#ifndef __JDETECTOR__JDETECTORTOOLKIT__
#define __JDETECTOR__JDETECTORTOOLKIT__

#include <limits>
#include <istream>
#include <ostream>
#include <iostream>
#include <fstream>
#include <cmath>
#include <vector>
#include <set>
#include <algorithm>
#include <cstdlib>

#include "JDetector/JDetector.hh"
#include "JDetector/JMonteCarloDetector.hh"
#include "JDetector/JTimeRange.hh"
#include "JDetector/JLocation.hh"
#include "JDetector/JStringCounter.hh"
#include "JDetector/JPMTIdentifier.hh"
#include "JGeometry3D/JVector3D.hh"
#include "JGeometry3D/JVersor3D.hh"
#include "JGeometry3D/JCylinder3D.hh"
#include "JGeometry3D/JRotation3D.hh"
#include "JPhysics/JConstants.hh"
#include "JMath/JConstants.hh"
#include "JMath/JMathToolkit.hh"
#include "JTools/JRange.hh"
#include "JIO/JFileStreamIO.hh"
#include "Jeep/JeepToolkit.hh"
#include "JLang/JException.hh"
#include "JLang/gzstream.h"
#include "JLang/JManip.hh"
#include "JLang/Jpp.hh"


/**
 * \author mdejong
 */

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

/**
 * Auxiliary classes and methods for detector calibration and simulation.
 */
namespace JDETECTOR {

  using JLANG::JException;
  using JLANG::JFileOpenException;
  using JLANG::JProtocolException;
  using JLANG::JValueOutOfRange;
  using JGEOMETRY3D::JRotation3D;


  /**
   * File name extensions.
   */
  static const char* const GENDET_DETECTOR_FILE_FORMAT   = "det";             //!< file format used by gendet
  static const char* const BINARY_DETECTOR_FILE_FORMAT[] = { "dat", "datx" }; //!< JIO binary file format
  static const char* const KM3NET_DETECTOR_FILE_FORMAT   = "detx";            //!< %KM3NeT standard ASCII format
  static const char* const ZIPPED_DETECTOR_FILE_FORMAT   = "gz";              //!< zipped %KM3NeT standard ASCII format
  static const char* const GDML_DETECTOR_FILE_FORMAT     = "gdml";            //!< KM3Sim input format

  static const char* const GDML_SCHEMA                   = getenv("GDML_SCHEMA_DIR");  //!< directory necessary for correct GDML header output
  static const char* const CAN_MARGIN_M	                 = getenv("CAN_MARGIN_M");     //!< extension of the detector size to comply with the can definition
  static const char* const G4GDML_DEFAULT_SCHEMALOCATION = "http://service-spi.web.cern.ch/service-spi/app/releases/GDML/schema/gdml.xsd";


  /**
   * Get maximal distance between modules in detector.
   * The option can be used to include base modules, if any.
   *
   * \param  detector          detector
   * \param  option            option
   * \return                   maximal distance [m]
   */
  inline double getMaximalDistance(const JDetector& detector, const bool option = false)
  {
    using namespace JPP;

    double dmax = 0.0;
    
    for (JDetector::const_iterator i1 = detector.begin(); i1 != detector.end(); ++i1) {
      for (JDetector::const_iterator i2 = detector.begin(); i2 != i1; ++i2) {

	if (option || (i1->getFloor() != 0 && i2->getFloor() != 0)) {

	  const double ds = getDistance(i1->getPosition(), i2->getPosition());

	  if (ds > dmax) {
	    dmax = ds;
	  }
	}
      }
    }
    
    return dmax;
  }


  /**
   * Get maximal time between optical modules in detector following causality.
   *
   * \param  detector          detector
   * \return                   maximal time [ns]
   */
  inline double getMaximalTime(const JDetector& detector)
  {
    using namespace JPP;

    return getMaximalDistance(detector) * getIndexOfRefraction() * getInverseSpeedOfLight();
  }


  /**
   * Get maximal time between optical modules in detector following causality.
   * The road width corresponds to the maximal distance traveled by the light.
   *
   * \param  detector          detector
   * \param  roadWidth_m       road width [m]
   * \return                   maximal time [ns]
   */
  inline double getMaximalTime(const JDetector& detector, const double roadWidth_m)
  {
    using namespace JPP;

    const double Dmax_m = getMaximalDistance(detector);

    return (sqrt((Dmax_m + roadWidth_m*getSinThetaC()) * 
		 (Dmax_m - roadWidth_m*getSinThetaC()))  +  
	    roadWidth_m * getSinThetaC() * getTanThetaC()) * getInverseSpeedOfLight();
  }


  /**
   * Get de-calibrated time range.
   *
   * The de-calibrated time range is corrected for minimal and maximal time offset of PMTs in given module.
   *
   * \param  timeRange         time range [ns]
   * \param  module            module
   * \return                   time range [ns]
   */
  inline JTimeRange getTimeRange(const JTimeRange& timeRange, const JModule& module)
  {
    if (!module.empty()) {

      JTimeRange time_range(JTimeRange::DEFAULT_RANGE());
      
      for (JModule::const_iterator pmt = module.begin(); pmt != module.end(); ++pmt) {
	
	const JCalibration& calibration = pmt->getCalibration();
	
	time_range.include(putTime(timeRange.getLowerLimit(), calibration));
	time_range.include(putTime(timeRange.getUpperLimit(), calibration));
      }
      
      return time_range;

    } else {
      
      return timeRange;
    }
  }

  
  /**
   * Get number of PMTs.
   *
   * \param  module            module
   * \return                   number of PMTs
   */
  inline int getNumberOfPMTs(const JModule& module)
  {
    return module.size();
  }

  
  /**
   * Get number of PMTs.
   *
   * \param  detector          detector
   * \return                   number of PMTs
   */
  inline int getNumberOfPMTs(const JDetector& detector)
  {
    int number_of_pmts = 0;
    
    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
      number_of_pmts += getNumberOfPMTs(*module);
    }

    return number_of_pmts;
  }

  
  /**
   * Get list of strings identifiers.
   *
   * \param  detector          detector
   * \return                   list of string identifiers
   */
  inline std::set<int> getStringIDs(const JDetector& detector)
  {
    std::set<int> buffer;
    
    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
      buffer.insert(module->getString());
    }

    return buffer;
  }

  
  /**
   * Get list of modules identifiers.
   *
   * \param  detector          detector
   * \param  option            option to include base modules
   * \return                   list of module identifiers
   */
  inline std::set<int> getModuleIDs(const JDetector& detector, const bool option = false)
  {
    std::set<int> buffer;
    
    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
      if (option || !module->empty()) {
	buffer.insert(module->getID());
      }
    }

    return buffer;
  }

  
  /**
   * Get number of floors.
   *
   * \param  detector          detector
   * \return                   number of floors
   */
  inline int getNumberOfFloors(const JDetector& detector)
  {
    std::set<int> buffer;
    
    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
      buffer.insert(module->getFloor());
    }

    return buffer.size();
  }

  
  /**
   * Type definition for range of floors.
   */
  typedef JTOOLS::JRange<int>  floor_range;


  /**
   * Get range of floors.
   *
   * \param  detector          detector
   * \return                   range of floors
   */
  inline floor_range getRangeOfFloors(const JDetector& detector)
  {
    floor_range buffer = floor_range::DEFAULT_RANGE();
    
    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
      buffer.include(module->getFloor());
    }

    return buffer;
  }

  
  /**
   * Get number of modules.
   *
   * \param  detector          detector
   * \return                   number of modules
   */
  inline int getNumberOfModules(const JDetector& detector)
  {
    std::set<int> buffer;
    
    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
      buffer.insert(module->getID());
    }

    return buffer.size();
  }


  /**
   * Get rotation over X axis in Geant4 coordinate system
   *
   * \param  dir            direction
   * \return                X-rotation [deg]
   */
  inline double GetXrotationG4(const JVersor3D& dir)
  {
    using namespace JPP;

    const double phi = atan2(dir.getDY(), dir.getDZ())*(180.0/PI);

    if (phi < 0.0){
      return phi + 360.0;
    }
    else{
      return phi;	
    }
  }


  /**
   * Get rotation over Y axis in Geant4 coordinate system
   *
   * \param  dir            direction
   * \return                Y-rotation [deg]
   */
  inline double GetYrotationG4(const JVersor3D& dir)
  {
    using namespace JPP;

    return asin(-dir.getDX())*(180.0/PI);
  }


  inline void read_gdml(std::istream&, JDetector&)
  {
    THROW(JException, "Operation not defined.");
  }


  /**
   * Writes KM3Sim GDML input file from detector
   *
   * \param  out           output stream
   * \param  detector      detector
   */
  inline void write_gdml(std::ostream& out, const JDetector& detector)
  {
    using namespace std;
    using namespace JPP;

    const double DEFAULT_CAN_MARGIN_M   = 350.0;  // default can   margin [m]
    const double DEFAULT_WORLD_MARGIN_M =  50.0;  // default world margin [m]

    const JCylinder3D cylinder(detector.begin(), detector.end());

    double can_margin;

    if (CAN_MARGIN_M) {
      can_margin = atof(CAN_MARGIN_M);
    } else {
      cerr << "CAN_MARGIN_M not defined! Setting standard CAN_MARGIN_M " << DEFAULT_CAN_MARGIN_M << " m." << endl;
      can_margin = DEFAULT_CAN_MARGIN_M; //this is given in meters like all the dimensions in the GDML file (look at the unit field of every position and solid definition)
    }

    const double WorldCylinderHeight = 2*(cylinder.getZmax() - cylinder.getZmin() + can_margin + DEFAULT_WORLD_MARGIN_M);
    const double WorldRadius         = cylinder.getLength() + cylinder.getRadius() + can_margin + DEFAULT_WORLD_MARGIN_M;

    const double Crust_Z_size     =  WorldCylinderHeight/2;
    const double Crust_Z_position = -WorldCylinderHeight/4;			

    out << "<?xml version=\"1.0\" encoding=\"UTF-8\" ?>\n<gdml xmlns:gdml=\"http://cern.ch/2001/Schemas/GDML\" xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xsi:noNamespaceSchemaLocation=\"";
    if (!GDML_SCHEMA) {
      cerr << "GDML_SCHEMA_DIR NOT DEFINED! Setting default path." << endl;
      out << G4GDML_DEFAULT_SCHEMALOCATION << "\">\n\n\n";
    } else {
      out << GDML_SCHEMA << "gdml.xsd\">\n\n\n";
    }
    out << "<!-- Jpp version: "<< getGITVersion() << " -->\n";
    out << "<define>\n";
    out << "<rotation name=\"identity\"/>\n<position name=\"zero\"/>\n";

    int PMTs_NO = 0;

    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
			
    if(module->empty()) continue;

	const JVector3D center = module->getCenter();

	out << "<position name=\"PosString" << module->getString() << "_Dom" << module->getID() << "\" unit=\"m\" x=\"" << module->getX() << "\" y=\"" << module->getY() << "\" z=\"" << module->getZ() << "\"/>\n";
					
	for (JModule::const_iterator pmt = module->begin(); pmt != module->end(); ++pmt) {
				
	  const JVector3D vec = static_cast<JVector3D>(*pmt).sub(center);
	  out << "<position name=\"CathodPosition" << pmt->getID() << "_" << module->getID() << "\" unit=\"m\" x=\"" << vec.getX() << "\" y=\"" << vec.getY() << "\" z=\"" << vec.getZ() << "\"/>\n";
	  out << "<rotation name=\"CathodRotation" << pmt->getID() << "_" << module->getID() << "\" unit=\"deg\" x=\"" << GetXrotationG4(*pmt) << "\" y=\"" << GetYrotationG4(*pmt) << "\" z=\"0.000000\"/>\n";
	  out << "<constant name=\"CathodID_" << PMTs_NO << "\" value=\"" << pmt->getID() << "\"/>\n";
	  PMTs_NO++;
	}
    
    }

    out << "<position name=\"OMShift\" unit=\"m\" x=\"0\" y=\"0\" z=\"0.0392\"/>\n";
    out << "\n\n\n";
    out << "<!-- end of DU position definitions -->\n<position name=\"CrustPosition\"   unit=\"m\" x=\"0\" y=\"0\" z=\"" << Crust_Z_position << "\"/>\n";
		
    out << "</define>\n";
    out << "<materials>\n";
    out << "</materials>\n";
    out << "<solids>\n";
    out << "	<box name=\"CrustBox\" lunit=\"m\" x=\"2200\" y=\"2200\" z=\"" << Crust_Z_size << "\"/>\n";
    out << "	<box name=\"StoreyBox\" lunit=\"m\" x=\"0.6\" y=\"0.6\" z=\"0.6\"/>\n";
    out << "	<sphere name=\"OMSphere\" lunit=\"cm\" aunit=\"deg\" rmin=\"0.0\" rmax=\"21.6\" startphi=\"0.0\" deltaphi=\"360.0\" starttheta=\"0.0\" deltatheta=\"180.0\"/>\n";
    out << "	<tube name=\"CathodTube\" lunit=\"cm\" aunit=\"deg\" rmin=\"0.0\" rmax=\"4.7462\" z=\"0.5\" startphi=\"0.0\" deltaphi=\"360.0\"/>\n";
    out << "	<tube name=\"WorldTube\" lunit=\"m\" aunit=\"deg\" rmin=\"0.0\" rmax=\"" << WorldRadius << "\" z=\"" << WorldCylinderHeight << "\" startphi=\"0.0\" deltaphi=\"360.0\"/>\n";	
    out << "</solids>\n\n\n";
		
    out << "<structure>\n";
    out << "	<volume name=\"CathodVolume\">\n";
    out << "		<materialref ref=\"Cathod\"/>\n";
    out << "		<solidref ref=\"CathodTube\"/>\n";
    out << "	</volume>\n";

    out << "<!-- OMVolume(s) construction -->\n";

    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
		
	  if(module->empty()) continue;
      out << "	<volume name=\"OMVolume" << module->getID() << "\">\n";
      out << "		<materialref ref=\"Water\"/>\n";
      out << "		<solidref ref=\"OMSphere\"/>\n";

      for (JModule::const_iterator pmt = module->begin(); pmt != module->end(); ++pmt) {
        out << "		<physvol>\n";
        out << "			<volumeref ref=\"CathodVolume\"/>\n";
        out << "			<positionref ref=\"CathodPosition" << pmt->getID() << "_" << module->getID() << "\"/>\n";
        out << "			<rotationref ref=\"CathodRotation" << pmt->getID() << "_" << module->getID() << "\"/>\n";
        out << "		</physvol>\n";
      }

      out << "	</volume>\n";
    }

    out << "<!-- StoreyVolume(s) construction -->\n";

    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
      if(module->empty()) continue;		
      out << "	<volume name=\"StoreyVolume" << module->getID() << "\">\n";
      out << "		<materialref ref=\"Water\"/>\n";
      out << "		<solidref ref=\"StoreyBox\"/>\n";
      out << "		<physvol>\n";
      out << "			<volumeref ref=\"OMVolume" << module->getID() << "\"/>\n";
      out << "			<positionref ref=\"OMShift\"/>\n";
      out << "			<rotationref ref=\"identity\"/>\n";
      out << "		</physvol>\n";
      out << "	</volume>\n";
    }

    out << "<!-- Crust Volume construction-->\n";
    out << "<volume name=\"CrustVolume\">\n";
    out << "	<materialref ref=\"Crust\"/>\n";
    out << "	<solidref ref=\"CrustBox\"/>\n";
    out << "</volume>\n";

    out << "<!-- World Volume construction-->\n";
    out << "<volume name=\"WorldVolume\">\n";
    out << "	<materialref ref=\"Water\"/>\n";
    out << "	<solidref ref=\"WorldTube\"/>\n";

    out << "	<physvol>\n";
    out << "		<volumeref ref=\"CrustVolume\"/>\n";
    out << "		<positionref ref=\"CrustPosition\"/>\n";
    out << "		<rotationref ref=\"identity\"/>\n";
    out << "	</physvol>\n";

    for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
      if(module->empty()) continue;		
      out << "	<physvol>\n";
      out << "		<volumeref ref=\"StoreyVolume" << module->getID() << "\"/>\n";
      out << "		<positionref ref=\"PosString" <<  module->getString()  << "_Dom" <<  module->getID()  << "\"/>\n";
      out << "			<rotationref ref=\"identity\"/>\n";
      out << "	</physvol>\n";	
    }

    out << "</volume>\n";

    out << "</structure>\n";
    out << "<setup name=\"Default\" version=\"1.0\">\n";
    out << "<world ref=\"WorldVolume\"/>\n";
    out << "</setup>\n";
    out << "</gdml>\n";
  }
  
  
  /**
   * Load detector from input file.
   *
   * Supported file formats:
   *   - ASCII   file, extension JDETECTOR::GENDET_DETECTOR_FILE_FORMAT, gendet           format; 
   *   - binary  file, extension JDETECTOR::BINARY_DETECTOR_FILE_FORMAT, Jpp     internal format;
   *   - ASCII   file, extension JDETECTOR::KM3NET_DETECTOR_FILE_FORMAT, %KM3NeT standard format; 
   *   - gzipped file, extension JDETECTOR::ZIPPED_DETECTOR_FILE_FORMAT, %KM3NeT standard format; 
   *
   * \param  file_name         file name
   * \param  detector          detector
   */
  inline void load(const std::string& file_name, JDetector& detector)
  {
    using namespace std;
    using namespace JPP;
    
    if        (getFilenameExtension(file_name) == GENDET_DETECTOR_FILE_FORMAT) {

      JMonteCarloDetector buffer(true);

      ifstream in(file_name.c_str());

      if (!in) {
	THROW(JFileOpenException, "File not opened for reading: " << file_name);
      }
      
      in >> buffer;

      in.close();

      detector.swap(buffer);

    } else if (getFilenameExtension(file_name) == BINARY_DETECTOR_FILE_FORMAT[0] ||
	       getFilenameExtension(file_name) == BINARY_DETECTOR_FILE_FORMAT[1]) {

      JFileStreamReader in(file_name.c_str());

      if (!in) {
	THROW(JFileOpenException, "File not opened for reading: " << file_name);
      }
      
      try {

	detector.read(in);

      } catch(const exception& error) {

	// recovery procedure for old format of comments

	JDetector::setStartOfComment(JDetector::OLD_START_OF_COMMENT);

	in.clear();
	in.rewind();

	detector.read(in);

	JDetector::setStartOfComment(JDetector::NEW_START_OF_COMMENT);
      }

      in.close();
 
    } else if (getFilenameExtension(file_name) == KM3NET_DETECTOR_FILE_FORMAT) {
      
      ifstream in(file_name.c_str());

      if (!in) {
	THROW(JFileOpenException, "File not opened for reading: " << file_name);
      }

      in >> detector;

      in.close();

    } else if (getFilenameExtension(file_name) == ZIPPED_DETECTOR_FILE_FORMAT) {
      
      igzstream in(file_name.c_str());

      if (!in) {
	THROW(JFileOpenException, "File not opened for reading: " << file_name);
      }
      
      in >> detector;

      in.close();

    } else {

      THROW(JProtocolException, "Protocol not defined: " << file_name);
    }

    if (!detector.hasVersion()) {
      THROW(JValueOutOfRange, "Invalid version <" << detector.getVersion() << ">");
    }
  }
  
  
  /**
   * Store detector to output file.
   *
   * Supported file formats:
   *   - binary  file, extension JDETECTOR::BINARY_DETECTOR_FILE_FORMAT, Jpp internal format;
   *   - ASCII   file, extension JDETECTOR::KM3NET_DETECTOR_FILE_FORMAT, %KM3NeT standard format; 
   *   - gzipped file, extension JDETECTOR::ZIPPED_DETECTOR_FILE_FORMAT, %KM3NeT standard format; 
   *   - gdml    file, extension JDETECTOR::GDML_DETECTOR_FILE_FORMAT,   KM3Sim input format;
   *
   * \param  file_name         file name
   * \param  detector          detector
   */
  inline void store(const std::string& file_name, const JDetector& detector)
  {
    using namespace std;
    using namespace JPP;
    
    if        (getFilenameExtension(file_name) == BINARY_DETECTOR_FILE_FORMAT[1]) {

      JFileStreamWriter out(file_name.c_str());

      if (!out) {
	THROW(JFileOpenException, "File not opened for writing: " << file_name);
      }

      detector.write(out);

      out.close();
 
    } else if (getFilenameExtension(file_name) == KM3NET_DETECTOR_FILE_FORMAT) {
      
      std::ofstream out(file_name.c_str());

      if (!out) {
	THROW(JFileOpenException, "File not opened for writing: " << file_name);
      }

      out << detector;

      out.close();

    } else if (getFilenameExtension(file_name) == ZIPPED_DETECTOR_FILE_FORMAT) {
      
      ogzstream out(file_name.c_str());

      if (!out) {
	THROW(JFileOpenException, "File not opened for writing: " << file_name);
      }

      out << detector;

      out.close();

    } else if (getFilenameExtension(file_name) == GDML_DETECTOR_FILE_FORMAT) {
      
      std::ofstream out(file_name.c_str());

      if (!out) {
	THROW(JFileOpenException, "File not opened for writing: " << file_name);
      }

      write_gdml(out, detector);

      out.close();

    } else {

      THROW(JProtocolException, "Protocol not defined: " << file_name);
    }
  }


  /**
   * Auxiliary class to get rotation matrix between two optical modules.
   */
  struct JRotation :
    public JRotation3D
  {

    static const size_t NUMBER_OF_DIMENSIONS = 3;    //!< Number of dimensions
    

    /**
     * Get rotation matrix to go from first to second module.
     *
     * \param  first       first  module
     * \param  second      second module
     * \return             rotation matrix
     */ 
    const JRotation3D& operator()(const JModule& first, const JModule& second) 
    {
      this->setIdentity();

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

	const size_t N = first.size();

	if (N >= NUMBER_OF_DIMENSIONS) {

	  in .resize(N);
	  out.resize(N);
	
	  for (size_t i = 0; i != N; ++i) {
	    in [i] = first .getPMT(i).getDirection();
	    out[i] = second.getPMT(i).getDirection();
	  }

	  for (size_t i = 0; i != NUMBER_OF_DIMENSIONS; ++i) {
	    if (!orthonormalise(i)) {
	      THROW(JException, "Failure to orthonormalise direction " << i);
	    }
	  }

	  this->a00 = out[0].getX() * in[0].getX()  +  out[1].getX() * in[1].getX()  +  out[2].getX() * in[2].getX();
	  this->a01 = out[0].getX() * in[0].getY()  +  out[1].getX() * in[1].getY()  +  out[2].getX() * in[2].getY();
	  this->a02 = out[0].getX() * in[0].getZ()  +  out[1].getX() * in[1].getZ()  +  out[2].getX() * in[2].getZ();
	  
	  this->a10 = out[0].getY() * in[0].getX()  +  out[1].getY() * in[1].getX()  +  out[2].getY() * in[2].getX();
	  this->a11 = out[0].getY() * in[0].getY()  +  out[1].getY() * in[1].getY()  +  out[2].getY() * in[2].getY();
	  this->a12 = out[0].getY() * in[0].getZ()  +  out[1].getY() * in[1].getZ()  +  out[2].getY() * in[2].getZ();
	  
	  this->a20 = out[0].getZ() * in[0].getX()  +  out[1].getZ() * in[1].getX()  +  out[2].getZ() * in[2].getX();
	  this->a21 = out[0].getZ() * in[0].getY()  +  out[1].getZ() * in[1].getY()  +  out[2].getZ() * in[2].getY();
	  this->a22 = out[0].getZ() * in[0].getZ()  +  out[1].getZ() * in[1].getZ()  +  out[2].getZ() * in[2].getZ();

	} else {

	  THROW(JException, "Module " << first.getID() << " size " << N << " < " << NUMBER_OF_DIMENSIONS);
	}

      } else {

	THROW(JException, "Module " << first.getID() << " size " << first.size() << " != " << second.size());
      }

      return *this;
    }

  private:
    /**
     * Put normalised primary direction at specified index and orthoganilise following directions.\n
     * This procedure follows Gram-Schmidt process.
     *
     * \param  index       index 
     * \param  precision   precision
     * \return             true if primary direction exists; else false
     */
    bool orthonormalise(const size_t index, const double precision = std::numeric_limits<double>::epsilon())
    {
      using namespace std;

      size_t pos = index;

      for (size_t i = index + 1; i != in.size(); ++i) {
	if (in[i].getLengthSquared() > in[pos].getLengthSquared()) {
	  pos = i;
	}
      }

      const double u = in[pos].getLength();

      if (u > precision) {

	in [pos] /= u;
	out[pos] /= u;

	if (pos != index) {
	  swap(in [pos], in [index]);
	  swap(out[pos], out[index]);
	}

	for (size_t i = index + 1; i != in.size(); ++i) {

	  const double dot = in[index].getDot(in[i]);

	  in [i] -= dot * in [index];
	  out[i] -= dot * out[index];
	}

	return true;

      } else {

	return false;
      }
    }


    std::vector<JVector3D> in;
    std::vector<JVector3D> out;
  };

  
  /**
   * Function object to get rotation matrix to go from first to second module.
   */
  static JRotation getRotation;

  
  /**
   * Get position to go from first to second module.
   *
   * \param  first       first  module
   * \param  second      second module
   * \return             position
   */ 
  inline JPosition3D getPosition(const JModule& first, const JModule& second)
  {
    return second.getPosition() - first.getPosition();
  }

  
  /**
   * Get calibration to go from first to second calibration.
   *
   * \param  first       first  calibration
   * \param  second      second calibration
   * \return             calibration
   */ 
  inline JCalibration getCalibration(const JCalibration& first, const JCalibration& second)
  {
    return JCalibration(second.getT0() - first.getT0());
  }
}

#endif