/**
 * 第2章の内容
 * 
 * This program is derived from C language programs 
 * which are described in "実用GPSプログラミング" wrote by Dr.Sakai,
 * and re-written and modified with C++ language by fenrir(M.Naruoka)
 * 
 * All copyright is held by fenrir(M.Naruoka).
 * 
 * @see https://fenrir.naruoka.org/archives/000620.html
 */

#include <iostream>
#include <cmath>

using namespace std;

#include "WGS84.h"
#include "util/util.h"
#include "param/matrix.h"

typedef WGS84 Earth; ///< 地球モデル

#ifndef pow2
#define pow2(x) ((x)*(x))
#endif
#ifndef pow3
#define pow3(x) ((x)*(x)*(x))
#endif

#ifndef __GPS_PROGRAMMING_SECT2_HEADER
#define __GPS_PROGRAMMING_SECT2_HEADER

template <class FloatT> class System_XYZ;
template <class FloatT> class System_LLH;

template <class FloatT = double>
class System_3D {
  protected:
    typedef System_3D self_t;
    FloatT v[3];
  public:
    static const unsigned int value_boundary = 3;
    System_3D(){
      for(unsigned i(0); i < value_boundary; i++){
        v[i] = FloatT(0);
      }
    }
    System_3D(const FloatT &v0, const FloatT &v1, const FloatT &v2){
      v[0] = v0;
      v[1] = v1;
      v[2] = v2;
    }
    System_3D(const self_t &orig){
      for(unsigned i(0); i < value_boundary; i++){
        v[i] = orig.v[i];
      }
    }
    System_3D(const Matrix<FloatT> &matrix){
      Matrix<FloatT> target(matrix);
      if(matrix.rows() < matrix.columns()){
        target = target.transpose();
      }
      for(unsigned i(0); i < value_boundary; i++){
        v[i] = target(i, 0);
      }
    }
    ~System_3D(){}
    System_3D &operator=(const System_3D &another){
      if(this != &another){
        for(unsigned i(0); i < value_boundary; i++){
          v[i] = another.v[i];
        }
      }
      return *this;
    }
    
    FloatT &operator[](int i){return v[i];}
    
    friend ostream &operator<<(ostream &out, const self_t &self){
      out << const_cast<self_t *>(&self)->operator[](0) << " "
          << const_cast<self_t *>(&self)->operator[](1) << " "
          << const_cast<self_t *>(&self)->operator[](2);
      return out;
    }
    friend istream &operator>>(istream &in, self_t &self){
      in >> self[0];
      in >> self[1];
      in >> self[2];
      return in;
    }
};


template <class FloatT = double>
class System_XYZ : public System_3D<FloatT> {
  protected:
    typedef System_XYZ<FloatT> self_t;
    typedef System_3D<FloatT> super_t;
  public:
    System_XYZ() : super_t() {}
    System_XYZ(const FloatT &x, const FloatT &y, const FloatT &z) 
        : super_t(x, y, z) {}
    System_XYZ(const self_t &xyz)
        : super_t(xyz) {}
    System_XYZ(const Matrix<FloatT> &matrix) 
        : super_t(matrix) {}
    ~System_XYZ(){}
    
    FloatT &x(){return super_t::operator[](0);}
    FloatT &y(){return super_t::operator[](1);}
    FloatT &z(){return super_t::operator[](2);}
    
    self_t &operator+=(const self_t &another){
      x() += const_cast<self_t *>(&another)->x();
      y() += const_cast<self_t *>(&another)->y();
      z() += const_cast<self_t *>(&another)->z();
      return *this;
    }
    self_t operator+(const self_t &another) const {
      self_t copy(*this);
      return copy += another;
    }
    self_t &operator-=(const self_t &another){
      x() -= const_cast<self_t *>(&another)->x();
      y() -= const_cast<self_t *>(&another)->y();
      z() -= const_cast<self_t *>(&another)->z();
      return *this;
    }
    self_t operator-(const self_t &another) const {
      self_t copy(*this);
      return copy -= another;
    }
    
    FloatT dist() const {
      
      const FloatT _x(const_cast<self_t *>(this)->x());
      const FloatT _y(const_cast<self_t *>(this)->y());
      const FloatT _z(const_cast<self_t *>(this)->z());
      
      return FloatT(sqrt(
          pow2(_x) + pow2(_y) + pow2(_z)
        ));
    }
    
    FloatT dist(const self_t &another) const {
      return (*this - another).dist();
    }
    
    /**
     * 緯度、経度、高度へ変換
     * 
     */
    System_LLH<FloatT> llh() const {
      
      static const FloatT f(Earth::F_e);
      static const FloatT a(Earth::R_e);
      static const FloatT b(a * (1.0 - f));
      static const FloatT e(sqrt(f * (2.0 - f)));
      //cout << f << ", "  << a << ", "  << b << ", "  << e << endl; 
      
      const FloatT _x(const_cast<self_t *>(this)->x());
      const FloatT _y(const_cast<self_t *>(this)->y());
      const FloatT _z(const_cast<self_t *>(this)->z());
      
      if((_x == 0) && (_y == 0) && (_z == 0)){
        return System_LLH<FloatT>(0, 0, -a);
      }

      FloatT h(pow2(a) - pow2(b));
      FloatT p(sqrt(pow2(_x) + pow2(_y)));
      //cout << "p => " << p << endl;
      FloatT t(atan2(_z*a, p*b));
      FloatT sint(sin(t)), cost(cos(t));
      
      FloatT _lat(atan2(_z + (h / b * pow3(sint)), p - (h / a * pow3(cost))));
      FloatT n(a/sqrt(1.0 - pow2(e) * pow2(sin(_lat))));
      
      return System_LLH<FloatT>(
          _lat,
          atan2(_y, _x),
          (p / cos(_lat) - n)
        );
    }
};

template <class FloatT = double>
class System_LLH : public System_3D<FloatT> {
  protected:
    typedef System_LLH<FloatT> self_t;
    typedef System_3D<FloatT> super_t;
  public:
    System_LLH() : super_t() {}
    System_LLH(const FloatT &latitude, const FloatT &longitude, const FloatT &height) 
        : super_t(latitude, longitude, height) {}
    System_LLH(const self_t &llh) 
        : super_t(llh) {}
    ~System_LLH(){}
    
    FloatT &latitude()  {return super_t::operator[](0);}
    FloatT &longitude() {return super_t::operator[](1);}
    FloatT &height()    {return super_t::operator[](2);}
    
    /**
     * 直交座標系へ変換
     * 
     */
    System_XYZ<FloatT> xyz() const {
      static const FloatT f(Earth::F_e);
      static const FloatT a(Earth::R_e);
      static const FloatT b(a * (1.0 - f));
      static const FloatT e(sqrt(f * (2.0 - f)));
      
      const FloatT _lat (const_cast<self_t *>(this)->latitude());   ///< 緯度[rad]
      const FloatT _long(const_cast<self_t *>(this)->longitude());  ///< 経度[rad]
      const FloatT _h   (const_cast<self_t *>(this)->height());     ///< 高度[m]
      
      FloatT n(a/sqrt(1.0 - pow2(e) * pow2(sin(_lat))));
      
      return System_XYZ<FloatT>(
          (n + _h) * cos(_lat) * cos(_long),
          (n + _h) * cos(_lat) * sin(_long),
          (n * (1.0 -pow2(e)) + _h) * sin(_lat)
        );
    }
    
    friend ostream &operator<<(ostream &out, const self_t &self){
      out << rad2deg(const_cast<self_t *>(&self)->latitude()) << " "
          << rad2deg(const_cast<self_t *>(&self)->longitude()) << " "
          << const_cast<self_t *>(&self)->height();
      return out;
    }
    friend istream &operator>>(istream &in, self_t &self){
      FloatT _lat, _long;
      in >> _lat;
      in >> _long;
      in >> self[2];
      self.latitude() = deg2rad(_lat);
      self.longitude() = deg2rad(_long);
      return in;
    }
};

template <class FloatT = double>
class System_ENU : public System_3D<FloatT> {
  protected:
    typedef System_ENU<FloatT> self_t;
    typedef System_3D<FloatT> super_t;
    typedef System_XYZ<FloatT> xyz_t;
    typedef System_LLH<FloatT> llh_t;
  public:
    System_ENU() : super_t() {}
    System_ENU(const FloatT &east, const FloatT &north, const FloatT &up) 
        : super_t(east, north, up) {}
    System_ENU(const self_t &enu) 
        : super_t(enu) {}
    ~System_ENU() {}
    
    FloatT &east()  {return super_t::operator[](0);}  ///< 東成分[m]
    FloatT &north() {return super_t::operator[](1);}  ///< 北成分[m]
    FloatT &up()    {return super_t::operator[](2);}  ///< 上成分[m]
    
    static self_t relative(const xyz_t &pos, const xyz_t &base){
      FloatT rel_x(const_cast<xyz_t *>(&pos)->x() - const_cast<xyz_t *>(&base)->x());
      FloatT rel_y(const_cast<xyz_t *>(&pos)->y() - const_cast<xyz_t *>(&base)->y());
      FloatT rel_z(const_cast<xyz_t *>(&pos)->z() - const_cast<xyz_t *>(&base)->z());
      
      llh_t base_llh(base.llh());
      FloatT s1(sin(base_llh.longitude())), c1(cos(base_llh.longitude()));
      FloatT s2(sin(base_llh.latitude())), c2(cos(base_llh.latitude()));
      
      return self_t(
          -rel_x * s1 + rel_y * c1,
          -rel_x * c1 * s2 - rel_y * s1 * s2 + rel_z * c2,
          rel_x * c1 * c2 + rel_y * s1 * c2 + rel_z * s2
        );
    }
    
    xyz_t absolute(const xyz_t &base) const {
      llh_t base_llh(base.llh());
      FloatT s1(sin(base_llh.longitude())), c1(cos(base_llh.longitude()));
      FloatT s2(sin(base_llh.latitude())), c2(cos(base_llh.latitude()));
      
      FloatT _east(const_cast<self_t *>(this)->east());
      FloatT _north(const_cast<self_t *>(this)->north());
      FloatT _up(const_cast<self_t *>(this)->up());
      
      return xyz_t(
          -_east * s1 - _north * c1 * s2 + _up * c1 * c2 
            + const_cast<xyz_t *>(&base)->x(),
          _east * c1 - _north * s1 * s2 + _up * s1 * c2
            + const_cast<xyz_t *>(&base)->y(),
          _north * c2 + _up * s2
            + const_cast<xyz_t *>(&base)->z()
        );
    }
    
    FloatT elevation() const {
      FloatT _east(const_cast<self_t *>(this)->east());
      FloatT _north(const_cast<self_t *>(this)->north());
      FloatT _up(const_cast<self_t *>(this)->up());
      return FloatT(atan2(_up, sqrt(pow2(_east) + pow2(_north))));
    }
    FloatT azimuth() const {
      FloatT _east(const_cast<self_t *>(this)->east());
      FloatT _north(const_cast<self_t *>(this)->north());
      return FloatT(atan2(_east, _north));
    }
};

#endif // __GPS_PROGRAMMING_SECT2_HEADER

#ifndef NO_MAIN

typedef double precision_t;
typedef System_XYZ<precision_t> xyz_t;
typedef System_ENU<precision_t> enu_t;

typedef Matrix<precision_t> matrix_t;

int main() {
  
  /*xyz_t xyz1(0, 0, 100);
  xyz_t xyz2(0, 0, 200);
  cout << xyz1.llh() << endl;
  cout << xyz2.llh() << endl;
  
  enu_t enu_1_2(enu_t::relative(xyz1, xyz2));
  
  cout << enu_1_2 << endl;
  cout << enu_1_2.absolute(xyz2) << endl;
  cout << enu_1_2.elevation() << endl;
  cout << enu_1_2.azimuth() << endl;*/
  
#define SAT_NUM 5
  
  xyz_t sat_position[SAT_NUM] = {
        xyz_t(-13897607.6294, -10930188.6233,  19676689.6804),
        xyz_t(-17800899.1998,  15689920.8120,  11943543.3888),
        xyz_t( -1510958.2282,  26280096.7818,  -3117646.1949),
        xyz_t(-12210758.3517,  20413597.0201, -11649499.5474),
        xyz_t(  -170032.6981,  17261822.6784,  20555984.4061)
    };
  precision_t range[SAT_NUM] = {
      23634878.5219,
      20292688.3557,
      24032055.0372,
      24383229.3740,
      22170992.8178
    };
  
  // 初期解の設定
  xyz_t user_position;
  
  // デザイン行列の作成
  while(true){
    matrix_t G(SAT_NUM, 3);
    matrix_t delta_r(SAT_NUM, 1);
    for(unsigned i(0); i < G.rows(); i++){
      delta_r(i, 0) = range[i] - user_position.dist(sat_position[i]);
      G(i, 0) = -(sat_position[i].x() - user_position.x()) / range[i];
      G(i, 1) = -(sat_position[i].y() - user_position.y()) / range[i];
      G(i, 2) = -(sat_position[i].z() - user_position.z()) / range[i];
    }
    //cout << "delta_r:" << delta_r << endl;
    //cout << "G:" << G << endl;
    
    // 最小二乗法の適用
    xyz_t delta_user_position(
        (G.transpose() * G).inverse() * G.transpose() * delta_r);
    
    user_position += delta_user_position;
    cout << "user:" << user_position << endl;
    
    if(delta_user_position.dist() <= 1E-6){break;}
  }
  
  return 0; 
}

#endif