#ifndef __JDYNAMICS__JDYNAMICS__
#define __JDYNAMICS__JDYNAMICS__

#include <ostream>
#include <set>

#include "JDetector/JDetector.hh"
#include "JDetector/JDetectorToolkit.hh"
#include "JDetector/JDetectorAddressMapToolkit.hh"
#include "JUTC/JUTCTimeRange.hh"

#include "JCompass/JEvt.hh"
#include "JCompass/JEvtToolkit.hh"
#include "JCompass/JSupport.hh"

#include "JAcoustics/JModel.hh"
#include "JAcoustics/JGeometry.hh"
#include "JAcoustics/JEvt.hh"
#include "JAcoustics/JEvtToolkit.hh"
#include "JAcoustics/JSupport.hh"

#include "JLang/JObjectIterator.hh"
#include "JLang/JException.hh"

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

#include "JTools/JElement.hh"
#include "JTools/JCollection.hh"
#include "JTools/JPolfit.hh"
#include "JTools/JHashMap.hh"
#include "JTools/JRange.hh"

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


/**
 * \file
 *
 * Dynamic detector calibration.
 * \author mdejong
 */

namespace JDYNAMICS {}
namespace JPP { using namespace JDYNAMICS; }

/**
 * Main namespace for dynamic position and orientation calibration.
 */
namespace JDYNAMICS {

  using JLANG::JObjectIterator;
  using JLANG::JValueOutOfRange;
  using JDETECTOR::JDetector;
  using KM3NETDAQ::JDAQChronometer;
  using JUTC::JUTCTimeRange;


  /**
   * Dynamic detector calibration.
   */
  struct JDynamics :
    public JDetector
  {
    /**
     * Auxiliary data structure to track applicability period of calibration data.
     */
    struct JUTCTracker :
      public JUTCTimeRange
    {
      /**
       * Constructor.
       *
       * \param  Tmax_s                   applicability period [s]
       */
      JUTCTracker(const double Tmax_s) :
	JUTCTimeRange(-Tmax_s, +Tmax_s)
      {}


      /**
       * Reset.
       */
      void reset()
      {
	set(0.0);
      }


      /**
       * Set.
       *
       * \param  t0_s                     UTC time [s]
       */
      void set(const double t0_s)
      {
	const double Tmax_s = 0.5 * (getUpperLimit() - getLowerLimit());

	setRange(t0_s - Tmax_s, t0_s + Tmax_s);
      }
    };


    /**
     * Dynamic orientation calibration.
     */
    struct JOrientation :
      public JUTCTracker
    {
      enum {
	NUMBER_OF_POINTS  = 20,    //!< number of points  for interpolation
	NUMBER_OF_DEGREES =  1     //!< number of degrees for interpolation
      };

      typedef JTOOLS::JElement2D<double, JGEOMETRY3D::JQuaternion3D>          element_type;
      typedef JTOOLS::JPolfitFunction1D<NUMBER_OF_POINTS,
					NUMBER_OF_DEGREES,
					element_type, JTOOLS::JCollection>    function_type;
      typedef typename function_type::collection_type::container_type         container_type;
  
      typedef JTOOLS::JHashMap<int, JGEOMETRY3D::JQuaternion3D>               buffer_type;
      typedef JTOOLS::JHashMap<int, function_type>                            data_type;

      typedef data_type::const_iterator                                       const_iterator;
      typedef data_type::const_reverse_iterator                               const_reverse_iterator;
      

      /**
       * Constructor.
       *
       * \param  detector                 detector
       * \param  Tmax_s                   applicability period of calibration [s]
       */
      JOrientation(const JDetector& detector,
		   const double     Tmax_s) :
	JUTCTracker(Tmax_s)
      {
	using namespace JPP;

	if (getDetectorVersion(detector.getVersion()) < JDetectorVersion::V4) {
	  THROW(JValueOutOfRange, "Detector version " << detector.getVersion() << " < " << JDetectorVersion::V4);
	}

	if (hasDetectorAddressMap(detector.getID())) {

	  const JDetectorAddressMap& demo = getDetectorAddressMap(detector.getID());

	  for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
	    if (module->getFloor() != 0) {
	      buffer[module->getID()] = getRotation(JDETECTOR::getModule(demo.get(module->getID())), *module);
	    }
	  }

	} else {

	  THROW(JValueOutOfRange, "No detector address map for detector identier " << detector.getID());
	}
      }


      /**
       * Load calibration data.
       *
       * \param  input                    calibration data
       */
      void load(JObjectIterator<JCOMPASS::JOrientation>& input)
      {
	this->reset();

	while (input.hasNext()) {

	  const JCOMPASS::JOrientation* orientation = input.next();

	  if (buffer.has(orientation->id)) {
	    static_cast<container_type&>(calibration[orientation->id]).push_back(element_type(orientation->t, getQuaternion(*orientation)));
	  }
	}

	for (data_type::iterator i = calibration.begin(); i != calibration.end(); ++i) {
	  i->second.sort();
	  i->second.compile();
	}
      }


      bool                   empty()  const { return calibration.empty(); }    //!< empty
      size_t                 size()   const { return calibration.size(); }     //!< size
      const_iterator         begin()  const { return calibration.begin(); }    //!< begin of calibration data
      const_iterator         end()    const { return calibration.end(); }      //!< end   of calibration data
      const_reverse_iterator rbegin() const { return calibration.rbegin(); }   //!< begin of reverse of calibration data
      const_reverse_iterator rend()   const { return calibration.rend(); }     //!< begin of reverse of calibration data


      /**
       * Get coverage.
       *
       * \param  detector                 detector
       * \param  t1_s                     UTC time [s]
       * \return                          coverage [0,1]
       */
      double getCoverage(const JDetector& detector, const double t1_s) const
      {
	int n0 = 0;
	int n1 = 0;

	for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
          
	  if (module->getFloor() != 0 && !module->has(COMPASS_DISABLE)) {

	    ++n0;

	    if (calibration.has(module->getID()) && !calibration.get(module->getID()).empty()) {
	      if (calibration[module->getID()].getXmin() <= t1_s && 
		  calibration[module->getID()].getXmax() >= t1_s) {
		++n1;
	      }
	    }
	  }
	}

	if (n0 != 0)
	  return (double) n1 / (double) n0;
	else
	  return 0.0;
      }


      /**
       * Calibrate given detector at given UTC time.
       *
       * \param  detector                 detector (I/O)
       * \param  t1_s                     UTC time [s]
       * \return                          true if updated; else false
       */
      bool update(JDetector& detector, const double t1_s)
      {
	using namespace std;
	using namespace JPP;

	if (!calibration.empty()) {

	  if (!in_range(t1_s)) {

	    for (JDetector::iterator module = detector.begin(); module != detector.end(); ++module) {
          
	      if (module->getFloor() != 0 && !module->has(COMPASS_DISABLE) && calibration.has(module->getID()) && buffer.has(module->getID())) {

		const function_type& f1 = calibration.get(module->getID());

		if (!f1.empty()) {

		  if (t1_s >= f1.getXmin() && t1_s <= f1.getXmax()) {

		    JQuaternion3D Q0 = buffer[module->getID()];
		    JQuaternion3D Q1 = module->getQuaternion() * f1(t1_s);

		    const JPosition3D center = module->getPosition();

		    module->sub(center);

		    module->rotate(Q1 * Q0.conjugate());

		    module->add(center);

		    buffer[module->getID()] = Q1;
		  }
		}
	      }
	    }

	    set(t1_s);

	    return true;
	  }
	}

	return false;
      }

    protected:
      buffer_type buffer;
      data_type   calibration;
    };


    /**
     * Dynamic position calibration.
     */
    struct JPosition :
      public JUTCTracker
    {
      enum {
	NUMBER_OF_POINTS  = 7,    //!< number of points  for interpolation
	NUMBER_OF_DEGREES = 2     //!< number of degrees for interpolation
      };

      typedef JACOUSTICS::JGeometry                                           JGeometry;

      typedef JTOOLS::JElement2D<double, JACOUSTICS::JMODEL::JString>         element_type;
      typedef JTOOLS::JPolfitFunction1D<NUMBER_OF_POINTS,
					NUMBER_OF_DEGREES,
					element_type, JTOOLS::JCollection>    function_type;

      typedef JTOOLS::JHashMap<int, function_type>                            data_type;

      typedef data_type::const_iterator                                       const_iterator;
      typedef data_type::const_reverse_iterator                               const_reverse_iterator;

      
      /**
       * Constructor.
       *
       * \param  detector                 detector
       * \param  Tmax_s                   applicability period of calibration [s]
       */
      JPosition(const JDetector& detector,
		const double     Tmax_s) :
	JUTCTracker(Tmax_s),
	geometry(detector)
      {
	using namespace JPP;

	if (getDetectorVersion(detector.getVersion()) < JDetectorVersion::V4) {
	  THROW(JValueOutOfRange, "Detector version " << detector.getVersion() << " < " << JDetectorVersion::V4);
	}
      }


      /**
       * Load calibration data.
       *
       * \param  input                    calibration data
       */
      void load(JObjectIterator<JACOUSTICS::JEvt>& input)
      {
	using namespace JPP;
	
	this->reset();

	while (input.hasNext()) {

	  const JACOUSTICS::JEvt* evt = input.next();
	  const double t1_s = 0.5 * (evt->UNIXTimeStart + evt->UNIXTimeStop);

	  for (JACOUSTICS::JEvt::const_iterator i = evt->begin(); i != evt->end(); ++i) {
	    calibration[i->id][t1_s] = getModel(*i);
	  }
	}

	for (data_type::iterator i = calibration.begin(); i != calibration.end(); ++i) {
	  i->second.compile();
	}
      }


      bool                   empty()  const { return calibration.empty(); }    //!< empty
      size_t                 size()   const { return calibration.size(); }     //!< size
      const_iterator         begin()  const { return calibration.begin(); }    //!< begin of calibration data
      const_iterator         end()    const { return calibration.end(); }      //!< end   of calibration data
      const_reverse_iterator rbegin() const { return calibration.rbegin(); }   //!< begin of reverse of calibration data
      const_reverse_iterator rend()   const { return calibration.rend(); }     //!< begin of reverse of calibration data


      /**
       * Get coverage.
       *
       * \param  detector                 detector
       * \param  t1_s                     UTC time [s]
       * \return                          coverage [0,1]
       */
      double getCoverage(const JDetector& detector, const double t1_s) const
      {
	int n0 = 0;
	int n1 = 0;

	std::set<int> string;
	
	for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
	  if (module->getFloor() != 0) {
	    string.insert(module->getString());
	  }
	}

	for (std::set<int>::const_iterator i = string.begin(); i != string.end(); ++i) {

	  ++n0;

	  if (calibration.has(*i) && !calibration.get(*i).empty()) {
	    if (calibration[*i].getXmin() <= t1_s && 
		calibration[*i].getXmax() >= t1_s) {
	      ++n1;
	    }
	  }
	}

	if (n0 != 0)
	  return (double) n1 / (double) n0;
	else
	  return 0.0;
      }


      /**
       * Get detector geometry.
       *
       * \return                          detector geometry
       */
      const JGeometry&  getGeometry() const
      {
	return geometry;
      }


      /**
       * Calibrate given detector at given UTC time.
       *
       * \param  detector                 detector (I/O)
       * \param  t1_s                     UTC time [s]
       * \return                          true if updated; else false
       */
      bool update(JDetector& detector, const double t1_s)
      {
	using namespace std;
	using namespace JPP;

	if (!calibration.empty()) {

	  if (!in_range(t1_s)) {

	    JHashMap<int, JMODEL::JString> buffer;

	    for (JDetector::iterator module = detector.begin(); module != detector.end(); ++module) {

	      if (module->getFloor() != 0) {

		if (!buffer.has(module->getString())) {

		  if (calibration.has(module->getString())) {

		    const function_type& f1 = calibration.get(module->getString());

		    if (!f1.empty()) {
		      if (t1_s >= f1.getXmin() && t1_s <= f1.getXmax()) {
			buffer[module->getString()] = f1(t1_s);
		      }
		    }
		  }
		}

		if (buffer.has(module->getString())) {
		  module->set(geometry[module->getString()].getPosition(buffer[module->getString()], module->getFloor())  -  getPiezoPosition());
		}
	      }
	    }

	    set(t1_s);

	    return true;
	  }
	}

	return false;
      }

    protected:
      JGeometry  geometry;
      data_type  calibration;
    };
    

    /**
     * Constructor.
     *
     * \param  detector                 detector
     * \param  Tmax_s                   applicability period of calibration [s]
     */
    JDynamics(const JDetector& detector,
	      const double     Tmax_s) :
      JDetector  (detector),
      orientation(detector, Tmax_s),
      position   (detector, Tmax_s)
    {
      this->setUTCTimeRange(JUTCTimeRange::DEFAULT_RANGE);
    }


    /**
     * Get actual detector.
     *
     * \return                          detector
     */
    const JDetector& getDetector() const
    {
      return static_cast<const JDetector&>(*this);
    }


    /**
     * Load calibration data.
     *
     * \param  input                    calibration data
     */
    template<class JObjectIterator_t>
    void load(JObjectIterator_t& input)
    {
      this->setUTCTimeRange(JUTCTimeRange::DEFAULT_RANGE);

      try { orientation.load(dynamic_cast<JObjectIterator<JCOMPASS::JOrientation>&>(input)); } catch(const std::exception&) {}
      try { position   .load(dynamic_cast<JObjectIterator<JACOUSTICS::JEvt>&>      (input)); } catch(const std::exception&) {}
    }


    /**
     * Get detector calibrated at given time.
     *
     * \param  t1_s                     UTC time [s]
     * \return                          true if updated; else false
     */
    bool update(const double t1_s)
    {
      bool is_updated = false;

      if (!in_range(t1_s)) {

	JUTCTimeRange range;

	if (orientation.update(static_cast<JDetector&>(*this), t1_s)) { is_updated = true; }
	if (position   .update(static_cast<JDetector&>(*this), t1_s)) { is_updated = true; }

	range.join(orientation.getUTCTimeRange());
	range.join(position   .getUTCTimeRange());

	setUTCTimeRange(range);
      }

      return is_updated;
    }


    /**
     * Get detector calibrated at given time.
     *
     * \param  chronometer              chronometer
     * \return                          true if updated; else false
     */
    bool update(const JDAQChronometer& chronometer)
    {
      return update(chronometer.getTimesliceStart().getUTCseconds());
    }
    

    /**
     * Data structure for coverage of dynamic calibrations.
     */
    struct coverage_type {
      double orientation;        //!< coverage of detector by available orientation calibration [0,1]
      double position;           //!< coverage of detector by available position    calibration [0,1]
    };


    /**
     * Get coverage at given time.
     *
     * \param  t1_s                     UTC time [s]
     * \return                          coverage
     */
    coverage_type getCoverage(const double t1_s) const
    {
      return { orientation.getCoverage(*this, t1_s), position.getCoverage(*this, t1_s) };
    }


    /**
     * Get coverage at given time.
     *
     * \param  chronometer              chronometer
     * \return                          coverage
     */
    coverage_type getCoverage(const JDAQChronometer& chronometer) const
    {
      return getCoverage(chronometer.getTimesliceStart().getUTCseconds());
    }


    /**
     * Get actual coverage.
     *
     * \return                          coverage
     */
    coverage_type getCoverage() const
    {
      return getCoverage(0.5 * (getLowerLimit() + getUpperLimit()));
    }
    

    JOrientation orientation;    //!< orientation calibration
    JPosition    position;       //!< position    calibration
  };
}

#endif