/* Copyright 2002-2024 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.attitudes;
  
  import org.hipparchus.Field;
  import org.hipparchus.CalculusFieldElement;
  import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  import org.hipparchus.geometry.euclidean.threed.Rotation;
  import org.hipparchus.geometry.euclidean.threed.Vector3D;
  import org.orekit.frames.FieldStaticTransform;
  import org.orekit.frames.FieldTransform;
  import org.orekit.frames.Frame;
  import org.orekit.frames.StaticTransform;
  import org.orekit.frames.Transform;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.time.FieldAbsoluteDate;
  import org.orekit.utils.FieldPVCoordinates;
  import org.orekit.utils.FieldPVCoordinatesProvider;
  import org.orekit.utils.PVCoordinates;
  import org.orekit.utils.PVCoordinatesProvider;
  
  
  /**
   * This class handles a celestial body pointed attitude provider.
   * <p>The celestial body pointed law is defined by two main elements:
   * <ul>
   *   <li>a celestial body towards which some satellite axis is exactly aimed</li>
   *   <li>a phasing reference defining the rotation around the pointing axis</li>
   * </ul>
   *
   * <p>
   * The celestial body implicitly defines two of the three degrees of freedom
   * and the phasing reference defines the remaining degree of freedom. This definition
   * can be represented as first aligning exactly the satellite pointing axis to
   * the current direction of the celestial body, and then to find the rotation
   * around this axis such that the satellite phasing axis is in the half-plane
   * defined by a cut line on the pointing axis and containing the celestial
   * phasing reference.
   * </p>
   * <p>
   * In order for this definition to work, the user must ensure that the phasing
   * reference is <strong>never</strong> aligned with the pointing reference.
   * Since the pointed body moves as the date changes, this should be ensured
   * regardless of the date. A simple way to do this for Sun, Moon or any planet
   * pointing is to choose a phasing reference far from the ecliptic plane. Using
   * <code>Vector3D.PLUS_K</code>, the equatorial pole, is perfect in these cases.
   * </p>
   * <p>Instances of this class are guaranteed to be immutable.</p>
   * @author Luc Maisonobe
   */
  public class CelestialBodyPointed implements AttitudeProvider {
  
      /** Frame in which {@link #phasingCel} is defined. */
      private final Frame celestialFrame;
  
      /** Celestial body to point at. */
      private final PVCoordinatesProvider pointedBody;
  
      /** Phasing reference, in celestial frame. */
      private final Vector3D phasingCel;
  
      /** Satellite axis aiming at the pointed body, in satellite frame. */
      private final Vector3D pointingSat;
  
      /** Phasing reference, in satellite frame. */
      private final Vector3D phasingSat;
  
      /** Creates new instance.
       * @param celestialFrame frame in which <code>phasingCel</code> is defined
       * @param pointedBody celestial body to point at
       * @param phasingCel phasing reference, in celestial frame
       * @param pointingSat satellite vector defining the pointing direction
       * @param phasingSat phasing reference, in satellite frame
       */
      public CelestialBodyPointed(final Frame celestialFrame,
                                  final PVCoordinatesProvider pointedBody,
                                  final Vector3D phasingCel,
                                  final Vector3D pointingSat,
                                  final Vector3D phasingSat) {
          this.celestialFrame = celestialFrame;
          this.pointedBody    = pointedBody;
          this.phasingCel     = phasingCel;
          this.pointingSat    = pointingSat;
          this.phasingSat     = phasingSat;
     }
 
     /** {@inheritDoc} */
     @Override
     public Attitude getAttitude(final PVCoordinatesProvider pvProv,
                                 final AbsoluteDate date, final Frame frame) {
 
         final PVCoordinates satPV = pvProv.getPVCoordinates(date, celestialFrame);
 
         // compute celestial references at the specified date
         final PVCoordinates bodyPV    = pointedBody.getPVCoordinates(date, celestialFrame);
         final PVCoordinates pointing  = new PVCoordinates(satPV, bodyPV);
         final Vector3D      pointingP = pointing.getPosition();
         final double        r2        = Vector3D.dotProduct(pointingP, pointingP);
 
         // evaluate instant rotation axis due to sat and body motion only (no phasing yet)
         final Vector3D rotAxisCel = new Vector3D(1 / r2, Vector3D.crossProduct(pointingP, pointing.getVelocity()));
 
         // fix instant rotation to take phasing constraint into account
         // (adding a rotation around pointing axis ensuring the motion of the phasing axis
         //  is constrained in the pointing-phasing plane)
         final Vector3D v1    = Vector3D.crossProduct(rotAxisCel, phasingCel);
         final Vector3D v2    = Vector3D.crossProduct(pointingP,  phasingCel);
         final double   compensation = -Vector3D.dotProduct(v1, v2) / v2.getNormSq();
         final Vector3D phasedRotAxisCel = new Vector3D(1.0, rotAxisCel, compensation, pointingP);
 
         // compute transform from celestial frame to satellite frame
         final Rotation celToSatRotation =
             new Rotation(pointingP, phasingCel, pointingSat, phasingSat);
 
         // build transform combining rotation and instant rotation axis
         Transform transform = new Transform(date, celToSatRotation, celToSatRotation.applyTo(phasedRotAxisCel));
         if (frame != celestialFrame) {
             // prepend transform from specified frame to celestial frame
             transform = new Transform(date, frame.getTransformTo(celestialFrame, date), transform);
         }
 
         // build the attitude
         return new Attitude(date, frame, transform.getRotation(), transform.getRotationRate(), transform.getRotationAcceleration());
 
     }
 
     /** {@inheritDoc} */
     @Override
     public Rotation getAttitudeRotation(final PVCoordinatesProvider pvProv,
                                         final AbsoluteDate date,
                                         final Frame frame) {
         final Vector3D satPosition = pvProv.getPosition(date, celestialFrame);
 
         // compute celestial references at the specified date
         final Vector3D bodyPosition    = pointedBody.getPosition(date, celestialFrame);
         final Vector3D      pointingP  = bodyPosition.subtract(satPosition);
 
         // compute static transform from celestial frame to satellite frame
         final Rotation celToSatRotation = new Rotation(pointingP, phasingCel, pointingSat, phasingSat);
         StaticTransform staticTransform = StaticTransform.of(date, celToSatRotation);
 
         if (frame != celestialFrame) {
             // prepend static transform from specified frame to celestial frame
             staticTransform = StaticTransform.compose(date, frame.getStaticTransformTo(celestialFrame, date), staticTransform);
         }
         return staticTransform.getRotation();
     }
 
     /** {@inheritDoc} */
     @Override
     public <T extends CalculusFieldElement<T>> FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
                                                                             final FieldAbsoluteDate<T> date,
                                                                             final Frame frame) {
 
         final Field<T> field = date.getField();
         final FieldPVCoordinates<T> satPV = pvProv.getPVCoordinates(date, celestialFrame);
 
         // compute celestial references at the specified date
         final FieldPVCoordinates<T> bodyPV    = new FieldPVCoordinates<>(field,
                                                                          pointedBody.getPVCoordinates(date.toAbsoluteDate(),
                                                                                                       celestialFrame));
         final FieldPVCoordinates<T> pointing  = new FieldPVCoordinates<>(satPV, bodyPV);
         final FieldVector3D<T>      pointingP = pointing.getPosition();
         final T                     r2        = FieldVector3D.dotProduct(pointingP, pointingP);
 
         // evaluate instant rotation axis due to sat and body motion only (no phasing yet)
         final FieldVector3D<T> rotAxisCel =
             new FieldVector3D<>(r2.reciprocal(), FieldVector3D.crossProduct(pointingP, pointing.getVelocity()));
 
         // fix instant rotation to take phasing constraint into account
         // (adding a rotation around pointing axis ensuring the motion of the phasing axis
         //  is constrained in the pointing-phasing plane)
         final FieldVector3D<T> v1           = FieldVector3D.crossProduct(rotAxisCel, phasingCel);
         final FieldVector3D<T> v2           = FieldVector3D.crossProduct(pointingP,  phasingCel);
         final T                compensation = FieldVector3D.dotProduct(v1, v2).negate().divide(v2.getNormSq());
         final FieldVector3D<T> phasedRotAxisCel = new FieldVector3D<>(field.getOne(), rotAxisCel, compensation, pointingP);
 
         // compute transform from celestial frame to satellite frame
         final FieldRotation<T> celToSatRotation =
             new FieldRotation<>(pointingP, new FieldVector3D<>(field, phasingCel),
                             new FieldVector3D<>(field, pointingSat), new FieldVector3D<>(field, phasingSat));
 
         // build transform combining rotation and instant rotation axis
         FieldTransform<T> transform = new FieldTransform<>(date, celToSatRotation, celToSatRotation.applyTo(phasedRotAxisCel));
         if (frame != celestialFrame) {
             // prepend transform from specified frame to celestial frame
             transform = new FieldTransform<>(date, frame.getTransformTo(celestialFrame, date), transform);
         }
 
         // build the attitude
         return new FieldAttitude<>(date, frame,
                         transform.getRotation(), transform.getRotationRate(), transform.getRotationAcceleration());
 
     }
 
     /** {@inheritDoc} */
     @Override
     public <T extends CalculusFieldElement<T>> FieldRotation<T> getAttitudeRotation(final FieldPVCoordinatesProvider<T> pvProv,
                                                                                     final FieldAbsoluteDate<T> date,
                                                                                     final Frame frame) {
         final Field<T> field = date.getField();
         final FieldVector3D<T> satPosition = pvProv.getPosition(date, celestialFrame);
 
         // compute celestial references at the specified date
         final FieldVector3D<T> bodyPosition    = new FieldVector3D<>(field,
                 pointedBody.getPosition(date.toAbsoluteDate(), celestialFrame));
         final FieldVector3D<T>      pointingP = bodyPosition.subtract(satPosition);
 
         // compute rotation from celestial frame to satellite frame
         final FieldRotation<T> celToSatRotation =
                 new FieldRotation<>(pointingP, new FieldVector3D<>(field, phasingCel),
                         new FieldVector3D<>(field, pointingSat), new FieldVector3D<>(field, phasingSat));
 
         // build static transform combining rotation and instant rotation axis
         FieldStaticTransform<T> staticTransform = FieldStaticTransform.of(date, celToSatRotation);
         if (frame != celestialFrame) {
             // prepend static transform from specified frame to celestial frame
             staticTransform = FieldStaticTransform.compose(date, frame.getStaticTransformTo(celestialFrame, date), staticTransform);
         }
         return staticTransform.getRotation();
     }
 }