/* Copyright 2002-2025 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.leastsquares;
  
  import org.hipparchus.geometry.euclidean.threed.Vector3D;
  import org.hipparchus.util.FastMath;
  import org.junit.jupiter.api.Assertions;
  import org.junit.jupiter.api.Test;
  import org.orekit.KeyValueFileParser;
  import org.orekit.Utils;
  import org.orekit.attitudes.AttitudeProvider;
  import org.orekit.bodies.CelestialBody;
  import org.orekit.bodies.OneAxisEllipsoid;
  import org.orekit.errors.OrekitException;
  import org.orekit.estimation.common.AbstractOrbitDetermination;
  import org.orekit.estimation.common.ParameterKey;
  import org.orekit.estimation.common.ResultSequentialBatchLeastSquares;
  import org.orekit.forces.ForceModel;
  import org.orekit.forces.drag.DragForce;
  import org.orekit.forces.drag.DragSensitive;
  import org.orekit.forces.empirical.AccelerationModel;
  import org.orekit.forces.empirical.ParametricAcceleration;
  import org.orekit.forces.empirical.PolynomialAccelerationModel;
  import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
  import org.orekit.forces.gravity.OceanTides;
  import org.orekit.forces.gravity.Relativity;
  import org.orekit.forces.gravity.SolidTides;
  import org.orekit.forces.gravity.ThirdBodyAttraction;
  import org.orekit.forces.gravity.potential.GravityFieldFactory;
  import org.orekit.forces.gravity.potential.ICGEMFormatReader;
  import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
  import org.orekit.forces.radiation.KnockeRediffusedForceModel;
  import org.orekit.forces.radiation.RadiationSensitive;
  import org.orekit.forces.radiation.SolarRadiationPressure;
  import org.orekit.models.earth.atmosphere.Atmosphere;
  import org.orekit.orbits.Orbit;
  import org.orekit.orbits.PositionAngleType;
  import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
  import org.orekit.propagation.conversion.ODEIntegratorBuilder;
  import org.orekit.time.TimeScalesFactory;
  import org.orekit.utils.IERSConventions;
  import org.orekit.utils.ParameterDriver;
  
  import java.io.File;
  import java.io.IOException;
  import java.net.URISyntaxException;
  import java.text.ParseException;
  import java.util.List;
  import java.util.NoSuchElementException;
  
  public class NumericalSequentialOrbitDeterminationTest extends AbstractOrbitDetermination<NumericalPropagatorBuilder> {
  
      /** Gravity field. */
      private NormalizedSphericalHarmonicsProvider gravityField;
  
      /** {@inheritDoc} */
      @Override
      protected void createGravityField(final KeyValueFileParser<ParameterKey> parser)
          throws NoSuchElementException {
  
          final int degree = parser.getInt(ParameterKey.CENTRAL_BODY_DEGREE);
          final int order  = FastMath.min(degree, parser.getInt(ParameterKey.CENTRAL_BODY_ORDER));
          gravityField = GravityFieldFactory.getNormalizedProvider(degree, order);
  
      }
  
      /** {@inheritDoc} */
      @Override
      protected double getMu() {
          return gravityField.getMu();
      }
  
      /** {@inheritDoc} */
      @Override
      protected NumericalPropagatorBuilder createPropagatorBuilder(final Orbit referenceOrbit,
                                                                   final ODEIntegratorBuilder builder,
                                                                   final double positionScale) {
          return new NumericalPropagatorBuilder(referenceOrbit, builder, PositionAngleType.MEAN,
                                                positionScale);
      }
  
      /** {@inheritDoc} */
      @Override
      protected void setMass(final NumericalPropagatorBuilder propagatorBuilder,
                                  final double mass) {
         propagatorBuilder.setMass(mass);
     }
 
     /** {@inheritDoc} */
     @Override
     protected List<ParameterDriver> setGravity(final NumericalPropagatorBuilder propagatorBuilder,
                                                final OneAxisEllipsoid body) {
         final ForceModel gravityModel = new HolmesFeatherstoneAttractionModel(body.getBodyFrame(), gravityField);
         propagatorBuilder.addForceModel(gravityModel);
         return gravityModel.getParametersDrivers();
     }
 
     /** {@inheritDoc} */
     @Override
     protected List<ParameterDriver> setOceanTides(final NumericalPropagatorBuilder propagatorBuilder,
                                                   final IERSConventions conventions,
                                                   final OneAxisEllipsoid body,
                                                   final int degree, final int order) {
         final ForceModel tidesModel = new OceanTides(body.getBodyFrame(),
                                                      gravityField.getAe(), gravityField.getMu(),
                                                      degree, order, conventions,
                                                      TimeScalesFactory.getUT1(conventions, true));
         propagatorBuilder.addForceModel(tidesModel);
         return tidesModel.getParametersDrivers();
     }
 
     /** {@inheritDoc} */
     @Override
     protected List<ParameterDriver> setSolidTides(final NumericalPropagatorBuilder propagatorBuilder,
                                                   final IERSConventions conventions,
                                                   final OneAxisEllipsoid body,
                                                   final CelestialBody[] solidTidesBodies) {
         final ForceModel tidesModel = new SolidTides(body.getBodyFrame(),
                                                      gravityField.getAe(), gravityField.getMu(),
                                                      gravityField.getTideSystem(), conventions,
                                                      TimeScalesFactory.getUT1(conventions, true),
                                                      solidTidesBodies);
         propagatorBuilder.addForceModel(tidesModel);
         return tidesModel.getParametersDrivers();
     }
 
     /** {@inheritDoc} */
     @Override
     protected List<ParameterDriver> setThirdBody(final NumericalPropagatorBuilder propagatorBuilder,
                                                  final CelestialBody thirdBody) {
         final ForceModel thirdBodyModel = new ThirdBodyAttraction(thirdBody);
         propagatorBuilder.addForceModel(thirdBodyModel);
         return thirdBodyModel.getParametersDrivers();
     }
 
     /** {@inheritDoc} */
     @Override
     protected List<ParameterDriver> setDrag(final NumericalPropagatorBuilder propagatorBuilder,
                                             final Atmosphere atmosphere, final DragSensitive spacecraft) {
         final ForceModel dragModel = new DragForce(atmosphere, spacecraft);
         propagatorBuilder.addForceModel(dragModel);
         return dragModel.getParametersDrivers();
     }
 
     /** {@inheritDoc} */
     @Override
     protected List<ParameterDriver> setSolarRadiationPressure(final NumericalPropagatorBuilder propagatorBuilder, final CelestialBody sun,
                                                               final OneAxisEllipsoid body, final RadiationSensitive spacecraft) {
         final ForceModel srpModel = new SolarRadiationPressure(sun, body, spacecraft);
         propagatorBuilder.addForceModel(srpModel);
         return srpModel.getParametersDrivers();
     }
 
     /** {@inheritDoc} */
     @Override
     protected List<ParameterDriver> setAlbedoInfrared(final NumericalPropagatorBuilder propagatorBuilder,
                                                       final CelestialBody sun, final double equatorialRadius,
                                                       final double angularResolution,
                                                       final RadiationSensitive spacecraft) {
         final ForceModel albedoIR = new KnockeRediffusedForceModel(sun, spacecraft, equatorialRadius, angularResolution);
         propagatorBuilder.addForceModel(albedoIR);
         return albedoIR.getParametersDrivers();
     }
 
     /** {@inheritDoc} */
     @Override
     protected List<ParameterDriver> setRelativity(final NumericalPropagatorBuilder propagatorBuilder) {
         final ForceModel relativityModel = new Relativity(gravityField.getMu());
         propagatorBuilder.addForceModel(relativityModel);
         return relativityModel.getParametersDrivers();
     }
 
     /** {@inheritDoc} */
     @Override
     protected List<ParameterDriver> setPolynomialAcceleration(final NumericalPropagatorBuilder propagatorBuilder,
                                                              final String name, final Vector3D direction, final int degree) {
         final AccelerationModel accModel = new PolynomialAccelerationModel(name, null, degree);
         final ForceModel polynomialModel = new ParametricAcceleration(direction, true, accModel);
         propagatorBuilder.addForceModel(polynomialModel);
         return polynomialModel.getParametersDrivers();
     }
 
     /** {@inheritDoc} */
     @Override
     protected void setAttitudeProvider(final NumericalPropagatorBuilder propagatorBuilder,
                                        final AttitudeProvider attitudeProvider) {
         propagatorBuilder.setAttitudeProvider(attitudeProvider);
     }
 
     @Test
     // Orbit determination for Lageos2 based on position measurements
     public void testLageos2()
         throws URISyntaxException, IllegalArgumentException, IOException,
                OrekitException, ParseException {
 
         // input in resources directory
         final String inputPathModel = NumericalSequentialOrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/Lageos2/sequential_od_test_Lageos.in").toURI().getPath();
         final File inputModel  = new File(inputPathModel);
 
         // configure Orekit data access
         Utils.setDataRoot("orbit-determination/february-2016:potential/icgem-format");
         GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
 
 
         //orbit determination run.
         final boolean print = false;
         ResultSequentialBatchLeastSquares odLageos2 = runSequentialBLS(inputModel, print);
 
         //test
         //definition of the accuracy for the test
         final double distanceAccuracyBLS = 0.13;
         final double distanceAccuracySBLS = 0.06;
 
         //test on the convergence BLS
         final int numberOfIte  = 4;
         final int numberOfEval = 4;
 
         Assertions.assertEquals(numberOfIte, odLageos2.getNumberOfIteration());
         Assertions.assertEquals(numberOfEval, odLageos2.getNumberOfEvaluation());
 
         //test on the convergence SBLS
         final int numberOfIteS  = 5;
         final int numberOfEvalS = 5;
 
         Assertions.assertEquals(numberOfIteS, odLageos2.getNumberOfIterationSequential());
         Assertions.assertEquals(numberOfEvalS, odLageos2.getNumberOfEvaluationSequential());
 
         //test on the estimated position
         final Vector3D estimatedPos = odLageos2.getEstimatedPV().getPosition();
         final Vector3D estimatedPosSequential = odLageos2.getEstimatedPVSequential().getPosition();
 
         // Ref position from "sequential_least_squares_lageos2_cpf_160213_5441.sgf"
         // Position in the file is given in ITRF. It is converted in EME2000 here
         final Vector3D refPos = new Vector3D(-8470598.591629019, -656367.1112940479, 8683152.425956512);
 
         Assertions.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), distanceAccuracyBLS);
         Assertions.assertEquals(0.0, Vector3D.distance(refPos, estimatedPosSequential), distanceAccuracySBLS);
 
        }
 }