// Path: GNSSLogger/pseudorange/src/main/java/com/google/location/lbs/gnss/gps/pseudorange/Ecef2LlaConverter.java
// public static class GeodeticLlaValues {
// 
//   public final double latitudeRadians;
//   public final double longitudeRadians;
//   public final double altitudeMeters;
// 
//   public GeodeticLlaValues(double latitudeRadians,
//       double longitudeRadians, double altitudeMeters) {
//     this.latitudeRadians = latitudeRadians;
//     this.longitudeRadians = longitudeRadians;
//     this.altitudeMeters = altitudeMeters;
//   }
// }


import com.google.location.lbs.gnss.gps.pseudorange.Ecef2LlaConverter.GeodeticLlaValues;
import org.apache.commons.math3.linear.RealMatrix;
/*
 * Copyright (C) 2017 The Android Open Source Project
 *
 * 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.google.location.lbs.gnss.gps.pseudorange;


/**
 * Transformations from ECEF coordinates to Topocentric coordinates
 */
public class EcefToTopocentricConverter {
  private static final double MIN_DISTANCE_MAGNITUDE_METERS = 1.0e-22;
  private static final int EAST_IDX = 0;
  private static final int NORTH_IDX = 1;
  private static final int UP_IDX = 2;

  /**
   * Transformation of {@code inputVectorMeters} with origin at {@code originECEFMeters} into
   * topocentric coordinate system. The result is {@code TopocentricAEDValues} containing azimuth
   * from north +ve clockwise, radians; elevation angle, radians; distance, vector length meters
   *
   * <p>Source: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
   * http://kom.aau.dk/~borre/life-l99/topocent.m
   *
   */
  public static TopocentricAEDValues convertCartesianToTopocentricRadMeters(
      final double[] originECEFMeters, final double[] inputVectorMeters) {

    GeodeticLlaValues latLngAlt = Ecef2LlaConverter.convertECEFToLLACloseForm(originECEFMeters[0],
        originECEFMeters[1], originECEFMeters[2]);

    RealMatrix rotationMatrix =
        Ecef2EnuConverter.
            getRotationMatrix(latLngAlt.latitudeRadians, latLngAlt.longitudeRadians).transpose();
    double[] eastNorthUpVectorMeters = GpsMathOperations.matrixByColVectMultiplication(
        rotationMatrix.transpose().getData(), inputVectorMeters);
    double eastMeters = eastNorthUpVectorMeters[EAST_IDX];
    double northMeters = eastNorthUpVectorMeters[NORTH_IDX];
    double upMeters = eastNorthUpVectorMeters[UP_IDX];

    // calculate azimuth, elevation and height from the ENU values
    double horizontalDistanceMeters = Math.hypot(eastMeters, northMeters);
// Path: GNSSLogger/pseudorange/src/main/java/com/google/location/lbs/gnss/gps/pseudorange/Ecef2LlaConverter.java
// public static class GeodeticLlaValues {
// 
//   public final double latitudeRadians;
//   public final double longitudeRadians;
//   public final double altitudeMeters;
// 
//   public GeodeticLlaValues(double latitudeRadians,
//       double longitudeRadians, double altitudeMeters) {
//     this.latitudeRadians = latitudeRadians;
//     this.longitudeRadians = longitudeRadians;
//     this.altitudeMeters = altitudeMeters;
//   }
// }


// Path: GNSSLogger/pseudorange/src/main/java/com/google/location/lbs/gnss/gps/pseudorange/EcefToTopocentricConverter.java
import com.google.location.lbs.gnss.gps.pseudorange.Ecef2LlaConverter.GeodeticLlaValues;
import org.apache.commons.math3.linear.RealMatrix;

/*
 * Copyright (C) 2017 The Android Open Source Project
 *
 * 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.google.location.lbs.gnss.gps.pseudorange;


/**
 * Transformations from ECEF coordinates to Topocentric coordinates
 */
public class EcefToTopocentricConverter {
  private static final double MIN_DISTANCE_MAGNITUDE_METERS = 1.0e-22;
  private static final int EAST_IDX = 0;
  private static final int NORTH_IDX = 1;
  private static final int UP_IDX = 2;

  /**
   * Transformation of {@code inputVectorMeters} with origin at {@code originECEFMeters} into
   * topocentric coordinate system. The result is {@code TopocentricAEDValues} containing azimuth
   * from north +ve clockwise, radians; elevation angle, radians; distance, vector length meters
   *
   * <p>Source: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
   * http://kom.aau.dk/~borre/life-l99/topocent.m
   *
   */
  public static TopocentricAEDValues convertCartesianToTopocentricRadMeters(
      final double[] originECEFMeters, final double[] inputVectorMeters) {

    GeodeticLlaValues latLngAlt = Ecef2LlaConverter.convertECEFToLLACloseForm(originECEFMeters[0],
        originECEFMeters[1], originECEFMeters[2]);

    RealMatrix rotationMatrix =
        Ecef2EnuConverter.
            getRotationMatrix(latLngAlt.latitudeRadians, latLngAlt.longitudeRadians).transpose();
    double[] eastNorthUpVectorMeters = GpsMathOperations.matrixByColVectMultiplication(
        rotationMatrix.transpose().getData(), inputVectorMeters);
    double eastMeters = eastNorthUpVectorMeters[EAST_IDX];
    double northMeters = eastNorthUpVectorMeters[NORTH_IDX];
    double upMeters = eastNorthUpVectorMeters[UP_IDX];

    // calculate azimuth, elevation and height from the ENU values
    double horizontalDistanceMeters = Math.hypot(eastMeters, northMeters);
    double azimuthRadians;