JDynamics.hh

Go to the documentation of this file.

#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/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();

          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]
       */
      void 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);
          }
        }
      }

    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] = JMODEL::JString(i->tx, i->ty);
          }
        }

        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]
       */
      void 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);
          }
        }
      }

    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                          detector
     */
    const JDetector& update(const double t1_s)
    {
      if (!in_range(t1_s)) {

        JUTCTimeRange range;

        orientation.update(static_cast<JDetector&>(*this), t1_s);
        position   .update(static_cast<JDetector&>(*this), t1_s);

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

        setUTCTimeRange(range);
      }

      return getDetector();
    }


    /**
     * Get detector calibrated at given time.
     *
     * \param  chronometer              chronometer
     * \return                          detector
     */
    const JDetector& 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