#include <cmath>
#include <algorithm>
#include <numeric>
#include <boost/unordered_map.hpp>
#include <emmintrin.h>
#include <immintrin.h>
#include "rchord_meanshift.hpp"

#include <iostream>

static void _meanShiftIter(const std::vector<Eigen::Matrix3f> &votes,
			   const std::vector<float> &weights,
			   const Eigen::Matrix3f &guess, Eigen::Matrix3f &mode,  float b);
static void _RChordMeanShift(const std::vector<Eigen::Matrix3f> &votes,
			     const std::vector<float> &votesWeight,
			     const Eigen::Matrix3f &guess, Eigen::Matrix3f &mode,
			     float b, float stepSize, float stepThr, unsigned int nIterThr);
static inline void _cvtVEuler2Rotm(const std::vector<Eigen::Vector3f> &votesEul,
				   std::vector<Eigen::Matrix3f> &votesRotm);
static inline void _cvtVRotm2Euler(const std::vector<Eigen::Matrix3f> &votesRotm,
				   std::vector<Eigen::Vector3f> &votesEul);
static inline void _cvtEuler2Rotm(const Eigen::Vector3f &voteEul, Eigen::Matrix3f &voteRotm);
static inline void _cvtRotm2Euler(const Eigen::Matrix3f &voteRotm, Eigen::Vector3f &voteEul);
static inline float _rotmDist2(const Eigen::Matrix3f &m1, const Eigen::Matrix3f &m2);

void RChordMeanShift(const std::vector<Eigen::Vector3f> &votes,
		     const std::vector<float> &votesWeight,
		     const Eigen::Vector3f &guess, Eigen::Vector3f &mode,
		     float b, float stepSize, float stepThr, unsigned int nIterThr)
{
  std::vector<Eigen::Matrix3f> votesRotm(votes.size());
  Eigen::Matrix3f guessRotm, modeRotm;

  _cvtVEuler2Rotm(votes, votesRotm);
  _cvtEuler2Rotm(guess, guessRotm);

  _RChordMeanShift(votesRotm, votesWeight, guessRotm, modeRotm,
		   b, stepSize, stepThr, nIterThr);

  _cvtRotm2Euler(modeRotm, mode);
}

static void _RChordMeanShift(const std::vector<Eigen::Matrix3f> &votes,
			     const std::vector<float> &votesWeight,
			     const Eigen::Matrix3f &guess, Eigen::Matrix3f &mode,
			     float b, float stepSize, float stepThr, unsigned int nIterThr)
{
  Eigen::Matrix3f currGuess = guess;

  for (int i=0; i<nIterThr; i++)
  {
    Eigen::Matrix3f m;
    float dist;

    _meanShiftIter(votes, votesWeight, currGuess, m, b);
    dist = _rotmDist2(currGuess, m);

    // Move to the meanshift mode by stepSize. Use quaternion interpolation to
    // rotate towards the target rotation.
    Eigen::Quaternion<float> q1(currGuess);
    Eigen::Quaternion<float> q2(m);

    currGuess = q1.slerp(stepSize, q2);
    if (dist<stepThr*stepThr) break;
  }
  mode = currGuess;
}

struct CmpIdxByWeight
{
  const std::vector<float> *w;
  CmpIdxByWeight(const std::vector<float> &w):w(&w){};
  bool operator() (unsigned int i1, unsigned int i2) const {return w->at(i1)>w->at(i2);}
};

struct CmpIdxByNGuess
{
  const std::vector<int> *nGuess;
  CmpIdxByNGuess(const std::vector<int> &nGuess):nGuess(&nGuess){};
  bool operator() (unsigned int i1, unsigned int i2) const {return nGuess->at(i1)>nGuess->at(i2);}
};

unsigned int  MultiGuessRChordMeanShift(const std::vector<Eigen::Vector3f> &votes,
					const std::vector<float> &votesWeight,
					const std::vector<Eigen::Vector3f> guesses,
					std::vector<Eigen::Vector3f> &modes,
					std::vector<int> &guessModeID,
					float b, float stepSize, float stepThr,
					unsigned int nIterThr, float sameModeThr)
{
  std::vector<Eigen::Matrix3f> vRotm(votes.size());
  std::vector<Eigen::Matrix3f> gRotm(guesses.size());
  std::vector<Eigen::Matrix3f> mRotm(guesses.size());
  std::vector<Eigen::Matrix3f> cRotm;
  std::vector<int> cNGuess;
  unsigned int nCModes=1;

  _cvtVEuler2Rotm(votes, vRotm);
  _cvtVEuler2Rotm(guesses, gRotm);

  #pragma omp parallel for
  for (int i=0; i<gRotm.size(); i++)
  {
    _RChordMeanShift(vRotm, votesWeight,
		     gRotm.at(i), mRotm.at(i),
		     b, stepSize, stepThr, nIterThr);
  }

  // Greedy clustering of modes (count how many guesses reach each mode as weel)
  cRotm.push_back(mRotm.at(0));
  cNGuess.push_back(1);
  guessModeID.at(0) = 0;
  for (int i=1; i<mRotm.size(); i++)
  {
    bool newMode = true;
    for (int j=0; j<nCModes; j++)
    {
      if (_rotmDist2(mRotm.at(i), cRotm.at(j))<sameModeThr*sameModeThr)
      {
	cNGuess.at(j)++;
	guessModeID.at(i) = j;
	newMode = false;
	break;
      }
    }
    if (newMode)
    {
      cRotm.push_back(mRotm.at(i));
      cNGuess.push_back(1);
      guessModeID.at(i) = nCModes;
      nCModes++;
    }
  }

  // Sort the modes by the number of guesses that converged to it
  std::vector<unsigned int> idx;
  for (unsigned int i=0; i<nCModes; i++) idx.push_back(i);
  if (idx.size()>1) std::sort(idx.begin(), idx.end(), CmpIdxByNGuess(cNGuess));

  int outNModes = std::min(nCModes, static_cast<unsigned int>(modes.size()));
  for (int i=0; i<outNModes; i++)
  {
    _cvtRotm2Euler(cRotm.at(idx[i]), modes.at(i));
  }

  // Update the mode id for votes that reached the current node
  // Note: Set the mode id to -1 if the guess reached a discarded mode
  boost::unordered_map<int, int> sortedModeIDMap;
  for (int i=0; i<nCModes; i++) sortedModeIDMap[idx[i]] = (i<outNModes) ? i : -1;
  for (int i=0; i<votes.size(); i++) guessModeID.at(i) = sortedModeIDMap.at(guessModeID.at(i));

  return outNModes;
}

void _meanShiftIter(const std::vector<Eigen::Matrix3f> &votes,
		    const std::vector<float> &weights,
		    const Eigen::Matrix3f &guess, Eigen::Matrix3f &mode, float b)
{
  Eigen::Matrix3f sumR = Eigen::Matrix3f::Zero();

  // Weighted sum of rotations
  for (int i=0; i<votes.size(); i++)
  {
    // Compute the current vote weight (i.e. provided weight multiplied by Guassian kernel
    // on chordal quaternion distance (i.e. Euclidean norm of R3 representation))
    float w = weights.at(i) * exp(-_rotmDist2(guess, votes.at(i))/(b*b));
    /*
    Eigen::Quaternion<float> q1(guess);
    Eigen::Quaternion<float> q2(votes.at(i));
    float dist = q1.angularDistance(q2);
    float w = weights.at(i) * exp(-(dist*dist)/(b*b));
    */
    sumR += w*votes.at(i);
  }

  // Compute the average rotation as in
  // Hartley, Richard, et al. "Rotation averaging." International journal of computer vision 103.3 (2013): 267-305.
  Eigen::JacobiSVD<Eigen::Matrix3f> SVDR(sumR, Eigen::ComputeFullU|Eigen::ComputeFullV);
  mode = SVDR.matrixU()*SVDR.matrixV().transpose();
  if (mode.determinant()<0)
  {
    mode = SVDR.matrixU()*Eigen::Vector3f(1,1,-1).asDiagonal()*SVDR.matrixV().transpose();
  }
}

void _cvtVEuler2Rotm(const std::vector<Eigen::Vector3f> &votesEul,
		    std::vector<Eigen::Matrix3f> &votesRotm)
{
  for (int i=0; i<votesEul.size(); i++)
  {
    const Eigen::Vector3f &currVoteEul = votesEul.at(i);
    Eigen::Matrix3f &currVoteRotm = votesRotm.at(i);
    _cvtEuler2Rotm(currVoteEul, currVoteRotm);
  }
}

void _cvtVRotm2Euler(const std::vector<Eigen::Matrix3f> &votesRotm,
		    std::vector<Eigen::Vector3f> &votesEul)
{
  for (int i=0; i<votesEul.size(); i++)
  {
    const Eigen::Matrix3f &currVoteRotm = votesRotm.at(i);
    Eigen::Vector3f &currVoteEul = votesEul.at(i);
    _cvtRotm2Euler(currVoteRotm, currVoteEul);
  }
}

void _cvtEuler2Rotm(const Eigen::Vector3f &voteEul, Eigen::Matrix3f &voteRotm)
{
  voteRotm = Eigen::AngleAxisf(voteEul.x(), Eigen::Vector3f::UnitZ()) *
             Eigen::AngleAxisf(voteEul.y(), Eigen::Vector3f::UnitY()) *
             Eigen::AngleAxisf(voteEul.z(), Eigen::Vector3f::UnitX());
}

void _cvtRotm2Euler(const Eigen::Matrix3f &voteRotm, Eigen::Vector3f &voteEul)
{
  voteEul = voteRotm.eulerAngles(2,1,0);
}

float _rotmDist2(const Eigen::Matrix3f &m1, const Eigen::Matrix3f &m2)
{
  Eigen::Matrix3f rotDiff = m1-m2;
  return rotDiff.cwiseProduct(rotDiff).sum();
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/airy.hpp>

EVE_TEST_TYPES( "Check return types of airy_bi"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::airy_bi(T(0)), T);
  TTS_EXPR_IS(eve::airy_bi(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of airy_bi on wide"
         , eve::test::simd::ieee_reals
         , eve::test::generate(eve::test::randoms(-20.0, 0.0),
                               eve::test::randoms(0.0, 20.0)
                              )
         )
   <typename T>(T a0, T a1)
{
  using v_t = eve::element_type_t<T>;
  v_t abstol = 1000*eve::eps(eve::as<v_t>());
  auto eve__airy_bi =  [](auto x) { return eve::airy_bi(x); };
  auto std__airy_bi =  [](auto x)->v_t { return boost::math::airy_bi(x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__airy_bi(eve::minf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::inf(eve::as<v_t>())), eve::inf(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::minf(eve::as< T>())), eve::zero(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::inf(eve::as< T>())),  eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::nan(eve::as< T>())), eve::nan(eve::as<T>()), 0);
  }
  TTS_ULP_EQUAL(eve__airy_bi(v_t(1500)), eve::inf(eve::as<v_t>()), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(500)), eve::inf(eve::as<v_t>()), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(10)),  std__airy_bi(v_t(10))  , 13.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(5)),  std__airy_bi(v_t(5))   , 10.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(2)),  std__airy_bi(v_t(2))   , 35.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(1.5)),std__airy_bi(v_t(1.5)) , 10.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(0.5)),std__airy_bi(v_t(0.5)) , 10.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(1)),  std__airy_bi(v_t(1))   , 2.5);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(0)),  std__airy_bi(v_t(0))   , 1.5);

  TTS_ULP_EQUAL(eve__airy_bi( T(1500)), eve::inf(eve::as<T>()), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(500)),  eve::inf(eve::as<T>()), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(10)) ,  T(std__airy_bi( v_t(10)) ), 13.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(5))  ,  T(std__airy_bi( v_t(5))  ), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(2))  ,  T(std__airy_bi( v_t(2))  ), 36.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(1.5)),  T(std__airy_bi( v_t(1.5))), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(0.5)),  T(std__airy_bi( v_t(0.5))), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(1))  ,  T(std__airy_bi( v_t(1))  ), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(0))  ,  T(0.614926627446001), 0.5);

  TTS_ABSOLUTE_EQUAL(eve__airy_bi(v_t(-1500)), std__airy_bi(v_t(-1500)), abstol);
  TTS_ABSOLUTE_EQUAL(eve__airy_bi(v_t(-500)), std__airy_bi(v_t(-500)), abstol);
  TTS_ABSOLUTE_EQUAL(eve__airy_bi(v_t(-10)),  std__airy_bi(v_t(-10)), abstol);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(-5)),  std__airy_bi(v_t(-5))   , 10.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(-2)),  std__airy_bi(v_t(-2))   , 35.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(-1.5)),std__airy_bi(v_t(-1.5)) , 10.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(-0.5)),std__airy_bi(v_t(-0.5)) , 10.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(-1)),  std__airy_bi(v_t(-1))   , 7.5);

  TTS_ABSOLUTE_EQUAL(eve__airy_bi( T(-1500)), T(std__airy_bi(v_t(-1500))), abstol);
  TTS_ABSOLUTE_EQUAL(eve__airy_bi( T(-500)),  T(std__airy_bi(v_t(-500)) ), abstol);
  TTS_ABSOLUTE_EQUAL(eve__airy_bi( T(-10)) ,  T(std__airy_bi( v_t(-10)) ), abstol);
  TTS_ULP_EQUAL(eve__airy_bi( T(-5))  ,  T(std__airy_bi( v_t(-5))  ), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(-2))  ,  T(std__airy_bi( v_t(-2))  ), 36.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(-1.5)),  T(std__airy_bi( v_t(-1.5))), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(-0.5)),  T(std__airy_bi( v_t(-0.5))), 10.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(-1))  ,  T(std__airy_bi( v_t(-1))  ), 10.0);

   TTS_ABSOLUTE_EQUAL(eve__airy_bi(a0), map(std__airy_bi, a0), abstol);
   TTS_RELATIVE_EQUAL(eve__airy_bi(a1), map(std__airy_bi, a1), 0.001);

};

EVE_TEST( "Check behavior of diff(airy_bi) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(-20.0, 0.0),
                              eve::test::randoms(0.0, 20.0)
                             )
        )
  <typename T>(T a0, T a1)
{
  using v_t = eve::element_type_t<T>;
  v_t reltol = 50000000*eve::eps(eve::as<v_t>());
  auto eve__airy_bi =  [](auto x) { return eve::diff(eve::airy_bi)(x); };
  auto std__airy_bi =  [](auto x)->v_t { return boost::math::airy_bi_prime(x); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__airy_bi(eve::minf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::inf(eve::as<v_t>())), eve::inf(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::minf(eve::as< T>())), eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::inf(eve::as< T>())), eve::inf(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__airy_bi(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__airy_bi(v_t(1500)), eve::inf(eve::as<v_t>()), 10.0);

  TTS_ULP_EQUAL(eve__airy_bi(v_t(10)),  std__airy_bi(v_t(10)) , 50.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(5)),  std__airy_bi(v_t(5))   , 50.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(2)),  std__airy_bi(v_t(2))   , 50.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(1.5)),std__airy_bi(v_t(1.5)) , 50.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(0.5)),std__airy_bi(v_t(0.5)) , 50.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(1)),  std__airy_bi(v_t(1))   , 50.0);
  TTS_ULP_EQUAL(eve__airy_bi(v_t(0)),  v_t(0.448288357353826), 0.5);

  TTS_ULP_EQUAL(eve__airy_bi( T(1500)), eve::inf(eve::as< T>()), 10.0);

  TTS_ULP_EQUAL(eve__airy_bi( T(10)) ,  T(std__airy_bi( v_t(10)) ),  50.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(5))  ,  T(std__airy_bi( v_t(5))  ),  50.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(2))  ,  T(std__airy_bi( v_t(2))  ),  50.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(1.5)),  T(std__airy_bi( v_t(1.5))),  50.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(0.5)),  T(std__airy_bi( v_t(0.5))),  50.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(1))  ,  T(std__airy_bi( v_t(1))  ),  50.0);
  TTS_ULP_EQUAL(eve__airy_bi( T(0))  ,  T(0.448288357353826), 0.5);

  TTS_RELATIVE_EQUAL(eve__airy_bi(v_t(-1500)), std__airy_bi(v_t(-1500)), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi(v_t(-50)), std__airy_bi(v_t(-50)), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi(v_t(-10)),  std__airy_bi(v_t(-10)) , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi(v_t(-5)),  std__airy_bi(v_t(-5))   , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi(v_t(-2)),  std__airy_bi(v_t(-2))   , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi(v_t(-1.5)),std__airy_bi(v_t(-1.5)) , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi(v_t(-0.5)),std__airy_bi(v_t(-0.5)) , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi(v_t(-1)),  std__airy_bi(v_t(-1))   , reltol);

  TTS_ABSOLUTE_EQUAL(eve__airy_bi( T(-1500)), T(std__airy_bi(v_t(-1500))), reltol);
  TTS_ABSOLUTE_EQUAL(eve__airy_bi( T(-500)),  T(std__airy_bi(v_t(-500)) ), reltol);
  TTS_ABSOLUTE_EQUAL(eve__airy_bi( T(-10)) ,  T(std__airy_bi( v_t(-10)) ), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi( T(-5))  ,  T(std__airy_bi( v_t(-5))  ), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi( T(-2))  ,  T(std__airy_bi( v_t(-2))  ), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi( T(-1.5)),  T(std__airy_bi( v_t(-1.5))), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi( T(-0.5)),  T(std__airy_bi( v_t(-0.5))), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_bi( T(-1))  ,  T(std__airy_bi( v_t(-1))  ), reltol);

  TTS_ABSOLUTE_EQUAL(eve__airy_bi(a0), map(std__airy_bi, a0), 0.0004);
  TTS_RELATIVE_EQUAL(eve__airy_bi(a1), map(std__airy_bi, a1), 0.0004);
};


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Core>

#include <tudat/astro/basic_astro/physicalConstants.h>
#include <tudat/basics/testMacros.h>
#include <tudat/math/basic/mathematicalConstants.h>
#include "tudat/astro/basic_astro/unitConversions.h"
#include <tudat/astro/basic_astro/orbitalElementConversions.h>
#include "tudat/astro/gravitation/librationPoint.h"
#include "tudat/astro/gravitation/unitConversionsCircularRestrictedThreeBodyProblem.h"
#include "tudat/simulation/propagation_setup/propagationCR3BPFullProblem.h"

#include <tudat/simulation/simulation.h>
#include <tudat/io/basicInputOutput.h>
#include <tudat/io/applicationOutput.h>
#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/gravitation/unitConversionsCircularRestrictedThreeBodyProblem.h"

int main( )
{
    using namespace tudat;
    using namespace tudat::input_output;
    using namespace tudat::simulation_setup;

    spice_interface::loadStandardSpiceKernels( );

    // Global characteristics of the problem
    double distanceSunJupiter = 778.0e9;

    // Initialise the spacecraft state (B. Taylor, D. (1981). Horseshoe periodic orbits in the restricted problem of three bodies
    // for a sun-Jupiter mass ratio. Astronomy and Astrophysics. 103. 288-294.)
    Eigen::Vector6d initialState = Eigen::Vector6d::Zero();
    initialState[0] = - 7.992e11;
    initialState[4] =  -1.29e4;

    // Create integrator settings.
    double initialTime = 0.0;
    const double fixedStepSize = 100000.0;
    std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
            std::make_shared < numerical_integrators::IntegratorSettings < > >
            ( numerical_integrators::rungeKutta4, initialTime, fixedStepSize );

    // Create system of bodies.
    std::vector < std::string > bodiesCR3BP;
    bodiesCR3BP.push_back( "Sun" );
    bodiesCR3BP.push_back( "Jupiter" );

    // Define propagator settings variables.
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;
    bodiesToPropagate.push_back( "Spacecraft" );
    centralBodies.push_back( "SSB" );

    // Define final time for the propagation.
    double gravitationalParameterSun = createGravityFieldModel(
                getDefaultGravityFieldSettings(
                    "Sun", TUDAT_NAN, TUDAT_NAN ), "Sun" )->getGravitationalParameter( );
    double gravitationalParameterJupiter = createGravityFieldModel(
                getDefaultGravityFieldSettings(
                    "Jupiter", TUDAT_NAN, TUDAT_NAN ), "Jupiter" )->getGravitationalParameter( );
    double finalTime = tudat::circular_restricted_three_body_problem::convertDimensionlessTimeToDimensionalTime(
                29.2386 * ( 2.0 * mathematical_constants::PI ), gravitationalParameterSun, gravitationalParameterJupiter, distanceSunJupiter);

    SystemOfBodies idealBodyMap = propagators::setupBodyMapCR3BP(
                distanceSunJupiter, "Sun", "Jupiter", "Spacecraft" );

    std::map< double, Eigen::Vector6d> fullPropagation;
    std::map< double, Eigen::Vector6d> cr3bpPropagation;

    /// Ideal case: full dynamics problem with the CR3BP assumtions
    {

        // Create acceleration map.
        basic_astrodynamics::AccelerationMap accelerationModelMap = propagators::setupAccelerationMapCR3BP(
                    "Sun", "Jupiter", bodiesToPropagate.at( 0 ), centralBodies.at( 0 ), idealBodyMap );

        // Calculate the difference between CR3BP and full problem.
        propagators::propagateCR3BPAndFullDynamicsProblem(
                    initialTime, finalTime, initialState, integratorSettings, accelerationModelMap,
                    bodiesToPropagate, centralBodies, idealBodyMap, bodiesCR3BP, fullPropagation,
                    cr3bpPropagation );

        Eigen::Vector6d stateDifference =
                fullPropagation.rbegin( )->second - cr3bpPropagation.rbegin( )->second;

        std::cout << "state difference at final time: " << stateDifference << std::endl;
    }

    std::map< double, Eigen::Vector6d> fullPropagationPerturbedCase;
    std::map< double, Eigen::Vector6d> cr3bpPropagationPerturbedCase;

    /// Perturbed case
    {

        std::string frameOrigin = "SSB";
        std::string frameOrientation = "ECLIPJ2000";

        SystemOfBodies perturbedBodyMap;

        std::vector< std::string > additionalBodies = { "Earth", "Mars", "Venus", "Saturn" };
        for( unsigned int i = 0; i < additionalBodies.size( ); i++ )
        {

            perturbedBodyMap[ additionalBodies.at( i ) ] = std::make_shared< Body >( );
            perturbedBodyMap[ additionalBodies.at( i ) ]->setEphemeris(
                        std::make_shared< ephemerides::ApproximatePlanetPositions>(
                                additionalBodies.at( i ) ) );
            perturbedBodyMap[ additionalBodies.at( i ) ]->setGravityFieldModel(
                        createGravityFieldModel(
                            std::make_shared< CentralGravityFieldSettings >(
                                spice_interface::getBodyGravitationalParameter(
                                    additionalBodies.at( i ) ) ), additionalBodies.at( i ) ) );
        }

        perturbedBodyMap[ "Sun" ] = idealBodyMap[ "Sun" ];
        perturbedBodyMap[ "Jupiter" ] = idealBodyMap[ "Jupiter" ];

        // Create the body to be propagated.
        perturbedBodyMap[ "Spacecraft" ] = std::make_shared< Body >( );
        perturbedBodyMap[ "Spacecraft" ]->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                                            std::shared_ptr< interpolators::OneDimensionalInterpolator
                                                            < double, Eigen::Vector6d > >( ), "SSB", frameOrientation ) );

        setGlobalFrameBodyEphemerides( perturbedBodyMap, frameOrigin, frameOrientation );

        // Set of accelerations experienced by the spacecraft.
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > bodyToPropagateAccelerations;
        bodyToPropagateAccelerations["Sun"].push_back(std::make_shared< AccelerationSettings >(
                                                          basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Jupiter"].push_back(std::make_shared< AccelerationSettings >(
                                                              basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Earth"].push_back(std::make_shared< AccelerationSettings >(
                                                            basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Mars"].push_back(std::make_shared< AccelerationSettings >(
                                                           basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Venus"].push_back(std::make_shared< AccelerationSettings >(
                                                            basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Saturn"].push_back(std::make_shared< AccelerationSettings >(
                                                             basic_astrodynamics::central_gravity ) );

        SelectedAccelerationMap accelerationMap;
        accelerationMap[ "Spacecraft" ] = bodyToPropagateAccelerations;

        // Create the acceleration map.
        basic_astrodynamics::AccelerationMap accelerationModelMapPerturbedCase = createAccelerationModelsMap(
                    perturbedBodyMap, accelerationMap, bodiesToPropagate, centralBodies );

        // Calculate the difference between CR3BP and full problem.
        propagators::propagateCR3BPAndFullDynamicsProblem(
                    initialTime, finalTime, initialState, integratorSettings,
                    accelerationModelMapPerturbedCase,
                    bodiesToPropagate, centralBodies, perturbedBodyMap, bodiesCR3BP,
                    fullPropagationPerturbedCase,
                    cr3bpPropagationPerturbedCase );

        Eigen::Vector6d stateDifferencePerturbedCase =
                fullPropagationPerturbedCase.rbegin( )->second - cr3bpPropagationPerturbedCase.rbegin( )->second;

        std::cout << "state difference at final time for the perturbed case: " << stateDifferencePerturbedCase << std::endl;

    }

    /// Ouputs

    // Outputs for the ideal case
    {
        std::map< double, Eigen::Vector6d > fullPropagationNormalisedCoRotatingFrame;
        for( std::map< double, Eigen::Vector6d >::iterator itr = fullPropagation.begin( );
             itr != fullPropagation.end( ); itr++ ){
            fullPropagationNormalisedCoRotatingFrame[ itr->first ] = tudat::circular_restricted_three_body_problem::convertCartesianToCorotatingNormalizedCoordinates(
                        gravitationalParameterSun, gravitationalParameterJupiter, distanceSunJupiter, itr->second, itr->first);
        }

        std::map< double, Eigen::Vector6d > cr3bpNormalisedCoRotatingFrame;
        for( std::map< double, Eigen::Vector6d >::iterator itr = cr3bpPropagation.begin( );
             itr != cr3bpPropagation.end( ); itr++ ){
            cr3bpNormalisedCoRotatingFrame[ itr->first ] = tudat::circular_restricted_three_body_problem::convertCartesianToCorotatingNormalizedCoordinates(
                        gravitationalParameterSun, gravitationalParameterJupiter, distanceSunJupiter, itr->second, itr->first);
        }

        input_output::writeDataMapToTextFile( fullPropagation,
                                              "fullProblemPropagation.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( fullPropagationNormalisedCoRotatingFrame,
                                              "fullProblemPropagationNormalisedCoRotatingFrame.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( cr3bpPropagation,
                                              "CR3BPsolution.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( cr3bpNormalisedCoRotatingFrame,
                                              "CR3BPnormalisedCoRotatingFrame.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );
    }

    // Outputs for the perturbed case
    {
        std::map< double, Eigen::Vector6d > fullPropagationNormalisedCoRotatingFramePerturbedCase;
        for( std::map< double, Eigen::Vector6d >::iterator itr = fullPropagationPerturbedCase.begin( );
             itr != fullPropagationPerturbedCase.end( ); itr++ ){
            fullPropagationNormalisedCoRotatingFramePerturbedCase[ itr->first ] = tudat::circular_restricted_three_body_problem::
                    convertCartesianToCorotatingNormalizedCoordinates(gravitationalParameterSun, gravitationalParameterJupiter,
                                                                      distanceSunJupiter, itr->second, itr->first);
        }

        std::map< double, Eigen::Vector6d > cr3bpNormalisedCoRotatingFramePerturbedCase;
        for( std::map< double, Eigen::Vector6d >::iterator itr = cr3bpPropagationPerturbedCase.begin( );
             itr != cr3bpPropagationPerturbedCase.end( ); itr++ ){
            cr3bpNormalisedCoRotatingFramePerturbedCase[ itr->first ] = tudat::circular_restricted_three_body_problem::
                    convertCartesianToCorotatingNormalizedCoordinates(
                        gravitationalParameterSun, gravitationalParameterJupiter, distanceSunJupiter, itr->second, itr->first);
        }

        input_output::writeDataMapToTextFile( fullPropagationPerturbedCase,
                                              "fullProblemPropagationPerturbedCase.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( fullPropagationNormalisedCoRotatingFramePerturbedCase,
                                              "fullProblemPropagationNormalisedCoRotatingFramePerturbedCase.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( cr3bpPropagationPerturbedCase,
                                              "CR3BPsolutionPerturbedCase.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( cr3bpNormalisedCoRotatingFramePerturbedCase,
                                              "CR3BPnormalisedCoRotatingFramePerturbedCase.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );
    }

    // Final statement.
    // The exit code EXIT_SUCCESS indicates that the program was successfully executed.
    return EXIT_SUCCESS;
}


#include <tiny.h>
#include <geometry.h>
#include <convex.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <cmath>

typedef tiny::MathTypes<double>  M;
typedef M::quaternion_type       Q;
typedef M::vector3_type          V;
typedef M::real_type             T;
typedef M::coordsys_type         C;
typedef M::value_traits          VT;

BOOST_AUTO_TEST_SUITE(convex_get_scale);

BOOST_AUTO_TEST_CASE(get_scale)
{
  geometry::Box<V>          box;
  convex::Capsule<M>          capsule;
  convex::Cone<M>             cone;
  convex::ConvexHull<M>       hull;
  convex::Cylinder<M>         cylinder;
  convex::Ellipsoid<M>        ellipsoid;
  geometry::Sphere<V>         sphere;

  //get_scale is supposed to return the minimum extent of the shape
  //spheres
  {
    sphere.radius() = 0.75f;
    BOOST_CHECK( sphere.get_scale() == 1.5f); //for a sphere, the scale is the diameter
    sphere.radius() = 0.5f;
    BOOST_CHECK( sphere.get_scale() == 1.0f); //for a sphere, the scale is the diameter
  }
  //boxes
  {
    box.half_extent()(0) = 0.5f;//
    box.half_extent()(1) = 0.5f;// a unit cube, scale should be 1.0f
    box.half_extent()(2) = 0.5f;//
    BOOST_CHECK( box.get_scale() == 1.0f);
    box.half_extent()(1) = 0.2f; //decreasing the extent should change the scale
    BOOST_CHECK(box.get_scale() == 0.40f);
    box.half_extent()(2) = 1.2f; //this should not change the scale
    BOOST_CHECK( box.get_scale() == 0.40f);
    box.half_extent()(2) = 0.0f; //dropping one dimension, this should not change the scale
    BOOST_CHECK( box.get_scale() == 0.40f);
  }
  //capsule
  {
    capsule.half_height() = 1.50f;
    capsule.radius() = 0.7f;
    BOOST_CHECK( capsule.get_scale() == 1.4f);
    capsule.radius() = 0.5f;
    BOOST_CHECK( capsule.get_scale() == 1.0f);
    capsule.half_height() = 0.25f;
    BOOST_CHECK( capsule.get_scale() == 0.5f);
    capsule.radius() = 2.5f;
    BOOST_CHECK( capsule.get_scale() == 0.5f);
  }
  //cone
  {
    cone.half_height() = 1.50f;
    cone.base_radius() = 0.7f;
    BOOST_CHECK( cone.get_scale() == 1.4f);
    cone.base_radius() = 0.5f;
    BOOST_CHECK( cone.get_scale() == 1.0f);
    cone.half_height() = 0.25f;
    BOOST_CHECK( cone.get_scale() == 0.5f);
    cone.base_radius() = 2.5f;
    BOOST_CHECK( cone.get_scale() == 0.5f);
    cone.base_radius() = 2.5f;
    BOOST_CHECK( cone.get_scale() == 0.5f);
  }
  //cylinder
  {
    cylinder.half_height() = 1.50f;
    cylinder.radius() = 0.7f;
    BOOST_CHECK( cylinder.get_scale() == 1.4f);
    cylinder.radius() = 0.5f;
    BOOST_CHECK( cylinder.get_scale() == 1.0f);
    cylinder.half_height() = 0.25f;
    BOOST_CHECK( cylinder.get_scale() == 0.5f);
    cylinder.radius() = 2.5f;
    BOOST_CHECK( cylinder.get_scale() == 0.5f);
  }
  //ellipsoid
  {
    ellipsoid.scale()(0) = 1.40f;
    ellipsoid.scale()(1) = 0.7f;
    ellipsoid.scale()(2) = 0.35f;
    BOOST_CHECK( ellipsoid.get_scale() == 0.7f);
    ellipsoid.scale()(2) = 0.7f;
    BOOST_CHECK( ellipsoid.get_scale() == 1.4f);
    ellipsoid.scale()(2) = 0.9f;
    BOOST_CHECK( ellipsoid.get_scale() == 1.4f);
    ellipsoid.scale()(2) = 0.0f; //dropping one dimension, should not change the scale
    BOOST_CHECK( ellipsoid.get_scale() == 1.4f);
  }
  //convex hull
  {
    hull.add_point( V::make( 1.0,  4.0,  6.0 ) );
    hull.add_point( V::make( 1.0,  6.0,  8.0 ) );
    hull.add_point( V::make( 1.0,  7.0,  4.0 ) );
    hull.add_point( V::make( 1.0,  8.0,  7.0 ) );
    hull.add_point( V::make( 3.0,  5.0,  9.0 ) );
    hull.add_point( V::make( 3.0,  7.0,  5.0 ) );
    hull.add_point( V::make( 3.0,  9.0,  7.0 ) );
    hull.add_point( V::make( 4.0,  7.0,  9.0 ) );
    hull.add_point( V::make( 4.0,  9.0,  6.0 ) );
    hull.add_point( V::make( 5.0,  6.0,  9.0 ) );
    hull.add_point( V::make( 5.0,  7.0,  8.0 ) );
    hull.add_point( V::make( 5.0,  8.0,  6.0 ) );
    BOOST_CHECK(hull.get_scale() == 4.0f);

    hull.add_point( V::make( 10.0, 10.0, 10.0 ) );
    BOOST_CHECK(hull.get_scale() == 6.0f);

    hull.add_point( V::make( 1.0,  4.0,  4.0 ) );
    BOOST_CHECK(hull.get_scale() == 6.0f);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <string>
#include <vector>

#include <thread>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>

#include <stdio.h>

#include "patch.h"

using std::cout;
using std::endl;
using std::vector;

namespace OFC
{

  typedef __v4sf v4sf;

  PatClass::PatClass(
    const camparam* cpt_in,
    const camparam* cpo_in,
    const optparam* op_in,
    const int patchid_in)
  :
    cpt(cpt_in),
    cpo(cpo_in),
    op(op_in),
    patchid(patchid_in)
{
  pc = new patchstate;
  CreateStatusStruct(pc);

  tmp.resize(op->novals,1);
  dxx_tmp.resize(op->novals,1);
  dyy_tmp.resize(op->novals,1);
}

void PatClass::CreateStatusStruct(patchstate * psin)
{
  // get reference / template patch
  psin->pdiff.resize(op->novals,1);
  psin->pweight.resize(op->novals,1);
}

PatClass::~PatClass()
{
  delete pc;
}

void PatClass::InitializePatch(Eigen::Map<const Eigen::MatrixXf> * im_ao_in, Eigen::Map<const Eigen::MatrixXf> * im_ao_dx_in, Eigen::Map<const Eigen::MatrixXf> * im_ao_dy_in, const Eigen::Vector2f pt_ref_in)
{
  im_ao = im_ao_in;
  im_ao_dx = im_ao_dx_in;
  im_ao_dy = im_ao_dy_in;

  pt_ref = pt_ref_in;
  ResetPatch();

  getPatchStaticNNGrad(im_ao->data(), im_ao_dx->data(), im_ao_dy->data(), &pt_ref, &tmp, &dxx_tmp, &dyy_tmp);

  ComputeHessian();
}

void PatClass::ComputeHessian()
{
  #if (SELECTMODE==1)
  pc->Hes(0,0) = (dxx_tmp.array() * dxx_tmp.array()).sum();
  pc->Hes(0,1) = (dxx_tmp.array() * dyy_tmp.array()).sum();
  pc->Hes(1,1) = (dyy_tmp.array() * dyy_tmp.array()).sum();
  pc->Hes(1,0) = pc->Hes(0,1);
  if (pc->Hes.determinant()==0)
  {
    pc->Hes(0,0)+=1e-10;
    pc->Hes(1,1)+=1e-10;
  }
  #else
  pc->Hes(0,0) = (dxx_tmp.array() * dxx_tmp.array()).sum();
  if (pc->Hes.sum()==0)
    pc->Hes(0,0)+=1e-10;
  #endif
}

void PatClass::SetTargetImage(Eigen::Map<const Eigen::MatrixXf> * im_bo_in, Eigen::Map<const Eigen::MatrixXf> * im_bo_dx_in, Eigen::Map<const Eigen::MatrixXf> * im_bo_dy_in)
{
  im_bo = im_bo_in;
  im_bo_dx = im_bo_dx_in;
  im_bo_dy = im_bo_dy_in;

  ResetPatch();
}

void PatClass::ResetPatch()
{
  pc->hasconverged=0;
  pc->hasoptstarted=0;

  pc->pt_st = pt_ref;
  pc->pt_iter = pt_ref;

  pc->p_in.setZero();
  pc->p_iter.setZero();
  pc->delta_p.setZero();

  pc->delta_p_sqnorm = 1e-10;
  pc->delta_p_sqnorm_init = 1e-10;
  pc->mares = 1e20;
  pc->mares_old = 1e20;
  pc->cnt=0;
  pc->invalid = false;
}

#if (SELECTMODE==1)
void PatClass::OptimizeStart(const Eigen::Vector2f p_in_arg)
#else
void PatClass::OptimizeStart(const Eigen::Matrix<float, 1, 1> p_in_arg)
#endif
{
  pc->p_in   = p_in_arg;
  pc->p_iter = p_in_arg;

  // convert from input parameters to 2D query location(s) for patches
  paramtopt();

  // save starting location, only needed for outlier check
  pc->pt_st = pc->pt_iter;

  //Check if initial position is already invalid
  if (pc->pt_iter[0] < cpt->tmp_lb  || pc->pt_iter[1] < cpt->tmp_lb ||    // check if patch left valid image region
      pc->pt_iter[0] > cpt->tmp_ubw || pc->pt_iter[1] > cpt->tmp_ubh)
  {
    pc->hasconverged=1;
    pc->pdiff = tmp;
    pc->hasoptstarted=1;
  }
  else
  {
    pc->cnt=0; // reset iteration counter
    pc->delta_p_sqnorm = 1e-10;
    pc->delta_p_sqnorm_init = 1e-10;  // set to arbitrary low value, s.t. that loop condition is definitely true on first iteration
    pc->mares = 1e5;          // mean absolute residual
    pc->mares_old = 1e20; // for rate of change, keep mares from last iteration in here. Set high so that loop condition is definitely true on first iteration
    pc->hasconverged=0;

    OptimizeComputeErrImg();

    pc->hasoptstarted=1;
    pc->invalid = false;
  }
}

#if (SELECTMODE==1)
void PatClass::OptimizeIter(const Eigen::Vector2f p_in_arg, const bool untilconv)
#else
void PatClass::OptimizeIter(const Eigen::Matrix<float, 1, 1> p_in_arg, const bool untilconv)
#endif
{
  if (!pc->hasoptstarted)
  {
    ResetPatch();
    OptimizeStart(p_in_arg);
  }
  int oldcnt=pc->cnt;

  // optimize patch until convergence, or do only one iteration if DIS visualization is used
  while (  ! (pc->hasconverged || (untilconv == false && (pc->cnt > oldcnt)))  )
  {
    pc->cnt++;

    // Projection onto sd_images
    #if (SELECTMODE==1)
      pc->delta_p[0] = (dxx_tmp.array() * pc->pdiff.array()).sum();
      pc->delta_p[1] = (dyy_tmp.array() * pc->pdiff.array()).sum();
    #else
      pc->delta_p[0] = (dxx_tmp.array() * pc->pdiff.array()).sum();
    #endif

    pc->delta_p = pc->Hes.llt().solve(pc->delta_p); // solve linear system

    pc->p_iter -= pc->delta_p; // update flow vector

    #if (SELECTMODE==2) // if stereo depth
    if (cpt->camlr==0)
      pc->p_iter[0] = std::min(pc->p_iter[0],0.0f); // disparity in t can only be negative (in right image)
    else
      pc->p_iter[0] = std::max(pc->p_iter[0],0.0f); // ... positive (in left image)
    #endif

    // compute patch locations based on new parameter vector
    paramtopt();

    // check if patch(es) moved too far from starting location, if yes, stop iteration and reset to starting location
    if ((pc->pt_st - pc->pt_iter).norm() > op->outlierthresh  // check if query patch moved more than >padval from starting location -> most likely outlier
        ||
        pc->pt_iter[0] < cpt->tmp_lb  || pc->pt_iter[1] < cpt->tmp_lb ||    // check patch left valid image region
        pc->pt_iter[0] > cpt->tmp_ubw || pc->pt_iter[1] > cpt->tmp_ubh)
    {
      pc->p_iter = pc->p_in; // reset
      paramtopt();
      pc->hasconverged=1;
      pc->hasoptstarted=1;
    }

    OptimizeComputeErrImg();
  }
}

inline void PatClass::paramtopt()
{
    #if (SELECTMODE==1)
      pc->pt_iter = pt_ref + pc->p_iter;    // for optical flow the point displacement and the parameter vector are equivalent
    #else
      pc->pt_iter[0] = pt_ref[0] + pc->p_iter[0];
    #endif
}

void PatClass::LossComputeErrorImage(Eigen::Matrix<float, Eigen::Dynamic, 1>* patdest, Eigen::Matrix<float, Eigen::Dynamic, 1>* wdest, const Eigen::Matrix<float, Eigen::Dynamic, 1>* patin,  const Eigen::Matrix<float, Eigen::Dynamic, 1>*  tmpin)
{
  v4sf * pd = (v4sf*) patdest->data(),
       * pa = (v4sf*) patin->data(),
       * te = (v4sf*) tmpin->data(),
       * pw = (v4sf*) wdest->data();

  if (op->costfct==0) // L2 cost function
  {
    for (int i=op->novals/4; i--; ++pd, ++pa, ++te, ++pw)
    {
      (*pd) = (*pa)-(*te);  // difference image
      (*pw) = __builtin_ia32_andnps(op->negzero,  (*pd) );
    }
  }
  else if (op->costfct==1) // L1 cost function
  {
    for (int i=op->novals/4; i--; ++pd, ++pa, ++te, ++pw)
    {
      (*pd) = (*pa)-(*te);   // difference image
      (*pd) = __builtin_ia32_orps( __builtin_ia32_andps(op->negzero,  (*pd) )  , __builtin_ia32_sqrtps (__builtin_ia32_andnps(op->negzero,  (*pd) )) );  // sign(pdiff) * sqrt(abs(pdiff))
      (*pw) = __builtin_ia32_andnps(op->negzero,  (*pd) );
    }
  }
  else if (op->costfct==2) // Pseudo Huber cost function
  {
    for (int i=op->novals/4; i--; ++pd, ++pa, ++te, ++pw)
    {
      (*pd) = (*pa)-(*te);   // difference image
      (*pd) = __builtin_ia32_orps(__builtin_ia32_andps(op->negzero,  (*pd) ),
                                  __builtin_ia32_sqrtps (
                                    __builtin_ia32_mulps(                                                                                // PSEUDO HUBER NORM
                                          __builtin_ia32_sqrtps (op->ones + __builtin_ia32_divps(__builtin_ia32_mulps((*pd),(*pd)) , op->normoutlier_tmpbsq)) - op->ones, // PSEUDO HUBER NORM
                                          op->normoutlier_tmp2bsq)                                                                                // PSEUDO HUBER NORM
                                     )
                                    ); // sign(pdiff) * sqrt( 2*b^2*( sqrt(1+abs(pdiff)^2/b^2)+1)  )) // <- looks like this without SSE instruction
      (*pw) = __builtin_ia32_andnps(op->negzero,  (*pd) );
    }
  }
}

void PatClass::OptimizeComputeErrImg()
{
  getPatchStaticBil(im_bo->data(), &(pc->pt_iter), &(pc->pdiff));

  // Get photometric patch error
  LossComputeErrorImage(&pc->pdiff, &pc->pweight, &pc->pdiff, &tmp);

  // Compute step norm
  pc->delta_p_sqnorm = pc->delta_p.squaredNorm();
  if (pc->cnt==1)
    pc->delta_p_sqnorm_init = pc->delta_p_sqnorm;

  // Check early termination criterions
  pc->mares_old = pc->mares;
  pc->mares = pc->pweight.lpNorm<1>() / (op->novals);
  if ( !  ((pc->cnt < op->max_iter) &  (pc->mares  > op->res_thresh) &
          ((pc->cnt < op->min_iter) |  (pc->delta_p_sqnorm / pc->delta_p_sqnorm_init >= op->dp_thresh)) &
          ((pc->cnt < op->min_iter) |  (pc->mares / pc->mares_old <= op->dr_thresh)))  )
    pc->hasconverged=1;

}

// Extract patch on integer position, and gradients, No Bilinear interpolation
void PatClass::getPatchStaticNNGrad(const float* img, const float* img_dx, const float* img_dy,
                    const Eigen::Vector2f* mid_in,
                    Eigen::Matrix<float, Eigen::Dynamic, 1>* tmp_in_e,
                    Eigen::Matrix<float, Eigen::Dynamic, 1>*  tmp_dx_in_e,
                    Eigen::Matrix<float, Eigen::Dynamic, 1>* tmp_dy_in_e)
{
  float *tmp_in    = tmp_in_e->data();
  float *tmp_dx_in = tmp_dx_in_e->data();
  float *tmp_dy_in = tmp_dy_in_e->data();

  Eigen::Vector2i pos;
  Eigen::Vector2i pos_it;

  pos[0] = round((*mid_in)[0]) + cpt->imgpadding;
  pos[1] = round((*mid_in)[1]) + cpt->imgpadding;

  int posxx = 0;

  int lb = -op->p_samp_s/2;
  int ub = op->p_samp_s/2-1;

  for (int j=lb; j <= ub; ++j)
  {
    for (int i=lb; i <= ub; ++i, ++posxx)
    {
      pos_it[0] = pos[0]+i;
      pos_it[1] = pos[1]+j;
      int idx = pos_it[0] + pos_it[1] * cpt->tmp_w;

      #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // Single channel
      tmp_in[posxx] = img[idx];
      tmp_dx_in[posxx] = img_dx[idx];
      tmp_dy_in[posxx] = img_dy[idx];
      #else  // 3 RGB channels
      idx *= 3;
      tmp_in[posxx] = img[idx]; tmp_dx_in[posxx] = img_dx[idx]; tmp_dy_in[posxx] = img_dy[idx]; ++posxx; ++idx;
      tmp_in[posxx] = img[idx]; tmp_dx_in[posxx] = img_dx[idx]; tmp_dy_in[posxx] = img_dy[idx]; ++posxx; ++idx;
      tmp_in[posxx] = img[idx]; tmp_dx_in[posxx] = img_dx[idx]; tmp_dy_in[posxx] = img_dy[idx];
      #endif
    }
  }

  // PATCH NORMALIZATION
  if (op->patnorm>0) // Subtract Mean
    tmp_in_e->array() -= (tmp_in_e->sum() / op->novals);
}

// Extract patch on float position with bilinear interpolation, no gradients.
void PatClass::getPatchStaticBil(const float* img, const Eigen::Vector2f* mid_in,  Eigen::Matrix<float, Eigen::Dynamic, 1>* tmp_in_e)
{
  float *tmp_in    = tmp_in_e->data();

  Eigen::Vector2f resid;
  Eigen::Vector4f we; // bilinear weight vector
  Eigen::Vector4i pos;
  Eigen::Vector2i pos_it;

  // Compute the bilinear weight vector, for patch without orientation/scale change -> weight vector is constant for all pixels
  pos[0] = ceil((*mid_in)[0]+.00001f); // ensure rounding up to natural numbers
  pos[1] = ceil((*mid_in)[1]+.00001f);
  pos[2] = floor((*mid_in)[0]);
  pos[3] = floor((*mid_in)[1]);

  resid[0] = (*mid_in)[0] - (float)pos[2];
  resid[1] = (*mid_in)[1] - (float)pos[3];
  we[0] = resid[0]*resid[1];
  we[1] = (1-resid[0])*resid[1];
  we[2] = resid[0]*(1-resid[1]);
  we[3] = (1-resid[0])*(1-resid[1]);

  pos[0] += cpt->imgpadding;
  pos[1] += cpt->imgpadding;

  float * tmp_it = tmp_in;
  const float * img_a, * img_b, * img_c, * img_d, *img_e;

  #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // 1 channel image
    img_e = img    + pos[0]-op->p_samp_s/2;
  #else                                       // 3-channel RGB image
    img_e = img    + (pos[0]-op->p_samp_s/2)*3;
  #endif

  int lb = -op->p_samp_s/2;
  int ub = op->p_samp_s/2-1;

  for (pos_it[1]=pos[1]+lb; pos_it[1] <= pos[1]+ub; ++pos_it[1])
  {
    #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // 1 channel image
      img_a = img_e +  pos_it[1]    * cpt->tmp_w;
      img_c = img_e + (pos_it[1]-1) * cpt->tmp_w;
      img_b = img_a-1;
      img_d = img_c-1;
    #else                                     // 3-channel RGB image
      img_a = img_e +  pos_it[1]    * cpt->tmp_w * 3;
      img_c = img_e + (pos_it[1]-1) * cpt->tmp_w * 3;
      img_b = img_a-3;
      img_d = img_c-3;
    #endif

    for (pos_it[0]=pos[0]+lb; pos_it[0] <= pos[0]+ub; ++pos_it[0],
            ++tmp_it,++img_a,++img_b,++img_c,++img_d)
    {
      #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // Single channel
        (*tmp_it)     = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d);
      #else // 3-channel RGB image
        (*tmp_it)     = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d); ++tmp_it; ++img_a; ++img_b; ++img_c; ++img_d;
        (*tmp_it)     = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d); ++tmp_it; ++img_a; ++img_b; ++img_c; ++img_d;
        (*tmp_it)     = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d);
      #endif
    }
  }
  // PATCH NORMALIZATION
  if (op->patnorm>0) // Subtract Mean
    tmp_in_e->array() -= (tmp_in_e->sum() / op->novals);
}

}


#include "cbe-loader.h"

#include <spdlog/spdlog.h>
#include <vector>
#include <Eigen/Dense>
#include <glm/glm.hpp>
#include <sstream>
#include <fstream>

#include "components/physics/transform.h"
#include "maths/rbf.h"
#include "history/generation/rbf-history.h"

CbeLoader::CbeLoader(Context& ctx) : m_ctx(ctx) {}

CbeLoader::~CbeLoader() {}

void CbeLoader::loadFile(const char* cbeFilePath) {
    std::ifstream fileStream(cbeFilePath);
    if (!fileStream) {
		spdlog::error("[CBEloader] Cannot load file : {}", cbeFilePath);
		assert(false);
	}

    const nlohmann::json parsed = nlohmann::json::parse(fileStream);
    fileStream.close();

    for (const auto& element : parsed.items()) {
        if (element.key() == "geometry") {

        } else if (element.key() == "palette") {

        } else if (element.key() == "generation") {
            generation(parsed);
        }
    }
}

void CbeLoader::generation(const nlohmann::json& json) {
    const std::string& type = json["generation"].at(0)["type"];
    const std::string& interpolation = json["generation"].at(0)["interpolation"];
    const std::string& mode = json["generation"].at(0)["mode"];
    const auto& controlPoints = json["generation"].at(0)["controlPoints"];

    // Get values
    std::vector<glm::ivec3> controlPointsXYZ(controlPoints.size());
    Eigen::VectorXd controlPointWeights(controlPoints.size());
    for (unsigned int i = 0; i < controlPoints.size(); i++) {
        const float& weight = controlPoints.at(i)["weight"];
        controlPointWeights[i] = weight;

        const glm::vec3 pos = glm::vec3(controlPoints.at(i)["position"].at(0), controlPoints.at(i)["position"].at(1), controlPoints.at(i)["position"].at(2));
        controlPointsXYZ.at(i) = pos;
    }

    // Get entities
    std::vector<glm::ivec3> coordWithYtoFind;
    std::vector<met::entity> entityToChange;
    m_ctx.registry.view<comp::Transform>().each([&](met::entity id, comp::Transform& transform){
        coordWithYtoFind.push_back(transform.position);
        entityToChange.push_back(id);
    });

    m_ctx.history.pushHistory(new RBFHistory(m_ctx.registry, coordWithYtoFind));
    voxmt::rbfInterpolate(coordWithYtoFind, controlPointsXYZ, controlPointWeights, voxmt::RBFType::LINEAR, 0.5f, voxmt::RBFTransformAxis::Y);

    // Changes entities
    for (size_t i = 0; i < coordWithYtoFind.size(); i++) {
        comp::Transform& trans =  m_ctx.registry.get<comp::Transform>(entityToChange.at(i));
        trans.position = coordWithYtoFind.at(i);
    }
}


#include <iostream>
#include <fstream>
#include <cmath>
#include <boost/assign/list_of.hpp>
#include "source/newtonian/common/hllc.hpp"
#include "source/newtonian/common/ideal_gas.hpp"
#include "source/newtonian/one_dimensional/hdsim.hpp"
#include "source/newtonian/one_dimensional/pcm1d.hpp"
#include "source/newtonian/one_dimensional/plm1d.hpp"
#include "source/newtonian/one_dimensional/eos_consistent1d.hpp"
#include "source/newtonian/one_dimensional/spatial_distribution1d.hpp"
#include "source/newtonian/one_dimensional/eulerian1d.hpp"
#include "source/newtonian/one_dimensional/periodic_1d.hpp"
#include "source/newtonian/one_dimensional/zero_force_1d.hpp"
#include "source/misc/int2str.hpp"
#include "source/misc/utils.hpp"
#include "source/misc/simple_io.hpp"
#include "source/newtonian/one_dimensional/hdf5_diagnostics1d.hpp"
#include "source/newtonian/test_1d/main_loop_1d.hpp"

using namespace std;
using namespace interpolations1d;
using namespace simulation1d;
using namespace diagnostics1d;

namespace {
SpatialReconstruction1D const& choose_between
(SpatialReconstruction1D const& opt1,
 SpatialReconstruction1D const& opt2,
 SpatialReconstruction1D const& opt3,
 string const& name1,
 string const& name2,
 string const& name3,
 string const& choice)
{
  if(name1==choice)
    return opt1;
  else if(name2==choice)
    return opt2;
  else if(name3==choice)
    return opt3;
  else
    throw "Unknown option "+choice;
}

class SimData
{
public:

  SimData(string const& interp_method_name):
    vertices_(linspace(0,1,100)),
    eos_(5./3.),
    pcm_(),
    plm_naive_(),
    plm_(plm_naive_,eos_),
    interpm_(choose_between(pcm_,plm_naive_,plm_,
			    "pcm","plm_naive","plm",
			    interp_method_name)),
    density_(1,0.3,2,0.7,1),
    pressure_(1),
    xvelocity_(1),
    yvelocity_(0),
    rs_(),
    vm_(),
    bc_(),
    force_(),
    sim_(pg_,
	 vertices_,
	 interpm_,
	 density_,
	 pressure_,
	 xvelocity_,
	 yvelocity_,
	 eos_,
	 rs_,
	 vm_,
	 bc_,
	 force_) {}

  hdsim1D& getSim(void)
  {
    return sim_;
  }

private:
  const SlabSymmetry1D pg_;
  const vector<double> vertices_;
  const IdealGas eos_;
  PCM1D pcm_;
  PLM1D plm_naive_;
  EOSConsistent plm_;
  SpatialReconstruction1D const& interpm_;
  const TwoSteps density_;
  const Uniform pressure_;
  const Uniform xvelocity_;
  const Uniform yvelocity_;
  const Hllc rs_;
  const Eulerian1D vm_;
  const Periodic1D bc_;
  const ZeroForce1D force_;
  hdsim1D sim_;
};
}

int main(void)
{
  const vector<string> interp_names =
    boost::assign::list_of("pcm")("plm_naive")("plm");
  for(size_t i=0;i<interp_names.size();++i){
    SimData sim_data(interp_names[i]);

    main_loop(sim_data.getSim(),
	      1, 1e6, 2,
	      "time.txt");

    write_snapshot_to_hdf5(sim_data.getSim(),
			   interp_names[i]+"_final.h5");
  }

  return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the quantum computing
    simulation classes from the dlib C++ Library.

    This example assumes you are familiar with quantum computing and
    Grover's search algorithm and Shor's 9 bit error correcting code
    in particular.   The example shows how to simulate both of these
    algorithms.

    The code to simulate Grover's algorithm is primarily here to show
    you how to make custom quantum gate objects.  The Shor ECC example
    is simpler and uses just the default gates that come with the
    library.

*/

#include <iostream>
#include <complex>
#include <ctime>
#include <dlib/quantum_computing.h>
#include <dlib/string.h>

using namespace std;
using namespace dlib;

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

// This declares a random number generator that we will be using below
dlib::rand rnd;

// ----------------------------------------------------------------------------------------

void shor_encode (
    quantum_register& reg
)
/*!
    requires
        - reg.num_bits() == 1
    ensures
        - #reg.num_bits() == 9
        - #reg == the Shor error coding of the input register
!*/
{
    DLIB_CASSERT(reg.num_bits() == 1,"");

    quantum_register zeros;
    zeros.set_num_bits(8);
    reg.append(zeros);

    using namespace dlib::quantum_gates;
    const gate<1> h = hadamard();
    const gate<1> i = noop();

    // Note that the expression (h,i) represents the tensor product of the 1 qubit
    // h gate with the 1 qubit i gate and larger versions of this expression
    // represent even bigger tensor products.  So as you see below, we make gates
    // big enough to apply to our quantum register by listing out all the gates we
    // want to go into the tensor product and then we just apply the resulting gate
    // to the quantum register.

    // Now apply the gates that constitute Shor's encoding to the input register.
    (cnot<3,0>(),i,i,i,i,i).apply_gate_to(reg);
    (cnot<6,0>(),i,i).apply_gate_to(reg);
    (h,i,i,h,i,i,h,i,i).apply_gate_to(reg);
    (cnot<1,0>(),i,cnot<1,0>(),i,cnot<1,0>(),i).apply_gate_to(reg);
    (cnot<2,0>(),cnot<2,0>(),cnot<2,0>()).apply_gate_to(reg);
}

// ----------------------------------------------------------------------------------------

void shor_decode (
    quantum_register& reg
)
/*!
    requires
        - reg.num_bits() == 9
    ensures
        - #reg.num_bits() == 1
        - #reg == the decoded qubit that was in the given input register
!*/
{
    DLIB_CASSERT(reg.num_bits() == 9,"");

    using namespace dlib::quantum_gates;
    const gate<1> h = hadamard();
    const gate<1> i = noop();

    // Now apply the gates that constitute Shor's decoding to the input register

    (cnot<2,0>(),cnot<2,0>(),cnot<2,0>()).apply_gate_to(reg);
    (cnot<1,0>(),i,cnot<1,0>(),i,cnot<1,0>(),i).apply_gate_to(reg);

    (toffoli<0,1,2>(),toffoli<0,1,2>(),toffoli<0,1,2>()).apply_gate_to(reg);

    (h,i,i,h,i,i,h,i,i).apply_gate_to(reg);

    (cnot<6,0>(),i,i).apply_gate_to(reg);
    (cnot<3,0>(),i,i,i,i,i).apply_gate_to(reg);
    (toffoli<0,3,6>(),i,i).apply_gate_to(reg);

    // Now that we have decoded the value we don't need the extra 8 bits any more so
    // remove them from the register.
    for (int i = 0; i < 8; ++i)
        reg.measure_and_remove_bit(0,rnd);
}

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

// This is the function we will use in Grover's search algorithm.  In this
// case the value we are searching for is 257.
bool is_key (unsigned long n)
{
    return n == 257;
}

// ----------------------------------------------------------------------------------------

template <int bits>
class uf_gate;

namespace dlib {
template <int bits>
struct gate_traits<uf_gate<bits> >
{
    static const long num_bits = bits;
    static const long dims = dlib::qc_helpers::exp_2_n<num_bits>::value;
};}

template <int bits>
class uf_gate : public gate_exp<uf_gate<bits> >
{
    /*!
        This gate represents the black box function in Grover's search algorithm.
        That is, it is the gate defined as follows:
            Uf|x>|y> = |x>|y XOR is_key(x)>

        See the documentation for the gate_exp object for the details regarding
        the compute_state_element() and operator() functions defined below.
    !*/
public:
    uf_gate() : gate_exp<uf_gate>(*this) {}

    static const long num_bits = gate_traits<uf_gate>::num_bits;
    static const long dims = gate_traits<uf_gate>::dims;

    const qc_scalar_type operator() (long r, long c) const
    {
        unsigned long output = c;
        // if the input control bit is set
        if (is_key(output>>1))
        {
            output = output^0x1;
        }

        if ((unsigned long)r == output)
            return 1;
        else
            return 0;
    }

    template <typename exp>
    qc_scalar_type compute_state_element (
        const matrix_exp<exp>& reg,
        long row_idx
    ) const
    {
        unsigned long output = row_idx;
        // if the input control bit is set
        if (is_key(output>>1))
        {
            output = output^0x1;
        }

        return reg(output);
    }
};

// ----------------------------------------------------------------------------------------

template <int bits>
class w_gate;

namespace dlib {
template <int bits>
struct gate_traits<w_gate<bits> >
{
    static const long num_bits = bits;
    static const long dims = dlib::qc_helpers::exp_2_n<num_bits>::value;
}; }

template <int bits>
class w_gate : public gate_exp<w_gate<bits> >
{
    /*!
        This is the W gate from the Grover algorithm
    !*/
public:

    w_gate() : gate_exp<w_gate>(*this) {}

    static const long num_bits = gate_traits<w_gate>::num_bits;
    static const long dims = gate_traits<w_gate>::dims;

    const qc_scalar_type operator() (long r, long c) const
    {
        qc_scalar_type res = 2.0/dims;
        if (r != c)
            return res;
        else
            return res - 1.0;
    }

    template <typename exp>
    qc_scalar_type compute_state_element (
        const matrix_exp<exp>& reg,
        long row_idx
    ) const
    {
        qc_scalar_type temp = sum(reg)*2.0/dims;
        // compute this value: temp = temp - reg(row_idx)*2.0/dims + reg(row_idx)*(2.0/dims - 1.0)
        temp = temp - reg(row_idx);

        return temp;
    }
};

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

int main()
{
    // seed the random number generator
    rnd.set_seed(cast_to_string(time(0)));

    // Pick out some of the gates we will be using below
    using namespace dlib::quantum_gates;
    const gate<1> h = quantum_gates::hadamard();
    const gate<1> z = quantum_gates::z();
    const gate<1> x = quantum_gates::x();
    const gate<1> i = quantum_gates::noop();

    quantum_register reg;

    // We will be doing the 12 qubit version of Grover's search algorithm.
    const int bits=12;
    reg.set_num_bits(bits);

    // set the quantum register to its initial state
    (i,i, i,i,i,i,i, i,i,i,i,x).apply_gate_to(reg);

    // Print out the starting bits
    cout << "starting bits: ";
    for (int i = reg.num_bits()-1; i >= 0; --i)
        cout << reg.probability_of_bit(i);
    cout << endl;

    // Now apply the Hadamard gate to all the input bits
    (h,h, h,h,h,h,h, h,h,h,h,h).apply_gate_to(reg);

    // Here we do the grover iteration
    for (int j = 0; j < 35; ++j)
    {
        (uf_gate<bits>()).apply_gate_to(reg);
        (w_gate<bits-1>(),i).apply_gate_to(reg);

        cout << j << " probability: bit 1 = " << reg.probability_of_bit(1) << ", bit 9 = " << reg.probability_of_bit(9) << endl;
    }

    cout << endl;

    // Print out the final probability of measuring a 1 for each of the bits
    for (int i = reg.num_bits()-1; i >= 1; --i)
        cout << "probability for bit " << i << " = " << reg.probability_of_bit(i) << endl;
    cout << endl;

    cout << "The value we want grover's search to find is 257 which means we should measure a bit pattern of 00100000001" << endl;
    cout << "Measured bits: ";
    // finally, measure all the bits and print out what they are.
    for (int i = reg.num_bits()-1; i >= 1; --i)
        cout << reg.measure_bit(i,rnd);
    cout << endl;

    // Now lets test out the Shor 9 bit encoding
    cout << "\n\n\n\nNow lets try playing around with Shor's 9bit error correcting code" << endl;

    // Reset the quantum register to contain a single bit
    reg.set_num_bits(1);
    // Set the state of this single qubit to some random mixture of the two computational bases
    reg.state_vector()(0) = qc_scalar_type(rnd.get_random_double(),rnd.get_random_double());
    reg.state_vector()(1) = qc_scalar_type(rnd.get_random_double(),rnd.get_random_double());
    // Make sure the state of the quantum register is a unit vector
    reg.state_vector() /= sqrt(sum(norm(reg.state_vector())));

    cout << "state: " << trans(reg.state_vector());

    shor_encode(reg);
    cout << "x bit corruption on bit 8" << endl;
    (x,i,i,i,i,i,i,i,i).apply_gate_to(reg); // mess up the high order bit
    shor_decode(reg); // try to decode the register

    cout << "state: " << trans(reg.state_vector());

    shor_encode(reg);
    cout << "x bit corruption on bit 1" << endl;
    (i,i,i,i,i,i,i,x,i).apply_gate_to(reg);
    shor_decode(reg);

    cout << "state: " << trans(reg.state_vector());

    shor_encode(reg);
    cout << "z bit corruption on bit 8" << endl;
    (z,i,i,i,i,i,i,i,i).apply_gate_to(reg);
    shor_decode(reg);

    cout << "state: " << trans(reg.state_vector());

    cout << "\nThe state of the input qubit survived all the corruptions in tact so the code works." << endl;

}


#include <cstdlib>
#include <cstdio>
#include <random>
#include <sstream>
#include <string>
#include <iostream>

#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>

using boost::uuids::uuid;

struct Test
{
    uuid id;
    boost::random::mt19937_64 rng;
    boost::random::uniform_real_distribution<double> dist01;

    Test(boost::random::mt19937_64 engine, boost::random::uniform_real_distribution<double> dist) : id(boost::uuids::random_generator()()), rng(engine), dist01(dist) {};
    ~Test(){};

    double Shift(double delta=0.75)
    {
        return dist01(rng);
    }
};

int main()
{
    boost::random::random_device rd;
    boost::random::mt19937_64 engine(rd);
    boost::random::uniform_real_distribution<double> dist(0,1);
    std::string id;

    Test testStruct(engine, dist);
    id = boost::uuids::to_string(testStruct.id);

    // Converting from UUID -> stringstream -> printf'able output is rough. Use std::cout instead
    // printf("UUID: %s",id.c_str());
    std::cout << "UUID: " << id << "\n";
    for(int i = 0; i < 16; i++)
    {
        printf("%f\n", testStruct.Shift());
    }

    return 0;
}


#ifndef MYPOISSON_HH
#define MYPOISSON_HH

#include <boost/math/distributions/poisson.hpp>

double poissonpdf( const double rate, int counts )
{
  if ( rate == 0 ) {
    return ( counts == 0 );
  } else {
    return boost::math::pdf( boost::math::poisson( rate ), counts );
  }
}

#endif


///////////////////////////////////////////////////////////////////////////////
// distribution::model::likelihood::iid::log_unnormalized_pdf.hpp    	     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_MODELS_LIKELIHOOD_IID_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_MODELS_LIKELIHOOD_IID_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <boost/statistics/detail/distribution/model/models/likelihood/iid/iid.hpp>
#include <boost/statistics/detail/distribution/model/models/likelihood/iid/detail/lpdf.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace model{

    template<typename M,typename D,typename P>
    typename distribution::model::detail::lpdf_iid_likelihood<M,D>::value_type
    log_unnormalized_pdf(
        const distribution::model::iid_likelihood<M,D>& l,
        const P& p
    )
    {
        typedef distribution::model::detail::lpdf_iid_likelihood<M,D> lpdf_;
        return lpdf_::call(l,p);
    }

}// model
}// distribution
}// detail
}// statistics
}// boost

#endif


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <random>
#include <chrono>

typedef Eigen::Triplet<float, uint32_t> triplet_t;
typedef std::vector<triplet_t> triplets_t;
typedef Eigen::SparseMatrix<float, Eigen::RowMajor> sparse_mat_t;
typedef Eigen::VectorXf vec_t;

int main() {
  int rows = 100 * 1000;
  int cols = 1000 * 1000;
  int nonzeros_per_row = 6000;
  int num_nonzeros = rows * nonzeros_per_row;

  std::random_device rd;  //Will be used to obtain a seed for the random number engine
  std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
  std::uniform_int_distribution<float> col_dist(0, cols - 1);
  std::uniform_real_distribution<float> val_dist(0, 99);

  triplets_t trips;
  trips.reserve(num_nonzeros);
  for (int i = 0; i < rows; ++i) {
    if (i % 10000 == 0) std::cout << "row = " << i << std::endl;
    for (int n = 0; n < nonzeros_per_row; ++n) {
      int j = col_dist(gen);
      int val = val_dist(gen);
      trips.emplace_back(i, j, val);
    }
  }

  std::cout << "setting from triplets, with non-zero entries = " << trips.size() << std::endl;

  sparse_mat_t x(rows, cols);
  x.setFromTriplets(trips.begin(), trips.end());

  std::cout << "x.size() = " << x.size() << std::endl;

  vec_t beta(cols);
  for (int i = 0; i < cols; ++i) {
    beta(i) = val_dist(gen);
  }

  std::cout << "start multiply" << std::endl;
  auto t1 = std::chrono::high_resolution_clock::now();
  vec_t mu = x * beta;
  auto t2 = std::chrono::high_resolution_clock::now();
  std::cout << "done multiplying" << std::endl;
  std::chrono::duration<double, std::milli> dur = t2 - t1;
  std::cout << "multiply time = " << dur.count() << "ms" << std::endl;

  std::cout << "mu = " << mu(100) << ", " << mu(1000) << std::endl;

  std::cout << "Fini." << std::endl;
}