/* Copyright 2022-2025 Luc Maisonobe
    * 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.CalculusFieldElement;
  import org.hipparchus.Field;
  import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
  import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
  import org.hipparchus.analysis.differentiation.UnivariateDerivative2Field;
  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.RotationConvention;
  import org.hipparchus.geometry.euclidean.threed.Vector3D;
  import org.orekit.annotation.DefaultDataContext;
  import org.orekit.bodies.OneAxisEllipsoid;
  import org.orekit.errors.OrekitException;
  import org.orekit.errors.OrekitMessages;
  import org.orekit.frames.Frame;
  import org.orekit.frames.FramesFactory;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.time.FieldAbsoluteDate;
  import org.orekit.utils.AngularCoordinates;
  import org.orekit.utils.ExtendedPositionProvider;
  import org.orekit.utils.FieldAngularCoordinates;
  import org.orekit.utils.FieldPVCoordinatesProvider;
  import org.orekit.utils.PVCoordinatesProvider;
  import org.orekit.utils.TimeStampedFieldPVCoordinates;
  import org.orekit.utils.TimeStampedPVCoordinates;
  
  import java.util.HashMap;
  import java.util.Map;
  
  /**
   * Attitude provider with one satellite vector aligned and another one constrained to two targets.
   * @author Luc Maisonobe
   * @since 12.2
   */
  public class AlignedAndConstrained implements AttitudeProvider
  {
  
      /** Satellite vector for primary target. */
      private final FieldVector3D<UnivariateDerivative2> primarySat;
  
      /** Primary target. */
      private final TargetProvider primaryTarget;
  
      /** Satellite vector for secondary target. */
      private final FieldVector3D<UnivariateDerivative2> secondarySat;
  
      /** Secondary target. */
      private final TargetProvider secondaryTarget;
  
      /** Sun model. */
      private final ExtendedPositionProvider sun;
  
      /** Earth model. */
      private final OneAxisEllipsoid earth;
  
      /** Reference inertial frame. */
      private final Frame inertialFrame;
  
      /** Cached field-based satellite vectors. */
      private final transient Map<Field<? extends CalculusFieldElement<?>>, Cache<? extends CalculusFieldElement<?>>>
          cachedSatelliteVectors;
  
      /**
       * Simple constructor.
       * @param primarySat      satellite vector for primary target
       * @param primaryTarget   primary target
       * @param secondarySat    satellite vector for secondary target
       * @param secondaryTarget secondary target
       * @param inertialFrame   reference inertial frame
       * @param sun             Sun model
       * @param earth           Earth model
       * @since 13.0
       */
      public AlignedAndConstrained(final Vector3D primarySat, final TargetProvider primaryTarget,
                                   final Vector3D secondarySat, final TargetProvider secondaryTarget,
                                   final Frame inertialFrame, final ExtendedPositionProvider sun,
                                   final OneAxisEllipsoid earth) {
          if (!inertialFrame.isPseudoInertial()) {
              throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, inertialFrame.getName());
          }
          this.primarySat             = new FieldVector3D<>(UnivariateDerivative2Field.getInstance(), primarySat);
         this.primaryTarget          = primaryTarget;
         this.secondarySat           = new FieldVector3D<>(UnivariateDerivative2Field.getInstance(), secondarySat);
         this.secondaryTarget        = secondaryTarget;
         this.inertialFrame          = inertialFrame;
         this.sun                    = sun;
         this.earth                  = earth;
         this.cachedSatelliteVectors = new HashMap<>();
     }
 
     /**
      * Constructor with default inertial frame.
      * @param primarySat      satellite vector for primary target
      * @param primaryTarget   primary target
      * @param secondarySat    satellite vector for secondary target
      * @param secondaryTarget secondary target
      * @param sun             Sun model
      * @param earth           Earth model
      */
     @DefaultDataContext
     public AlignedAndConstrained(final Vector3D primarySat, final TargetProvider primaryTarget,
                                  final Vector3D secondarySat, final TargetProvider secondaryTarget,
                                  final ExtendedPositionProvider sun,
                                  final OneAxisEllipsoid earth)
     {
         this(primarySat, primaryTarget, secondarySat, secondaryTarget, FramesFactory.getGCRF(), sun, earth);
     }
 
     /** {@inheritDoc} */
     @Override
     public Rotation getAttitudeRotation(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) {
         final TimeStampedPVCoordinates satPV = pvProv.getPVCoordinates(date, inertialFrame);
 
         // compute targets references at the specified date
         final Vector3D primaryDirection   = primaryTarget.getTargetDirection(sun, earth, satPV, inertialFrame);
         final Vector3D secondaryDirection = secondaryTarget.getTargetDirection(sun, earth, satPV, inertialFrame);
 
         // compute transform from inertial frame to satellite frame
         final Rotation rotation = new Rotation(primaryDirection, secondaryDirection, primarySat.toVector3D(),
                 secondarySat.toVector3D());
         if (inertialFrame != frame) {
             // prepend transform from specified frame to inertial frame
             final Rotation prepended = frame.getStaticTransformTo(inertialFrame, date).getRotation();
             return rotation.compose(prepended, RotationConvention.VECTOR_OPERATOR);
         }
         return rotation;
     }
 
     /** {@inheritDoc} */
     @Override
     public Attitude getAttitude(final PVCoordinatesProvider pvProv,
                                 final AbsoluteDate date,
                                 final Frame frame)
     {
         final TimeStampedPVCoordinates satPV = pvProv.getPVCoordinates(date, inertialFrame);
 
         // compute targets references at the specified date
         final FieldVector3D<UnivariateDerivative2> primaryDirection   = primaryTarget.getDerivative2TargetDirection(sun,
                 earth, satPV, inertialFrame);
         final FieldVector3D<UnivariateDerivative2> secondaryDirection = secondaryTarget.getDerivative2TargetDirection(sun,
                 earth, satPV, inertialFrame);
 
         // compute transform from inertial frame to satellite frame
         final FieldRotation<UnivariateDerivative2> inertToSatRotation =
             new FieldRotation<>(primaryDirection, secondaryDirection, primarySat, secondarySat);
 
         // build the angular coordinates
         final AngularCoordinates angularCoordinates = new AngularCoordinates(inertToSatRotation);
         final Attitude attitude = new Attitude(date, inertialFrame, angularCoordinates);
         return attitude.withReferenceFrame(frame);
     }
 
     /** {@inheritDoc} */
     @Override
     public <T extends CalculusFieldElement<T>> FieldRotation<T> getAttitudeRotation(final FieldPVCoordinatesProvider<T> pvProv,
                                                                                     final FieldAbsoluteDate<T> date,
                                                                                     final Frame frame) {
         final TimeStampedFieldPVCoordinates<T> satPV = pvProv.getPVCoordinates(date, inertialFrame);
 
         // compute targets references at the specified date
         final FieldVector3D<T> primaryDirection   = primaryTarget.getTargetDirection(sun, earth, satPV, inertialFrame);
         final FieldVector3D<T> secondaryDirection = secondaryTarget.getTargetDirection(sun, earth, satPV, inertialFrame);
 
         // compute transform from inertial frame to satellite frame
         final Field<T> field = date.getField();
         final FieldRotation<T> rotation = new FieldRotation<>(primaryDirection, secondaryDirection,
                 new FieldVector3D<>(field, primarySat.toVector3D()), new FieldVector3D<>(field, secondarySat.toVector3D()));
         if (inertialFrame != frame) {
             // prepend transform from specified frame to inertial frame
             final FieldRotation<T> prepended = frame.getStaticTransformTo(inertialFrame, date).getRotation();
             return rotation.compose(prepended, RotationConvention.VECTOR_OPERATOR);
         }
         return rotation;
     }
 
     /** {@inheritDoc} */
     @Override
     public <T extends CalculusFieldElement<T>> FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
                                                                             final FieldAbsoluteDate<T> date,
                                                                             final Frame frame)
     {
         // get the satellite vectors for specified field
         @SuppressWarnings("unchecked")
         final Cache<T> satVectors =
             (Cache<T>) cachedSatelliteVectors.computeIfAbsent(date.getField(),
                                                               f -> new Cache<>(date.getField(), primarySat, secondarySat));
 
         final TimeStampedFieldPVCoordinates<T> satPV = pvProv.getPVCoordinates(date, inertialFrame);
 
         // compute targets references at the specified date
         final FieldVector3D<FieldUnivariateDerivative2<T>> primaryDirection   = primaryTarget.getDerivative2TargetDirection(sun,
                 earth, satPV, inertialFrame);
         final FieldVector3D<FieldUnivariateDerivative2<T>> secondaryDirection = secondaryTarget.getDerivative2TargetDirection(sun,
                 earth, satPV, inertialFrame);
 
         // compute transform from inertial frame to satellite frame
         final FieldRotation<FieldUnivariateDerivative2<T>> inertToSatRotation =
             new FieldRotation<>(primaryDirection, secondaryDirection, satVectors.primarySat, satVectors.secondarySat);
 
         // build the attitude
         final FieldAngularCoordinates<T> angularCoordinates = new FieldAngularCoordinates<>(inertToSatRotation);
         final FieldAttitude<T> attitude = new FieldAttitude<>(date, inertialFrame, angularCoordinates);
         return attitude.withReferenceFrame(frame);
     }
 
     /** Container for cached satellite vectors. */
     private static class Cache<T extends CalculusFieldElement<T>> {
 
         /** Satellite vector for primary target. */
         private final FieldVector3D<FieldUnivariateDerivative2<T>> primarySat;
 
         /** Satellite vector for primary target. */
         private final FieldVector3D<FieldUnivariateDerivative2<T>> secondarySat;
 
         /** Simple constructor.
          * @param field field to which the elements belong
          * @param primarySat satellite vector for primary target
          * @param secondarySat satellite vector for primary target
          */
         Cache(final Field<T> field,
               final FieldVector3D<UnivariateDerivative2> primarySat,
               final FieldVector3D<UnivariateDerivative2> secondarySat) {
             final FieldUnivariateDerivative2<T> zero =
                 new FieldUnivariateDerivative2<>(field.getZero(), field.getZero(), field.getZero());
             this.primarySat   = new FieldVector3D<>(zero.newInstance(primarySat.getX().getValue()),
                                                     zero.newInstance(primarySat.getY().getValue()),
                                                     zero.newInstance(primarySat.getZ().getValue()));
             this.secondarySat = new FieldVector3D<>(zero.newInstance(secondarySat.getX().getValue()),
                                                     zero.newInstance(secondarySat.getY().getValue()),
                                                     zero.newInstance(secondarySat.getZ().getValue()));
         }
 
     }
 
 }