/* Copyright (c) 2017, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once
#include <random>
#include <cmath>
#include <iostream>
#include <Eigen/Dense>

#include "vmf.hpp"

template<typename T>
class vMFprior {
 public:
  vMFprior(const Eigen::Matrix<T,3,1>& m0, T a, T b)
    : m0_(m0), b_(b), a_(a), unif_(0.,1.)
  {}

  T logMarginal(const Eigen::Matrix<T,3,1>& x) const {
    const T bTilde = (x + b_*m0_).norm();
    const T bOverTan = b_ < 1e-9 ? 2./M_PI : b_/tan(M_PI*0.5*b_);
    const T sinc = bTilde < 1e-9 ? 1. : sin(bTilde*M_PI)/(bTilde*M_PI);
    const T sinus = sin(bTilde*0.5*M_PI);
    return log(bOverTan*0.125) + log(1.-sinc) - 2*log(sinus);
  }

  vMF<T,3> sample(std::mt19937& rnd) {
//    std::cout << "sample from vMF prior " << a_ << " " << b_
//      << " " << m0_.transpose() << std::endl;
    Eigen::Matrix<T,3,1> mu;
    T tau = 1.;
    vMF<T,3> vmf(m0_, tau*b_);
//    std::cout << "sampling from base" << std::endl;
    for (size_t it=0; it<10; ++it) {
      vmf.tau_ = tau*b_;
      mu = vmf.sample(rnd);
//      std::cout << "mu " << mu.transpose() << std::endl;
      const T dot = mu.dot(m0_);
      tau = sampleConcentration(dot, rnd, 3, tau);
//      std::cout <<"@" << it << "tau " << tau << " mu " << mu.transpose() << std::endl;
    }
    return vMF<T,3>(mu, tau);
  }

  vMF<T,3> MAP() {
    Eigen::Matrix<T,3,1> xSum=b_*m0_;
    float tau = MLEstimateTau<float,3>(xSum, m0_, a_);
    return vMF<T,3>(m0_, tau);
  }

  vMFprior<T> posterior(const Eigen::Matrix<T,3,1>& xSum, const T count) const {
    T aN = a_+count;
    Eigen::Matrix<T,3,1> muN = xSum + b_*m0_;
    T bN = muN.norm();
    muN /= bN;
    return vMFprior<T>(muN, aN, bN);
  }

  vMFprior<T> posterior(const Eigen::Matrix<T,4,1>& ss) const {
    T aN = a_+ss(3);
    Eigen::Matrix<T,3,1> muN = ss.topRows(3) + b_*m0_;
    T bN = muN.norm();
    muN /= bN;
    return vMFprior<T>(muN, aN, bN);
  }

  Eigen::Matrix<T,3,1> m0_;
  T b_;
  T a_;
 private:

  T propToConcentrationLogPdf(const T tau, const T dot) const
  {
    if (tau < 1e-16) {
      return 0.;
    } else {
      return a_*(log(tau) + LOG_2 - log(1.-exp(-2.*tau))) + tau*(b_*dot-a_);
    }
  };

  T propToConcentrationLogPdfDeriv(const T tau, const T dot) const
  {
    // this is only for 3D case
    if (tau < 1e-16) {
      return b_*dot;
    } else {
      return a_/tau - (2.*a_*exp(-2.*tau)/(1.-exp(-2.*tau))) + b_*dot -a_;
    }
  };
  T propToConcentrationLogPdfDerivDeriv(const T tau, const T dot) const
  {
    // this is only for 3D case
    if (tau < 1e-16) {
      return -a_/3.;
    } else {
//      return -a_/(tau*tau) + (4.*a_*exp(2.*tau)/(1.-2.*exp(2.*tau)+exp(4.*tau)));
      return -a_/(tau*tau) + (4.*a_*exp(-2.*tau)/(1.-2.*exp(-2.*tau)+exp(-4.*tau)));
    }
  };

  T maximum(const T dot) {
    if (dot*b_ <= 0)
      return 0.;
    T tau = 1.;
    for (size_t it=0; it<100; ++it) {
      T f = propToConcentrationLogPdfDeriv(tau, dot);
      T df = propToConcentrationLogPdfDerivDeriv(tau, dot);
      tau -= f/df;
      if (fabs(f/df) < 1e-6)
        break;
    }
    return tau;
  };

  T intersect(const T c, const T dot, const T tau0) {
    T tau = tau0;
    for (size_t it=0; it<100; ++it) {
      T f = propToConcentrationLogPdf(tau, dot) - c;
      T df = propToConcentrationLogPdfDeriv(tau, dot);
      tau = std::max(0.f, tau-f/df);
//      std::cout << "   __ " <<it << ": " << f << " " << df
//        << "\t f/df " << fabs(f/df)
//        << "\t step to " << tau-f/df << ": "<< tau << std::endl;
      if (fabs(f/df) < 1e-6 || tau == 0.)
        break;
    }
    return tau;
  };

  /// slice sampler for concentration paramter tau
  T sampleConcentration(const T dot, std::mt19937& rnd, size_t maxIt, T tau0 = 0.3)
  {
    T tauMax = maximum(dot);
    T tau = tau0;
//    std::cout << " ----- max " << tauMax << " " << tau0
//      << " dot " << dot << std::endl;
    T tauL = 0.;
    T tauR = tauMax;
    for(size_t t=0; t<maxIt; ++t)
    {
      const T f = propToConcentrationLogPdf(tau,dot);
      const T u = log(unif_(rnd)) + f;

      if (tauMax > 0.) {
        tauL = intersect(u, dot, tauMax*0.001);
        tauR = intersect(u, dot, tauMax*1.5);
      } else {
        tauL = 0.;
        tauR = intersect(u, dot, 0.5);
      }
      tau = unif_(rnd)*(tauR-tauL)+tauL;
//      std::cout << tauL << " - " << tauMax << " - " << tauR
//        << " tau= " << tau
//        << " : u " << u << " f(tau) " << f
//        << " f(tau^star) " << propToConcentrationLogPdf(tauMax,dot)
//        << std::endl;
    }
    return tau;
  };

  /// Old backstepping slice sampler implementation
  T sampleConcentrationStepping(const T dot, std::mt19937& rnd, size_t maxIt, T tau0 = 0.3)
  {
//    std::cout << "start sampling concentration ---" << std::endl;
    // slice sampler for concentration paramter tau
    const T w = 0.1;  // width for expansions of search region
    T tau = tau0;      // arbitrary starting point
    for(size_t t=0; t<maxIt; ++t)
    {
      const T yMax = propToConcentrationLogPdf(tau,dot);
      const T y = log(unif_(rnd)) + yMax;
      T tauMin = tau-w;
      T tauMax = tau+w;
//      std::cout << "before " << tauMin << " " << tauMax
//        << ": " << propToConcentrationLogPdf(tauMin,dot)
//        << " " << propToConcentrationLogPdf(tauMax,dot) << std::endl;
      while (tauMin >=0. && propToConcentrationLogPdf(tauMin,dot) >= y) tauMin -= w;
      tauMin = std::max(static_cast<T>(0.),tauMin);
      while (propToConcentrationLogPdf(tauMax,dot) >= y) tauMax += w;
//      std::cout << "after "  << tauMin << " " << tauMax
//        << ": " << propToConcentrationLogPdf(tauMin,dot)
//        << " " << propToConcentrationLogPdf(tauMax,dot) << std::endl;
      while(42) {
        T tauNew = unif_(rnd)*(tauMax-tauMin)+tauMin;

//        std::cout << "@"<< t << ": " << tauMin << " " << tauMax << " " << tauNew << " " << tau
//          << ": " << propToConcentrationLogPdf(tauNew,dot)
//          << " >=? " << y
//          << ",  " << propToConcentrationLogPdf(tauMin,dot)
//          << " "  << propToConcentrationLogPdf(tauMax,dot) << std::endl;

        if(propToConcentrationLogPdf(tauNew,dot) >= y)
        {
          tau = tauNew; break;
        }else{
          if (tauNew < tau) tauMin = tauNew; else tauMax = tauNew;
        }
      };
    }
    return tau;
  };
  std::uniform_real_distribution<T> unif_;
};


/** \file QuantumChannel.cc
 *  \todo clean Clean code when it is simple.
 *  \todo doc Write doxygen documentation.
 *  \authors takaakimatsuo
 *
 *  \brief QuantumChannel
 */
#include <omnetpp.h>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
//#include <Eigen/Dense>
#include <PhotonicQubit_m.h>

using namespace Eigen;
using namespace omnetpp;
using namespace quisp::messages;

namespace quisp {
namespace channels {

/*The sum of Z,X and Y error rate equates to pauli_error_rate. Value could potentially between 0 ~ 1. */
struct channel_error_model {
  double pauli_error_rate;  // Overall error rate
  double Z_error_rate;
  double X_error_rate;
  double Y_error_rate;
};

/** \class QuantumChannel QuantumChannel.cc
 *  \todo Documentation of the class header.
 *
 *  \brief QuantumChannel
 */
class QuantumChannel : public cDatarateChannel {
 public:
  channel_error_model err;
  double photon_loss_rate;
  double distance = 0;  // in km
  // int less = 0, more = 0;
 private:
  double No_error_ceil;
  double X_error_ceil;
  double Y_error_ceil;
  double Z_error_ceil;
  double Lost_ceil;
  int DEBUG_darkcount_count = 0;
  MatrixXd Q_to_the_distance;
  virtual void initialize();
  virtual void processMessage(cMessage *msg, simtime_t t, result_t &result);

 public:
  QuantumChannel();
};

Define_Channel(QuantumChannel)

    QuantumChannel::QuantumChannel() {}

void QuantumChannel::initialize() {
  cDatarateChannel::initialize();
  Q_to_the_distance(5, 5);
  distance = par("distance");  // in km

  /*double Z_error_ratio = par("Z_error_ratio");//par("name") will be read from .ini or .ned file
  double X_error_ratio = par("X_error_ratio");
  double Y_error_ratio = par("Y_error_ratio");
  double Loss_error_ratio = par("photon_loss_ratio");
  if(Z_error_ratio==0 && X_error_ratio==0 && Y_error_ratio==0 && Loss_error_ratio==0){
      Z_error_ratio=1;//To avoid bug.
      X_error_ratio=1;
      Y_error_ratio=1;
      Loss_error_ratio=1;
  }

  double ratio_sum = Z_error_ratio + X_error_ratio + Y_error_ratio + Loss_error_ratio;//Get the sum of x:y:z for normalization
  err.pauli_error_rate = par("channel_error_rate");//This is per km.
  err.X_error_rate = err.pauli_error_rate * (X_error_ratio/ratio_sum);
  err.Y_error_rate = err.pauli_error_rate * (Y_error_ratio/ratio_sum);
  err.Z_error_rate = err.pauli_error_rate * (Z_error_ratio/ratio_sum);
  photon_loss_rate = err.pauli_error_rate * (Loss_error_ratio/ratio_sum);//Photon Loss rate per km.
  */

  photon_loss_rate = par("channel_Loss_error_rate");
  err.X_error_rate = par("channel_X_error_rate");
  err.Y_error_rate = par("channel_Y_error_rate");
  err.Z_error_rate = par("channel_Z_error_rate");
  err.pauli_error_rate = err.X_error_rate + err.Y_error_rate + err.Z_error_rate + photon_loss_rate;

  /*
  int num_err_type = 0;
  if(err.X_error_rate !=0){
      num_err_type++;
  }
  if(err.Z_error_rate !=0){
      num_err_type++;
  }
  if(err.Y_error_rate !=0){
      num_err_type++;
  }

  if((1-err.pauli_error_rate) < double(1)/double(num_err_type)){
      //error("Error rate inaccurate.");
      std::cout<<"Inaccurate error rate \n";
  }*/

  // std::cout<<"Sum of errors must be ... = "<<err.X_error_rate+err.Y_error_rate+err.Z_error_rate+photon_loss_rate<<"\n";
  // std::cout<<"Channel err:"<<err.pauli_error_rate<<" X = " <<err.X_error_rate << "Y = "<< err.Y_error_rate << ", Z = "<< err.Z_error_rate<<",Loss"<<photon_loss_rate<<"\n";
  MatrixXd Transition_matrix(5, 5);

  Transition_matrix << 1 - err.pauli_error_rate, err.X_error_rate, err.Z_error_rate, err.Y_error_rate, photon_loss_rate, err.X_error_rate, 1 - err.pauli_error_rate,
      err.Y_error_rate, err.Z_error_rate, photon_loss_rate, err.Z_error_rate, err.Y_error_rate, 1 - err.pauli_error_rate, err.X_error_rate, photon_loss_rate, err.Y_error_rate,
      err.Z_error_rate, err.X_error_rate, 1 - err.pauli_error_rate, photon_loss_rate, 0, 0, 0, 0, 1;

  std::cout << "Transition mat per km = \n" << Transition_matrix << "\n";
  MatrixPower<MatrixXd> Apow(Transition_matrix);
  Q_to_the_distance = Apow(distance);
  std::cout << "Transition mat = " << Q_to_the_distance << "\n";

  // std::cout<<"\nNo_error_ceil = "<<No_error_ceil<<", X_error_ceil = "<< X_error_ceil << ", Z_error_ceil"<<Z_error_ceil<<", Y_error_ceil"<<Y_error_ceil<<" pauli err rate is
  // "<<err.pauli_error_rate<<"\n"; std::cout<<" 1-err.pauli_error_rate"
  // <<1-err.pauli_error_rate<<"err.X_error_rate"<<err.X_error_rate<<"err.Z_error_rate"<<err.Z_error_rate<<"err.Y_error_rate"<<err.Y_error_rate<<"photon_loss_rate"<<photon_loss_rate<<"\n";
}

void QuantumChannel::processMessage(cMessage *msg, simtime_t t, result_t &result) {
  cDatarateChannel::processMessage(msg, t, result);  // Call the original processMessage

  try {
    PhotonicQubit *q = check_and_cast<PhotonicQubit *>(msg);

    bool lost = q->getPhotonLost();
    bool Zerr = q->getPauliZerr();
    bool Xerr = q->getPauliXerr();

    // The photon may have an error when emitted.
    MatrixXd Initial_condition(1, 5);  // I, X, Z, Y, Photon Lost
    if (lost) {
      Initial_condition << 0, 0, 0, 0, 1;  // Photon already lost. Maybe by emission time. Not implemented though.
    } else if (Zerr && Xerr) {
      Initial_condition << 0, 0, 0, 1, 0;  // Has a Y error
    } else if (Zerr && !Xerr) {
      Initial_condition << 0, 0, 1, 0, 0;  // Has a Z error
    } else if (!Zerr && Xerr) {
      Initial_condition << 0, 1, 0, 0, 0;  // Has an X error
    } else {
      Initial_condition << 1, 0, 0, 0, 0;  // No error
    }
    MatrixXd Output_condition(1, 5);
    Output_condition = Initial_condition * Q_to_the_distance;

    // std::cout<<"Q_to_the_distance"<<Q_to_the_distance<<"\n";
    // std::cout<<"Output_condition = "<<Output_condition<<"\n";
    No_error_ceil = Output_condition(0, 0);
    X_error_ceil = No_error_ceil + Output_condition(0, 1);
    Z_error_ceil = X_error_ceil + Output_condition(0, 2);
    Y_error_ceil = Z_error_ceil + Output_condition(0, 3);
    Lost_ceil = Y_error_ceil + Output_condition(0, 4);

    // std::cout<<"NO error ceil = "<<No_error_ceil<<", X = "<<X_error_ceil<<"Z, "<<Z_error_ceil<<", Y = "<<Y_error_ceil<<", Lost = "<<Lost_ceil<<"\n";

    double rand = dblrand();  // Gives a random double between 0.0 ~ 1.0
    /* if(rand<0.5){
         less++;
     }else{
         more++;
     }*/
    // double rand = std::rand()/(RAND_MAX + 1.);
    if (rand < No_error_ceil) {
      // Qubit will end up with no error
    } else if (No_error_ceil <= rand && rand < X_error_ceil && (No_error_ceil != X_error_ceil)) {
      // X error
      bool xerr = q->getPauliXerr();
      q->setPauliXerr(!xerr);  // if xerr already was true, then another x error will make it false
    } else if (X_error_ceil <= rand && rand < Z_error_ceil && (X_error_ceil != Z_error_ceil)) {
      // Z error
      bool zerr = q->getPauliZerr();
      q->setPauliZerr(!zerr);
    } else if (Z_error_ceil <= rand && rand < Y_error_ceil && (Z_error_ceil != Y_error_ceil)) {
      // Y error
      bool xerr = q->getPauliXerr();
      q->setPauliXerr(!xerr);
      bool zerr = q->getPauliZerr();
      q->setPauliZerr(!zerr);
    } else {
      // Photon was lost
      DEBUG_darkcount_count++;
      // std::cout<<"less = "<<less<<", more = "<<more<<"\n";
      // std::cout<<"dbl="<<rand<<" count = "<<DEBUG_darkcount_count<<"\n";
      q->setPhotonLost(true);
    }
    q->setError_random_for_debug(rand);  // For debugging purpose
  } catch (std::exception &e) {
    // error("Only PhotonicQubit is allowed in quantum channel");
    EV << "Only PhotonicQubit is allowed in quantum channel";
  }
}

}  // namespace channels
}  // namespace quisp


/*=============================================================================

  LeastSquaresPackage: A software package for estimating the rotation and translation of rigid bodies.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include <boost/program_options.hpp>
#include "lsqLeastSquare3D.h"
#include <iostream>
#include <fstream>
#include <algorithm>
#include <iterator>

// A helper function to simplify the help command.
template<class T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& v)
{
    std::copy(v.begin(), v.end(), std::ostream_iterator<T>(os, " "));
    return os;
}

/**
* \file lsqComputeRotTrans.cpp
* \brief End-user application for computing the rotation and translation connecting two sets of 3D points.
* \ingroup applications
*
* This command line application can read from two separate files, each of them containing the coordinates of many 3D points,
* and compute the rotation and translation connecting the two sets. The user can choose between two different methods to
* compute the rotation, and the methods can be selected with a specific option (see help). The two methods are based, respectively,
* on the singular value decomposition of a matrix obtained from the sets of points (K.S. Arun et al., IEEE PAMI-9, 698-700, 1987),
* and on the quaternionic representation of matrices (B.K.P. Horn, J. Opt. Soc. Am. A 4, 629-642, 1987). Rotation and translation
* are saved in file "rotation.dat" and "translation.dat" respectively.
*/
int main(int argc, char* argv[]) {

  try {

    std::string algorithm_strategy;
    std::string output_filename;

    // Allowed options, shown in the help.
    boost::program_options::options_description desc("Options");

    desc.add_options()
      ("help,h", "produce help message")
      ("version,v", "return the version of the application")
      ("method,m", boost::program_options::value< std::string >(&algorithm_strategy)->default_value("svd"),
        "set the algorithm for computing the rotation, either svd or quat")
      ("output,o", boost::program_options::value< std::string >(&output_filename)->default_value("output.dat"),
        "set the name of the output file where rotation and translation are saved")
    ;

    // Hidden options, are allowed on command line, but are not shown to the user.
    boost::program_options::options_description hidden("Hidden options");

    hidden.add_options()
      ("input-files", boost::program_options::value< std::vector<std::string> >(),
        "input files with the coordinates of the two sets of 3D points")
    ;

    boost::program_options::options_description cmdline_options;
    cmdline_options.add(desc).add(hidden);

    boost::program_options::positional_options_description p;
    p.add("input-files", -1);

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(cmdline_options).positional(p).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("help")) {
      std::cout << "Usage: options_description [options]" << std::endl;
      std::cout << desc;  // The helper function is used by the help to print the list of commands.
      return 0;
    }

    if (vm.count("version")) {
      std::cout << "lsqComputeRotTrans version 0.0.1" << std::endl;
      return 0;
    }

    lsq::LeastSquare3D reference_frames;

    if (vm.count("input-files")) {

      auto input_filenames = vm["input-files"].as<std::vector<std::string>>();

      if (input_filenames.size() != 2) {
        std::cerr << "Provide two files with the sets of coordinates of the 3D points." << std::endl;
        return 1;
      }

      // ---------------------------- Read the coordinates and fill the two sets of 3D points ---------------------------
      std::ifstream filein;
      double x_value, y_value, z_value;
      Eigen::Vector3d point;

      for( int i = 0 ; i < 2 ;  ++i ) {

        filein.open( input_filenames[i] );

        if (!filein.is_open()) {
          std::cerr << "Error opening the input file " << input_filenames[i] << "." << std::endl;
          return 1;
        }

        std::cout << "Input file " << input_filenames[i] << " is read and points are saved..." << std::endl;

        while(!filein.eof()) {
          filein >> x_value >> y_value >> z_value;
          point = {x_value, y_value, z_value};
          if ( i == 0 )
            reference_frames.add_point_first_vector(point);
          else
            reference_frames.add_point_second_vector(point);
        }

        filein.close();
      }

      if ( !reference_frames.same_number_of_points() ) {
        std::cout << "warning: The two sets of points have different size. The additional points will be neglected." << std::endl;
      }

    }
    else {
      std::cerr << "No input files are provided. Provide two files with the sets of coordinates of the 3D points." << std::endl;
      return 1;
    }

    //  ----------------------------------- Set the algorithm to compute the rotation -----------------------------------
    std::unique_ptr<lsq::ComputeRotation> algorithm;

    if ( algorithm_strategy == std::string("svd") ) {
      std::cout << "The chosen algorithm is svd." << std::endl;
      algorithm = std::unique_ptr<lsq::ComputeRotation>( new lsq::SVDMethod() );
    }
    else if ( algorithm_strategy == std::string("quat") ) {
      std::cout << "The chosen algorithm is quat." << std::endl;
      algorithm = std::unique_ptr<lsq::ComputeRotation>( new lsq::QuaternionMethod() );
    }
    else {
      std::cerr << "The chosen method is not available. Refer to the --help for the available methods." << std::endl;
      return 1;
    }

    reference_frames.set_rotation_strategy( std::move(algorithm) );

    // ---------------------- Compute the centroids and the update the sets of points around them -----------------------
    reference_frames.centroid_first_vector();
    reference_frames.centroid_second_vector();
    reference_frames.update_first_points_around_centroid();
    reference_frames.update_second_points_around_centroid();

    // ---------------------------------------------- Compute the H matrix ----------------------------------------------
    reference_frames.compute_H_matrix();

    // ---------------------------------------- Compute rotation and translation ----------------------------------------
    reference_frames.compute_rotation_matrix();
    reference_frames.compute_translation_vector();

    // ----------------------------------------- Save rotation and translation -----------------------------------------
    Eigen::Matrix3d rotation = reference_frames.get_rotation_matrix();
    Eigen::Vector3d translation = reference_frames.get_translation_vector();

    std::ofstream fileout;

    fileout.open( output_filename );

    if (!fileout.is_open()) {
      std::cerr << "Error opening the output file " << output_filename << "." << std::endl;
      return 1;
    }

    std::cout << "Rotation and translation are saved in the output file " << output_filename << "." << std::endl;

    fileout << "The rotation matrix is:" << std::endl;
    fileout << rotation << std::endl;
    fileout << "The translation vector is:" << std::endl;
    fileout << translation << std::endl;

    fileout.close();

    }
    catch(std::exception& e)
    {
        std::cerr << e.what() << std::endl;
        return 1;
    }
    return 0;
}


/*
Copyright <2017> <Benjamin Santos>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <iostream>
#include <fstream>
#include <utility>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include <boost/phoenix/core.hpp>

#include <boost/phoenix/core.hpp>
#include <boost/phoenix/operator.hpp>

using namespace std;
using namespace boost::numeric::odeint;
namespace phoenix = boost::phoenix;

// vector type
typedef double value_type;
typedef vector< value_type > state_type;

// system definition
struct stiff_system{

  // constructor, sets a and b
  stiff_system(double a_=-101.0, double b_=-100.0): a(a_), b(b_){  }

  // rhs of ode system
  inline
  void operator()( const state_type &x, state_type &dxdt, const value_type t) const {
    dxdt[0] = a*x[0] + b*x[1];
    dxdt[1] = x[0];
  }
  double a;
  double b;
};

int main(int argc, char **argv) {

  state_type xini( 2.0 , 1.0 );

  // system
  //   stiff_system ssys();
  stiff_system* ssys = new stiff_system();

  auto stepper = make_controlled(1.0e-6, 1.0e-6,
                                runge_kutta_cash_karp54< state_type >());

  size_t num_of_steps = integrate_adaptive(stepper, *ssys, xini,
                                           0.01, 50.0, 0.01,
                        cout << phoenix::arg_names::arg2 << '\t'
                             << phoenix::arg_names::arg1[0] << '\t'
                             << phoenix::arg_names::arg1[1] << '\t'
                             << phoenix::arg_names::arg1[1]
                            *phoenix::arg_names::arg1[1] << "\n" );

  cerr << "\n[ii] Number of steps: " <<  num_of_steps << endl;

  // deallocate memory for ssys
  delete(ssys);

  return 0;
}


#ifndef MPP_HAMILTONIAN_KINETIC_ENERGY_MULTIVAR_NORMAL_HPP
#define MPP_HAMILTONIAN_KINETIC_ENERGY_MULTIVAR_NORMAL_HPP

#include <random>
#include <exception>
#include <sstream>
#include <string>
#include <cmath>
#include <cstddef>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/assert.hpp>

namespace mpp { namespace hamiltonian {

template<class real_scalar_type>
class multivariate_normal {
public:
    typedef boost::numeric::ublas::vector<real_scalar_type> real_vector_type;
    typedef std::normal_distribution<real_scalar_type> normal_distribution_type;

    explicit multivariate_normal(real_vector_type const & sigma_inv) throw()
    :m_sigma_inv(sigma_inv) {
        if( sigma_inv.size() == std::size_t(0)) {
            std::stringstream msg;
            msg << "The number of dimensions = "
                << m_sigma_inv.size()
                << " of the multivariate normal kinetic energy should be"
                << " greater than zero.";
            throw std::length_error(msg.str());
        }

        for(std::size_t i=0;i<sigma_inv.size();++i) {
            if( not std::isfinite(sigma_inv (i)) ) {
                std::stringstream msg;
                msg << i << "th value of sigma_inv is not finite";
                throw std::out_of_range(msg.str());
            }
        }
    }

    real_scalar_type log_posterior(real_vector_type const & p) const {
        BOOST_ASSERT_MSG( p.size() == m_sigma_inv.size(),
            "p should have the same dimensionality of the log_posterior.");
        real_scalar_type val(0);
        for(std::size_t i=0;i<p.size();++i) {
            val -= p(i)*p(i)*m_sigma_inv(i);
        }
        return real_scalar_type(0.5)*val;
    }

    real_vector_type grad_log_posterior(real_vector_type const & p) const {
        BOOST_ASSERT_MSG( p.size() == m_sigma_inv.size(),
            "p should have the same dimensionality of the log_posterior.");
        real_vector_type dp(p.size());
        for(std::size_t i=0;i<p.size();++i) {
            dp(i) = -m_sigma_inv(i)*p(i);
        }
        return dp;
    }

    template<class rng_type>
    real_vector_type generate_sample(rng_type & rng) {
        real_vector_type sample(m_sigma_inv.size());
        for(std::size_t i=0;i<m_sigma_inv.size();++i) {
            real_scalar_type scale
                = m_sigma_inv(i) > real_scalar_type(0) ?
                    std::sqrt( real_scalar_type(1)/m_sigma_inv(i) ) : 0 ;
            sample(i) = scale*m_norm_dist(rng);
        }
        return sample;
    }
private:
    real_vector_type m_sigma_inv;
    normal_distribution_type m_norm_dist;
};

}}

#endif //MPP_HAMILTONIAN_KINETIC_ENERGY_MULTIVAR_NORMAL_HPP


// Author: Dmitry Anisimov.
// In this test we compute mean value coordinates at some particular points,
// where the computation might break. The used polygon is a concave polygon with 7 vertices.
// We also use inexact kernel and epsilon = 1.0e-15.

// Works with an exact kernel, too.

#include <cmath>
#include <cassert>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Barycentric_coordinates_2/Mean_value_2.h>
#include <CGAL/Barycentric_coordinates_2/Generalized_barycentric_coordinates_2.h>
#include <boost/math/special_functions/fpclassify.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

typedef Kernel::FT      Scalar;
typedef Kernel::Point_2 Point;

typedef std::vector<Scalar> Coordinate_vector;
typedef std::vector<Point>  Point_vector;

typedef std::back_insert_iterator<Coordinate_vector> Vector_insert_iterator;

typedef CGAL::Barycentric_coordinates::Mean_value_2<Kernel> Mean_value;
typedef CGAL::Barycentric_coordinates::Generalized_barycentric_coordinates_2<Mean_value, Kernel> Mean_value_coordinates;

typedef boost::optional<Vector_insert_iterator> Output_type;

using std::cout; using std::endl; using std::string;

int main()
{
    Point_vector vertices(7);

    vertices[0] = Point(0, 0);                                     vertices[1] = Point(1, 1);
    vertices[2] = Point(Scalar(7)/Scalar(4), Scalar(1)/Scalar(2)); vertices[3] = Point(Scalar(7)/Scalar(4), Scalar(5)/Scalar(2));
    vertices[4] = Point(1, 2);                                     vertices[5] = Point(0, 3);
    vertices[6] = Point(Scalar(1)/Scalar(2), Scalar(3)/Scalar(2));

    Mean_value_coordinates mean_value_coordinates(vertices.begin(), vertices.end());

    const Point query_points[11] = { Point(Scalar(1) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))            , Scalar(2) - (Scalar(1) / Scalar(std::pow(10.0, 300.0)))            ),
                                     Point(Scalar(1) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))            , Scalar(1) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))            ),
                                     Point(1                                                                  , Scalar(3) / Scalar(2)                                              ),
                                     Point(Scalar(5) / Scalar(4)                                              , Scalar(5) / Scalar(4)                                              ),
                                     Point(Scalar(5) / Scalar(4)                                              , Scalar(7) / Scalar(4)                                              ),
                                     Point(Scalar(3) / Scalar(2)                                              , Scalar(3) / Scalar(2)                                              ),

                                     Point(Scalar(7) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(7) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0)))),
                                     Point(Scalar(7) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(5) / Scalar(4) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))),

                                     Point(Scalar(3) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(3) / Scalar(4) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))),
                                     Point(Scalar(3) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(9) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0)))),
                                     Point(Scalar(1) / Scalar(2) + (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(3) / Scalar(2)                                              )
                                   };

    Coordinate_vector coordinates;

    int count = 0;
    const Scalar epsilon = Scalar(1) / Scalar(std::pow(10.0, 15.0));

    for(int i = 0; i < 11; ++i) {
        const Output_type result = mean_value_coordinates(query_points[i], coordinates);

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 0])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 0])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 1])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 1])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 2])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 2])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 3])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 3])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 4])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 4])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 5])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 5])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 6])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 6])));

        const Scalar coordinate_sum = coordinates[count + 0] +
                                      coordinates[count + 1] +
                                      coordinates[count + 2] +
                                      coordinates[count + 3] +
                                      coordinates[count + 4] +
                                      coordinates[count + 5] +
                                      coordinates[count + 6] ;

        const Point linear_combination( vertices[0].x()*coordinates[count + 0] +
                                        vertices[1].x()*coordinates[count + 1] +
                                        vertices[2].x()*coordinates[count + 2] +
                                        vertices[3].x()*coordinates[count + 3] +
                                        vertices[4].x()*coordinates[count + 4] +
                                        vertices[5].x()*coordinates[count + 5] +
                                        vertices[6].x()*coordinates[count + 6] ,
                                        vertices[0].y()*coordinates[count + 0] +
                                        vertices[1].y()*coordinates[count + 1] +
                                        vertices[2].y()*coordinates[count + 2] +
                                        vertices[3].y()*coordinates[count + 3] +
                                        vertices[4].y()*coordinates[count + 4] +
                                        vertices[5].y()*coordinates[count + 5] +
                                        vertices[6].y()*coordinates[count + 6] );

        const Point difference(linear_combination.x() - query_points[i].x(), linear_combination.y() - query_points[i].y());

        assert( ((coordinate_sum - Scalar(1)) < epsilon) && difference.x() < epsilon && difference.y() < epsilon );

        if( ((coordinate_sum - Scalar(1)) > epsilon) || difference.x() > epsilon || difference.y() > epsilon )
        {
            cout << endl << "MV_special_points_test: FAILED." << endl << endl;
            exit(EXIT_FAILURE);
        }
        count += 7;
    }

    cout << endl << "MV_special_points_test: PASSED." << endl << endl;

    return EXIT_SUCCESS;
}


/*
 * odeint_rk4_lorenz_def_alg.cpp
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/array.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>
#include <boost/numeric/odeint/algebra/array_algebra.hpp>

#include "rk_performance_test_case.hpp"

#include "lorenz.hpp"

typedef boost::array< double , 3 > state_type;
typedef boost::numeric::odeint::runge_kutta4_classic< state_type > rk4_odeint_type;

class odeint_wrapper
{
public:
    void reset_init_cond()
    {
        m_x[0] = 10.0 * rand() / RAND_MAX;
        m_x[1] = 10.0 * rand() / RAND_MAX;
        m_x[2] = 10.0 * rand() / RAND_MAX;
        m_t = 0.0;
    }

    inline void do_step( const double dt )
    {
        m_stepper.do_step( lorenz() , m_x , m_t , dt );
        //m_t += dt;
    }

    double state( const size_t i ) const
    { return m_x[i]; }

private:
    state_type m_x;
    double m_t;
    rk4_odeint_type m_stepper;
};

int main()
{
    odeint_wrapper stepper;

    run( stepper );
}


#ifndef M_PI
#    define M_PI 3.14159265358979323846
#endif

#include <boost/shared_array.hpp>
#include <boost/range.hpp>

#include <smp/planner_utils/trajectory.hpp>
#include <smp/planner_utils/vertex_edge.hpp>
#include <smp/components/extenders/pos.h>

#include <smp/components/extenders/state_array_double.hpp>
#include <smp/components/extenders/input_array_double.hpp>
#include <smp/components/extenders/base.hpp>
#include <cmath>

#include <cstdlib>
/// Waypoint Position Controller WP=1
#define WP 1

using namespace smp;
using namespace std;

template< class typeparams, int NUM_DIMENSIONS >
int smp::extender_pos<typeparams,NUM_DIMENSIONS>
::ex_update_insert_vertex (vertex_t *vertex_in) {

    return 1;
}

template< class typeparams, int NUM_DIMENSIONS >
int smp::extender_pos<typeparams,NUM_DIMENSIONS>
::ex_update_insert_edge (edge_t *edge_in) {

    return 1;
}

template< class typeparams , int NUM_DIMENSIONS>
int smp::extender_pos<typeparams,NUM_DIMENSIONS>
::ex_update_delete_vertex (vertex_t *vertex_in){

    return 1;
}

template< class typeparams, int NUM_DIMENSIONS >
int smp::extender_pos<typeparams,NUM_DIMENSIONS>
::ex_update_delete_edge (edge_t *edge_in) {

    return 1;
}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::set_angle_to_range(double alpha, double min)
{

    while (alpha >= min + 2.0 * M_PI) {
        alpha -= 2.0 * M_PI;
    }
    while (alpha < min) {
        alpha += 2.0 * M_PI;
    }
    return alpha;
}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::diff_angle_unwrap(double alpha1, double alpha2)
{
    double delta;

    // normalize angles alpha1 and alpha2
    alpha1 = set_angle_to_range(alpha1, 0);
    alpha2 = set_angle_to_range(alpha2, 0);

    // take difference and unwrap
    delta = alpha1 - alpha2;
    if (alpha1 > alpha2) {
        while (delta > M_PI) {
            delta -= 2.0 * M_PI;
        }
    } else if (alpha2 > alpha1) {
        while (delta < -M_PI) {
            delta += 2.0 * M_PI;
        }
    }
    return delta;
}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::normangle(double a, double mina){

    double ap,minap;
    ap=a;
    minap=mina;
    while (ap>= (minap+M_PI*2)){
        ap=ap-M_PI*2;
    }
    while(ap<minap){
        ap=ap+M_PI*2;

    }

    return ap;

}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::f(double rho){

    double Kv;
    /// New implementation  Kv= max atanh(rho)/rho
    Kv=1;
//    Kv=1;

    return tanh(rho*Kv);
    /// German Paper Version

    //  return (2*v0/M_PI)*atan((M_PI/(2*v0))*rho);

}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::g(double t){
    /// German Paper Version
//        double a;
//        a=pow((t/T),4);
//        return a/(a+1);

    /// New implementation
    return 1;

}

template< class typeparams, int NUM_DIMENSIONS >
double* smp::extender_pos<typeparams,NUM_DIMENSIONS>
::posctrlstep (double x_c, double y_c, double t_c,
               double x_end, double y_end, double t_end, double ct, double b, int dir) {

    /** This function will generate a vector of double as output:

     *  [0] Vl velocity of the left wheel;
     *  [1] Vr velocity of the right wheel;
     *  [2] V translational Velocity;
     *  [3] W Angular Velocity.
     *  [4] EOT End Of Trajectory

    **/

    static double oldBeta,controllerType;

    double Krho,Kalpha,Kbeta,Kphi,Vmax,RhoThreshold1,RhoThreshold2,RhoEndCondition,PhiEndCondition;
    // [1 3 -1 -1]
    Krho    = 1.2;
    Kalpha  = 3;
    Kbeta   = -1;
    Kphi    = -1;
    Vmax    = Krho;
    //  RhoThreshold1   = 0.04;
    RhoThreshold1   = 0.005;

    RhoThreshold2   = 0.02;
    //  RhoThreshold2   = 0.2;
/// The RRT* edges' lenght is related to the RhoEndCondition
#if WP>0
    RhoEndCondition = 0.35;
#else
    RhoEndCondition = 0.35;
#endif

/// the PhiEndCondition has to be setted properly,
/// small Value, local minima can occur, tested --> greater than 35*M_PI/180

    //  PhiEndCondition = 1*M_PI/180; [NOT IN THE ASTOLFI PAPER :)]
    PhiEndCondition = 50*M_PI/180;

    if(ct==0){
        oldBeta=0;
        controllerType=1;
    }

    double dx,dy,rho,fRho,alpha,phi,beta,v,w,vl,vr,eot,vi,di;
    vi=0.1;
    // rho
    eot=1;
    dx=x_end-x_c;
    dy=y_end -y_c;
    rho=sqrt(dx*dx+dy*dy);
    fRho=rho;

    if(fRho>(Vmax/Krho)){
        fRho=Vmax/Krho;
    }

    //alpha

    alpha=atan2(dy,dx)-t_c;
    alpha=normangle(alpha,-M_PI);

    //direction

    if (dir==0){

        if(alpha>(M_PI/2)) {
            fRho=-fRho;
            alpha=alpha-M_PI;
        }else if(alpha<=-M_PI/2){
            fRho=-fRho;
            alpha=alpha+M_PI;
        }
    }
    else if(dir==-1){
        fRho=-fRho;
        alpha=alpha+M_PI;
        if(alpha>M_PI){
            alpha=alpha-2*M_PI;
        }
    }

    //phi

    phi=t_end-t_c;
    phi=normangle(phi, -M_PI);

    beta=normangle(phi-alpha, -M_PI);

    if ((abs(oldBeta-beta)>M_PI)){
        beta=oldBeta;
    }
    oldBeta=beta;

    //set speed

#if WP>0
    v=Krho*f(fRho)*g(ct+1);
    w=(Kalpha*alpha+Kbeta*beta)*g(ct+1);
#else
    v=Krho*fRho;
    w=Kalpha*alpha+Kbeta*beta;
#endif

#if WP>0
    if (rho<this->rho_endcondition && abs(phi)<this->phi_endcondition ){
#else
    if (rho<RhoEndCondition && abs(phi)<PhiEndCondition){

#endif

        eot=1;
        //    cout<<"eot evaluated... "<<eot<<endl;
    }
    else {
        eot=0;
        //   cout<<"eot evaluated... "<<eot<<endl;

    }

    if(eot){
#if WP>0
#else
        v=0.0;
#endif
        w=0.;
    }

    //Convert speed to wheel speed

    vl=v-w*b/2;

    if(abs(vl)>Vmax){

        if(vl<0){
            vl=Vmax*-1;}
        else{
            vl=Vmax;}
    }

    vr=v+w*b/2;

    if(abs(vr)>Vmax){
        if(vr<0){
            vr=Vmax*-1;}
        else{
            vr=Vmax;}
    }

    result[0]=vl;
    result[1]=vr;
    result[2]=v;
    result[3]=w;
    result[4]=eot;

    return result;

}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
:: posctrl(state_t *state_ini, state_t *state_fin,int dir,double b, double dt ,
           list<state_t *> *list_states_out, list<input_t *> *list_inputs_out) {

    double sl,sr,oldSl,oldSr,t,eot,dSl,dSr,dSm,dSd,vl,vr,enc_l,enc_r;

    enc_l=0;
    enc_r=0;
    sl=0;
    sr=0;
    oldSl=0;
    oldSr=0;
    eot=0;
    t=0;
    vl=0;
    vr=0;

    double x,y,th;
    x=(*state_ini)[0];
    y=(*state_ini)[1];
    th=(*state_ini)[2];

    this->xi=x;
    this->yi=y;

    double vv,ww,ths;
    vv=0;
    ww=0;
    ths=0.1;

    double dist;
    dist=0;

    if(list_states_out){

        while(eot==0){
            // calculate distance for both wheels
            dSl=sl-oldSl;
            dSr=sr-oldSr;
            dSm=(dSl+dSr)/2;

            dSd=(dSr-dSl)/b;
            state_t *curr = new state_t;
            input_t *ve = new input_t;

            (*curr)[0]=x+dSm*cos(th+dSd/2);
            (*curr)[1]=y+dSm*sin(th+dSd/2);
            (*curr)[2]=normangle(th+dSd, -M_PI);

            intRes= posctrlstep ((*curr)[0],(*curr)[1],(*curr)[2],(*state_fin)[0], (*state_fin)[1],(*state_fin)[2], t,b,dir);
            //Save the velocity commands,eot
            vv=intRes[2];
            ww=intRes[3];
            (*ve)[0]=intRes[2];
            (*ve)[1]=intRes[3];
            eot=intRes[4];
            vl=intRes[0];
            vr=intRes[1];

            //Increase the timer
            t=t+dt;

            // keep track of previous wheel position
            oldSl=sl;
            oldSr=sr;

            // increase encoder values
            enc_l=enc_l+dt*vl;
            enc_r=enc_r+dt*vr;

            sl=enc_l;
            sr=enc_r;

            //
            float dxl,dyl;
            dxl=(*state_fin)[0]-(*curr)[0];
            dyl=(*state_fin)[1]-(*curr)[1];
            dist=sqrt(dxl*dxl+dyl*dyl);

            //save the state for the next sample
            x=(*curr)[0];
            y=(*curr)[1];
            th=(*curr)[2];

            if(eot==1){

            /// save the last state!!!
            state_t *save = new state_t;
            input_t *vesave = new input_t;
            (*vesave)[0]=intRes[2];
            (*vesave)[1]=intRes[3];
            dSl=sl-oldSl;
            dSr=sr-oldSr;
            dSm=(dSl+dSr)/2;
            dSd=(dSr-dSl)/b;
            (*save)[0]=x+dSm*cos(th+dSd/2);
            (*save)[1]=y+dSm*sin(th+dSd/2);
            (*save)[2]=normangle(th+dSd, -M_PI);
            // Add current values to the Trajectory
            list_states_out->push_back ((curr));
            list_inputs_out->push_back ((ve));

            list_states_out->push_back ((save));
            list_inputs_out->push_back ((vesave));

            }
            else

            {
            // Add current values to the Trajectory
            list_states_out->push_back ((curr));
            list_inputs_out->push_back ((ve));
            }

        }
    }
    return dist;

}

template< class typeparams, int NUM_DIMENSIONS >
smp::extender_pos<typeparams,NUM_DIMENSIONS>
::extender_pos () {
     result= (double*)malloc(sizeof(double)*5);
     intRes= (double*) malloc(sizeof(double)*5);
     this->dt_=0.01;
     this->rho_endcondition=0.15;
     this->L_axis=0.5;
     this->phi_endcondition = 20*M_PI/180;

}

template< class typeparams, int NUM_DIMENSIONS >
smp::extender_pos<typeparams,NUM_DIMENSIONS>
::~extender_pos () {

}

template< class typeparams, int NUM_DIMENSIONS>
int smp::extender_pos<typeparams, NUM_DIMENSIONS>
::extend (state_t *state_from_in, state_t *state_towards_in,
          int *exact_connection_out, trajectory_t *trajectory_out,
          list<state_t*> *intermediate_vertices_out) {

    int dir;
    dir=1;

    double b,dt,d,myEps;
    dt=this->dt_;
    T=0.31;
    /// Base
    // b=0.4;
    b=this->L_axis;
    /// myEps=0.1 for pursuit
    /// myEps=0.25 no pursuit
#if WP>0
    myEps=0.50;
#else
    myEps=0.5;
#endif
    //   cout<<"call posctrl /...."<<endl<<endl;
    intermediate_vertices_out->clear ();
    trajectory_out->clear ();
    d=posctrl(state_from_in, state_towards_in,dir, b, dt,&(trajectory_out->list_states),&(trajectory_out->list_inputs));

    if(d<myEps)
    {
        (*exact_connection_out)=1;
        return 1;

    }   else

    {

        (*exact_connection_out)=0;
        return 0;

    }

}


#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <ros/time.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <math.h>
#include "pose.h"

#define DEBUG_ODOM false
#define DEBUG_IMU false
#define DEBUG_TF false
#define DEBUG_COV false

using namespace std;
using namespace Eigen;
ros::Publisher odom_pub;
MatrixXd Q = MatrixXd::Identity(12, 12); // For propogate, IMU noise
MatrixXd Rt = MatrixXd::Identity(6, 6);   // For update, Odometry noise

// ros::Time current_time, last_time;
ros::Time current_time;
ros::Time last_time;

/*  Mean and covariance matrixs
    ps: present state
    ba: state propogated
    ns: next state
	[x1] x(0)  x(1)  x(2) = position, x y z
	[x2] x(3)  x(4)  x(5) = orientation, roll pitch yaw, ZXY Euler
	[x3] x(6)  x(7)  x(8) = linear velocity, x y z
	[x4] x(9)  x(10) x(11)= gyroscope bias
	[x5] x(12) x(13) x(14)= accelerator bias   							*/
VectorXd mean_ps= MatrixXd::Zero(15, 1 );
VectorXd mean_ba= MatrixXd::Zero(15, 1 );
VectorXd mean_ns= MatrixXd::Zero(15, 1 );
MatrixXd cov_ps = MatrixXd::Identity(15, 15);
MatrixXd cov_ba = MatrixXd::Identity(15, 15);
MatrixXd cov_ns = MatrixXd::Identity(15, 15);
/* Observation Model	*/
VectorXd g_ut(6);

// Initially use a constant, later need to read from the environment
float g = 9.82;
bool IMU_UPDATED = false;
bool CAMERA_UPDATED = false;

/*  Process model, since IMU is an internal measurement
    the gyro bias and accelerator bias is part to the state
    From the process model, IMU measurement - bias = real state
    Nov. 16th, 2016 note:
    1. Everything IMU has an input, propogate once;
    2. Getting g ~ 9.8 m.s^-2 is tricky
    3. Use MATLAB to from the A, B, and U
    Optimal:
    4. The best way to handle timestamp difference is to store the propogated
       and origin data, and trace back to the most recent propogated value
       and repropogate after the odometry reading comes
*/
void imu_callback(const sensor_msgs::Imu::ConstPtr &msg)
{
	IMU_UPDATED = true;
	#if DEBUG_IMU
		// cout << "Before process, the mean_ps is: " << endl << mean_ps << endl;
		// cout << "The cov_ps is: " << endl << cov_ps << endl;
    	ROS_INFO("Imu Seq: [%d]", msg->header.seq);
    	ROS_INFO("Imu linear acceleration x: [%f], y: [%f], z: [%f]", \
		msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z);
    	ROS_INFO("Imu angular velocity x: [%f], y: [%f], z: [%f]", \
    	msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z);
	#endif
    MatrixXd At = MatrixXd::Identity(15, 15);
    MatrixXd Ut = MatrixXd::Identity(15, 12);
    MatrixXd Ft = MatrixXd::Identity(15, 15);
    MatrixXd Vt = MatrixXd::Identity(15, 12);
    last_time = current_time;
	float dt = 0.0;
	if	(current_time < msg->header.stamp) { // Check for its first time
		current_time = msg->header.stamp;
    	dt = current_time.toSec() - last_time.toSec();
	}  else  {
		current_time = msg->header.stamp;
		dt = 0.0;
		return;
	}

	// double x11= mean_ps(0), x12= mean_ps(1), x13= mean_ps(2);
	double x21= mean_ps(3), x22= mean_ps(4), x23= mean_ps(5);
	double x31= mean_ps(6), x32= mean_ps(7), x33= mean_ps(8);
	double x41= mean_ps(9), x42= mean_ps(10),x43= mean_ps(11);
	double x51= mean_ps(12),x52= mean_ps(13),x53= mean_ps(14);
	// input u : acceleration, augular_velocity
	double am1= msg->linear_acceleration.x;
	double am2= msg->linear_acceleration.y;
	double am3= msg->linear_acceleration.z;
	double wm1= msg->angular_velocity.x;
	double wm2= msg->angular_velocity.y;
	double wm3= msg->angular_velocity.z;

	/**************************************************************************/
	/* updated to f(mu_t_1, u_t, 0), At(mu_t_1, u_t, 0), and Ut               */
	/* Generated by matlab ELEC6910P_Project2Phase2.m                         */
	/**************************************************************************/

	VectorXd f_t_1(15);
	f_t_1 <<
		x31,
		x32,
		x33,
		wm1*cos(x22) - 0*cos(x22) - x41*cos(x22) - 0*sin(x22) + wm3*sin(x22) - x43*sin(x22),
		-(0*cos(x21) - wm2*cos(x21) + x42*cos(x21) - 0*cos(x22)*sin(x21) + wm3*cos(x22)*sin(x21) - x43*cos(x22)*sin(x21) + 0*sin(x21)*sin(x22) - wm1*sin(x21)*sin(x22) + x41*sin(x21)*sin(x22))/cos(x21),
		-(0*cos(x22) - wm3*cos(x22) + x43*cos(x22) - 0*sin(x22) + wm1*sin(x22) - x41*sin(x22))/cos(x21),
		cos(x21)*sin(x23)*(0 - am2 + x52) - (cos(x23)*sin(x22) + cos(x22)*sin(x21)*sin(x23))*(0 - am3 + x53) - (cos(x22)*cos(x23) - sin(x21)*sin(x22)*sin(x23))*(0 - am1 + x51),
		- (cos(x22)*sin(x23) + cos(x23)*sin(x21)*sin(x22))*(0 - am1 + x51) - (sin(x22)*sin(x23) - cos(x22)*cos(x23)*sin(x21))*(0 - am3 + x53) - cos(x21)*cos(x23)*(0 - am2 + x52),
		cos(x21)*sin(x22)*(0 - am1 + x51) - cos(x21)*cos(x22)*(0 - am3 + x53) - sin(x21)*(0 - am2 + x52) - g,
		0,
		0,
		0,
		0,
		0,
		0;

	At <<
		0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
		0, 0, 0,                                                                                0,                                                 wm3*cos(x22) - 0*cos(x22) - x43*cos(x22) + 0*sin(x22) - wm1*sin(x22) + x41*sin(x22),                                                                                0, 0, 0, 0,                     -cos(x22),  0,                    -sin(x22),                                                0,                  0,                                                0,
		0, 0, 0,                              (0*cos(x22) - wm3*cos(x22) + x43*cos(x22) - 0*sin(x22) + wm1*sin(x22) - x41*sin(x22))/cos(x21)^2,                          -(sin(x21)*(0*cos(x22) - wm1*cos(x22) + x41*cos(x22) + 0*sin(x22) - wm3*sin(x22) + x43*sin(x22)))/cos(x21),                                                                                0, 0, 0, 0, -(sin(x21)*sin(x22))/cos(x21), -1, (cos(x22)*sin(x21))/cos(x21),                                                0,                  0,                                                0,
		0, 0, 0,                  -(sin(x21)*(0*cos(x22) - wm3*cos(x22) + x43*cos(x22) - 0*sin(x22) + wm1*sin(x22) - x41*sin(x22)))/cos(x21)^2,                                      (0*cos(x22) - wm1*cos(x22) + x41*cos(x22) + 0*sin(x22) - wm3*sin(x22) + x43*sin(x22))/cos(x21),                                                                                0, 0, 0, 0,             sin(x22)/cos(x21),  0,           -cos(x22)/cos(x21),                                                0,                  0,                                                0,
		0, 0, 0, cos(x21)*sin(x22)*sin(x23)*(0 - am1 + x51) - cos(x21)*cos(x22)*sin(x23)*(0 - am3 + x53) - sin(x21)*sin(x23)*(0 - am2 + x52), (cos(x23)*sin(x22) + cos(x22)*sin(x21)*sin(x23))*(0 - am1 + x51) - (cos(x22)*cos(x23) - sin(x21)*sin(x22)*sin(x23))*(0 - am3 + x53), (cos(x22)*sin(x23) + cos(x23)*sin(x21)*sin(x22))*(0 - am1 + x51) + (sin(x22)*sin(x23) - cos(x22)*cos(x23)*sin(x21))*(0 - am3 + x53) + cos(x21)*cos(x23)*(0 - am2 + x52), 0, 0, 0,                             0,  0,                            0,   sin(x21)*sin(x22)*sin(x23) - cos(x22)*cos(x23),  cos(x21)*sin(x23), - cos(x23)*sin(x22) - cos(x22)*sin(x21)*sin(x23),
		0, 0, 0, cos(x23)*sin(x21)*(0 - am2 + x52) - cos(x21)*cos(x23)*sin(x22)*(0 - am1 + x51) + cos(x21)*cos(x22)*cos(x23)*(0 - am3 + x53), (sin(x22)*sin(x23) - cos(x22)*cos(x23)*sin(x21))*(0 - am1 + x51) - (cos(x22)*sin(x23) + cos(x23)*sin(x21)*sin(x22))*(0 - am3 + x53), cos(x21)*sin(x23)*(0 - am2 + x52) - (cos(x23)*sin(x22) + cos(x22)*sin(x21)*sin(x23))*(0 - am3 + x53) - (cos(x22)*cos(x23) - sin(x21)*sin(x22)*sin(x23))*(0 - am1 + x51), 0, 0, 0,                             0,  0,                            0, - cos(x22)*sin(x23) - cos(x23)*sin(x21)*sin(x22), -cos(x21)*cos(x23),   cos(x22)*cos(x23)*sin(x21) - sin(x22)*sin(x23),
		0, 0, 0,                            cos(x22)*sin(x21)*(0 - am3 + x53) - cos(x21)*(0 - am2 + x52) - sin(x21)*sin(x22)*(0 - am1 + x51),                                                               cos(x21)*cos(x22)*(na1 - am1 + x51) + cos(x21)*sin(x22)*(0 - am3 + x53),                                                                                0, 0, 0, 0,                             0,  0,                            0,                                cos(x21)*sin(x22),          -sin(x21),                               -cos(x21)*cos(x22),
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

	Ut <<
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0,                     -cos(x22),  0,                    -sin(x22), 0, 0, 0, 0, 0, 0,
		0, 0, 0, -(sin(x21)*sin(x22))/cos(x21), -1, (cos(x22)*sin(x21))/cos(x21), 0, 0, 0, 0, 0, 0,
		0, 0, 0,             sin(x22)/cos(x21),  0,           -cos(x22)/cos(x21), 0, 0, 0, 0, 0, 0,
		   sin(x21)*sin(x22)*sin(x23) - cos(x22)*cos(x23),  cos(x21)*sin(x23), - cos(x23)*sin(x22) - cos(x22)*sin(x21)*sin(x23), 0, 0, 0, 0, 0, 0, 0, 0, 0,
		 - cos(x22)*sin(x23) - cos(x23)*sin(x21)*sin(x22), -cos(x21)*cos(x23),   cos(x22)*cos(x23)*sin(x21) - sin(x22)*sin(x23), 0, 0, 0, 0, 0, 0, 0, 0, 0,
		                                cos(x21)*sin(x22),          -sin(x21),                               -cos(x21)*cos(x22), 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
		0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;

	Ft = MatrixXd::Identity(15, 15) + dt * At;
    Vt = dt * Ut;

    // Calculate the propogagted mean and covariance
    mean_ba = mean_ps + dt * f_t_1;
    cov_ba = Ft * cov_ps * Ft.transpose() + Vt * Q * Vt.transpose();
	#if DEBUG_IMU
		// cout<< "After process, the mean_ba became" << endl << mean_ba << endl;
		// cout<< "The cov_ba became" << endl << cov_ba << endl;
	#endif
	#if DEBUG_COV
		ROS_INFO("Imu Seq: [%d]", msg->header.seq);
		cout<< "the mean_ba (0-5):" << endl << mean_ba.head(6) << endl;
		// cout<< "The cov_ba[6,6]  :" << endl << cov_ba.topLeftCorner(6, 6) << endl;
	#endif
}

// store singularity info
double phi_ppg;
double the_ppg;
double psi_ppg;
void odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
	CAMERA_UPDATED = true;
    //your code for update
    //camera position in the IMU frame = (0, -0.04, -0.02)
    //camera orientaion in the IMU frame = Quaternion(0, 0, 1, 0); w x y z, respectively
    //			   RotationMatrix << -1, 0, 0,
    //							      0, 1, 0,
    //                                0, 0, -1;

	VectorXd zt(6); // Camera reading in x,y,z, ZXY Euler

	/*                     Transformation from TF                           */
	// Get the world frame in camera frame transformation from the msg
	tf::Transform camera_pose_cw;
	tf::poseMsgToTF(msg->pose.pose, camera_pose_cw);

	// Record the camera frame in the IMU frame from TA
	tf::Transform transform_ic;
	transform_ic.setOrigin( tf::Vector3(0, -0.04, -0.02) );
	transform_ic.setRotation( tf::Quaternion(0, 0, -1, 0) );
	tf::Transform camera_pose_iw = transform_ic * camera_pose_cw;
	tf::Transform camera_pose_wi = camera_pose_iw.inverse();

	geometry_msgs::Transform camera_pose_wi_geo;
	tf::transformTFToMsg(camera_pose_wi, camera_pose_wi_geo);

	/*                     Transformation from Eigen                       */
	geometry_msgs::Quaternion q_cw = msg->pose.pose.orientation;
	// camera to tag world
	Matrix3d R_cw = Quaterniond(q_cw.w, q_cw.x, q_cw.y, q_cw.z).toRotationMatrix();
	Vector3d T_cw;
	T_cw << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z;
//	MatrixXd H_cw(4,4);
//	H_cw.col(0) << R_cw.col(0), 0;
//	H_cw.col(1) << R_cw.col(1), 0;
//	H_cw.col(2) << R_cw.col(2), 0;
//	H_cw.col(3) << T_cw   , 1;

	// IMU to camera frame
	Matrix3d R_ic = Quaterniond(0, 0, 1, 0).toRotationMatrix();
    Vector3d T_ic = Vector3d(0, -0.04, -0.02);
//	MatrixXd H_ic(4,4);
//	H_ic << -1, 0,  0,  0,
//			 0, 1,  0, -0.04,
//			 0, 0, -1, -0.02,
//			 0, 0,  0,  1;

	// IMU to tag world
	// Matrix3d R_iw = R_ic * R_cw;
	// Vector3d T_iw = R_ic * T_cw + T_ic;

	// tag world to IMU
//	MatrixXd H_wi(4,4);
//	H_wi = H_ic.inverse() * H_cw.inverse();
	// Matrix3d R_wi = R_iw.inverse();
	// Vector3d T_wi = -R_wi*T_iw;
	Matrix3d R_wi;
	Vector3d T_wi;
	R_wi =  R_cw.transpose() *  R_ic.transpose();
	T_wi = -R_cw.transpose() * (R_ic.transpose() * T_ic + T_cw);
//	R_wi = H_wi.topLeftCorner(3, 3);
//	T_wi = H_wi.topRightCorner(3, 1);

	#if DEBUG_TF
		Quaterniond R_wi_q(R_wi);
		cout << "camera_pose_wi_geo transformation from TF is: " << endl;
		cout << camera_pose_wi_geo.translation.x << endl;
		cout << camera_pose_wi_geo.translation.y << endl;
		cout << camera_pose_wi_geo.translation.z << endl;
		cout << "quaternion from TF is: " << endl;
		cout << camera_pose_wi_geo.rotation.w << endl;
		cout << camera_pose_wi_geo.rotation.x << endl;
		cout << camera_pose_wi_geo.rotation.y << endl;
		cout << camera_pose_wi_geo.rotation.z << endl;
		cout << "camera_pose_wi_geo transformation from Eigen is: " << endl;
		cout << T_wi(0) << endl;
		cout << T_wi(1) << endl;
		cout << T_wi(2) << endl;
		cout << "quaternion from Eigen is: " << endl;
		cout << R_wi_q.w() << endl;
		cout << R_wi_q.x() << endl;
		cout << R_wi_q.y() << endl;
		cout << R_wi_q.z() << endl;
	#endif
	if (DEBUG_TF) {
		zt(0) = camera_pose_wi_geo.translation.x;
		zt(1) = camera_pose_wi_geo.translation.y;
		zt(2) = camera_pose_wi_geo.translation.z;
		// From quaternion to rotation matrix and then ZXY Euler
		Eigen::Quaterniond R_wi_quat;
		R_wi_quat.w() = camera_pose_wi_geo.rotation.w;
		R_wi_quat.x() = camera_pose_wi_geo.rotation.x;
		R_wi_quat.y() = camera_pose_wi_geo.rotation.y;
		R_wi_quat.z() = camera_pose_wi_geo.rotation.z;
		Matrix3d R_wi_tf = R_wi_quat.toRotationMatrix();
		zt(3) = asin(R_wi_tf(2, 1)); // roll
		zt(4) = atan2(-R_wi_tf(2, 0) / cos(zt(3)), R_wi_tf(2, 2) / cos(zt(3)) ); // pitch
		zt(5) = atan2(-R_wi_tf(0, 1) / cos(zt(3)), R_wi_tf(1, 1) / cos(zt(3)) ); // yaw
	}
	else {
		// Use the result from Eigen in rviz
		Vector3d rpy_wi = R_to_rpy(R_wi);
		zt << T_wi, rpy_wi;
//		zt.head(3) = T_wi;
//		zt(3) = asin(R_wi(2, 1)); // roll
//		zt(4) = atan2(-R_wi(2, 0) / cos(zt(3)), R_wi(2, 2)) / cos(zt(3)) ; // pitch
//		zt(5) = atan2(-R_wi(0, 1) / cos(zt(3)), R_wi(1, 1)) / cos(zt(3)) ; // yaw
	}

	if (msg->header.seq == 0) { // first time callback, initialize all messages
		mean_ps << zt, MatrixXd::Zero(9, 1);
		mean_ba << zt, MatrixXd::Zero(9, 1);
		mean_ns << zt, MatrixXd::Zero(9, 1);
		phi_ppg = mean_ba(3);
		the_ppg = mean_ba(4);
		psi_ppg = mean_ba(5);
	}

	// Check if the angle passes the singularity point for ZXY Euler angle

	double phi = zt(3);
	double the = zt(4);
	double psi = zt(5);
	if (phi_ppg - phi >  2 * M_PI) { zt(3) += 2 * M_PI; cout<<" phi changes up   2*pi: " << phi << endl; }
	if (phi_ppg - phi < -2 * M_PI) { zt(3) -= 2 * M_PI; cout<<" phi changes down 2*pi: " << phi << endl; }
	if (the_ppg - the >  2 * M_PI) { zt(4) += 2 * M_PI; cout<<" the changes up   2*pi: " << the << endl; }
	if (the_ppg - the < -2 * M_PI) { zt(4) -= 2 * M_PI; cout<<" the changes down 2*pi: " << the << endl; }
	if (psi_ppg - psi >  2 * M_PI) { zt(5) += 2 * M_PI; cout<<" psi changes up   2*pi: " << psi << endl; }
	if (psi_ppg - psi < -2 * M_PI) { zt(5) -= 2 * M_PI; cout<<" psi changes down 2*pi: " << psi << endl; }
	phi_ppg = phi;
	the_ppg = the;
	psi_ppg = psi;

    #if DEBUG_ODOM
        // cout<<" The x of camera: " << zt(0) <<endl;
        // cout<<" The y of camera: " << zt(1) <<endl;
        // cout<<" The z of camera: " << zt(2) <<endl;
        cout<<" mean_ba(3) : " << mean_ba(3) <<endl;
        cout<<" mean_ba(4) : " << mean_ba(4) <<endl;
        cout<<" mean_ba(5) : " << mean_ba(5) <<endl;
		cout<<" zt(3) : " << zt(3) <<endl;
        cout<<" zt(4) : " << zt(4) <<endl;
        cout<<" zt(5) : " << zt(5) <<endl;
        // cout<<" The pose of camera is in the coordinate frame: " <<  msg->header.frame_id <<endl;
        // cout<<" The twist of camera is in the child frame: " <<  msg->child_frame_id <<endl;
    #endif

    /*     Update, with C and W matrix										  */
    /*     Linear for this case, but still use  Extended Kalman Filter        */
    MatrixXd Kt = MatrixXd::Identity(15, 6); // Kalman
    MatrixXd Ct = MatrixXd::Identity(6, 15);
    // MatrixXd Wt = MatrixXd::Identity(6, 6);

    Kt = cov_ba * Ct.transpose() * ((Ct * cov_ba * Ct.transpose() + Rt).inverse());
	g_ut << mean_ba(0), mean_ba(1), mean_ba(2), mean_ba(3), mean_ba(4), mean_ba(5);
    mean_ns = mean_ba + Kt * (zt - g_ut);
    cov_ns  = cov_ba  - Kt * Ct * cov_ba;
    mean_ps = mean_ns;
    cov_ps  = cov_ns;

	// 2016.12.8 reconstruction
	Vector3d X_rpy;
	X_rpy(0) = mean_ns(3);
	X_rpy(1) = mean_ns(4);
	X_rpy(2) = mean_ns(5);
	Quaterniond Q_output;
	Q_output = rpy_to_R(X_rpy);

    nav_msgs::Odometry ekf_odom;
	if	(IMU_UPDATED) {
		ekf_odom.header.seq = msg->header.seq;
		ekf_odom.header.stamp = msg->header.stamp;
		ekf_odom.header.frame_id = "world";
		ekf_odom.pose.pose.position.x = mean_ns(0);
		ekf_odom.pose.pose.position.y = mean_ns(1);
		ekf_odom.pose.pose.position.z = mean_ns(2);
		ekf_odom.pose.pose.orientation.w = Q_output.w();
		ekf_odom.pose.pose.orientation.x = Q_output.x();
		ekf_odom.pose.pose.orientation.y = Q_output.y();
		ekf_odom.pose.pose.orientation.z = Q_output.z();
		ekf_odom.twist.twist.linear.x = mean_ns(6);
		ekf_odom.twist.twist.linear.y = mean_ns(7);
		ekf_odom.twist.twist.linear.z = mean_ns(8);
	}
	else {
		ekf_odom = *msg;
	}

    odom_pub.publish(ekf_odom);

	#if DEBUG_ODOM
		// cout<<" The Kalman gain is:" << endl << Kt << endl;
		// cout<<" The mean_ns is:" << endl << mean_ns << endl;
		cout<<" The cov_ns is:" << endl << cov_ns << endl;
		// cout<<" The g_ut is:" << endl << g_ut << endl;
		cout<<" The odometry before EKF:" << endl;
        ROS_INFO("Seq: [%d]", msg->header.seq);
		ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", camera_pose_wi_geo.translation.x,camera_pose_wi_geo.translation.y, camera_pose_wi_geo.translation.z);
		ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", camera_pose_wi_geo.rotation.x, camera_pose_wi_geo.rotation.y, camera_pose_wi_geo.rotation.z, camera_pose_wi_geo.rotation.w);
		cout<<" The odometry after EKF:" << endl;
        ROS_INFO("Seq: [%d]", ekf_odom.header.seq);
		ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", ekf_odom.pose.pose.position.x,ekf_odom.pose.pose.position.y, ekf_odom.pose.pose.position.z);
		ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", ekf_odom.pose.pose.orientation.x, ekf_odom.pose.pose.orientation.y, ekf_odom.pose.pose.orientation.z, ekf_odom.pose.pose.orientation.w);
		ROS_INFO("Vel-> Linear: [%f], Angular: [%f]", ekf_odom.twist.twist.linear.x,ekf_odom.twist.twist.angular.z);
    #endif
	#if DEBUG_COV
		ROS_INFO("Cam Seq: [%d]", ekf_odom.header.seq);
		cout<< "the mean_ns (0-5):" << endl << mean_ns.head(6) << endl;
		// cout<< "The cov_ns[6,6]  :" << endl << cov_ns.topLeftCorner(6, 6) << endl;
	#endif
	// cout<<" The end of Odometry callback" << endl << endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ekf");
    ros::NodeHandle n("~");
	ros::Time::init();
	current_time = ros::Time::now();
    ros::Subscriber s1 = n.subscribe("imu", 1000, imu_callback);
    ros::Subscriber s2 = n.subscribe("tag_odom", 1000, odom_callback);
    odom_pub = n.advertise<nav_msgs::Odometry>("ekf_odom", 100);
    // Q imu covariance matrix; Rt visual odomtry covariance matrix
    Q.topLeftCorner(6, 6) = 0.01 * Q.topLeftCorner(6, 6);
    Q.bottomRightCorner(6, 6) = 0.01 * Q.bottomRightCorner(6, 6);
    Rt.topLeftCorner(3, 3) = 0.5 * Rt.topLeftCorner(3, 3);
    Rt.bottomRightCorner(3, 3) = 0.5 * Rt.bottomRightCorner(3, 3);
    Rt.bottomRightCorner(1, 1) = 0.1 * Rt.bottomRightCorner(1, 1);

    ros::spin();
}


#include "joint_tracker/pdf/NonLinearRevoluteMeasurementPdf.h"

#include <Eigen/Geometry>
#include <lgsm/Lgsm>
#include <ros/ros.h>

#include "omip_common/OMIPUtils.h"

using namespace BFL;

#define REV_PDF_DIM 6
#define REV_CONDITIONAL_VAR_DIM 7
#define REV_NUM_CONDITIONAL_ARGS 2

NonLinearRevoluteMeasurementPdf::NonLinearRevoluteMeasurementPdf(const Gaussian& additiveNoise) :
    AnalyticConditionalGaussianAdditiveNoise(additiveNoise,REV_NUM_CONDITIONAL_ARGS),
    dfx(REV_PDF_DIM, REV_CONDITIONAL_VAR_DIM)
{

}

NonLinearRevoluteMeasurementPdf::~NonLinearRevoluteMeasurementPdf()
{

}

MatrixWrapper::ColumnVector NonLinearRevoluteMeasurementPdf::ExpectedValueGet() const
{
    ColumnVector state = ConditionalArgumentGet(0);

    double phi = state(1);
    double theta = state(2);
    double sp = sin(phi);
    double cp = cos(phi);
    double st = sin(theta);
    double ct = cos(theta);

    double px = state(3);
    double py = state(4);
    double pz = state(5);

    Eigen::Vector3d rev_joint_orientation = Eigen::Vector3d(cp * st, sp * st, ct);
    Eigen::Vector3d rev_joint_position = Eigen::Vector3d(px, py, pz);

    double rv = state(6);

    Eigen::Vector3d joint_translation = (-rv * rev_joint_orientation).cross(rev_joint_position);

    Eigen::Twistd joint_pose = Eigen::Twistd(rv * rev_joint_orientation.x(),
                                             rv * rev_joint_orientation.y(),
                                             rv * rev_joint_orientation.z(),
                                             joint_translation.x(),
                                             joint_translation.y(),
                                             joint_translation.z());

    ColumnVector expected_pose(REV_PDF_DIM);
    expected_pose(1) = joint_pose.vx();
    expected_pose(2) = joint_pose.vy();
    expected_pose(3) = joint_pose.vz();
    expected_pose(4) = joint_pose.rx();
    expected_pose(5) = joint_pose.ry();
    expected_pose(6) = joint_pose.rz();

    return expected_pose + this->AdditiveNoiseMuGet();
}

MatrixWrapper::Matrix NonLinearRevoluteMeasurementPdf::dfGet(unsigned int i) const
{
    if (i == 0) //derivative to the first conditional argument (x)
    {
        double phi = ConditionalArgumentGet(0)(1);
        double theta = ConditionalArgumentGet(0)(2);
        double sp = sin(phi);
        double cp = cos(phi);
        double st = sin(theta);
        double ct = cos(theta);

        double px = ConditionalArgumentGet(0)(3);
        double py = ConditionalArgumentGet(0)(4);
        double pz = ConditionalArgumentGet(0)(5);
        double rv = ConditionalArgumentGet(0)(6);

        dfx = 0;

        dfx(1, 1) = -rv * cp * st * pz;
        dfx(2, 1) = -rv * sp * st * pz;
        dfx(3, 1) = rv * st * (cp * px + sp * py);
        dfx(4, 1) = -rv * sp * st;
        dfx(5, 1) = rv * cp * st;

        dfx(1, 2) = -rv * (st * py + sp * ct * pz);
        dfx(2, 2) = rv * (cp * ct * pz + st * px);
        dfx(3, 2) = rv * ct * (sp * px - cp * py);
        dfx(4, 2) = rv * cp * ct;
        dfx(5, 2) = rv * sp * ct;
        dfx(6, 2) = -rv * st;

        dfx(2, 3) = -rv * ct;
        dfx(3, 3) = rv * sp * st;

        dfx(1, 4) = rv * ct;
        dfx(3, 4) = -rv * cp * st;

        dfx(1, 5) = -rv * sp * st;
        dfx(2, 5) = rv * cp * st;

        dfx(1, 6) = ct * py - sp * st * pz;
        dfx(2, 6) = cp * st * pz - ct * px;
        dfx(3, 6) = st * (sp * px - cp * py);
        dfx(4, 6) = cp * st;
        dfx(5, 6) = sp * st;
        dfx(6, 6) = ct;

        dfx(1, 7) = 0;
        dfx(2, 7) = 0;
        dfx(3, 7) = 0;
        dfx(4, 7) = 0;
        dfx(5, 7) = 0;
        dfx(6, 7) = 0;
        return dfx;
    }
    else
    {
        ROS_ERROR_STREAM_NAMED( "NonLinearRevoluteMeasurementPdf::dfGet",
                    "The derivative is not implemented for the " << i << "th conditional argument");
        exit(BFL_ERRMISUSE);
    }
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// Filename: matrix_free_2.cpp (part of MTL4)

#include <iostream>
#include <cassert>
#include <boost/numeric/mtl/mtl.hpp>

struct poisson2D_dirichlet
{
    poisson2D_dirichlet(int m, int n) : m(m), n(n) {}

    template <typename Vector>
    Vector operator*(const Vector& v) const
    {
	mtl::vampir_trace<9901> tracer;
	assert(int(size(v)) == m * n);
	Vector w(m * n);

	// Inner domain
	for (int i= 1; i < m-1; i++)
	    for (int j= 1, k= i * n + j; j < n-1; j++, k++)
		w[k]= 4 * v[k] - v[k-n] - v[k+n] - v[k-1] - v[k+1];

	// Upper border
	for (int j= 1; j < n-1; j++)
	    w[j]= 4 * v[j] - v[j+n] - v[j-1] - v[j+1];

	// Lower border
	for (int j= 1, k= (m-1) * n + j; j < n-1; j++, k++)
	    w[k]= 4 * v[k] - v[k-n] - v[k-1] - v[k+1];

	// Left border
	for (int i= 1, k= n; i < m-1; i++, k+= n)
	    w[k]= 4 * v[k] - v[k-n] - v[k+n] - v[k+1];

	// Right border
	for (int i= 1, k= n+n-1; i < m-1; i++, k+= n)
	    w[k]= 4 * v[k] - v[k-n] - v[k+n] - v[k-1];

	// Corners
	w[0]= 4 * v[0] - v[1] - v[n];
	w[n-1]= 4 * v[n-1] - v[n-2] - v[2*n - 1];
	w[(m-1)*n]= 4 * v[(m-1)*n] - v[(m-2)*n] - v[(m-1)*n+1];
	w[m*n-1]= 4 * v[m*n-1] - v[m*n-2] - v[m*n-n-1];

	return w;
    }
    int m, n;
};

namespace mtl { namespace ashape {
    template <> struct ashape_aux<poisson2D_dirichlet>
    {	typedef nonscal type;    };
}}

int main(int, char**)
{
    using namespace std;

    mtl::compressed2D<double> B;
    laplacian_setup(B, 1000, 1000);

    mtl::dense_vector<double> v(1000000), w(1000000);
    iota(v);

    poisson2D_dirichlet A(1000, 1000);
    // cout << "A * v is " << A * v << endl;
    w= A * v;

    w= B * v;

     return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#ifndef MTL_MATRIX_CUPPEN_INCLUDE
#define MTL_MATRIX_CUPPEN_INCLUDE

#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/utility/assert.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/iota.hpp>
#include <boost/numeric/mtl/operation/secular.hpp>
#include <boost/numeric/mtl/operation/sort.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/utility/domain.hpp>
#include <boost/numeric/mtl/matrix/permutation.hpp>

#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/itl/iteration/basic_iteration.hpp>
#include <boost/numeric/itl/krylov/fsm.hpp>

namespace mtl { namespace mat {

/// Eigenvalues of triangle matrix A with Cuppen's divide and conquer algorithm
/** Eigenvalues are returned in vector lambda. A is overwritten. **/
template <typename Matrix, typename Vector>
void inline cuppen_inplace(Matrix& A, Matrix& Q, Vector& lambda)
{
    using std::abs; using mtl::irange; using mtl::imax; using mtl::iall; using vec::iota;

    typedef typename Collection<Matrix>::value_type     value_type;
    typedef typename Collection<Matrix>::size_type      size_type;
    typedef vec::dense_vector<size_type, vec::parameters<> >   size_vector; // todo: with type trait

    size_type        nrows= num_rows(A);
    MTL_CRASH_IF(nrows != num_cols(A), "Matrix not square");
    const value_type zero= 0, one= 1;

    if (nrows == 1){
	lambda[0]= A[0][0];
	Q= one;
    } else {
	size_type     m= size_type(nrows/2);
	irange        till_m(m), from_m(m, imax);

	size_vector   perm(nrows);
	Matrix        T1(A[till_m][till_m]), T2(A[from_m][from_m]),                               // sub-matrices of A
	              Q0(nrows, nrows), Q1(Q0[till_m][till_m]), Q2(Q0[from_m][from_m]);           // Q0 and sub-matrices
	Vector        v(nrows, zero), diag(nrows), lambda1(diag[till_m]), lambda2(diag[from_m]);  // sub-vectors of diag

	//DIVIDE
	value_type    b= A[m-1][m];
	T1[m-1][m-1]-= abs(b);
	T2[0][0]-= abs(b);

	v[m-1]= b > zero ? one : -one;
	v[m]= one;

	cuppen_inplace(T1, Q1, lambda1);
	cuppen_inplace(T2, Q2, lambda2);

	Q0[till_m][from_m]= zero; Q0[from_m][till_m]= zero; // zero out non-diagonal blocks

	T1[m-1][m-1]+= abs(b);
	T2[0][0]+= abs(b);

	iota(perm);
	sort(diag, perm);

	// CONQUER, start with eq. (3.0.2) using rows (not columns)
	v[till_m]= b < zero ? Vector(-trans(Q1[m-1][iall])) : trans(Q1[m-1][iall]);
	v[from_m]= trans(Q2[0][iall]);

	// permutation on v
	mtl::mat::traits::permutation<>::type P= mtl::mat::permutation(perm);
	Vector v1(P * v);

	lambda= secular(v1, diag, abs(b));   // solve secular equation

	// std::cout << "lambda is " << lambda << "\ndiag is " << diag << '\n';
	//Lemma 3.0.2  ... calculate eigenvectors
	Matrix Q_tilde(nrows, nrows);
	for (size_type i = 0; i < nrows; i++) {
	    for (size_type j= 0; j < size(diag); ++j)
		MTL_THROW_IF (diag[j] == lambda[i],
			      logic_error("Can't compute eigenvector, probably due to double eigenvalue."));
	    Vector    li(nrows, lambda[i]), lambda_i(ele_quot(v1, diag - li));
	    Q_tilde[iall][i]= lambda_i / two_norm(lambda_i); // normalized eigenvector in Matrix Q
	}

	Q= Q0 * P * Q_tilde;

#if 0
	for (size_type i = 0; i < nrows; i++) {
	    // Vector    qi(Q[iall][i]); // Todo: find out valgrind complains about the memory of qi for reasons inexplicable
	    Vector qi(nrows);
	    for (size_type j= 0; j < nrows; j++)
		qi[j]= Q[j][i];

	    std::cout << "q[" << i << "] = " << qi << ", lambda[i] = " << lambda[i]
		      << ", diff = " << two_norm(Vector(A*qi - lambda[i]*qi)) << ", A*qi = " << Vector(A*qi) << ", li*qi = " << Vector(lambda[i]*qi) << '\n';
	    itl::basic_iteration<double>   iter(1.0, 20, 1e-5, 1e-5);
	    fsm(A, qi, lambda[i], 0.1, iter);
	    std::cout << "q[" << i << "] = " << qi << ", diff = " << two_norm(Vector(A*qi - lambda[i]*qi)) << '\n';
	    Q[iall][i]= qi;
	}
#endif
    }
}

/// Eigenvalues of triangle matrix A with Cuppen's divide and conquer algorithm
/** Eigenvalues are returned in vector lambda. A is copied. **/
// A not as reference to force copy
template <typename Matrix, typename Vector>
void inline cuppen(Matrix A, Matrix& Q, Vector& lambda)
{
    Q= 0.0;
    cuppen_inplace(A, Q, lambda);
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_CUPPEN_INCLUDE


#include "wave/optimization/ceres/odom_gp_reduced/point_to_line_gp.hpp"
#include <Eigen/QR>

namespace wave {

SE3PointToLineGPRed::SE3PointToLineGPRed(const double *const p,
                                         const double *const pA,
                                         const double *const pB,
                                         const Mat12 &hat,
                                         const Mat12 &candle,
                                         const Mat3 &CovZ,
                                         bool calculate_weight)
    : pt(p), ptA(pA), ptB(pB), hat(hat), candle(candle) {
    this->JP_T.setZero();
    this->JP_T.block<3, 3>(0, 3).setIdentity();

    this->diff[0] = this->ptB[0] - this->ptA[0];
    this->diff[1] = this->ptB[1] - this->ptA[1];
    this->diff[2] = this->ptB[2] - this->ptA[2];
    this->bottom = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2];

    if (this->bottom < 1e-10) {
        // The points defining the line are too close to each other
        throw std::out_of_range("Points defining line are too close!");
    }

    this->Jres_P(0, 0) = 1 - (diff[0] * diff[0] / bottom);
    this->Jres_P(0, 1) = -(diff[0] * diff[1] / bottom);
    this->Jres_P(0, 2) = -(diff[0] * diff[2] / bottom);
    this->Jres_P(1, 0) = -(diff[1] * diff[0] / bottom);
    this->Jres_P(1, 1) = 1 - (diff[1] * diff[1] / bottom);
    this->Jres_P(1, 2) = -(diff[1] * diff[2] / bottom);
    this->Jres_P(2, 0) = -(diff[2] * diff[0] / bottom);
    this->Jres_P(2, 1) = -(diff[2] * diff[1] / bottom);
    this->Jres_P(2, 2) = 1 - (diff[2] * diff[2] / bottom);

    Eigen::Vector3d unitdiff;
    double invlength = 1.0 / sqrt(this->bottom);
    if (this->diff[2] > 0) {
        unitdiff[0] = this->diff[0] * invlength;
        unitdiff[1] = this->diff[1] * invlength;
        unitdiff[2] = this->diff[2] * invlength;
    } else {
        unitdiff[0] = -this->diff[0] * invlength;
        unitdiff[1] = -this->diff[1] * invlength;
        unitdiff[2] = -this->diff[2] * invlength;
    }

    Eigen::Vector3d unitz;
    unitz << 0, 0, 1;

    auto v = unitdiff.cross(unitz);
    auto s = v.norm();
    auto c = unitz.dot(unitdiff);
    Mat3 skew;
    Transformation<>::skewSymmetric3(v, skew);
    this->rotation = Eigen::Matrix3d::Identity() + skew + skew * skew * ((1 - c) / (s * s));

    this->Jres_P = this->rotation * this->Jres_P;

    if (calculate_weight) {
        auto rotated = this->Jres_P * CovZ * this->Jres_P.transpose();
        this->weight_matrix = rotated.block<2, 2>(0, 0).inverse().sqrt();
    } else {
        this->weight_matrix.setIdentity();
    }
}

bool SE3PointToLineGPRed::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    Eigen::Map<const Mat34> tk_map(parameters[0], 3, 4);
    Eigen::Map<const Mat34> tkp1_map(parameters[1], 3, 4);

    Transformation<Eigen::Map<const Mat34>, true> Tk(tk_map);
    Transformation<Eigen::Map<const Mat34>, true> Tkp1(tkp1_map);

    Eigen::Map<const Vec6> vel_k(parameters[2], 6, 1);

    if (jacobians) {
        Transformation<Eigen::Map<const Eigen::Matrix<double, 3, 4>>, true>::interpolateAndJacobians(
          Tk,
          Tkp1,
          vel_k,
          vel_k,
          this->hat,
          this->candle,
          this->T_current,
          this->JT_Ti,
          this->JT_Tip1,
          this->JT_Wi,
          this->JT_Wip1);
    } else {
        Transformation<Eigen::Map<const Eigen::Matrix<double, 3, 4>>, true>::interpolate(
          Tk, Tkp1, vel_k, vel_k, this->hat, this->candle, this->T_current);
    }

    Eigen::Map<const Vec3> PT(this->pt, 3, 1);
    Vec3 point = this->T_current.transform(PT);

    double p_A[3] = {point(0) - this->ptA[0], point(1) - this->ptA[1], point(2) - this->ptA[2]};

    double scaling = ceres::DotProduct(p_A, diff);
    // point on line closest to point
    double p_Tl[3] = {this->ptA[0] + (scaling / bottom) * diff[0],
                      this->ptA[1] + (scaling / bottom) * diff[1],
                      this->ptA[2] + (scaling / bottom) * diff[2]};

    Eigen::Map<const Vec3> pt_Tl(p_Tl, 3, 1);
    Eigen::Map<Eigen::Vector2d> reduced(residuals, 2, 1);

    reduced = this->weight_matrix * (this->rotation * (point - pt_Tl)).block<2, 1>(0, 0);

    if (jacobians != nullptr) {
        this->JP_T(0, 1) = point(2);
        this->JP_T(0, 2) = -point(1);
        this->JP_T(1, 0) = -point(2);
        this->JP_T(1, 2) = point(0);
        this->JP_T(2, 0) = point(1);
        this->JP_T(2, 1) = -point(0);

        // Jres_P already has rotation incorporated during construction
        this->Jr_T = this->Jres_P * this->JP_T;

        if (jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> Jr_Tk(jacobians[0], 2, 12);
            Jr_Tk.block<2, 6>(0, 0) = this->weight_matrix * this->Jr_T.block<2, 6>(0, 0) * this->JT_Ti;
            Jr_Tk.block<2, 6>(0, 6).setZero();
        }
        if (jacobians[1]) {
            Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> Jr_Tkp1(jacobians[1], 2, 12);
            Jr_Tkp1.block<2, 6>(0, 0) = this->weight_matrix * this->Jr_T.block<2, 6>(0, 0) * this->JT_Tip1;
            Jr_Tkp1.block<2, 6>(0, 6).setZero();
        }
        if (jacobians[2]) {
            Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> jac_map(jacobians[2], 2, 6);
            jac_map = this->weight_matrix * this->Jr_T.block<2, 6>(0, 0) * (this->JT_Wi + this->JT_Wip1);
        }
    }

    return true;
}

}  // namespace wave


#include "segments.hpp"

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <boost/foreach.hpp>

using namespace std;

int orientation(Point A, Point B, Point C)
{
    int val = (B[1] - A[1]) * (C[0] - B[0]) - (B[0] - A[0]) * (C[1] - B[1]);
    return val == 0 ? 0 : (val > 0 ? 1 : 2);
}

bool onSegment(Point P, Segment S) //P collinear with S
{
    if (P[0] <= std::max(S.A[0], S.B[0]) &&
        P[0] >= std::min(S.A[0], S.B[0]) &&
        P[1] <= std::max(S.A[1], S.B[1]) &&
        P[1] >= std::min(S.A[1], S.B[1]) )
    {
        return true;
    }
    return false;
}

int intersect(Segment sA, Segment sB)
{
    Point PA = sA.A, QA = sA.B, PB = sB.B, QB = sB.B;
    int o1 = orientation(PA, QA, PB);
    int o2 = orientation(PA, QA, QB);
    int o3 = orientation(PB, QB, PA);
    int o4 = orientation(PB, QB, QA);

    if (o1 != o2 && o3 != o4)
        return 1;

    if (o1 == 0 && onSegment(PB, sA)) return 1;
    if (o2 == 0 && onSegment(QB, sA)) return 1;
    if (o3 == 0 && onSegment(PA, sB)) return 1;
    if (o4 == 0 && onSegment(QA, sB)) return 1;

    return 0;

}

bool isInPolygon(Polygon polygon, Point P)
{
    Point Q = P;
    Q[0] = INF;
    int nInter = 0;
    Segment halfLine;
    halfLine.A = P;
    halfLine.B = Q;

    BOOST_FOREACH(Segment seg, polygon.seg)
    {
        if (orientation(seg.A, P, seg.B) == 0)
            return onSegment(P, seg);
        nInter += intersect(seg, halfLine);
    }

    return nInter&1;
}


#ifndef TRIUMF_BNMR_NUCLEI_HPP
#define TRIUMF_BNMR_NUCLEI_HPP

#include <boost/math/constants/constants.hpp>

#include <triumf/nmr/utilities.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// β-NMR probe nuclei and their properties
namespace nuclei {

// lithium-8
template <typename T = double> struct lithium_8 {
  // radioactive half-life (s)
  static inline constexpr T half_life() { return 1e-3 * 838.40; }
  // radioactive lifetime (s)
  static inline constexpr T lifetime() {
    return half_life() / boost::math::constants::ln_two<T>();
  }
  // spin quantum number
  static inline constexpr T spin() { return 2.0; }
  // magnetic dipole moment (nm)
  static inline constexpr T magnetic_dipole_moment() { return 1.65350; }
  // electric dipole moment (b)
  static inline constexpr T electric_quadrupole_moment() { return 0.0326; }
  // gyromagnetic ratio (s^-1 T^-1)
  static inline constexpr T gyromagnetic_ratio() {
    return triumf::nmr::utilities::calculate_gamma<T>(magnetic_dipole_moment(),
                                                      spin());
  }
  // gyromagnetic ratio (MHz / T)
  static inline constexpr T gyromagnetic_ratio_in_MHz_T() {
    return gyromagnetic_ratio() / 1e6 / boost::math::constants::two_pi<T>();
  }
};

// beryllium-11
template <typename T = double> struct beryllium_11 {
  // radioactive half-life (s)
  static inline constexpr T half_life() { return 13.76; }
  // radioactive lifetime (s)
  static inline constexpr T lifetime() {
    return half_life() / boost::math::constants::ln_two<T>();
  }
  // spin quantum number
  static inline constexpr T spin() { return 1.0 / 2.0; }
  // magnetic dipole moment (nm)
  static inline constexpr T magnetic_dipole_moment() { return -1.6816; }
  // electric dipole moment (b)
  static inline constexpr T electric_quadrupole_moment() { return 0.0; }
  // gyromagnetic ratio (s^-1 T^-1)
  static inline constexpr T gyromagnetic_ratio() {
    return triumf::nmr::utilities::calculate_gamma<T>(magnetic_dipole_moment(),
                                                      spin());
  }
  // gyromagnetic ratio (MHz / T)
  static inline constexpr T gyromagnetic_ratio_in_MHz_T() {
    return gyromagnetic_ratio() / 1e6 / boost::math::constants::two_pi<T>();
  }
};

// boron-12
template <typename T = double> struct boron_12 {
  // radioactive half-life (s)
  static inline constexpr T half_life() { return 1e-3 * 20.20; }
  // radioactive lifetime (s)
  static inline constexpr T lifetime() {
    return half_life() / boost::math::constants::ln_two<T>();
  }
  // spin quantum number
  static inline constexpr T spin() { return 1.0; }
  // magnetic dipole moment (nm)
  static inline constexpr T magnetic_dipole_moment() { return 1.003; }
  // electric dipole moment (b)
  static inline constexpr T electric_quadrupole_moment() { return 0.0132; }
  // gyromagnetic ratio (s^-1 T^-1)
  static inline constexpr T gyromagnetic_ratio() {
    return triumf::nmr::utilities::calculate_gamma<T>(magnetic_dipole_moment(),
                                                      spin());
  }
  // gyromagnetic ratio (MHz / T)
  static inline constexpr T gyromagnetic_ratio_in_MHz_T() {
    return gyromagnetic_ratio() / 1e6 / boost::math::constants::two_pi<T>();
  }
};

// magnesium-31
template <typename T = double> struct magnesium_31 {
  // radioactive half-life (s)
  static inline constexpr T half_life() { return 1e-3 * 236.0; }
  // radioactive lifetime (s)
  static inline constexpr T lifetime() {
    return half_life() / boost::math::constants::ln_two<T>();
  }
  // spin quantum number
  static inline constexpr T spin() { return 1.0 / 2.0; }
  // magnetic dipole moment (nm)
  static inline constexpr T magnetic_dipole_moment() { return -0.88340; }
  // electric dipole moment (b)
  static inline constexpr T electric_quadrupole_moment() { return 0.0; }
  // gyromagnetic ratio (s^-1 T^-1)
  static inline constexpr T gyromagnetic_ratio() {
    return triumf::nmr::utilities::calculate_gamma<T>(magnetic_dipole_moment(),
                                                      spin());
  }
  // gyromagnetic ratio (MHz / T)
  static inline constexpr T gyromagnetic_ratio_in_MHz_T() {
    return gyromagnetic_ratio() / 1e6 / boost::math::constants::two_pi<T>();
  }
};

} // namespace nuclei

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_NUCLEI_HPP


// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Copyright Paul A. Bristow 2015 - 2016.
// Copyright Christopher Kormanyos 2015 - 2016.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

// Below are snippets of code that can be included into a Quickbook file.

#include <iostream>
#include <iomanip>
#include <exception>
#include <typeinfo>

#include <boost/fixed_point/fixed_point.hpp>

//[fixed_point_type_representation_example_3

template <typename T>
void show_representation_types(std::ostream& os = std::cout)
{
  os << "Fixed_point Type "
    << typeid(T).name()
    // class boost::fixed_point::negatable<7,-1,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined>
    << " with range " << T::range << ", resolution " << T::resolution << std::endl;

  os << "value_type is " << typeid(T::value_type).name() << std::endl; // signed char
  os << "float_type is " << typeid(T::float_type).name() << std::endl; // float
} // template <typename T>void show_representation_types()

//] [/fixed_point_type_representation_example_3]

int main()
{
  using boost::fixed_point::negatable;

  typedef negatable<7, -1> fixed_point_type_7m1;
  typedef negatable<15, -16> fixed_point_type_15m16;
  typedef negatable<0, -15> fixed_point_type_0m15;
  typedef negatable<0, -31> fixed_point_type_0m31;
  typedef negatable<10, -21> fixed_point_type_10m21;
  typedef negatable<0, -63> fixed_point_type_0m63;
  typedef negatable<10, -53> fixed_point_type_10m53;
  typedef negatable<2, -78> fixed_point_type_2m78;
  typedef negatable<0, -126> fixed_point_type_0m126; //
  typedef negatable<10, -116> fixed_point_type_10m116; //
  typedef negatable<0, -254> fixed_point_type_0m254;

  try
  {
    std::cout.setf(std::ios_base::boolalpha | std::ios_base::showpoint); // Show any trailing zeros.
    std::cout << std::endl;

//[fixed_point_type_representation_example_1
     std::cout << typeid(negatable<0, -7>::value_type).name() << std::endl;
//] [/fixed_point_type_representation_example_1]

//[fixed_point_type_representation_example_2
     std::cout << typeid(negatable<0, -7>::float_type).name() << std::endl;
//] [/fixed_point_type_representation_example_2]

   show_representation_types<fixed_point_type_7m1>();
   show_representation_types<fixed_point_type_15m16>();
   show_representation_types<fixed_point_type_0m15>();
   show_representation_types<fixed_point_type_0m31>();
   show_representation_types<fixed_point_type_10m21>();
   show_representation_types<fixed_point_type_0m63>();
   show_representation_types<fixed_point_type_10m53>();
   show_representation_types<fixed_point_type_2m78>();
   show_representation_types<fixed_point_type_0m126>();
   show_representation_types<fixed_point_type_10m116>();
   show_representation_types<fixed_point_type_0m254>();

  }
  catch (std::exception ex)
  {
    std::cout << ex.what() << std::endl;
  }
}

/*
//[fixed_point_representation_examples_output_1

GCC5.1.0

signed char
float
Fixed_point Type class boost::fixed_point::negatable<7,-1,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 7, resolution -1
value_type is short
float_type is float
Fixed_point Type class boost::fixed_point::negatable<15,-16,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 15, resolution -16
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<0,-15,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -15
value_type is short
float_type is float
Fixed_point Type class boost::fixed_point::negatable<0,-31,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -31
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<10,-21,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -21
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<0,-63,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -63
value_type is __int64
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<63,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<10,-53,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -53
value_type is __int64
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<63,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<2,-78,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 2, resolution -78
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<80,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<0,-126,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -126
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<126,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<10,-116,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -116
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<126,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<0,-254,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -254
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<256,256,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<254,2,void,int,0,0>,0>

//] [/fixed_point_representation_examples_output_1]

//[fixed_point_representation_examples_output_2

VS 2013

Fixed_point Type class boost::fixed_point::negatable<7,-1,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 7, resolution -1
value_type is short
float_type is float
Fixed_point Type class boost::fixed_point::negatable<15,-16,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 15, resolution -16
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<0,-15,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -15
value_type is short
float_type is float
Fixed_point Type class boost::fixed_point::negatable<0,-31,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -31
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<10,-21,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -21
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<0,-63,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -63
value_type is __int64
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<63,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<10,-53,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -53
value_type is __int64
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<63,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<2,-78,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 2, resolution -78
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<80,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<0,-126,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -126
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<126,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<10,-116,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -116
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<126,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<0,-254,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -254
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<256,256,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<254,2,void,int,0,0>,0>

//] [/fixed_point_representation_examples_output_2]

*/