#ifndef __JDB_JAHRSCALIBRATIONTOOLKIT__
#define __JDB_JAHRSCALIBRATIONTOOLKIT__

#include "JDB/JAHRSCalibration.hh"

#include "JTools/JRange.hh"
#include "JLang/JLangToolkit.hh"
#include "JLang/JManip.hh"

/**
 * \author mdejong
 */
namespace JDATABASE {}
namespace JPP { using namespace JDATABASE; }

namespace JDATABASE {


  /**
   * Read AHRS calibration from input stream.
   *
   * \param  in            input stream
   * \param  calibration   AHRS calibration
   * \return               input stream
   */
  inline std::istream& operator>>(std::istream& in, JAHRSCalibration& calibration)
  {
    using namespace std;
    using namespace JPP;

    in >> calibration.ACC_OFFSET_X
       >> calibration.ACC_OFFSET_Y
       >> calibration.ACC_OFFSET_Z;

    in >> calibration.ACC_ROT_XX >> calibration.ACC_ROT_XY >> calibration.ACC_ROT_XZ;
    in >> calibration.ACC_ROT_YX >> calibration.ACC_ROT_YY >> calibration.ACC_ROT_YZ;
    in >> calibration.ACC_ROT_ZX >> calibration.ACC_ROT_ZY >> calibration.ACC_ROT_ZZ;

    in >> calibration.MAG_XMIN
       >> calibration.MAG_YMIN
       >> calibration.MAG_ZMIN;
    
    in >> calibration.MAG_XMAX
       >> calibration.MAG_YMAX
       >> calibration.MAG_ZMAX;
    
    in >> calibration.MAG_ROT_XX >> calibration.MAG_ROT_XY >> calibration.MAG_ROT_XZ;
    in >> calibration.MAG_ROT_YX >> calibration.MAG_ROT_YY >> calibration.MAG_ROT_YZ;
    in >> calibration.MAG_ROT_ZX >> calibration.MAG_ROT_ZY >> calibration.MAG_ROT_ZZ;

    return in;
  }


  /**
   * Write AHRS calibration to output stream.
   *
   * \param  out           output stream
   * \param  calibration   AHRS calibration
   * \return               output stream
   */
  inline std::ostream& operator<<(std::ostream& out, const JAHRSCalibration& calibration)
  {
    using namespace std;
    using namespace JPP;

    out << FIXED(9,6) << calibration.ACC_OFFSET_X << ' '
	<< FIXED(9,6) << calibration.ACC_OFFSET_Y << ' '
	<< FIXED(9,6) << calibration.ACC_OFFSET_Z << ' ';

    out << FIXED(9,6) << calibration.ACC_ROT_XX << ' ' << FIXED(9,6) << calibration.ACC_ROT_XY << ' ' << FIXED(9,6) << calibration.ACC_ROT_XZ << ' ';
    out << FIXED(9,6) << calibration.ACC_ROT_YX << ' ' << FIXED(9,6) << calibration.ACC_ROT_YY << ' ' << FIXED(9,6) << calibration.ACC_ROT_YZ << ' ';
    out << FIXED(9,6) << calibration.ACC_ROT_ZX << ' ' << FIXED(9,6) << calibration.ACC_ROT_ZY << ' ' << FIXED(9,6) << calibration.ACC_ROT_ZZ << ' ';

    out << FIXED(9,6) << calibration.MAG_XMIN << ' '
	<< FIXED(9,6) << calibration.MAG_YMIN << ' '
	<< FIXED(9,6) << calibration.MAG_ZMIN << ' ';
    
    out << FIXED(9,6) << calibration.MAG_XMAX << ' '
	<< FIXED(9,6) << calibration.MAG_YMAX << ' '
	<< FIXED(9,6) << calibration.MAG_ZMAX << ' ';
    
    out << FIXED(9,6) << calibration.MAG_ROT_XX << ' ' << FIXED(9,6) << calibration.MAG_ROT_XY << ' ' << FIXED(9,6) << calibration.MAG_ROT_XZ << ' ';
    out << FIXED(9,6) << calibration.MAG_ROT_YX << ' ' << FIXED(9,6) << calibration.MAG_ROT_YY << ' ' << FIXED(9,6) << calibration.MAG_ROT_YZ << ' ';
    out << FIXED(9,6) << calibration.MAG_ROT_ZX << ' ' << FIXED(9,6) << calibration.MAG_ROT_ZY << ' ' << FIXED(9,6) << calibration.MAG_ROT_ZZ << ' ';

    return out;
  }


  /**
   * Get numerical value of AHRS calibration version.
   *
   * \param  version       version
   * \return               version
   */
  inline int getVersion(const std::string& version)
  {
    using namespace std;
    using namespace JPP;

    size_t pos = version.find_last_of("vV");

    if (pos != string::npos) 
      return to_value<int>(version.substr(pos+1));
    else
      return -1;
  }

  
  /**
   * Auxiliary data structure to check validity of AHRS calibration data.
   */
  struct JAHRSCalibrationValidity {

    typedef JTOOLS::JRange<double> range_type;

    /**
     * Default constructor.
     *
     * This constructor sets default ranges of acceptance of AHRS calibration values.
     */
    JAHRSCalibrationValidity() 
    {
      ACC_OFFSET_X  = range_type(-1.0, +1.0);
      ACC_OFFSET_Y  = range_type(-1.0, +1.0);
      ACC_OFFSET_Z  = range_type(-1.0, +1.0);

      ACC_ROT[0][0] = range_type(-1.8, +1.8);  ACC_ROT[0][1] = range_type(-1.0, +1.0);  ACC_ROT[0][2] = range_type(-1.0, +1.0);
      ACC_ROT[1][0] = range_type(-1.0, +1.0);  ACC_ROT[1][1] = range_type(+0.6, +1.8);  ACC_ROT[1][2] = range_type(-1.0, +1.0);
      ACC_ROT[2][0] = range_type(-1.0, +1.0);  ACC_ROT[2][1] = range_type(-1.0, +1.0);  ACC_ROT[2][2] = range_type(+0.6, +1.8);
    
      MAG_OFFSET_X  = range_type(-1.0, +1.0);
      MAG_OFFSET_Y  = range_type(-1.0, +1.0);
      MAG_OFFSET_Z  = range_type(-1.0, +1.0);

      MAG_ROT[0][0] = range_type(-1.8, +1.8);  MAG_ROT[0][1] = range_type(-1.0, +1.0);  MAG_ROT[0][2] = range_type(-1.0, +1.0);
      MAG_ROT[1][0] = range_type(-1.0, +1.0);  MAG_ROT[1][1] = range_type(+0.6, +1.8);  MAG_ROT[1][2] = range_type(-1.0, +1.0);
      MAG_ROT[2][0] = range_type(-1.0, +1.0);  MAG_ROT[2][1] = range_type(-1.0, +1.0);  MAG_ROT[2][2] = range_type(+0.6, +1.8);    
    }


    /**
     * Check validity of AHRS calibration data.
     *
     * \param  calibration   AHRS calibration
     * \return               true if valid; else false
     */
    bool operator()(const JAHRSCalibration& calibration) const
    {
      return (ACC_OFFSET_X(calibration.ACC_OFFSET_X)   &&
	      ACC_OFFSET_Y(calibration.ACC_OFFSET_Y)   &&
	      ACC_OFFSET_Z(calibration.ACC_OFFSET_Z)   &&
	    
	      ACC_ROT[0][0](calibration.ACC_ROT_XX)  &&  ACC_ROT[0][1](calibration.ACC_ROT_XY)  &&  ACC_ROT[0][2](calibration.ACC_ROT_XZ)  &&
	      ACC_ROT[1][0](calibration.ACC_ROT_YX)  &&  ACC_ROT[1][1](calibration.ACC_ROT_YY)  &&  ACC_ROT[1][2](calibration.ACC_ROT_YZ)  &&
	      ACC_ROT[2][0](calibration.ACC_ROT_ZX)  &&  ACC_ROT[2][1](calibration.ACC_ROT_ZY)  &&  ACC_ROT[2][2](calibration.ACC_ROT_ZZ)  &&

	      MAG_OFFSET_X(0.5 * (calibration.MAG_XMIN + calibration.MAG_XMAX))  &&
	      MAG_OFFSET_Y(0.5 * (calibration.MAG_YMIN + calibration.MAG_YMAX))  &&
	      MAG_OFFSET_Z(0.5 * (calibration.MAG_ZMIN + calibration.MAG_ZMAX))  &&
	      
	      MAG_ROT[0][0](calibration.MAG_ROT_XX)  &&  MAG_ROT[0][1](calibration.MAG_ROT_XY)  &&  MAG_ROT[0][2](calibration.MAG_ROT_XZ)  &&
	      MAG_ROT[1][0](calibration.MAG_ROT_YX)  &&  MAG_ROT[1][1](calibration.MAG_ROT_YY)  &&  MAG_ROT[1][2](calibration.MAG_ROT_YZ)  &&
	      MAG_ROT[2][0](calibration.MAG_ROT_ZX)  &&  MAG_ROT[2][1](calibration.MAG_ROT_ZY)  &&  MAG_ROT[2][2](calibration.MAG_ROT_ZZ));
    }


    range_type ACC_OFFSET_X;
    range_type ACC_OFFSET_Y;
    range_type ACC_OFFSET_Z;

    range_type ACC_ROT[3][3];
    
    range_type MAG_OFFSET_X;
    range_type MAG_OFFSET_Y;
    range_type MAG_OFFSET_Z;

    range_type MAG_ROT[3][3];
  };


  /**
   * Auxiliary data structure for sorting of AHRS calibrations.
   */
  struct JAHRSCalibrationComparator {
    /**
     * Comparison of operation identifiers.
     *
     * \param  first         first  operation identifiers
     * \param  second        second operation identifiers
     * \return               true if first calibration better than second; else false
     */
    inline bool operator()(const std::string& first,
			   const std::string& second) const
    {
      using namespace std;
      using namespace JPP;

      if (!first.empty() && !second.empty()) {

	if (first[0] == second[0])
	  return to_value<int>(first.substr(1)) > to_value<int>(second.substr(1));
	else
	  return first[0] > second[0];

      } else {

	return first > second;
      }
    }


    /**
     * Comparison of AHRS calibrations.
     *
     * \param  first         first  AHRS calibration
     * \param  second        second AHRS calibration
     * \return               true if first AHRS calibration better than second; else false
     */
    inline bool operator()(const JAHRSCalibration& first,
			   const JAHRSCalibration& second) const
    {
      if (first.SERIALNUMBER == second.SERIALNUMBER) {

	if (first.TESTEND == second.TESTEND) {

	  if (getVersion(first.TESTNAME) == getVersion(second.TESTNAME))
	    return (*this)(first.TESTOPID, second.TESTOPID);
	  else
	    return getVersion(first.TESTNAME) > getVersion(second.TESTNAME);

	} else {
	  
	  return first.TESTEND > second.TESTEND;
	}
	
      } else {

	return first.SERIALNUMBER > second.SERIALNUMBER;
      }
    }
  };
}

#endif