#ifndef __COORDINATE_H__
#define __COORDINATE_H__

/**
 * 座標系について記述したヘッダ
 * 
 */

/*
 * $Log$
 */

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

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

#include "WGS84.h"
#include "util/iostream_format.h"

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];}

#if ENABLE_IOSTREAM
    friend std::ostream &operator<<(std::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 std::istream &operator>>(std::istream &in, self_t &self){
      in >> self[0];
      in >> self[1];
      in >> self[2];
      return in;
    }
#endif
};

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

template <class FloatT = double, class Earth = WGS84>
class System_XYZ : public System_3D<FloatT> {
  protected:
    typedef System_XYZ<FloatT, Earth> 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, Earth> 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, Earth>(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, Earth>(
          _lat,
          atan2(_y, _x),
          (p / cos(_lat) - n)
        );
    }
};

template <class FloatT = double, class Earth = WGS84>
class System_LLH : public System_3D<FloatT> {
  protected:
    typedef System_LLH<FloatT, Earth> 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, Earth> 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, Earth>(
          (n + _h) * cos(_lat) * cos(_long),
          (n + _h) * cos(_lat) * sin(_long),
          (n * (1.0 -pow2(e)) + _h) * sin(_lat)
        );
    }

#if ENABLE_IOSTREAM
    friend std::ostream &operator<<(std::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 std::istream &operator>>(std::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;
    }
#endif
};

template <class FloatT = double, class Earth = WGS84>
class System_ENU : public System_3D<FloatT> {
  protected:
    typedef System_ENU<FloatT, Earth> self_t;
    typedef System_3D<FloatT> super_t;
    typedef System_XYZ<FloatT, Earth> xyz_t;
    typedef System_LLH<FloatT, Earth> 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 // __COORDINATE_H__