#ifndef __JDETECTOR__JCOMPASS__
#define __JDETECTOR__JCOMPASS__

#include <istream>
#include <ostream>
#include <cmath>
#include <limits>

#include "JLang/JEquals.hh"
#include "JDB/JAHRS.hh"
#include "JDB/JAHRSCalibration.hh"
#include "JIO/JSerialisable.hh"
#include "JGeometry3D/JRotation3D.hh"
#include "JGeometry3D/JQuaternion3D.hh"


/**
 * \author mdejong
 */

namespace JDETECTOR {}
namespace JPP { using namespace JDETECTOR; }

namespace JDETECTOR {

  using JLANG::JEquals;
  using JDATABASE::JAHRS;
  using JDATABASE::JAHRSCalibration;
  using JGEOMETRY3D::JRotation3D;
  using JGEOMETRY3D::JQuaternion3D;
  using JIO::JReader;
  using JIO::JWriter;


  /**
   * Data structure for compass in three dimensions.
   *
   * \image html compass.png "Definitions of yaw, pitch and roll."
   *
   * Note that the z-axis of the %KM3NeT reference system points up.\n
   * So, the yaw angle, measured by the compass is measured from North to East
   * (since in the compass system z-axis points down).\n
   * Note also that the sign of the angle JCompass::pitch is maintained and 
   * the signs of the angles JCompass::yaw and JCompass::roll inverted 
   * when converting to/from a rotation matrix or quaternion.
   *
   * This class implements the JMATH::JMath and JLANG::JEquals interfaces.
   */
  class JCompass :
    public JEquals<JCompass>
  {
  public:
    /**
     * Default constructor.
     */
    JCompass() :
      yaw  (0.0),
      pitch(0.0),
      roll (0.0)
    {}


    /**
     * Constructor.
     *
     * \param  yaw      yaw   angle [rad]
     * \param  pitch    pitch angle [rad]
     * \param  roll     roll  angle [rad]
     */
    JCompass(const double yaw,
	     const double pitch,
	     const double roll) :
      yaw  (yaw),
      pitch(pitch),
      roll (roll)
    {}


    /**
     * Constructor.
     *
     * \param  data            AHRS data
     * \param  calibration     AHRS calibration
     */
    JCompass(const JAHRS& data, const JAHRSCalibration& calibration)
    {
      using namespace std;
      using namespace JPP;

      double A0 = data.AHRS_A0;
      double A1 = data.AHRS_A1;
      double A2 = data.AHRS_A2;
    
      double H0 = data.AHRS_H0;
      double H1 = data.AHRS_H1;
      double H2 = data.AHRS_H2;
    
      A0  -=  calibration.ACC_OFFSET_X;
      A1  -=  calibration.ACC_OFFSET_Y;
      A2  -=  calibration.ACC_OFFSET_Z;

      JMatrix3D(calibration.ACC_ROT_XX, calibration.ACC_ROT_XY, calibration.ACC_ROT_XZ,
		calibration.ACC_ROT_YX, calibration.ACC_ROT_YY, calibration.ACC_ROT_YZ,
		calibration.ACC_ROT_ZX, calibration.ACC_ROT_ZY, calibration.ACC_ROT_ZZ).transform(A0, A1, A2);
    
      H0  -=  0.5 * (calibration.MAG_XMIN + calibration.MAG_XMAX);
      H1  -=  0.5 * (calibration.MAG_YMIN + calibration.MAG_YMAX);
      H2  -=  0.5 * (calibration.MAG_ZMIN + calibration.MAG_ZMAX);

      JMatrix3D(calibration.MAG_ROT_XX, calibration.MAG_ROT_XY, calibration.MAG_ROT_XZ,
		calibration.MAG_ROT_YX, calibration.MAG_ROT_YY, calibration.MAG_ROT_YZ,
		calibration.MAG_ROT_ZX, calibration.MAG_ROT_ZY, calibration.MAG_ROT_ZZ).transform(H0, H1, H2);

      // invert axis for central-logic board being upside down in optical module

      A0 =  A0;
      A1 = -A1;
      A2 = -A2;
    
      H0 =  H0;
      H1 = -H1;
      H2 = -H2;

      this->roll  = atan2(-A1, -A2);
      this->pitch = atan2(A0, sqrt(A1*A1 + A2*A2));
      this->yaw   = atan2(H2 * sin(roll) - H1 * cos(roll), H0 * cos(pitch) + H1 * sin(pitch) * sin(roll) + H2 * sin(pitch) * cos(roll));
    }


    /**
     * Constructor.
     *
     * \param  Q        quaternion
     */
    JCompass(const JQuaternion3D& Q) :
      yaw  (0.0),
      pitch(0.0),
      roll (0.0)
    {
      using namespace std;

      this->yaw   = -atan2(2.0*(Q.getA()*Q.getD() + Q.getB()*Q.getC()), 1.0 - 2.0*(Q.getC()*Q.getC() + Q.getD()*Q.getD())); 

      const double sp = 2.0*(Q.getA()*Q.getC() - Q.getD()*Q.getB());

      if      (sp >= +1.0)
	this->pitch = asin(+1.0);
      else if (sp <= -1.0)
	this->pitch = asin(-1.0);
      else
	this->pitch = asin(sp);

      this->roll  = -atan2(2.0*(Q.getA()*Q.getB() + Q.getC()*Q.getD()), 1.0 - 2.0*(Q.getB()*Q.getB() + Q.getC()*Q.getC()));
    }


    /**
     * Get compass.
     *
     * \return          this compass
     */
    const JCompass& getCompass() const
    {
      return static_cast<const JCompass&>(*this);
    }


    /**
     * Set compass.
     *
     * \param  compass  compass
     */
    void setCompass(const JCompass& compass)
    {
      static_cast<JCompass&>(*this) = compass;
    }


    /**
     * Get rotation matrix.
     *
     * \return          rotation matrix
     */
    JRotation3D getRotation() const
    {
      using namespace JPP;

      const JRotation3D Rx = JRotation3X(-this->getRoll());
      const JRotation3D Ry = JRotation3Y(+this->getPitch());
      const JRotation3D Rz = JRotation3Z(-this->getYaw());

      return (Rz * Ry * Rx);
    }


    /**
     * Get quaternion.
     *
     * \return          quaternion
     */
    JQuaternion3D getQuaternion() const
    {
      using namespace JPP;

      const double cy = cos(-0.5 * yaw);
      const double sy = sin(-0.5 * yaw);
      const double cp = cos(+0.5 * pitch);
      const double sp = sin(+0.5 * pitch);
      const double cr = cos(-0.5 * roll);
      const double sr = sin(-0.5 * roll);

      return JQuaternion3D(cr * cp * cy + sr * sp * sy,
			   sr * cp * cy - cr * sp * sy,
			   cr * sp * cy + sr * cp * sy,
			   cr * cp * sy - sr * sp * cy);
    }


    /**
     * Get yaw compass.
     *
     * \return          yaw compass
     */
    double getYaw() const 
    { 
      return yaw;
    }


    /**
     * Get pitch compass.
     *
     * \return          pitch compass
     */
    double getPitch() const 
    { 
      return pitch;
    }


    /**
     * Get roll compass.
     *
     * \return          roll compass
     */
    double getRoll() const
    { 
      return roll;
    }
  

    /**
     * Check equality.
     *
     * \param  compass       compass
     * \param  precision     numerical precision
     * \return               true if compass's are equal; else false
     */
    bool equals(const JCompass& compass,
                const double    precision = std::numeric_limits<double>::min()) const
    {
      return (fabs(getYaw()   - compass.getYaw())   <= precision &&
              fabs(getPitch() - compass.getPitch()) <= precision &&
              fabs(getRoll()  - compass.getRoll())  <= precision);
    }


    /**
     * Correct compass for magnetic declination and meridian convergence angle.
     *
     * \param  declination   magnetic declination       [rad]
     * \param  meridian      meridian convergence angle [rad]
     */
    void correct(const double declination, const double meridian)
    {
      this->yaw += declination;
      this->yaw -= meridian;
    }


    /**
     * Read compasss from input.
     *
     * \param  in            input stream
     * \param  compass       compasss
     * \return               input stream
     */
    friend inline std::istream& operator>>(std::istream& in, JCompass& compass)
    {
      in >> compass.yaw >> compass.pitch  >> compass.roll;

      return in;
    }


    /**
     * Write compasss to output.
     *
     * \param  out           output stream
     * \param  compass       compass
     * \return               output stream
     */
    friend inline std::ostream& operator<<(std::ostream& out, const JCompass& compass)
    {
      out << compass.getYaw() << ' ' << compass.getPitch()  << ' ' << compass.getRoll();

      return out;
    }


    /**
     * Read compasss from input.
     *
     * \param  in         reader
     * \param  compass    compasss
     * \return            reader
     */
    friend inline JReader& operator>>(JReader& in, JCompass& compass)
    {
      in >> compass.yaw;
      in >> compass.pitch;
      in >> compass.roll;

      return in;
    }


    /**
     * Write compasss to output.
     *
     * \param  out        writer
     * \param  compass    compasss
     * \return            writer
     */
    friend inline JWriter& operator<<(JWriter& out, const JCompass& compass)
    {
      out << compass.yaw;
      out << compass.pitch;
      out << compass.roll;

      return out;
    }


  protected:
    double yaw;
    double pitch;
    double roll;
  };
}

#endif