/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * 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.
 *
*/
#include <string>
#include <math.h>

#include "gazebo/common/Console.hh"
#include "gazebo/common/SphericalCoordinates.hh"
#include "gazebo/common/SphericalCoordinatesPrivate.hh"

using namespace gazebo;
using namespace common;

// Parameters for EARTH_WGS84 model
// wikipedia: World_Geodetic_System#A_new_World_Geodetic_System:_WGS_84

// a: Equatorial radius. Semi-major axis of the WGS84 spheroid (meters).
const double g_EarthWGS84AxisEquatorial = 6378137.0;

// b: Polar radius. Semi-minor axis of the wgs84 spheroid (meters).
const double g_EarthWGS84AxisPolar = 6356752.314245;

// if: WGS84 inverse flattening parameter (no units)
const double g_EarthWGS84Flattening = 1.0/298.257223563;

// Radius of the Earth (meters).
const double g_EarthRadius = 6371000.0;

//////////////////////////////////////////////////
SphericalCoordinates::SurfaceType SphericalCoordinates::Convert(
  const std::string &_str)
{
  if ("EARTH_WGS84" == _str)
    return EARTH_WGS84;

  gzerr << "SurfaceType string not recognized, "
        << "EARTH_WGS84 returned by default" << std::endl;
  return EARTH_WGS84;
}

//////////////////////////////////////////////////
SphericalCoordinates::SphericalCoordinates()
  : dataPtr(new SphericalCoordinatesPrivate)
{
  this->SetSurfaceType(EARTH_WGS84);
  this->SetElevationReference(0.0);
}

//////////////////////////////////////////////////
SphericalCoordinates::SphericalCoordinates(const SurfaceType _type)
  : dataPtr(new SphericalCoordinatesPrivate)
{
  this->SetSurfaceType(_type);
  this->SetElevationReference(0.0);
}

//////////////////////////////////////////////////
SphericalCoordinates::SphericalCoordinates(const SurfaceType _type,
    const ignition::math::Angle &_latitude,
    const ignition::math::Angle &_longitude,
    double _elevation,
    const ignition::math::Angle &_heading)
: dataPtr(new SphericalCoordinatesPrivate)
{
  // Set the reference and calculate ellipse parameters
  this->SetSurfaceType(_type);

  // Set the coordinate transform parameters
  this->dataPtr->latitudeReference = _latitude;
  this->dataPtr->longitudeReference = _longitude;
  this->dataPtr->elevationReference = _elevation;
  this->dataPtr->headingOffset = _heading;

  // Generate transformation matrix
  this->UpdateTransformationMatrix();
}

//////////////////////////////////////////////////
SphericalCoordinates::~SphericalCoordinates()
{
  delete this->dataPtr;
  this->dataPtr = NULL;
}

//////////////////////////////////////////////////
SphericalCoordinates::SurfaceType SphericalCoordinates::GetSurfaceType() const
{
  return this->dataPtr->surfaceType;
}

//////////////////////////////////////////////////
ignition::math::Angle SphericalCoordinates::LatitudeReference() const
{
  return this->dataPtr->latitudeReference;
}

//////////////////////////////////////////////////
ignition::math::Angle SphericalCoordinates::LongitudeReference() const
{
  return this->dataPtr->longitudeReference;
}

//////////////////////////////////////////////////
double SphericalCoordinates::GetElevationReference() const
{
  return this->dataPtr->elevationReference;
}

//////////////////////////////////////////////////
ignition::math::Angle SphericalCoordinates::HeadingOffset() const
{
  return this->dataPtr->headingOffset;
}

//////////////////////////////////////////////////
void SphericalCoordinates::SetSurfaceType(const SurfaceType &_type)
{
  this->dataPtr->surfaceType = _type;

  switch (this->dataPtr->surfaceType)
  {
    case EARTH_WGS84:
      {
      // Set the semi-major axis
      this->dataPtr->ellA = g_EarthWGS84AxisEquatorial;

      // Set the semi-minor axis
      this->dataPtr->ellB = g_EarthWGS84AxisPolar;

      // Set the flattening parameter
      this->dataPtr->ellF = g_EarthWGS84Flattening;

      // Set the first eccentricity ellipse parameter
      // https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses
      this->dataPtr->ellE = sqrt(1.0 -
          std::pow(this->dataPtr->ellB, 2) / std::pow(this->dataPtr->ellA, 2));

      // Set the second eccentricity ellipse parameter
      // https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses
      this->dataPtr->ellP = sqrt(
          std::pow(this->dataPtr->ellA, 2) / std::pow(this->dataPtr->ellB, 2) -
          1.0);

      break;
      }
    default:
      gzerr << "Unknown surface type[" << this->dataPtr->surfaceType << "]\n";
      break;
  }
}

//////////////////////////////////////////////////
void SphericalCoordinates::SetLatitudeReference(
    const ignition::math::Angle &_angle)
{
  this->dataPtr->latitudeReference = _angle;
  this->UpdateTransformationMatrix();
}

//////////////////////////////////////////////////
void SphericalCoordinates::SetLongitudeReference(
    const ignition::math::Angle &_angle)
{
  this->dataPtr->longitudeReference = _angle;
  this->UpdateTransformationMatrix();
}

//////////////////////////////////////////////////
void SphericalCoordinates::SetElevationReference(double _elevation)
{
  this->dataPtr->elevationReference = _elevation;
  this->UpdateTransformationMatrix();
}

//////////////////////////////////////////////////
void SphericalCoordinates::SetHeadingOffset(const ignition::math::Angle &_angle)
{
  this->dataPtr->headingOffset.Radian(_angle.Radian());
  this->UpdateTransformationMatrix();
}

//////////////////////////////////////////////////
ignition::math::Vector3d SphericalCoordinates::SphericalFromLocal(
    const ignition::math::Vector3d &_xyz) const
{
  ignition::math::Vector3d result =
    this->PositionTransform(_xyz, LOCAL, SPHERICAL);
  result.X(IGN_RTOD(result.X()));
  result.Y(IGN_RTOD(result.Y()));
  return result;
}

//////////////////////////////////////////////////
ignition::math::Vector3d SphericalCoordinates::LocalFromSpherical(
    const ignition::math::Vector3d &_xyz) const
{
  ignition::math::Vector3d result = _xyz;
  result.X(IGN_DTOR(result.X()));
  result.Y(IGN_DTOR(result.Y()));
  return this->PositionTransform(result, SPHERICAL, LOCAL);
}

//////////////////////////////////////////////////
ignition::math::Vector3d SphericalCoordinates::GlobalFromLocal(
    const ignition::math::Vector3d &_xyz) const
{
  return this->VelocityTransform(_xyz, LOCAL, GLOBAL);
}

//////////////////////////////////////////////////
ignition::math::Vector3d SphericalCoordinates::LocalFromGlobal(
    const ignition::math::Vector3d &_xyz) const
{
  return this->VelocityTransform(_xyz, GLOBAL, LOCAL);
}

//////////////////////////////////////////////////
/// Based on Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula).
double SphericalCoordinates::Distance(const ignition::math::Angle &_latA,
                                      const ignition::math::Angle &_lonA,
                                      const ignition::math::Angle &_latB,
                                      const ignition::math::Angle &_lonB)
{
  ignition::math::Angle dLat = _latB - _latA;
  ignition::math::Angle dLon = _lonB - _lonA;

  double a = sin(dLat.Radian() / 2) * sin(dLat.Radian() / 2) +
             sin(dLon.Radian() / 2) * sin(dLon.Radian() / 2) *
             cos(_latA.Radian()) * cos(_latB.Radian());

  double c = 2 * atan2(sqrt(a), sqrt(1 - a));
  double d = g_EarthRadius * c;
  return d;
}

//////////////////////////////////////////////////
void SphericalCoordinates::UpdateTransformationMatrix()
{
  // Cache trig results
  double cosLat = cos(this->dataPtr->latitudeReference.Radian());
  double sinLat = sin(this->dataPtr->latitudeReference.Radian());
  double cosLon = cos(this->dataPtr->longitudeReference.Radian());
  double sinLon = sin(this->dataPtr->longitudeReference.Radian());

  // Create a rotation matrix that moves ECEF to GLOBAL
  // http://www.navipedia.net/index.php/
  // Transformations_between_ECEF_and_ENU_coordinates
  this->dataPtr->rotECEFToGlobal = ignition::math::Matrix3d(
                      -sinLon,           cosLon,          0.0,
                      -cosLon * sinLat, -sinLon * sinLat, cosLat,
                       cosLon * cosLat,  sinLon * cosLat, sinLat);

  // Create a rotation matrix that moves GLOBAL to ECEF
  // http://www.navipedia.net/index.php/
  // Transformations_between_ECEF_and_ENU_coordinates
  this->dataPtr->rotGlobalToECEF = ignition::math::Matrix3d(
                      -sinLon, -cosLon * sinLat, cosLon * cosLat,
                       cosLon, -sinLon * sinLat, sinLon * cosLat,
                       0,      cosLat,           sinLat);

  // Cache heading transforms -- note that we have to negate the heading in
  // order to preserve backward compatibility. ie. Gazebo has traditionally
  // expressed positive angle as a CLOCKWISE rotation that takes the GLOBAL
  // frame to the LOCAL frame. However, right hand coordinate systems require
  // this to be expressed as an ANTI-CLOCKWISE rotation. So, we negate it.
  this->dataPtr->cosHea = cos(-this->dataPtr->headingOffset.Radian());
  this->dataPtr->sinHea = sin(-this->dataPtr->headingOffset.Radian());

  // Cache the ECEF coordinate of the origin
  this->dataPtr->origin = ignition::math::Vector3d(
    this->dataPtr->latitudeReference.Radian(),
    this->dataPtr->longitudeReference.Radian(),
    this->dataPtr->elevationReference);
  this->dataPtr->origin =
    this->PositionTransform(this->dataPtr->origin, SPHERICAL, ECEF);
}

/////////////////////////////////////////////////
ignition::math::Vector3d SphericalCoordinates::PositionTransform(
    const ignition::math::Vector3d &_pos,
    const CoordinateType &_in, const CoordinateType &_out) const
{
  ignition::math::Vector3d tmp = _pos;

  // Cache trig results
  double cosLat = cos(_pos.X());
  double sinLat = sin(_pos.X());
  double cosLon = cos(_pos.Y());
  double sinLon = sin(_pos.Y());

  // Radius of planet curvature (meters)
  double curvature = 1.0 -
    this->dataPtr->ellE * this->dataPtr->ellE * sinLat * sinLat;
  curvature = this->dataPtr->ellA / sqrt(curvature);

  // Convert whatever arrives to a more flexible ECEF coordinate
  switch (_in)
  {
    // East, North, Up (ENU), note no break at end of case
    case LOCAL:
      {
        tmp.X(-_pos.X() * this->dataPtr->cosHea + _pos.Y() *
            this->dataPtr->sinHea);
        tmp.Y(-_pos.X() * this->dataPtr->sinHea - _pos.Y() *
            this->dataPtr->cosHea);
      }

    case GLOBAL:
      {
        tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp;
        break;
      }

    case SPHERICAL:
      {
        tmp.X((_pos.Z() + curvature) * cosLat * cosLon);
        tmp.Y((_pos.Z() + curvature) * cosLat * sinLon);
        tmp.Z(((this->dataPtr->ellB * this->dataPtr->ellB)/
              (this->dataPtr->ellA * this->dataPtr->ellA) *
              curvature + _pos.Z()) * sinLat);
        break;
      }

    // Do nothing
    case ECEF:
      break;
    default:
      gzerr << "Invalid coordinate type[" << _in << "]\n";
      return _pos;
  }

  // Convert ECEF to the requested output coordinate system
  switch (_out)
  {
    case SPHERICAL:
      {
        // Convert from ECEF to SPHERICAL
        double p = sqrt(tmp.X() * tmp.X() + tmp.Y() * tmp.Y());
        double theta = atan((tmp.Z() * this->dataPtr->ellA) /
            (p * this->dataPtr->ellB));

        // Calculate latitude and longitude
        double lat = atan(
            (tmp.Z() + std::pow(this->dataPtr->ellP, 2) * this->dataPtr->ellB *
             std::pow(sin(theta), 3)) /
            (p - std::pow(this->dataPtr->ellE, 2) *
             this->dataPtr->ellA * std::pow(cos(theta), 3)));

        double lon = atan2(tmp.Y(), tmp.X());

        // Recalculate radius of planet curvature at the current latitude.
        double nCurvature = 1.0 - std::pow(this->dataPtr->ellE, 2) *
          std::pow(sin(lat), 2);
        nCurvature = this->dataPtr->ellA / sqrt(nCurvature);

        tmp.X(lat);
        tmp.Y(lon);

        // Now calculate Z
        tmp.Z(p/cos(lat) - nCurvature);
        break;
      }

    // Convert from ECEF TO GLOBAL
    case GLOBAL:
      tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin);
      break;

    // Convert from ECEF TO LOCAL
    case LOCAL:
      tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin);

      tmp = ignition::math::Vector3d(
          tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea,
          tmp.X() * this->dataPtr->sinHea + tmp.Y() * this->dataPtr->cosHea,
          tmp.Z());
      break;

    // Return ECEF (do nothing)
    case ECEF:
      break;

    default:
      gzerr << "Unknown coordinate type[" << _out << "]\n";
      return _pos;
  }

  return tmp;
}

//////////////////////////////////////////////////
ignition::math::Vector3d SphericalCoordinates::VelocityTransform(
    const ignition::math::Vector3d &_vel,
    const CoordinateType &_in, const CoordinateType &_out) const
{
  // Sanity check -- velocity should not be expressed in spherical coordinates
  if (_in == SPHERICAL || _out == SPHERICAL)
  {
    gzwarn << "Spherical velocities are not supported";
    return _vel;
  }

  // Intermediate data type
  ignition::math::Vector3d tmp = _vel;

  // First, convert to an ECEF vector
  switch (_in)
  {
    // ENU (note no break at end of case)
    case LOCAL:
      tmp.X(-_vel.X() * this->dataPtr->cosHea + _vel.Y() *
            this->dataPtr->sinHea);
      tmp.Y(-_vel.X() * this->dataPtr->sinHea - _vel.Y() *
            this->dataPtr->cosHea);
    // spherical
    case GLOBAL:
      tmp = this->dataPtr->rotGlobalToECEF * tmp;
      break;
    // Do nothing
    case ECEF:
      tmp = _vel;
      break;
    default:
      gzerr << "Unknown coordinate type[" << _in << "]\n";
      return _vel;
  }

  // Then, convert to the request coordinate type
  switch (_out)
  {
    // ECEF, do nothing
    case ECEF:
      break;

    // Convert from ECEF to global
    case GLOBAL:
      tmp = this->dataPtr->rotECEFToGlobal * tmp;
      break;

    // Convert from ECEF to local
    case LOCAL:
      tmp = this->dataPtr->rotECEFToGlobal * tmp;
      tmp = ignition::math::Vector3d(
          tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea,
          tmp.X() * this->dataPtr->sinHea + tmp.Y() * this->dataPtr->cosHea,
          tmp.Z());
      break;

    default:
      gzerr << "Unknown coordinate type[" << _out << "]\n";
      return _vel;
  }

  return tmp;
}