/* Copyright 2002-2021 CS GROUP
    * Licensed to CS GROUP (CS) under one or more
    * contributor license agreements.  See the NOTICE file distributed with
    * this work for additional information regarding copyright ownership.
    * CS licenses this file to You 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 org.orekit.estimation.iod;
  
  import org.hipparchus.geometry.euclidean.threed.Vector3D;
  import org.hipparchus.util.FastMath;
  import org.orekit.errors.OrekitException;
  import org.orekit.errors.OrekitMessages;
  import org.orekit.estimation.measurements.PV;
  import org.orekit.estimation.measurements.Position;
  import org.orekit.frames.Frame;
  import org.orekit.orbits.KeplerianOrbit;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.utils.PVCoordinates;
  
  /**
   * Gibbs initial orbit determination.
   * An orbit is determined from three position vectors.
   *
   * Reference:
   *  Vallado, D., Fundamentals of Astrodynamics and Applications
   *
   * @author Joris Olympio
   * @since 8.0
   *
   */
  public class IodGibbs {
  
      /** Threshold for checking coplanr vectors. */
      private static final double COPLANAR_THRESHOLD = FastMath.toRadians(5.);
  
      /** gravitationnal constant. */
      private final double mu;
  
      /** Creator.
       *
       * @param mu gravitational constant
       */
      public IodGibbs(final double mu) {
          this.mu = mu;
      }
  
      /** Give an initial orbit estimation, assuming Keplerian motion.
       * All observations should be from the same location.
       *
       * @param frame measurements frame
       * @param p1 First position measurement
       * @param p2 Second position measurement
       * @param p3 Third position measurement
       * @return an initial orbit estimation
       * @since 11.0
       */
      public KeplerianOrbit estimate(final Frame frame, final Position p1, final Position p2, final Position p3) {
          return estimate(frame,
                          p1.getPosition(), p1.getDate(),
                          p2.getPosition(), p2.getDate(),
                          p3.getPosition(), p3.getDate());
      }
  
      /** Give an initial orbit estimation, assuming Keplerian motion.
       * All observations should be from the same location.
       *
       * @param frame measure frame
       * @param pv1 PV measure 1 taken in frame
       * @param pv2 PV measure 2 taken in frame
       * @param pv3 PV measure 3 taken in frame
       * @return an initial orbit estimation
       */
      public KeplerianOrbit estimate(final Frame frame, final PV pv1, final PV pv2, final PV pv3) {
          return estimate(frame,
                          pv1.getPosition(), pv1.getDate(),
                          pv2.getPosition(), pv2.getDate(),
                          pv3.getPosition(), pv3.getDate());
      }
  
      /** Give an initial orbit estimation, assuming Keplerian motion.
       * All observations should be from the same location.
       *
       * @param frame measure frame
       * @param r1 position 1 measured in frame
       * @param date1 date of measure 1
       * @param r2 position 2 measured in frame
       * @param date2 date of measure 2
       * @param r3 position 3 measured in frame
       * @param date3 date of measure 3
      * @return an initial orbit estimation
      */
     public KeplerianOrbit estimate(final Frame frame,
                                    final Vector3D r1, final AbsoluteDate date1,
                                    final Vector3D r2, final AbsoluteDate date2,
                                    final Vector3D r3, final AbsoluteDate date3) {
         // Checks measures are not at the same date
         if (date1.equals(date2) || date1.equals(date3) || date2.equals(date3)) {
             throw new OrekitException(OrekitMessages.NON_DIFFERENT_DATES_FOR_OBSERVATIONS, date1, date2, date3,
                     date2.durationFrom(date1), date3.durationFrom(date1), date3.durationFrom(date2));
         }
 
         // Checks measures are in the same plane
         final double num = r1.normalize().dotProduct(r2.normalize().crossProduct(r3.normalize()));
         final double alpha = FastMath.PI / 2.0 - FastMath.acos(num);
         if (FastMath.abs(alpha) > COPLANAR_THRESHOLD) {
             throw new OrekitException(OrekitMessages.NON_COPLANAR_POINTS);
         }
 
         final Vector3D D = r1.crossProduct(r2).add(r2.crossProduct(r3).add(r3.crossProduct(r1)));
 
         final Vector3D N = (r2.crossProduct(r3)).scalarMultiply(r1.getNorm())
                 .add((r3.crossProduct(r1)).scalarMultiply(r2.getNorm()))
                 .add((r1.crossProduct(r2)).scalarMultiply(r3.getNorm()));
 
         final Vector3D B = D.crossProduct(r2);
 
         final Vector3D S = r1.scalarMultiply(r2.getNorm() - r3.getNorm())
                 .add(r2.scalarMultiply(r3.getNorm() - r1.getNorm())
                      .add(r3.scalarMultiply(r1.getNorm() - r2.getNorm())));
 
         // middle velocity
         final double vm = FastMath.sqrt(mu / (N.getNorm() * D.getNorm()));
         final Vector3D vlEci = B.scalarMultiply(vm / r2.getNorm()).add(S.scalarMultiply(vm));
 
         // compile a new middle point with position, velocity
         final PVCoordinates pv = new PVCoordinates(r2, vlEci);
         final AbsoluteDate date = date2;
 
         // compute the equivalent Keplerian orbit
         final KeplerianOrbit orbit = new KeplerianOrbit(pv, frame, date, mu);
 
         //define the reverse orbit
         final PVCoordinates pv2 = new PVCoordinates(r2, vlEci.scalarMultiply(-1));
         final KeplerianOrbit orbit2 = new KeplerianOrbit(pv2, frame, date, mu);
 
         //check which orbit is correct
         final Vector3D estP3 = orbit.shiftedBy(date3.durationFrom(date2)).
                 getPVCoordinates().getPosition();
         final double dist = estP3.subtract(r3).getNorm();
         final Vector3D estP3_2 = orbit2.shiftedBy(date3.durationFrom(date2)).
                 getPVCoordinates().getPosition();
         final double dist2 = estP3_2.subtract(r3).getNorm();
 
         if (dist <= dist2) {
             return orbit;
         } else {
             return orbit2;
         }
     }
 }