/*
    * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
    *
    * Licensed under the Apache License, Version 2.0 (the "License");
    * you may not use this file except in compliance with the License.
    * You may obtain a copy of the License at
    *
    *         http://www.apache.org/licenses/LICENSE-2.0
    *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  package com.irurueta.ar.sfm;
  
  import com.irurueta.geometry.CoordinatesType;
  import com.irurueta.geometry.PinholeCamera;
  import com.irurueta.geometry.Point2D;
  import com.irurueta.geometry.Point3D;
  import com.irurueta.geometry.estimators.LockedException;
  import com.irurueta.geometry.estimators.NotReadyException;
  
  import java.util.List;
  
  /**
   * Base class to triangulate matched 2D points into a single 3D one by using
   * 2D points correspondences on different views along with the corresponding
   * cameras on each of those views.
   * Subclasses will implement different types of triangulators that can provide
   * either LMSE or weighted solutions using either homogeneous or inhomogeneous
   * systems of equations.
   * Inhomogeneous methods are suitable only for cases where finite points and
   * cameras are being used. If points or cameras are located very far or at
   * infinity, triangulation will fail when using inhomogeneous methods.
   * Homogeneous methods are suitable for any case, however, if points and
   * cameras are close and well-defined, inhomogeneous methods might yield better
   * accuracy (although the difference is minimal).
   */
  public abstract class SinglePoint3DTriangulator {
  
      /**
       * Default triangulator type.
       */
      public static final Point3DTriangulatorType DEFAULT_TYPE = Point3DTriangulatorType.LMSE_HOMOGENEOUS_TRIANGULATOR;
  
      /**
       * Minimum required number of views to triangulate 3D points.
       */
      public static final int MIN_REQUIRED_VIEWS = 2;
  
      /**
       * Matched 2D points. Each point in the list is assumed to be projected by
       * the corresponding camera in the list.
       */
      protected List<Point2D> points2D;
  
      /**
       * List of cameras associated to the matched 2D point on the same position
       * as the camera on the list.
       */
      protected List<PinholeCamera> cameras;
  
      /**
       * Listener to handle events generated by instances of this class.
       */
      protected SinglePoint3DTriangulatorListener listener;
  
      /**
       * Indicates whether this instance is locked doing computations.
       */
      protected boolean locked;
  
      /**
       * Constructor.
       */
      protected SinglePoint3DTriangulator() {
      }
  
      /**
       * Constructor.
       *
       * @param points2D list of matched 2D points on each view. Each point in the
       *                 list is assumed to be projected by the corresponding camera in the list.
       * @param cameras  camera for each view where 2D points are represented.
       * @throws IllegalArgumentException if provided lists don't have the same
       *                                  length or their length is less than 2 views, which is the minimum
       *                                  required to compute triangulation.
       */
      protected SinglePoint3DTriangulator(final List<Point2D> points2D, final List<PinholeCamera> cameras) {
          internalSetPointsAndCameras(points2D, cameras);
      }
  
      /**
       * Constructor.
       *
       * @param listener listener to notify events generated by instances of this
       *                 class.
      */
     protected SinglePoint3DTriangulator(final SinglePoint3DTriangulatorListener listener) {
         this.listener = listener;
     }
 
     /**
      * Constructor.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  cameras for each view where 2D points are represented.
      * @param listener listener to notify events generated by instances of this
      *                 class.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  length or their length is less than 2 views, which is the minimum
      *                                  required to compute triangulation.
      */
     protected SinglePoint3DTriangulator(final List<Point2D> points2D, final List<PinholeCamera> cameras,
                                         final SinglePoint3DTriangulatorListener listener) {
         this(points2D, cameras);
         this.listener = listener;
     }
 
     /**
      * Returns list of matched 2D points on each view. Each point in the list is
      * assumed to be projected by the corresponding camera.
      *
      * @return list of matched 2D points on each view.
      */
     public List<Point2D> getPoints2D() {
         return points2D;
     }
 
     /**
      * Returns cameras for each view where 2D points are represented.
      *
      * @return cameras for each view where 2D points are represented.
      */
     public List<PinholeCamera> getCameras() {
         return cameras;
     }
 
     /**
      * Sets list of matched 2D points for each view and their corresponding
      * cameras used to project them.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  cameras for each view where 2D points are represented.
      * @throws LockedException          if this instance is locked.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  length or their length is less than 2 views, which is the minimum
      *                                  required to compute triangulation.
      */
     public void setPointsAndCameras(final List<Point2D> points2D, final List<PinholeCamera> cameras)
             throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         internalSetPointsAndCameras(points2D, cameras);
     }
 
     /**
      * Indicates whether this instance is locked because computations are being
      * done.
      *
      * @return true if instance is locked, false otherwise.
      */
     public boolean isLocked() {
         return locked;
     }
 
     /**
      * Indicates whether this instance is ready to start the triangulation.
      * An instance is ready when both lists of 2D points and cameras are
      * provided, both lists have the same length and at least data for 2 views
      * is provided.
      *
      * @return true if this instance is ready, false otherwise.
      */
     public boolean isReady() {
         return areValidPointsAndCameras(points2D, cameras);
     }
 
     /**
      * Returns listener to be notified of events generated by instances of this
      * class.
      *
      * @return listener to be notified of events generated by instances of this
      * class.
      */
     public SinglePoint3DTriangulatorListener getListener() {
         return listener;
     }
 
     /**
      * Sets listener to be notified of events generated by instances of this
      * class.
      *
      * @param listener listener to be notified of events generated by instances
      *                 of this class.
      * @throws LockedException if this instance is locked.
      */
     public void setListener(final SinglePoint3DTriangulatorListener listener) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
 
         this.listener = listener;
     }
 
     /**
      * Triangulates provided matched 2D points being projected by each
      * corresponding camera into a single 3D point.
      * At least 2 matched 2D points and their corresponding 2 cameras are
      * required to compute triangulation. If more views are provided, an
      * averaged solution can be found.
      *
      * @return computed triangulated 3D point.
      * @throws LockedException               if this instance is locked.
      * @throws NotReadyException             if lists of points and cameras don't have the
      *                                       same length or less than 2 views are provided.
      * @throws Point3DTriangulationException if triangulation fails for some
      *                                       other reason (i.e. degenerate geometry, numerical
      *                                       instabilities, etc.).
      */
     public Point3D triangulate() throws LockedException, NotReadyException, Point3DTriangulationException {
         final var result = Point3D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
         triangulate(result);
         return result;
     }
 
     /**
      * Triangulates provided matched 2D points being projected by each
      * corresponding camera into a single 3D point.
      * At least 2 matched 2D points and their corresponding 2 cameras are
      * required to compute triangulation. If more views are provided, an
      * averaged solution can be found.
      *
      * @param result instance where data for triangulated 3D point is stored.
      * @throws LockedException               if this instance is locked.
      * @throws NotReadyException             if lists of points and cameras don't have the
      *                                       same length or less than 2 views are provided.
      * @throws Point3DTriangulationException if triangulation fails for some
      *                                       other reason (i.e. degenerate geometry, numerical
      *                                       instabilities, etc.).
      */
     public void triangulate(final Point3D result) throws LockedException, NotReadyException,
             Point3DTriangulationException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (!isReady()) {
             throw new NotReadyException();
         }
 
         triangulate(points2D, cameras, result);
     }
 
     /**
      * Indicates whether provided points and cameras are valid to start the
      * triangulation.
      * In order to triangulate points, at least two cameras and their
      * corresponding 2 matched 2D points are required.
      * If more views are provided, an averaged solution can be found.
      *
      * @param points2D list of matched points on each view.
      * @param cameras  cameras for each view where 2D points are represented.
      * @return true if data is enough to start triangulation, false otherwise.
      */
     public static boolean areValidPointsAndCameras(final List<Point2D> points2D, final List<PinholeCamera> cameras) {
         return points2D != null && cameras != null && points2D.size() == cameras.size()
                 && points2D.size() >= MIN_REQUIRED_VIEWS;
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided type.
      *
      * @param type a triangulator type.
      * @return a 3D point triangulator instance.
      */
     public static SinglePoint3DTriangulator create(final Point3DTriangulatorType type) {
         return switch (type) {
             case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator();
             case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator();
             case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator();
             default -> new LMSEHomogeneousSinglePoint3DTriangulator();
         };
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided lists of
      * points and corresponding cameras along with provided type.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  camera for each view where 2D points are represented.
      * @param type     a triangulator type.
      * @return a 3D point triangulator instance.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  length or their length is less than 2 views, which is the minimum
      *                                  required to compute triangulation.
      */
     public static SinglePoint3DTriangulator create(
             final List<Point2D> points2D, final List<PinholeCamera> cameras, final Point3DTriangulatorType type) {
         return switch (type) {
             case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(points2D,
                     cameras);
             case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(points2D,
                     cameras);
             case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(points2D,
                     cameras);
             default -> new LMSEHomogeneousSinglePoint3DTriangulator(points2D, cameras);
         };
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided lists of
      * points, weights and corresponding cameras along with provided type.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  camera for each view where 2D points are represented.
      * @param weights  weights assigned to each view.
      * @param type     a triangulator type.
      * @return a 3D point triangulator instance.
      * @throws IllegalArgumentException if provided lists or weights don't have
      *                                  the same length or their length is less than 2 views, which is the
      *                                  minimum required to compute triangulation.
      */
     public static SinglePoint3DTriangulator create(
             final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights,
             final Point3DTriangulatorType type) {
         return switch (type) {
             case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(points2D,
                     cameras, weights);
             case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(points2D,
                     cameras, weights);
             case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(points2D, cameras);
             default -> new LMSEHomogeneousSinglePoint3DTriangulator(points2D, cameras);
         };
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided listener and
      * type.
      *
      * @param listener listener to notify events generated by instances of this
      *                 class.
      * @param type     a triangulator type.
      * @return a 3D point triangulator instance.
      */
     public static SinglePoint3DTriangulator create(
             final SinglePoint3DTriangulatorListener listener, final Point3DTriangulatorType type) {
         return switch (type) {
             case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(listener);
             case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(listener);
             case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(listener);
             default -> new LMSEHomogeneousSinglePoint3DTriangulator(listener);
         };
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided lists of
      * points and corresponding cameras, listener and provided type.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  camera for each view where 2D points are represented.
      * @param listener listener to notify events generated by instances of this
      *                 class.
      * @param type     a triangulator type.
      * @return a 3D point triangulator instance.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  length or their length is less than 2 views, which is the minimum
      *                                  required to compute triangulation.
      */
     public static SinglePoint3DTriangulator create(
             final List<Point2D> points2D, final List<PinholeCamera> cameras,
             final SinglePoint3DTriangulatorListener listener, final Point3DTriangulatorType type) {
         return switch (type) {
             case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(points2D,
                     cameras, listener);
             case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(points2D,
                     cameras, listener);
             case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(points2D, cameras,
                     listener);
             default -> new LMSEHomogeneousSinglePoint3DTriangulator(points2D, cameras, listener);
         };
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided lists of
      * points, weights, corresponding cameras, listener and provided type.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  camera for each view where 2D points are represented.
      * @param weights  weights assigned to each view.
      * @param listener listener to notify events generated by instances of this
      *                 class.
      * @param type     a triangulator type.
      * @return a 3D point triangulator instance.
      * @throws IllegalArgumentException if provided lists or weights don't have
      *                                  the same length or their length is less than 2 views, which is the
      *                                  minimum required to compute triangulation.
      */
     public static SinglePoint3DTriangulator create(
             final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights,
             final SinglePoint3DTriangulatorListener listener, final Point3DTriangulatorType type) {
         return switch (type) {
             case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(points2D,
                     cameras, weights, listener);
             case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(points2D,
                     cameras, weights, listener);
             case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(points2D, cameras,
                     listener);
             default -> new LMSEHomogeneousSinglePoint3DTriangulator(points2D, cameras, listener);
         };
     }
 
     /**
      * Creates a new 3D point triangulator instance using default type.
      *
      * @return a 3D point triangulator instance.
      */
     public static SinglePoint3DTriangulator create() {
         return create(DEFAULT_TYPE);
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided lists of
      * points and corresponding cameras along with default type.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  camera for each view where 2D points are represented.
      * @return a 3D point triangulator instance.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  length or their length is less than 2 views, which is the minimum
      *                                  required to compute triangulation.
      */
     public static SinglePoint3DTriangulator create(final List<Point2D> points2D, final List<PinholeCamera> cameras) {
         return create(points2D, cameras, DEFAULT_TYPE);
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided lists of
      * points, weights and corresponding cameras along with default type.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  camera for each view where 2D points are represented.
      * @param weights  weights assigned to each view.
      * @return a 3D point triangulator instance.
      * @throws IllegalArgumentException if provided lists or weights don't have
      *                                  the same length or their length is less than 2 views, which is the
      *                                  minimum required to compute triangulation.
      */
     public static SinglePoint3DTriangulator create(
             final List<Point2D> points2D, final List<PinholeCamera> cameras, double[] weights) {
         return create(points2D, cameras, weights, DEFAULT_TYPE);
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided listener and
      * default type.
      *
      * @param listener listener to notify events generated by instances of this
      *                 class.
      * @return a 3D point triangulator instance.
      */
     public static SinglePoint3DTriangulator create(final SinglePoint3DTriangulatorListener listener) {
         return create(listener, DEFAULT_TYPE);
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided lists of
      * points and corresponding cameras, listener and default type.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  camera for each view where 2D points are represented.
      * @param listener listener to notify events generated by instances of this
      *                 class.
      * @return a 3D point triangulator instance.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  length or their length is less than 2 views, which is the minimum
      *                                  required to compute triangulation.
      */
     public static SinglePoint3DTriangulator create(
             final List<Point2D> points2D, final List<PinholeCamera> cameras,
             final SinglePoint3DTriangulatorListener listener) {
         return create(points2D, cameras, listener, DEFAULT_TYPE);
     }
 
     /**
      * Creates a new 3D point triangulator instance using provided lists of
      * points, weights, corresponding cameras, listener and default type.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  camera for each view where 2D points are represented.
      * @param weights  weights assigned to each view.
      * @param listener listener to notify events generated by instances of this
      *                 class.
      * @return a 3D point triangulator instance.
      * @throws IllegalArgumentException if provided lists or weights don't have
      *                                  the same length or their length is less than 2 views, which is the
      *                                  minimum required to compute triangulation.
      */
     public static SinglePoint3DTriangulator create(final List<Point2D> points2D,
                                                    final List<PinholeCamera> cameras,
                                                    final double[] weights,
                                                    final SinglePoint3DTriangulatorListener listener) {
         return create(points2D, cameras, weights, listener, DEFAULT_TYPE);
     }
 
     /**
      * Returns type of triangulator (a combination of homogeneous or
      * inhomogeneous type along with an LMSE or weighted strategy).
      *
      * @return type of triangulator.
      */
     public abstract Point3DTriangulatorType getType();
 
     /**
      * Internal method to triangulate provided matched 2D points being projected
      * by each corresponding camera into a single 3D point.
      * At least 2 matched 2D points and their corresponding 2 cameras are
      * required to compute triangulation. If more views are provided, an
      * averaged solution is found.
      * This method does not check whether instance is locked or ready.
      *
      * @param points2D matched 2D points. Each point in the list is assumed to
      *                 be projected by the corresponding camera in the list.
      * @param cameras  list of cameras associated to the matched 2D point on the
      *                 same position as the camera on the list.
      * @param result   instance where triangulated 3D point is stored.
      * @throws Point3DTriangulationException if triangulation fails for some
      *                                       other reason (i.e. degenerate geometry, numerical
      *                                       instabilities, etc.).
      */
     protected abstract void triangulate(
             final List<Point2D> points2D, final List<PinholeCamera> cameras, final Point3D result)
             throws Point3DTriangulationException;
 
     /**
      * Internal method to sets list of matched 2D points for each view and their
      * corresponding cameras used to project them.
      * This method does not check whether instance is locked.
      *
      * @param points2D list of matched 2D points on each view. Each point in the
      *                 list is assumed to be projected by the corresponding camera in the list.
      * @param cameras  cameras for each view where 2D points are represented.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  length or their length is less than 2 views, which is the minimum
      *                                  required to compute triangulation.
      */
     private void internalSetPointsAndCameras(final List<Point2D> points2D, final List<PinholeCamera> cameras) {
         if (!areValidPointsAndCameras(points2D, cameras)) {
             throw new IllegalArgumentException();
         }
 
         this.points2D = points2D;
         this.cameras = cameras;
     }
 }