/**
 *  Moore-Greitzer engine DBA control with the specification-guided engine.
 *
 *  Created by Yinan Li on July 3, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <sys/stat.h>

#include <string>
#include <cmath>
#include <boost/numeric/odeint.hpp>

#include "src/DBAparser.h"
#include "src/abstraction.hpp"
#include "src/bsolver.hpp"
#include "src/hdf5io.h"

const double a = 1./3.5;
const double B = 2.0;
const double H = 0.18;
const double W = 0.25;
const double lc = 8.0;
const double cx = 1.0/lc;
const double cy = 1.0/(4*lc*B*B);
const double aH = a+H;
const double H2 = H/(2.0*W*W*W);
const double W2 = 3*W*W;

/* user defined dynamics */
struct mgode2 {

    static const int n = 2;  // system dimension
    static const int nu = 2;  // control dimension

    /* template constructor
     * @param[out] dx
     * @param[in] x
     * @param u
     */
    template<typename S>
    mgode2(S *dx, const S *x, rocs::Rn u) {
	dx[0] = cx * (aH+H2*(x[0]-W)*(W2-(x[0]-W)*(x[0]-W)) - x[1]) + u[0];
	dx[1] = cy * (x[0] - u[1]*sqrt(x[1]));
    }
};

int main(int argc, char *argv[])
{
    /**
     * Input arguments:
     * engine2dAbstII dbafile
     */
    if (argc != 2) {
	std::cout << "Improper number of arguments.\n";
	std::exit(1);
    }

    clock_t tb, te;
    /* set the state space */
    double xlb[]{0.44, 0.6};
    double xub[]{0.54, 0.7};

    /* set the control values */
    // double Lmu = 0.01;
    double Lu = 0.05;
    double ulb[]{-Lu, 0.5};
    double uub[]{Lu, 0.8};
    double mu[]{Lu/5, 0.01};

    /* set the sampling time and disturbance */
    double delta = 0.01;
    /* parameters for computing the flow */
    int kmax = 10;
    double tol = 0.01;
    double alpha = 0.5;
    double beta = 2;
    rocs::params controlparams(kmax, tol, alpha, beta);

    /* define the control system */
    double t= 0.1;
    rocs::CTCntlSys<mgode2> engine("Moore-Greitzer", t, mgode2::n, mgode2::nu,
				  delta, &controlparams);
    engine.init_workspace(xlb, xub);
    engine.init_inputset(mu, ulb, uub);
    engine.allocate_flows();

    /**
     * Abstraction
     */
    rocs::abstraction< rocs::CTCntlSys<mgode2> > abst(&engine);
    const double eta[]{0.00018, 0.00018};
    abst.init_state(eta, xlb, xub);
    std::cout << "The number of abstraction states: " << abst._x._nv << '\n';
    double obs[][2]{{0.497, 0.503}, {0.650, 0.656}};
    auto avoid = [&obs, abst, eta](size_t& id) {
		     std::vector<double> x(abst._x._dim);
    		     abst._x.id_to_val(x, id);
    		     double c1 = eta[0]/2.0; //+1e-10;
    		     double c2 = eta[1]/2.0; //+1e-10;
		     if ((obs[0][0]-c1) <= x[0] && x[0] <= (obs[0][1]+c1) &&
			 (obs[1][0]-c2) <= x[1] && x[1] <= (obs[1][1]+c2))
			 return -1;
		     return 0;
		 };
    abst.assign_labels(avoid);
    abst.assign_label_outofdomain(0);
    std::vector<size_t> obstacles;
    for (size_t i = 0; i < abst._x._nv; ++i) {
    	if (abst._labels[i] < 0)
    	    obstacles.push_back(i);
    }

    std::string transfile = "abstII_0.00018.h5";
    struct stat buffer;
    float tabst;
    if(stat(transfile.c_str(), &buffer) == 0) {
	/* Read from a file */
	std::cout << "Transitions have been computed. Reading transitions...\n";
	rocs::h5FileHandler transRdr(transfile, H5F_ACC_RDONLY);
	tb = clock();
	transRdr.read_transitions(abst._ts);
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of reading abstraction: " << tabst << '\n';
    } else {
	std::cout << "Transitions haven't been computed. Computing transitions...\n";
	/* Robustness margins */
	double e1[] = {0.0, 0.0};
	double e2[] = {0.0, 0.0};
	tb = clock();
	abst.assign_transitions(e1, e2);
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of computing abstraction: " << tabst << '\n';
	/* Write abstraction to file */
	rocs::h5FileHandler transWtr(transfile, H5F_ACC_TRUNC);
	transWtr.write_transitions(abst._ts);
    }
    std::cout << "# of transitions: " << abst._ts._ntrans << '\n';

    /**
     * Read DBA from spec*.txt file
     */
    std::cout << "Reading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    std::string specfile = std::string(argv[1]);
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
	std::exit(1);

    /*
     * Assign labels to states: has to be consistent with the dba file.
     */
    double e = 0.003;
    double goal[][2]{{0.4519-e, 0.4519+e}, {0.6513-e, 0.6513+e}};

    auto label_target = [&goal, &abst, &eta](size_t i) {
			    std::vector<double> x(abst._x._dim);
			    abst._x.id_to_val(x, i);
			    double c1= eta[0]/2.0; //+1e-10;
			    double c2= eta[1]/2.0; //+1e-10;

			    return (goal[0][0] <= (x[0]-c1) && (x[0]+c1) <= goal[0][1] &&
				    goal[1][0] <= (x[1]-c2) && (x[1]+c2) <= goal[1][1]) ?
				1: abst._labels[i];
			};
    abst.assign_labels(label_target);
    std::cout << "Specification assignment is done.\n";
    std::vector<size_t> targetIDs;
    std::vector<rocs::Rn> targetPts;   //initial invariant set
    rocs::Rn x(abst._x._dim);
    for(size_t i = 0; i < abst._labels.size(); ++i) {
    	if(abst._labels[i] > 0) {
    	    targetIDs.push_back(i);
    	    abst._x.id_to_val(x, i);
    	    targetPts.push_back(x);
    	}
    }

    /**
     * Solve a Buchi game on the product of NTS and DBA.
     */
    std::cout << "Start solving a Buchi game on the product of the abstraction and DBA...\n";
    rocs::BSolver solver; // memories will be allocated for psolver
    solver.construct_dba((int)nAP, (int)nNodes, (int)q0, acc, arrayM);
    tb = clock();
    solver.load_abstraction(abst);
    solver.generate_product(abst);
    solver.solve_buchigame_on_product();
    te = clock();
    float tsyn = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of synthesizing controller: " << tsyn << '\n';

    /**
     * Display and save memoryless controllers.
     */
    std::cout << "Writing the controller...\n";
    // std::string datafile = "controller_abstII.txt";
    // solver.write_controller_to_txt(const_cast<char*>(datafile.c_str()));
    std::string datafile = "controller_abstII_0.00018.h5";
    rocs::h5FileHandler ctlrWtr(datafile, H5F_ACC_TRUNC);
    ctlrWtr.write_problem_setting< rocs::CTCntlSys<mgode2> >(engine);
    ctlrWtr.write_2d_array<double>(targetPts, "G");
    ctlrWtr.write_array<double>(eta, mgode2::n, "eta");
    ctlrWtr.write_2d_array<double>(abst._x._data, "xgrid");
    ctlrWtr.write_discrete_controller(&(solver._sol));
    std::cout << "Controller writing is done.\n";

    std::cout << "Total time of used (abstraction+synthesis): " << tabst+tsyn << '\n';

    engine.release_flows();
    return 0;
}


/*
 * This file is modified from the sources of Gazebo 7.0
 *
 * Copyright (C) 2012-2016 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef _GAZEBO_MATH_FUNCTIONS_HH_
#define _GAZEBO_MATH_FUNCTIONS_HH_

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>
#include <algorithm>
#include <cmath>
#include <limits>
#include <string>
#include <iostream>
#include <vector>

/// \brief Double maximum value
#define GZ_DBL_MAX std::numeric_limits<double>::max()

/// \brief Double min value
#define GZ_DBL_MIN std::numeric_limits<double>::min()

/// \brief Double positive infinite value
#define GZ_DBL_INF std::numeric_limits<double>::infinity()

/// \brief Float maximum value
#define GZ_FLT_MAX std::numeric_limits<float>::max()

/// \brief Float minimum value
#define GZ_FLT_MIN std::numeric_limits<float>::min()

/// \brief 32bit unsigned integer maximum value
#define GZ_UINT32_MAX std::numeric_limits<uint32_t>::max()

/// \brief 32bit unsigned integer minimum value
#define GZ_UINT32_MIN std::numeric_limits<uint32_t>::min()

/// \brief 32bit integer maximum value
#define GZ_INT32_MAX std::numeric_limits<int32_t>::max()

/// \brief 32bit integer minimum value
#define GZ_INT32_MIN std::numeric_limits<int32_t>::min()

namespace gazebo
{
  namespace math
  {
    /// \addtogroup gazebo_math
    /// \brief A set of classes that encapsulate math related properties and
    ///        functions.
    /// \{

    /// \brief Returns the representation of a quiet not a number (NAN)
    static const double NAN_D = std::numeric_limits<double>::quiet_NaN();

    /// \brief Returns the representation of a quiet not a number (NAN)
    static const int NAN_I = std::numeric_limits<int>::quiet_NaN();

    /// \brief Simple clamping function
    /// \param[in] _v value
    /// \param[in] _min minimum
    /// \param[in] _max maximum
    template<typename T>
    inline T clamp(T _v, T _min, T _max)
    {
      return std::max(std::min(_v, _max), _min);
    }

    /// \brief check if a float is NaN
    /// \param[in] _v the value
    /// \return true if _v is not a number, false otherwise
    inline bool isnan(float _v)
    {
      return (boost::math::isnan)(_v);
    }

    /// \brief check if a double is NaN
    /// \param[in] _v the value
    /// \return true if _v is not a number, false otherwise
    inline bool isnan(double _v)
    {
      return (boost::math::isnan)(_v);
    }

    /// \brief Fix a nan value.
    /// \param[in] _v Value to correct.
    /// \return 0 if _v is NaN, _v otherwise.
    inline float fixnan(float _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
    }

    /// \brief Fix a nan value.
    /// \param[in] _v Value to correct.
    /// \return 0 if _v is NaN, _v otherwise.
    inline double fixnan(double _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
    }

    /// \brief get mean of vector of values
    /// \param[in] _values the vector of values
    /// \return the mean
    template<typename T>
    inline T mean(const std::vector<T> &_values)
    {
      T sum = 0;
      for (unsigned int i = 0; i < _values.size(); ++i)
        sum += _values[i];
      return sum / _values.size();
    }

    /// \brief get variance of vector of values
    /// \param[in] _values the vector of values
    /// \return the squared deviation
    template<typename T>
    inline T variance(const std::vector<T> &_values)
    {
      T avg = mean<T>(_values);

      T sum = 0;
      for (unsigned int i = 0; i < _values.size(); ++i)
        sum += (_values[i] - avg) * (_values[i] - avg);
      return sum / _values.size();
    }

    /// \brief get the maximum value of vector of values
    /// \param[in] _values the vector of values
    /// \return maximum
    template<typename T>
    inline T max(const std::vector<T> &_values)
    {
      T max = std::numeric_limits<T>::min();
      for (unsigned int i = 0; i < _values.size(); ++i)
        if (_values[i] > max)
          max = _values[i];
      return max;
    }

    /// \brief get the minimum value of vector of values
    /// \param[in] _values the vector of values
    /// \return minimum
    template<typename T>
    inline T min(const std::vector<T> &_values)
    {
      T min = std::numeric_limits<T>::max();
      for (unsigned int i = 0; i < _values.size(); ++i)
        if (_values[i] < min)
          min = _values[i];
      return min;
    }

    /// \brief check if two values are equal, within a tolerance
    /// \param[in] _a the first value
    /// \param[in] _b the second value
    /// \param[in] _epsilon the tolerance
    template<typename T>
    inline bool equal(const T &_a, const T &_b,
                      const T &_epsilon = 1e-6)
    {
      return std::fabs(_a - _b) <= _epsilon;
    }

    /// \brief get value at a specified precision
    /// \param[in] _a the number
    /// \param[in] _precision the precision
    /// \return the value for the specified precision
    template<typename T>
    inline T precision(const T &_a, const unsigned int &_precision)
    {
      if (!std::isinf(_a))
      {
        return boost::math::round(
          _a * pow(10, _precision)) / pow(10, _precision);
      }
      else
      {
        return _a;
      }
    }

    /// \brief is this a power of 2?
    /// \param[in] _x the number
    /// \return true if _x is a power of 2, false otherwise
    inline bool isPowerOfTwo(unsigned int _x)
    {
      return ((_x != 0) && ((_x & (~_x + 1)) == _x));
    }

    /// \brief Get the smallest power of two that is greater or equal to a given
    /// value
    /// \param[in] _x the number
    /// \return the same value if _x is already a power of two. Otherwise,
    /// it returns the smallest power of two that is greater than _x
    inline unsigned int roundUpPowerOfTwo(unsigned int _x)
    {
      if (_x == 0)
        return 1;

      if (isPowerOfTwo(_x))
        return _x;

      while (_x & (_x - 1))
        _x = _x & (_x - 1);

      _x = _x << 1;

      return _x;
    }

    /// \brief parse string into an integer
    /// \param[in] _input the string
    /// \return an integer, 0 or 0 and a message in the error stream
    inline int parseInt(const std::string& _input)
    {
      const char *p = _input.c_str();
      if (!*p || *p == '?')
        return NAN_I;

      int s = 1;
      while (*p == ' ')
        p++;

      if (*p == '-')
      {
        s = -1;
        p++;
      }

      double acc = 0;
      while (*p >= '0' && *p <= '9')
        acc = acc * 10 + *p++ - '0';

      if (*p)
      {
        std::cerr << "Invalid int numeric format[" << _input << "]\n";
        return 0.0;
      }

      return s * acc;
    }

    /// \brief parse string into float
    /// \param _input the string
    /// \return a floating point number (can be NaN) or 0 with a message in the
    /// error stream
    inline double parseFloat(const std::string& _input)
    {
      const char *p = _input.c_str();
      if (!*p || *p == '?')
        return NAN_D;
      int s = 1;
      while (*p == ' ')
        p++;

      if (*p == '-')
      {
        s = -1;
        p++;
      }

      double acc = 0;
      while (*p >= '0' && *p <= '9')
        acc = acc * 10 + *p++ - '0';

      if (*p == '.')
      {
        double k = 0.1;
        p++;
        while (*p >= '0' && *p <= '9')
        {
          acc += (*p++ - '0') * k;
          k *= 0.1;
        }
      }
      if (*p == 'e')
      {
        int es = 1;
        int f = 0;
        p++;
        if (*p == '-')
        {
          es = -1;
          p++;
        }
        else if (*p == '+')
        {
          es = 1;
          p++;
        }
        while (*p >= '0' && *p <= '9')
          f = f * 10 + *p++ - '0';

        acc *= pow(10, f*es);
      }

      if (*p)
      {
        std::cerr << "Invalid double numeric format[" << _input << "]\n";
        return 0.0;
      }
      return s * acc;
    }
    /// \}
  }
}
#endif


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

vec addTwo_parallel(vec x, vec y){
    vec z(x.n_elem);
    int i;
#pragma omp parallel for schedule(static) \
private(i) shared(x,y,z)
    for(i=0; i < x.n_elem; i++)
        z(i) = x(i) + 2 * y(i);
    return z;
}

int main()
{

    vec a = ones<vec>(10);
    vec b = ones<vec>(10);
    vec c = addTwo_parallel(a, b);
    c.print();
    return 0;
}


#include "powerlawCommon.h"
//#include <boost/program_options.hpp>
#include "tkdCmdParser.h"

/**
 * @author: W.M. Otte (wim@invivonmr.uu.nl); Image Sciences Institute, UMC Utrecht, NL.
 * @date: 19-11-2009
 *
 * Estimate powerlaw scaling parameter from input distribution.
 *
 * ***************************************************************************
 * Method: "Power-law distributions in empirical data", Clauset et al, 2009
 * http://www.santafe.edu/~aaronc/powerlaws/
 * ***************************************************************************
 */
class PowerLawFit
{

public:

	typedef double ValueType;
	typedef std::vector< ValueType > VectorType;

	/**
	 * Power law fit.
	 */
	void run( const std::string& inputFileName, bool nosmall, bool finite,
					double startXmin, double incrementXmin, double endXmin,
						bool bootstrap, unsigned int bootstrapIterations, bool verbose )
	{
		// [ 1 ] read input from text file ...
		VectorType values = getInput( inputFileName );

		// [ 2 ] bootstrap or single fit ...
		VectorType results;

		if ( bootstrap )
		{
			graph::Powerlaw< ValueType >::BootstrapFit( values, results, nosmall, finite, startXmin, incrementXmin, endXmin, bootstrapIterations, verbose );

			if ( ! results.empty() )
			{
				std::cout << "Alpha," << results.at( 0 ) <<  std::endl;
				std::cout << "Xmin," << results.at( 1 ) << std::endl;
				std::cout << "Log-likelihood," << results.at( 2 ) << std::endl;
				std::cout << "Alpha_sd," << results.at( 3 ) <<  std::endl;
				std::cout << "Xmin_sd," << results.at( 4 ) << std::endl;
				std::cout << "Log-likelihood_sd," << results.at( 5 ) << std::endl;

			}
			else
			{
				std::cerr << "*** ERROR ***: maximum likelihood "
					"bootstrap estimation failed! -> check input ..." << std::endl;
			}

		}
		else
		{
			graph::Powerlaw< ValueType >::SingleFit( values, results, nosmall, finite,
					startXmin, incrementXmin, endXmin );

			if ( ! results.empty() )
			{
				std::cout << "Alpha," << results.at( 0 ) << std::endl;
				std::cout << "Xmin," << results.at( 1 ) << std::endl;
				std::cout << "Log-likelihood," << results.at( 2 ) << std::endl;
			}
			else
			{
				std::cerr << "*** ERROR ***: maximum likelihood "
					"single estimation failed! -> check input ..." << std::endl;
			}
		}
	}

protected:

	/**
	 * Return input from given text file as vector.
	 */
	VectorType getInput( const std::string& input )
	{
		std::ifstream inFile;

		inFile.open( input.c_str() );
		if ( !inFile )
		{
			std::cout << "*** ERROR ***: Unable to open: " << input << "." << std::endl;
			exit( EXIT_FAILURE );
		}

		double x;
		VectorType output;

		while ( inFile >> x )
		{
			output.push_back( x );
		}
		inFile.close();

		/**
		 * Negative values will be converted to complex numbers in matlab,
		 * but not with the stl ...
		 *
		 * No support is given (yet) for complex number mle.
		 */
		if ( *( std::min_element( output.begin(), output.end() ) ) < 0 )
		{
			std::cerr << "*** ERROR ***: Negative input not supported!" << std::endl;
			exit (EXIT_FAILURE );
		}

		return output;
	}
};

// ************************************************************************************

/**
 * Option list.
 */
struct parameters
{
	std::string input;
	bool nosmall;
	bool finite;
	bool bootstrap;
	bool verbose;
	double startXmin;
	double incrementXmin;
	double endXmin;
	int bootstrapIterations;

};

/**
 * Fit powerlaw to list of numbers.
 */
int main(int argc, char* argv[])
{
	tkd::CmdParser p( argv[0], "Fits a power-law distributional model to data." );

		parameters list;

		list.nosmall = false;
		list.finite = false;
		list.bootstrap = false;
		list.verbose = false;
		list.startXmin = 1.5;
		list.incrementXmin = 0.01;
		list.endXmin = 3.5;
		list.bootstrapIterations = 1000;

		p.AddArgument( list.input, "input" )
				->AddAlias( "i" )
				->SetDescription( "Input file with distribution values in column format" )
				->SetRequired( true );

		p.AddArgument( list.nosmall, "nosmall" )
				->SetDescription( "Truncate the search over xmin values before the finite-size bias becomes significant (default: false)" );

		p.AddArgument( list.finite, "finite" )
				->SetDescription( "Use an experimental finite-size correction (default: false)" );

		p.AddArgument( list.bootstrap, "bootstrap" )
				->SetDescription( "Run non-parametric bootstrap instead of single estimation (default: false)" );

		p.AddArgument( list.verbose, "verbose" )
				->SetDescription( "Print boostrap status (default: false)" );

		p.AddArgument( list.startXmin, "start-x-min" )
				->SetDescription( "Start value for discrete xmin estimation (default: 1.5)" );

		p.AddArgument( list.incrementXmin, "increment-x-min" )
				->SetDescription( "Increment value for discrete xmin estimation (default: 0.01)" );

		p.AddArgument( list.endXmin, "end-x-min" )
				->SetDescription( "End value for discrete xmin estimation (default: 3.5)" );

		p.AddArgument( list.bootstrapIterations, "iterations" )
				->SetDescription( "Bootstrap iterations (default: 1000)" );

		if ( !p.Parse( argc, argv ) )
		{
			p.PrintUsage( std::cout );
			return EXIT_FAILURE;
		}

        // run application ...
        PowerLawFit powerlawFit;

    	powerlawFit.run( list.input, list.nosmall, list.finite,
							list.startXmin, list.incrementXmin, list.endXmin,
											list.bootstrap, list.bootstrapIterations, list.verbose );

    return EXIT_SUCCESS;
}


#ifndef HOPS_AUTOCORRELATION_HPP
#define HOPS_AUTOCORRELATION_HPP

#include <Eigen/Core>
#include <unsupported/Eigen/FFT>
#include <memory>

namespace hops {
    inline size_t nextGoodSizeFFT(size_t N) {
        if (N <= 2) {
            return 2;
        }
        while (true) {
            size_t m = N;
            while ((m % 2) == 0) {
            m /= 2;
            }
            while ((m % 3) == 0) {
            m /= 3;
            }
            while ((m % 5) == 0) {
            m /= 5;
            }
            if (m <= 1) {
            return N;
            }
            N++;
        }
    }

    template <typename StateType>
    void computeAutocorrelations (const std::vector<StateType>& draws,
                                  Eigen::VectorXd& autocorrelations,
                                  unsigned long dimension) {
        computeAutocorrelations(&draws, autocorrelations, dimension);
    }

    template <typename StateType>
    void computeAutocorrelations (const std::vector<StateType>* draws,
                                  Eigen::VectorXd& autocorrelations,
                                  unsigned long dimension) {
        size_t N = draws->size();
        Eigen::VectorXd X = Eigen::VectorXd::Zero(N);
        for (size_t n = 0; n < N; ++n) {
            X(n) = (*draws)[n](dimension);
        }

        Eigen::FFT<typename StateType::Scalar> fft;
        size_t M = nextGoodSizeFFT(N);
        size_t Mt2 = 2 * M;

        // center and pad X
        Eigen::VectorXd centeredX(Mt2);
        centeredX.setZero();
        centeredX.head(N) = X.array() - X.mean();

        // See https://en.wikipedia.org/wiki/Autocorrelation#Efficient_computation for a quick
        // explanation on what follows
        Eigen::VectorXcd frequency(Mt2);
        fft.fwd(frequency, centeredX);

        frequency = frequency.cwiseAbs2();

        Eigen::VectorXcd autocorrelationsTmp(Mt2);
        fft.inv(autocorrelationsTmp, frequency);

        // use "biased" estimate as recommended by Geyer (1992)
        autocorrelations = autocorrelationsTmp.head(N).real().array() / (N * N * 2);
        autocorrelations /= autocorrelations(0);
    }
}

#endif // HOPS_AUTOCORRELATION_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FLOOR_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FLOOR_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing floor capabilities

    Computes the floor of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = floor(x);
    @endcode

    is the greatest integral value of type @c T less or equal to @c x.

    @see  ceil, round, nearbyint, trunc, ifloor

  **/
  Value floor(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/floor.hpp>
#include <boost/simd/function/simd/floor.hpp>

#endif


/* -*- Mode: C++; c-basic-offset: 2; indent-tabs-mode: nil; tab-width: 8 -*- */

#include <boost/tokenizer.hpp>
#include <spotfinder/core_toolbox/distl.h>
#include <scitbx/vec3.h>

namespace af = scitbx::af;
namespace di = spotfinder::distltbx;
//this function was developed but never used; deprecate soon
Distl::spot::spot(af::flex_int::const_iterator intptr) :
  peakintensity(0.0),
  nmaxima(0),
  p_gotaxes(false)
{
  peak = Distl::point(*intptr,*(intptr+1));
  intptr+=2;
  int bodylen = *intptr; intptr++;
  bodypixels.reserve(bodylen);
  for (int i=0; i<bodylen; ++i, intptr+=2){
    bodypixels.push_back(Distl::point(*intptr,*(intptr+1)));}
}

void
di::w_spot::p_getaxes() {
//DEPRECATE SOON---DEPRECATE SOON
  if (p_gotaxes) return;
  if (bodypixels.size() < 2) {
    p_majoraxis = 1.0;
    p_minoraxis = 1.0;
    p_gotaxes = true;
    return;
  }
  // iteration to find average position
  af::tiny<double, 2> average_pixel_position = af::tiny<double, 2>(0.,0.);
  for (int i = 0; i < bodypixels.size() ; ++i) {
    average_pixel_position[0]+=bodypixels[i].x;
    average_pixel_position[1]+=bodypixels[i].y;
  }
  average_pixel_position[0]/=bodypixels.size();
  average_pixel_position[1]/=bodypixels.size();

  //iteration to find major axis
  std::vector<double> sq_distance;
  for (int i = 0; i < bodypixels.size() ; ++i) {
    sq_distance.push_back(
     (bodypixels[i].x - average_pixel_position[0]) *
     (bodypixels[i].x - average_pixel_position[0]) +
     (bodypixels[i].y - average_pixel_position[1]) *
     (bodypixels[i].y - average_pixel_position[1]));
  }
  std::vector<double>::const_iterator ptrmajor =
    std::max_element(sq_distance.begin(),sq_distance.end());
  double c_majoraxis = 2.0*std::sqrt(*ptrmajor);
  size_t posmajor = ptrmajor - sq_distance.begin();
  //printf("posmajor %7d\n",posmajor);

  //double check to verify the major axis vector:
  double dx = bodypixels[posmajor].x - average_pixel_position[0];
  double dy = bodypixels[posmajor].y - average_pixel_position[1];
  //double d_majoraxis = 2.0*std::sqrt( dx*dx + dy*dy );
  //printf("C_major %7.1f   D_major %7.1f\n",c_majoraxis,d_majoraxis);

  //iteration to find minor axis
  std::vector<double> abs_cross_product;
  for (int i = 0; i < bodypixels.size() ; ++i) {
    abs_cross_product.push_back( std::fabs(
       dx * (bodypixels[i].y - average_pixel_position[1]) -
       (bodypixels[i].x - average_pixel_position[0]) * dy
    ) );
  }
  std::vector<double>::const_iterator ptrminor =
    std::max_element(abs_cross_product.begin(),abs_cross_product.end());
  double c_minoraxis = 4.0*(*ptrminor)/c_majoraxis;
  //printf("C_major %7.1f   C_minor %7.1f\n",c_majoraxis,c_minoraxis);

  if (c_majoraxis >= c_minoraxis) {
    p_majoraxis = c_majoraxis;
    p_minoraxis = c_minoraxis;
  } else {
    p_majoraxis = c_minoraxis;
    p_minoraxis = c_majoraxis;
  }

  p_gotaxes=true;
}

// major and minor axes given in units of pixels
double
di::w_spot::get_majoraxis() {
  p_getaxes();
  return p_majoraxis;
}

double
di::w_spot::get_minoraxis() {
  p_getaxes();
  return p_minoraxis;
}

void di::w_spot::setstate(Distl::point const& pk){
  nmaxima=0;
  p_gotaxes=false;
  peak=pk;
}

/*
 * Does not take any charge sharing into account.  beam_center_x and
 * beam_center_y must be given in pixel units.
 */
scitbx::vec2<double>
di::w_spot::get_radial_and_azimuthal_size(
  double beam_center_x, double beam_center_y)
{
  if (bodypixels.size() == 0)
    return (scitbx::vec2<double>(0, 0));

  /*
   * Calculate unit-length radial and azimuthal (tangential) direction
   * vectors, r and a, respectively.  The azimuthal direction vector
   * is the radial vector rotated by 90 degrees counter-clockwise.
   */
  const scitbx::vec2<double> center(model_center());
  scitbx::vec2<double> r(center[0] - beam_center_x,
                         center[1] - beam_center_y);
  double h(r.length());
  if (h <= 0)
    return (scitbx::vec2<double>(0, 0));
  r /= h;
  scitbx::vec2<double> a(-r[1], +r[0]);

  /*
   * Determine the extent of the spot along the radial and azimuthal
   * directions from its center.
   */
  double a_max(-std::numeric_limits<double>::infinity());
  double a_min(+std::numeric_limits<double>::infinity());
  double r_max(-std::numeric_limits<double>::infinity());
  double r_min(+std::numeric_limits<double>::infinity());
  for (point_list_t::const_iterator q = bodypixels.begin();
       q != bodypixels.end();
       q++) {
    const scitbx::vec2<double> p(q->x - center[0], q->y - center[1]);
    const double pa(p * a);
    const double pr(p * r);

    if (pa > a_max)
      a_max = pa;
    if (pa < a_min)
      a_min = pa;
    if (pr > r_max)
      r_max = pr;
    if (pr < r_min)
      r_min = pr;
  }

  return (scitbx::vec2<double>(r_max - r_min, a_max - a_min));
}

/*
Commentary on DISTL, relevant to the use of DISTL for virus images.

1) the function pxlclassify considers background boxes in sequence across
   the detector face, but then when it comes to the final edge, it
   calculates the final box boundaries differently.  This makes for clumsy
   code, in particular the pxlclassify_scanbox() is called in 4 places.
   Duplicated code, harder to maintain.  Better to change the function so
   it establishes a single loop with an adjusted-size box so it only
   needs to call scanbox once.

2) consecutively scanned boxes overlap in 2 pixels.  This was probably
   intentional to avoid edge-related discontinuties; but it also has an
   unwanted consequence.  The pixelintensity[x][y] value is both used and
   modified by pxlclassify_scanbox().  Therefore, for the two pixels that
   overlap with the previous box, we are using
   pixelintensity[x][y] values modified in this cycle, while all other pixels
   have values from the last cycle.  It would be cleaner to implement boxes
   without overlaps.

3) on the same topic, it would help avoid discontinuties to determine a
   background plane using International Tables, volume F, p. 213.

4) the as-published gamma-I values don't really make sense:
   a) in cycle 1, all initial pixelintensity[x][y] values are supposed to
      be zero (but see 2 above) so it doesn't mean anything to have a
      bgupperint for this cycle.
   b) in cycle 2, we remove some box pixels from the background calculation.
      Then in cycle 3, the published procedure increases the bgupperint
      cutoff and recalculates the mask, having the effect of adding some
      pixels back to the background.  But this seems wrong; once a pixel
      is removed from background (because it may have signal in it) it
      probably shouldn't be added back.  So the bgupperint cutoff should
      remain constant across cycles.

5) regarding #3, for the HK97 image (with many spots) we are spending an
   enormous amount of time increasing the background-box size and retesting
   it to attain 2/3 of spots in background.  It is possible that by
   applying the background plane correction, we will attain the 2/3 threshhold
   much sooner.  No--just a modest improvement when the plane correction is
   applied to the pixelvalue[x][y].  Second try: the plane correction must
   also be applied when calculating the boxstd(); otherwise the estimate
   of standard deviation will be generally too high.  No:  no improvement in
   the occurrence of background-box increases (even though the sd is lower).
   There are dramatically more spots chosen around the water ring.

   Actions:
   -add a DetectorImageBase.debug_write() command to the iotbx.
   -add a w_Distl.mod_data() function to get a custom-written modified dataarray

6) Conclusions:
   - the get_underload() function is at fault; it is choosing a lower cutoff
   of 4035, which is much too high for reasonable interpretation of the
   virus diffraction.  Most true background is masked out.
   Change the code so it does a sanity check, and never masks out more than
   10% of the pixels as underloads (and thus ~90% of pixels are potential
   background pixels).

7) tackle some performance issues.  the function spotlist_to_flexint() takes
   as much CPU as the libdistl calls.  Solution: alleviate by coding in C++.

8) In tnear, there are numerous boost.python function calls for each spot,
   allowing for spot filtering.  This becomes prohibitive for large unit
   cells.  Solution: recode spot filtering tests in C++ code.
   Also: implement the SpotManager to give a more general way of subsetting
   the spot list.

   Tuesday goals:
     tweak parameters for this image--bg cutoff to 1.5, d1 to 2.5
       & see where I am.
     still don't understand why the spots bleed into each other when bg
       cutoff is at 1.5.  I thought the criteria for growing spots had to
       do with d1, not bg2.
*/

di::w_Distl::w_Distl(std::string optionstring, bool report_overloads){
/*
"       -s2     Smallest acceptible spot area. Spots with area smaller than this value are ignored.\n\n"
"               Recommended values: [3,5]\n\n"
"       -s3     Spot base area.\n\n"
"               Summary of spot shape, strength, etc. are based on spots no smaller than this size.\n\n"
"       -s7     Spot area upper bound factor. If area exceeds\n"
"                   median + (95th prctile - 5th prctile) * factor,\n"
"               the spot is eliminated.\n\n"
"               Recommended value: [2, 5]\n\n"
"       -s8     Spot peak intensity upper bound factor. If peak intensity exceeds \n"
"                   median + (95th prctile - 5th prctile) * factor,\n"
"               the spot is eliminated.\n\n"
"               Recommended value: [2, 10]\n\n"
"   Ice-Ring Detection Parameters:\n\n"
"       -i3     Intensity percentile as a measure of ice-ring strength.\n\n"
"                               Recommended value: [0.1, 0.3]\n\n"
        -d1     Diffraction lower intensity; a lower bound for finding maxima, default 3.5
    More parameters added Feb. 2006 for virus work
        -bx0,1,2 Scanboxsize integer values for cycles 1,2,and 3.
        -bg0,1,2 Bgupperint cutoff values for cycles 1,2,and 3.
*/
    finder.spotbasesize = 10; // distl initializes it as 16, but the LABELIT default is 10.
    finder.bgupperint[0] = 1.5; //See note 4a above; value shouldn't be used
    finder.bgupperint[1] = 1.5; //See note 4b above
    finder.bgupperint[2] = 1.5; //See note 4b above
    if (report_overloads) {finder.report_overloads = true;}
    typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
    boost::char_separator<char> sep(" ");
    tokenizer tok(optionstring,sep);
    for (tokenizer::iterator tok_iter = tok.begin();
         tok_iter!=tok.end(); ++tok_iter){
      if        (*tok_iter == "-s2") {
        finder.spotarealowcut = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-s3") {
        finder.spotbasesize = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-s7") {
        finder.spotareamaxfactor = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-s8") {
        finder.spotpeakintmaxfactor = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-i3") {
        finder.icering_strengthprctile = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-d1") {
        finder.difflowerint = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bx0") {
        finder.scanboxsize[0] = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bx1") {
        finder.scanboxsize[1] = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bx2") {
        finder.scanboxsize[2] = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bg0") {
        finder.bgupperint[0] = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bg1") {
        finder.bgupperint[1] = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bg2") {
        finder.bgupperint[2] = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-ro") {
        set_resolution_outer( atof((*(++tok_iter)).c_str()) );
      }
    }
}

void
di::w_Distl::set_resolution_outer(const double& newvalue)
{
  // only meaningful if the resolution value is a positive number.
  SCITBX_ASSERT(newvalue>0.0);
  finder.resolution_outer = newvalue;
  //SCITBX_EXAMINE(finder.resolution_outer);
}

void
di::w_Distl::setspotimg(const double& pixel_size,
                        const double& distance,
                        const double& wavelength,
                        const double& beamx,
                        const double& beamy,
                        af::flex_int const& intdata,
                        const int& peripheral_margin,
                        const double& saturation )
{
  finder.overloadvalue = saturation;
  //Take default angle settings because we don't use them anyway.
  finder.set_imageheader(pixel_size, distance, wavelength, 0., 0.,
                         beamx, beamy);

  int ncols = intdata.accessor().all()[0];
  int nrows = intdata.accessor().all()[1];

  finder.imgmargin = peripheral_margin;
  finder.set_imagedata(intdata.begin(),ncols,nrows);
}

void
di::w_Distl::set_tiling(const string& vendortype)
{
  if (vendortype=="Pilatus-6M") {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_pilatus6M(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty));
  } else if (vendortype=="Pilatus-2M") {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_pilatus2M(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty));
  } else if (vendortype=="Pilatus-300K") {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_pilatus300K(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty));
  } else if (vendortype.substr(0,5)=="Eiger") {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_eiger(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty,
      finder.pixelvalue.nx, finder.pixelvalue.ny));
  } else {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty));
  }
}

void
di::w_Distl::set_tiling(af::flex_int const& explicit_tiling,int const& peripheral_margin)
{
  finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_explicit(
      explicit_tiling, peripheral_margin));
}

af::flex_double
di::w_Distl::Z_data()
{
  int nrows=finder.pixelvalue.ny;
  af::flex_double z(af::flex_grid<>(finder.pixelvalue.nx,nrows));

  double* begin = z.begin();

  for (int x=0; x<finder.pixelvalue.nx; x++) {
    for (int y=0; y<nrows; y++){
        //capture DISTL's Z-function;
        *begin++ = finder.pixelintensity[x][y];
    }
  }
  return z;
}

af::flex_int
di::w_Distl::mod_data()
{
  int nrows=finder.pixelvalue.ny;
  af::flex_int z(af::flex_grid<>(finder.pixelvalue.nx,nrows));

  int* begin = z.begin();

  for (int x=0; x<finder.pixelvalue.nx; x++) {
    for (int y=0; y<nrows; y++){
        //example mod_data function; pixel value conditional on some property
        if (finder.pixelintensity[x][y]<1.5 &&
        finder.pixelvalue[x][y]>finder.underloadvalue &&
        finder.pixelvalue[x][y]<finder.overloadvalue
        ){
            *begin = 10;
        }else{
            *begin = finder.pixelvalue[x][y];
        }
        begin++;
    }
  }
  return z;
}

void
di::w_Distl::finish_analysis(){

  // Now done as python calls to individual methods-->finder.process();

  //get the spots out of std::list and in to af::shared

  spots.reserve(finder.spots.size());
  list<Distl::spot>::const_iterator position = finder.spots.begin();
  list<Distl::spot>::const_iterator end = finder.spots.end();
  for (; position!=end;++position ) {

    //It turns out that minimum spot area is an extremely important
    //  filter without which all sorts of junk are reported.
    /* 12oct2011-->the following line has a bug; should be >=
     * However, can't fix the bug without changing all previous results
     * For now simply work around it by decrementing distl.minimum_spot_area by 1
     */
    if ((*position).area() > finder.spotbasesize) {
      spots.push_back(*position);
    }
  }

  icerings.reserve(finder.icerings.size());
  vector<Distl::icering>::const_iterator iposition = finder.icerings.begin();
  vector<Distl::icering>::const_iterator iend = finder.icerings.end();

  for (; iposition!=iend;++iposition ) {
    icerings.push_back(*iposition);
  }

}

bool
di::w_Distl::isIsolated(const w_spot& spot, const double& mmradius) const {
  //Future:  1) find cases that use this algorithm
  //         2) change the algorithm so it relies on elliptical modelling
  //            rather than on the borderpixels data structure
  double pixelradius = mmradius / finder.pixel_size;
  typedef scitbx::vec2<double>                 vpoint;
  af::shared<Distl::point>::const_iterator sptr;
  af::shared<Distl::point>::const_iterator send;

  //vpoint focusspot(spot.ctr_mass_x(),spot.ctr_mass_y());
  vpoint focusspot(spot.max_pxl_x(),spot.max_pxl_y());

  spot_list_t::const_iterator p = spots.begin();
  spot_list_t::const_iterator e = spots.end();
  for (; p!=e; ++p) {
      //vpoint target((*p).ctr_mass_x(),(*p).ctr_mass_y());
      vpoint target((*p).max_pxl_x(),(*p).max_pxl_y());
      vpoint diff = target - focusspot;
      double dist = std::sqrt(diff*diff);
      if ( dist > pixelradius ) {continue;}

      // Consider border pixels of the focus spot
      vpoint bisector = 0.45*diff;
      double bisectorsq = bisector*bisector;
      sptr = spot.borderpixels.begin();
      send = spot.borderpixels.end();
      for (;sptr!=send; ++sptr) {
        vpoint borderpt((*sptr).x,(*sptr).y);
        vpoint bordervec = borderpt - focusspot;
        if ( bordervec*bisector > bisectorsq ) {return false;}
      }

      // Consider border pixels of the target spot
      bisector = 0.55*diff;
      bisectorsq = bisector*bisector;
      sptr = (*p).borderpixels.begin();
      send = (*p).borderpixels.end();
      for (;sptr!=send; ++sptr) {
        vpoint borderpt((*sptr).x,(*sptr).y);
        vpoint bordervec = borderpt - focusspot;
        if ( bordervec*bisector < bisectorsq ) {return false;}
      }
  }
  return true;
}


#include "pose.hpp"

#include <typeinfo> // operator typeid

#include <Eigen/Geometry>
#include <tbb/parallel_for.h>

#include <autodiff/autodiff.h>
#include <logger.hpp>
#include <profiler.hpp>
#include <utils/is_zero.hpp>
#include <utils/not_implemented_error.hpp>
#include <utils/sinc.hpp>
#include <utils/type_name.hpp>

namespace ipc::rigid {

template <typename T>
Pose<T>::Pose()
    : position()
    , rotation()
{
}

template <typename T>
Pose<T>::Pose(const VectorMax3<T>& position, const VectorMax3<T>& rotation)
    : position(position)
    , rotation(rotation)
{
}

template <typename T> Pose<T>::Pose(const VectorMax6<T>& dof)
{
    if (dof.size() == dim_to_ndof(2)) {
        position = dof.head(dim_to_pos_ndof(2));
        rotation = dof.tail(dim_to_rot_ndof(2));
    } else if (dof.size() == dim_to_ndof(3)) {
        position = dof.head(dim_to_pos_ndof(3));
        rotation = dof.tail(dim_to_rot_ndof(3));
    } else {
        throw NotImplementedError("Unknown pose convertion for given ndof");
    }
}

template <typename T>
Pose<T>::Pose(const T& x, const T& y, const T& theta)
    : Pose(Vector2<T>(x, y), Vector1<T>())
{
    rotation << theta;
}

template <typename T>
Pose<T>::Pose(
    const T& x,
    const T& y,
    const T& z,
    const T& theta_x,
    const T& theta_y,
    const T& theta_z)
    : Pose(Vector3<T>(x, y, z), Vector3<T>(theta_x, theta_y, theta_z))
{
}

template <typename T> Pose<T> Pose<T>::Zero(int dim)
{
    assert(dim == 2 || dim == 3);
    return Pose(
        VectorX<T>::Zero(Pose<T>::dim_to_pos_ndof(dim)),
        VectorX<T>::Zero(Pose<T>::dim_to_rot_ndof(dim)));
}

template <typename T>
Poses<T> Pose<T>::dofs_to_poses(const VectorX<T>& dofs, int dim)
{
    int ndof = dim_to_ndof(dim);
    int num_poses = dofs.size() / ndof;
    assert(dofs.size() % ndof == 0);
    Poses<T> poses;
    poses.reserve(num_poses);
    for (int i = 0; i < num_poses; i++) {
        poses.emplace_back(dofs.segment(i * ndof, ndof));
    }
    return poses;
}

template <typename T> VectorX<T> Pose<T>::poses_to_dofs(const Poses<T>& poses)
{
    const int ndof = poses.size() ? poses[0].ndof() : 0;
    VectorX<T> dofs(poses.size() * ndof);
    for (size_t i = 0; i < poses.size(); i++) {
        assert(poses[i].ndof() == ndof);
        dofs.segment(i * ndof, ndof) = poses[i].dof();
    }
    return dofs;
}

template <typename T> VectorMax6<T> Pose<T>::dof() const
{
    VectorMax6<T> pose_dof(ndof());
    pose_dof.head(pos_ndof()) = position;
    pose_dof.tail(rot_ndof()) = rotation;
    return pose_dof;
}

// Replace a selected dof with the dof in other.
template <typename T>
void Pose<T>::select_dof(
    const VectorMax6b& is_dof_selected,
    const Pose<T>& other,
    const MatrixMax3d& R)
{
    assert(is_dof_selected.size() == this->ndof());
    assert(other.dim() == this->dim());
    // R should be a rotation
    assert(R.isUnitary(1e-9));
    assert(fabs(R.determinant() - 1.0) < 1.0e-6);
    position =
        is_dof_selected.head(pos_ndof()).select(other.position, position);
    rotation = R.transpose()
        * is_dof_selected.tail(rot_ndof())
              .select(R * other.rotation, R * rotation);
}

// Zero out the i-th dof if is_dof_zero(i) == true.
template <typename T>
void Pose<T>::zero_dof(const VectorMax6b& is_dof_zero, const MatrixMax3d& R)
{
    select_dof(is_dof_zero, Pose<T>::Zero(dim()), R);
}

template <typename T> MatrixMax3<T> Pose<T>::construct_rotation_matrix() const
{
    return ipc::rigid::construct_rotation_matrix(rotation);
}

template <typename T> Eigen::Quaternion<T> Pose<T>::construct_quaternion() const
{
    return ipc::rigid::construct_quaternion(rotation);
}

template <typename T>
Pose<T> Pose<T>::interpolate(const Pose<T>& pose0, const Pose<T>& pose1, T t)
{
    assert(pose0.dim() == pose1.dim());
    return Pose<T>(
        (pose1.position - pose0.position) * t + pose0.position,
        (pose1.rotation - pose0.rotation) * t + pose0.rotation);
}

template <typename T> bool Pose<T>::operator==(const Pose<T>& other) const
{
    return this->position == other.position && this->rotation == other.rotation;
}

template <typename T> Pose<T>& Pose<T>::operator*=(const T& x)
{
    this->position *= x;
    this->rotation *= x;
    return *this;
}

template <typename T> Pose<T> Pose<T>::operator/(const T& x) const
{
    return Pose<T>(this->position / x, this->rotation / x);
}

///////////////////////////////////////////////////////////////////////////
// Operations on vector of Poses

template <typename T>
Poses<T> interpolate(const Poses<T>& poses0, const Poses<T>& poses1, T t)
{
    PROFILE_POINT(fmt::format("Poses<{}>::interpolate", get_type_name<T>()));
    PROFILE_START();
    Poses<T> poses(poses0.size());
    for (size_t i = 0; i < poses.size(); i++) {
        poses[i] = Pose<T>::interpolate(poses0[i], poses1[i], t);
    }
    PROFILE_END();
    return poses;
}

template <typename T> Poses<T> operator*(const Poses<T>& poses, const T& x)
{
    Poses<T> product = poses;
    for (size_t i = 0; i < product.size(); i++) {
        product[i] *= x;
    }
    return product;
}

template <typename T, typename U> Poses<T> cast(const Poses<U>& poses)
{
    Poses<T> poses_T;
    poses_T.reserve(poses_T.size());
    for (int i = 0; i < poses.size(); i++) {
        poses_T.push_back(poses[i].template cast<T>());
    }
    return poses_T;
}

template <typename T>
MatrixMax3<T> construct_rotation_matrix(const VectorMax3<T>& r)
{
    if (r.size() == 1) {
        return Eigen::Rotation2D<T>(r(0)).toRotationMatrix();
    } else {
        assert(r.size() == 3);
        T sinc_angle = sinc_normx(r);
        T sinc_half_angle = sinc_normx((r / T(2.0)).eval());
        Matrix3<T> K = Hat(r);
        Matrix3<T> K2 = K * K;
        Matrix3<T> R =
            sinc_angle * K + 0.5 * sinc_half_angle * sinc_half_angle * K2;
        R.diagonal().array() += T(1.0);
        return R;
    }
}

template <typename Derived, typename T>
Eigen::Quaternion<T> construct_quaternion(const Eigen::MatrixBase<Derived>& r)
{
    assert(r.size() == 3 && (r.rows() == 3 || r.cols() == 3));
    T angle = r.norm();
    if (angle == 0) {
        return Eigen::Quaternion<T>::Identity();
    }
    return Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, r / angle));
}

template <typename T> Matrix3<T> rotate_to_z(Vector3<T> n)
{
    if (n.norm() == T(0)) {
        return Matrix3<T>::Identity();
    }
    return Eigen::Quaternion<T>::FromTwoVectors(n, Vector3<T>::UnitZ())
        .toRotationMatrix();
}

template <typename T> Matrix3<T> rotate_around_z(const T& theta)
{
    Matrix3<T> R;
    R.row(0) << cos(theta), -sin(theta), T(0);
    R.row(1) << sin(theta), cos(theta), T(0);
    R.row(2) << T(0), T(0), T(1);
    return R;
}

template <typename T>
void decompose_to_z_screwing(
    const Pose<T>& pose_t0,
    const Pose<T>& pose_t1,
    Matrix3<T>& R0,
    Matrix3<T>& P,
    T& omega)
{
    // Decompose the inbetween rotation as a rotation around the z-axis:
    //     R = Pᵀ R_z P
    // Where R = R₁R₀ᵀ, P is a rotation from n̂ to ẑ, and R_z is a rotation
    // of ω around the z-axis.
    R0 = pose_t0.construct_rotation_matrix();
    Matrix3<T> R1 = pose_t1.construct_rotation_matrix();
    Eigen::AngleAxis<T> r(R1 * R0.transpose());
    omega = r.angle();
    P = rotate_to_z(r.axis());
}

} // namespace ipc::rigid


/*
Author: Rohan Chetan Thanki
Date created: 16-Oct-2021
*/

// This file contains functions which are used in the main file. This ensures that the main file is not cluttered

#include "Hedging_Portfolio.hpp"
#include <iostream>
#include <fstream>
#include <vector>

#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
#include <boost/date_time.hpp>

using namespace std;

/**************************************** GENERIC UTIL FUNCTIONS ****************************************************/

// split a string into substrings based on a delimiter
inline vector<string> string_splitter(const string& str, char delimiter)
{
    vector<string> str_vec;
    stringstream ss(str);
    string token;
    while (getline(ss, token, delimiter))
    {
        str_vec.push_back(token);
    }
    return str_vec;
}

// Writing a 2D Vector to a CSV file
inline void write2DVectorToCSV(const vector<vector<double>>& vect, const string& filepath)
{
    std::ofstream out(filepath);
    for (int i = 0; i <= vect.size() - 1; i++)
    {
        for (int j = 0; j <= vect[0].size() - 1; j++)
        {
            out << vect[i][j] << ",";
        }
        out << "\n";
    }
}

// Get CDF of Standard Normal
inline double N(const double& x)
{
    boost::math::normal_distribution<> stdNormal(0.0, 1.0);
    return(cdf(stdNormal, x));
}

// print a 2D vector to console
template <typename T>
inline void printVect(vector<vector<T>> vect)
{
    for (int i = 0; i <= vect[0].size() - 1; i++)
    {
        for (int j = 0; j <= vect.size() - 1; j++)
        {
            cout << vect[j][i] << "\t";
        }
        cout << "\n";
    }
}

// convert vector of string to vector of double
inline vector<double> stringToDoubleVect(const vector<string>& inVect)
{
    vector<double> outVect;
    for (int i = 0; i <= inVect.size() - 1; i++)
    {
        outVect.push_back(stod(inVect[i]));
    }
    return(outVect);
}

// convert vector of string to vector of dates
inline vector<boost::gregorian::date> stringtoDateVect(const vector<string>& inVect)
{
    vector<boost::gregorian::date> outVect;
    for (int i = 0; i <= inVect.size() - 1; i++)
    {
        outVect.push_back(boost::gregorian::from_simple_string(inVect[i]));
    }
    return(outVect);
}

// function to read a CSV where each column is returned as a vector
inline vector<vector<string>> readCSV(const string& filepath, const unsigned int& ignoreLines, const int& numCols)
{
    ifstream infile(filepath);
    string line;

    // ignore the first few lines of the file
    for (int lineCount = 1; lineCount <= ignoreLines && infile.good(); lineCount++)
        getline(infile, line);

    // initialising vectors which will be populated when the file is read
    vector<vector<string>> csvData;
    for (int i = 0; i <= numCols - 1; i++)
    {
        vector<string> temp;
        csvData.push_back(temp);
    }

    // Read the data line by line
    while (getline(infile, line))
    {
        vector<string> rowData = string_splitter(line, ',');
        for (int i = 0; i <= numCols - 1; i++)
        {
            csvData[i].push_back(rowData[i]);
        }
    }

    infile.close();
    return(csvData);
}

/************************************ FUNCTIONS SPECIFIC TO PART 1 ****************************************************/

// Simulate stock prices
inline vector<vector<double>> simulateStockPrices(unsigned int numPaths, unsigned int N, double S0, double T, double u, double sigma)
{
    double dt = T / N;          // time step
    vector < vector<double> > stockPrices;          // creating a vector to store stock prices for all paths

    boost::random::mt19937 rng;     // creating a random number generator
    //rng.seed(static_cast<unsigned int> (std::time(0)));     // setting the seed of the random number generator
    rng.seed(123);
    boost::random::normal_distribution<double> stdNormal(0, 1);     // creating the distribution object

    for (int i = 0; i <= numPaths - 1; i++)
    {
        vector<double> pathStockPrices;             // creating a vector to store stock prices of the current path
        double S = S0;
        pathStockPrices.push_back(S);

        // add simulated stock prices of the current path to the  current path vector
        for (int j = 1; j <= N; j++)
        {
            S = (S)+(S * u * dt) + (S * sigma * sqrt(dt) * stdNormal(rng));
            pathStockPrices.push_back(S);
        }

        // add the stock prices of the current path to the overall stock price vector
        stockPrices.push_back(pathStockPrices);
    }

    return(stockPrices);
}

// Computing Hedging Error of Hedged Portfolio
inline vector<vector<Hedging_Portfolio>> computeHedgingErrors(const vector<vector<double>>& stockPrices, const double& K, const double& r, const double& T, const double& sigma, const char& optionFlag)
{
    unsigned int numPaths = stockPrices.size(); // number of paths
    unsigned int pathLength = stockPrices[0].size() - 1;
    double dt = T / pathLength;     // time step

    vector<vector<Hedging_Portfolio>> allHedgingPortfolio;     // creating a vector to store the details of the hedging error

    for (int i = 0; i <= numPaths - 1; i++)
    {
        vector<Hedging_Portfolio> pathHedgingPortfolio;
        for (int j = 0; j <= pathLength; j++)
        {
            // creating variables
            double S = stockPrices[i][j];
            double TMat = T - (j * dt);
            double deltaPrev, BPrev, rPrev;
            double delta, V, B, HE;

            // computing d1 and d2 for Black Scholes model
            double d1 = (log(S / K) + ((r + (pow(sigma, 2) / 2)) * TMat)) / (sigma * sqrt(TMat));
            double d2 = d1 - (sigma * sqrt(TMat));

            // creating an object of the hedged portfolio at the current state
            Hedging_Portfolio portfolio(K, S, r, TMat, sigma, optionFlag);

            // setting delta
            delta = N(d1);
            portfolio.setDelta(delta);

            // setting option Price
            if (optionFlag == 'c' || optionFlag == 'C')
                V = (S * N(d1)) - (K * exp(-r * TMat) * N(d2));
            else if (optionFlag == 'p' || optionFlag == 'P')
                V = (K * exp(-r * TMat) * N(-d2)) - (S * N(-d1));
            else
            {
                cout << "Flag " << optionFlag << "is incorrect. Flag must be either 'c', 'C', 'p' or 'P'" << endl;
                throw(10);
            }
            portfolio.setOptionPrice(V);

            // setting B
            if (j == 0)
            {
                B = V - delta * S;
            }
            else
            {
                rPrev = pathHedgingPortfolio[j - 1].getRiskFreeRate();
                deltaPrev = pathHedgingPortfolio[j - 1].getDelta();
                BPrev = pathHedgingPortfolio[j - 1].getB();
                B = ((deltaPrev - delta) * S) + BPrev * exp(rPrev * dt);
            }
            portfolio.setB(B);

            // setting Hedging Error
            if (j == 0)
            {
                HE = 0;
            }
            else
            {
                HE = deltaPrev * S + BPrev * exp(rPrev * dt) - V;
            }
            portfolio.setHedgingError(HE);

            // adding current state of the portfolio to the vector of states of the portfolio for current path
            pathHedgingPortfolio.push_back(portfolio);
        }

        // adding the path of portfolio to the vector containinf portfolio states of all paths
        allHedgingPortfolio.push_back(pathHedgingPortfolio);
    }
    return(allHedgingPortfolio);
}

// function to create a 2D Vector of a specific attribute from the hedged portfolio state vector
inline vector<vector<double>> getHedgedPortfolioParam(const vector<vector<Hedging_Portfolio>>& allPortfolioStates, const string param)
{
    vector<vector<double>> allVect;
    int numPaths = allPortfolioStates.size();
    int pathSize = allPortfolioStates[0].size() - 1;

    for (int i = 0; i <= numPaths - 1; i++)
    {
        vector<double> pathVect;
        for (int j = 0; j <= pathSize; j++)
        {
            double val;

            if (param == "S")
                val = allPortfolioStates[i][j].getSpotPrice();
            else if (param == "DELTA")
                val = allPortfolioStates[i][j].getDelta();
            else if (param == "V")
                val = allPortfolioStates[i][j].getOptionPrice();
            else if (param == "B")
                val = allPortfolioStates[i][j].getB();
            else if (param == "HE")
                val = allPortfolioStates[i][j].getHedgingError();
            else
                val = 0;

            pathVect.push_back(val);
        }
        allVect.push_back(pathVect);
    }

    return(allVect);
}

/************************************** FUNCTIONS SPECIFIC TO PART 2 ****************************************************/

// Count the number of weekdays between 2 days
inline long countWeekDays(string d0str, string d1str)
{
    boost::gregorian::date d0(boost::gregorian::from_simple_string(d0str));
    boost::gregorian::date d1(boost::gregorian::from_simple_string(d1str));
    long ndays = (d1 - d0).days() + 1; // +1 for inclusive
    long nwkends = 2 * ((ndays + d0.day_of_week()) / 7); // 2*Saturdays
    if (d0.day_of_week() == boost::date_time::Sunday) ++nwkends;
    if (d1.day_of_week() == boost::date_time::Saturday) --nwkends;
    return ndays - nwkends;
}

// finding corresponding value from a vector given the date
inline double findVal(vector<string> dateVec, vector<double> valVec, string date)
{
    auto it = find(dateVec.begin(), dateVec.end(), date);
    return valVec.at(it - dateVec.begin());  // this will throw an exception if array is out of bounds
}


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2015 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_WELL_KNOWN_SRS_HPP
#define MAPNIK_WELL_KNOWN_SRS_HPP

// mapnik
#include <mapnik/global.hpp> // for M_PI on windows
#include <mapnik/enumeration.hpp>
#include <mapnik/geometry.hpp>

#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <boost/optional.hpp>
#pragma GCC diagnostic pop

// stl
#include <cmath>

namespace mapnik {

enum well_known_srs_enum : std::uint8_t {
    WGS_84,
    G_MERC,
    well_known_srs_enum_MAX
};

DEFINE_ENUM( well_known_srs_e, well_known_srs_enum );

static const double EARTH_RADIUS = 6378137.0;
static const double EARTH_DIAMETER = EARTH_RADIUS * 2.0;
static const double EARTH_CIRCUMFERENCE = EARTH_DIAMETER * M_PI;
static const double MAXEXTENT = EARTH_CIRCUMFERENCE / 2.0;
static const double M_PI_by2 = M_PI / 2;
static const double D2R = M_PI / 180;
static const double R2D = 180 / M_PI;
static const double M_PIby360 = M_PI / 360;
static const double MAXEXTENTby180 = MAXEXTENT / 180;
static const double MAX_LATITUDE = R2D * (2 * std::atan(std::exp(180 * D2R)) - M_PI_by2);
static const std::string MAPNIK_LONGLAT_PROJ = "+proj=longlat +ellps=WGS84 +datum=WGS84 +no_defs";
static const std::string MAPNIK_GMERC_PROJ = "+proj=merc +a=6378137 +b=6378137 +lat_ts=0.0 +lon_0=0.0 +x_0=0.0 +y_0=0.0 +k=1.0 +units=m +nadgrids=@null +wktext +no_defs +over";

boost::optional<well_known_srs_e> is_well_known_srs(std::string const& srs);

boost::optional<bool> is_known_geographic(std::string const& srs);

static inline bool lonlat2merc(double * x, double * y , int point_count)
{
    for(int i=0; i<point_count; ++i)
    {
        if (x[i] > 180) x[i] = 180;
        else if (x[i] < -180) x[i] = -180;
        if (y[i] > MAX_LATITUDE) y[i] = MAX_LATITUDE;
        else if (y[i] < -MAX_LATITUDE) y[i] = -MAX_LATITUDE;
        x[i] = x[i] * MAXEXTENTby180;
        y[i] = std::log(std::tan((90 + y[i]) * M_PIby360)) * R2D;
        y[i] = y[i] * MAXEXTENTby180;
    }
    return true;
}

static inline bool merc2lonlat(double * x, double * y , int point_count)
{
    for(int i=0; i<point_count; i++)
    {
        if (x[i] > MAXEXTENT) x[i] = MAXEXTENT;
        else if (x[i] < -MAXEXTENT) x[i] = -MAXEXTENT;
        if (y[i] > MAXEXTENT) y[i] = MAXEXTENT;
        else if (y[i] < -MAXEXTENT) y[i] = -MAXEXTENT;
        x[i] = (x[i] / MAXEXTENT) * 180;
        y[i] = (y[i] / MAXEXTENT) * 180;
        y[i] = R2D * (2 * std::atan(std::exp(y[i] * D2R)) - M_PI_by2);
    }
    return true;
}

static inline bool lonlat2merc(geometry::line_string<double> & ls)
{
    for(auto & p : ls)
    {
        if (p.x > 180) p.x = 180;
        else if (p.x < -180) p.x = -180;
        if (p.y > MAX_LATITUDE) p.y = MAX_LATITUDE;
        else if (p.y < -MAX_LATITUDE) p.y = -MAX_LATITUDE;
        p.x = p.x * MAXEXTENTby180;
        p.y = std::log(std::tan((90 + p.y) * M_PIby360)) * R2D;
        p.y = p.y * MAXEXTENTby180;
    }
    return true;
}

static inline bool merc2lonlat(geometry::line_string<double> & ls)
{
    for (auto & p : ls)
    {
        if (p.x > MAXEXTENT) p.x = MAXEXTENT;
        else if (p.x < -MAXEXTENT) p.x = -MAXEXTENT;
        if (p.y > MAXEXTENT) p.y = MAXEXTENT;
        else if (p.y < -MAXEXTENT) p.y = -MAXEXTENT;
        p.x = (p.x / MAXEXTENT) * 180;
        p.y = (p.y / MAXEXTENT) * 180;
        p.y = R2D * (2 * std::atan(std::exp(p.y * D2R)) - M_PI_by2);
    }
    return true;
}

}

#endif // MAPNIK_WELL_KNOWN_SRS_HPP