#include <string>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <utility>
#include <vector>

#include "km3net-dataformat/definitions/module_status.hh"

#include "JDB/JSupport.hh"
#include "JDB/JAHRS.hh"
#include "JDB/JAHRSCalibration_t.hh"
#include "JDB/JAHRSToolkit.hh"

#include "JSupport/JMultipleFileScanner.hh"
#include "JSupport/JFileRecorder.hh"
#include "JSupport/JMeta.hh"

#include "JDetector/JDetector.hh"
#include "JDetector/JDetectorToolkit.hh"
#include "JDetector/JModuleRouter.hh"
#include "JDetector/JCompass.hh"

#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JEigen3D.hh"

#include "JTools/JHashMap.hh"
#include "JLang/JVectorize.hh"
#include "JLang/JComparator.hh"
#include "JMath/JLegendre.hh"

#include "JCompass/JEvt.hh"
#include "JCompass/JEvtToolkit.hh"
#include "JCompass/JSupport.hh"
#include "JCompass/JNOAA.hh"
#include "JCompass/JPolicy.hh"
#include "JCompass/JCompassSupportkit.hh"

#include "Jeep/JPrint.hh"
#include "Jeep/JParser.hh"
#include "Jeep/JMessage.hh"

namespace {

  static const int NUMBER_OF_POINTS  = 20;    //!< number of points  for interpolation
  static const int NUMBER_OF_DEGREES =  1;    //!< number of degrees for interpolation

}


/**
 * \file
 *
 * Auxiliary program to process AHRS data.
 *
 * The AHRS calibration as well as the quaternion calibration are applied to the data.\n
 * Possible outliers are removed according the specified maximal angle (option <tt>-S \<angle_deg\></tt>.\n
 * For this, interpolations of the data are made based on Legendre polynomials with 
 * the number of points and the number of degrees\n
 * set to NUMBER_OF_POINTS and NUMBER_OF_DEGREES, respectively.\n
 * A policy for missing compass data is applied in which data from neighbouring compasses are used.\n
 * Finally, data within a given time window can be averaged (option <tt>-T \<T_s\></tt>.\n)
 * \author mdejong
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;

  JMultipleFileScanner_t inputFile;
  counter_type    numberOfEvents;
  JFileRecorder<JTYPELIST<JOrientation, JMeta>::typelist>  outputFile;
  string          detectorFile;
  string          ahrsFile;
  double          angle_deg;
  double          T_s;
  int             debug;

  try {

    JParser<> zap("Auxiliary program to process AHRS data.");

    zap['f'] = make_field(inputFile,    "output of JConvertDB -q ahrs");
    zap['n'] = make_field(numberOfEvents)                                    = JLimit::max();
    zap['a'] = make_field(detectorFile);
    zap['c'] = make_field(ahrsFile,     "output of JAHRSCalibration");
    zap['o'] = make_field(outputFile,   "ROOT formatted orientations file");
    zap['S'] = make_field(angle_deg,    "maximal angle w.r.t. fit")          = 0.0;
    zap['T'] = make_field(T_s,          "time window for averaging")         = 0.0;
    zap['d'] = make_field(debug)         = 1;

    zap(argc, argv);
  }
  catch(const exception &error) {
    FATAL(error.what() << endl);
  }


  JDetector detector;

  try {
    load(detectorFile, detector);
  }
  catch(const JException& error) {
    FATAL(error);
  }

  const JModuleRouter      router(detector);
  const JNOAAFunction1D_t& getMagneticDeclination = (isARCADetector(detector) ? static_cast<const JNOAAFunction1D_t&>(getARCAMagneticDeclination) :
						     isORCADetector(detector) ? static_cast<const JNOAAFunction1D_t&>(getORCAMagneticDeclination) :
						     static_cast<const JNOAAFunction1D_t&>(getZEROMagneticDeclination));
  const double             meridian               = (isARCADetector(detector) ? ARCA_MERIDIAN_CONVERGENCE_ANGLE_RAD :
						     isORCADetector(detector) ? ORCA_MERIDIAN_CONVERGENCE_ANGLE_RAD :
						     0.0);

  NOTICE("Magnetic declination " << getMagneticDeclination << endl);
  NOTICE("Meridian angle [rad] " << FIXED(5,3) << meridian << endl);

  const JAHRSCalibration_t calibration(ahrsFile.c_str());
  const JAHRSValidity      is_valid;


  typedef pair<double, JQuaternion3D>   element_type;
  typedef vector<element_type>          buffer_type;
  typedef JHashMap<int, buffer_type>    map_type;               // container for temporary storage of data

  JLegendre<JQuaternion3D, NUMBER_OF_DEGREES>  f1;              // interpolator for outlier removal


  outputFile.open();

  outputFile.put(JMeta(argc, argv));

  counter_type counter = 0;

  for (JMultipleFileScanner_t::const_iterator file_name = inputFile.begin(); file_name != inputFile.end(); ++file_name) {

    STATUS("processing file " << *file_name << "... " << flush);

    // collect data

    map_type data;

    for (JMultipleFileScanner<JAHRS> in(*file_name); in.hasNext() && counter != numberOfEvents; ++counter) {

      const JAHRS* parameters = in.next();
      const int    id         = parameters->DOMID; 

      if (is_valid(*parameters) && calibration.has(id) && router.hasModule(id)) {

	const JModule& module = router.getModule(id);

	if (module.getFloor() != 0 && !module.has(COMPASS_DISABLE)) {

	  JCompass compass(*parameters, calibration.get(id));

	  compass.correct(getMagneticDeclination(parameters->UNIXTIME * 1.0e-3), meridian);

	  const JQuaternion3D Q = compass.getQuaternion();

	  data[id].push_back(element_type(parameters->UNIXTIME * 1.0e-3, Q));
	}
      }
    }

    // remove outliers

    if (angle_deg > 0.0) {

      for (map_type::iterator module = data.begin(); module != data.end(); ++module) {

	if (!module->second.empty()) {

	  sort(module->second.begin(), module->second.end(), make_comparator(&element_type::first));

	  buffer_type::iterator       out = module->second.begin();       // output
	  buffer_type::const_iterator in  = module->second.begin();       // input
	  buffer_type::const_iterator p   = module->second.begin();       // begin interpolation data
	  buffer_type::const_iterator q   = module->second.begin();       // end   interpolation data
      
	  for (int i = 0; i != NUMBER_OF_POINTS && q != module->second.end(); ++i, ++q) {}

	  for (int i = 0; i != NUMBER_OF_POINTS/2 && in != q; ++i, ++in) {

	    f1.set(p, in, q);

	    if (getAngle(in->second, f1(in->first)) <= angle_deg) {
	      *out = *in;
	      ++out;
	    }
	  }

	  for (++p; q++ != module->second.end(); ++p, ++in) {

	    f1.set(p, in, q);

	    if (getAngle(in->second, f1(in->first)) <= angle_deg) {
	      *out = *in;
	      ++out;
	    }
	  }

	  for ( ; in != module->second.end(); ++in) {

	    f1.set(p, in, q);

	    if (getAngle(in->second, f1(in->first)) <= angle_deg) {
	      *out = *in;
	      ++out;
	    }
	  }

	  module->second.erase(out, module->second.end());                // remove leftovers
	}
      }
    }


    // apply policy for missing modules and write output
    
    const array_type<map_type::key_type>& buffer = make_array(data.begin(), data.end(), &map_type::value_type::first);

    const JPolicy policy(router, buffer.begin(), buffer.end());

    for (JPolicy::const_iterator module = policy.begin(); module != policy.end(); ++module) {
      
      buffer_type buffer;

      for (JPolicy::mapped_type::const_iterator in = module->second.begin(); in != module->second.end(); ++in) {
	for (map_type::mapped_type::const_iterator i = data[*in].begin(); i != data[*in].end(); ++i) {
	  buffer.push_back(element_type(i->first, i->second));
	}
      }

      // average

      if (T_s > 0.0) {

	sort(buffer.begin(), buffer.end(), make_comparator(&element_type::first));

	buffer_type::iterator       out = buffer.begin();                 // output
	buffer_type::const_iterator p   = buffer.begin();                 // begin averaging data
	buffer_type::const_iterator q   = buffer.begin();                 // end   averaging data

	for (p = buffer.begin(); p != buffer.end(); p = q) {

	  for (q = p; q != buffer.end() && q->first - p->first < T_s; ++q) {}

	  if (distance(p,q) > 1) {

	    const array_type<double>&        t1 = make_array(p, q, &element_type::first);
	    const array_type<JQuaternion3D>& q1 = make_array(p, q, &element_type::second);

	    out->first  = getAverage(t1.begin(), t1.end());
	    out->second = getAverage(q1.begin(), q1.end());

	  } else {

	    *out = *p;
	  }

	  ++out;
	}

	buffer.erase(out, buffer.end());                                  // remove leftovers
      }

      for (buffer_type::const_iterator i = buffer.begin(); i != buffer.end(); ++i) {
	outputFile.put(JOrientation(module->first, i->first, getQuaternion(i->second)));
      }
    }

    STATUS(endl);
  }

  JMultipleFileScanner<JMeta> io(inputFile);

  io >> outputFile;

  outputFile.close();
}