#ifndef _NAIVE_BAYES_
#define _NAIVE_BAYES_ 1

#include <string>
#include <vector>
#include <Eigen/Dense>
#include <cmath>
#include <exception>
#include <algorithm>

#define PI 3.141592653589793238462643383279502884L

#define _l_ std::cout<<__LINE__<<std::endl;

typedef struct {
  unsigned label;
  Eigen::VectorXd features;
} ClassificationObject;

typedef struct {
  unsigned label;
  double prior;
  Eigen::VectorXd mean;
  Eigen::MatrixXd covariance;
  double covarianceDeterminant;
  Eigen::MatrixXd covarianceInverse;
} ClassInfo;

ClassInfo
ComputeClassInfo(
    const std::vector<ClassificationObject>& data,
    const unsigned classLabel,
    const unsigned featureDim) {

  ClassInfo classInfo;
  classInfo.label = classLabel;
  classInfo.mean.setZero(featureDim);
  classInfo.covariance.setZero(featureDim, featureDim);

  // compute mean and prior probability at once
  std::vector<ClassificationObject> subset;
  for (const ClassificationObject& classificationObject : data) {
    if (classificationObject.label == classLabel) {
      classInfo.mean += classificationObject.features;
      subset.push_back(classificationObject);
    }
  }
  classInfo.mean /= subset.size();
  classInfo.prior = (double) subset.size() / (double) data.size();

  // covariance matrix
  // Note: I use a nifty trick that is simpler in code (not sure if
  //       simpler computationally). Let $P_i = x_i - mu$, where $i$
  //       corresponds to a particular observation vector. Then
  //       $A$ is the concatenation of all the $P_i$ as column
  //       vectors ($A = [P_1 ... P_m]$). Then
  //       $\frac{1}{m} * A * A^t$ results in the same computations
  //       that create the covariance matrix as more traditional
  //       formulae.
  Eigen::MatrixXd A(featureDim, subset.size());
  for (unsigned j = 0; j < subset.size(); ++j) {
    A.col(j) = subset[j].features - classInfo.mean;
  }

  classInfo.covariance = A * A.transpose();
  classInfo.covariance /= (double) subset.size();

  classInfo.covarianceDeterminant = classInfo.covariance.determinant();
  classInfo.covarianceInverse = classInfo.covariance.inverse();

  return classInfo;
}

double
GaussianPdf(
  const Eigen::VectorXd& testFeatureVector,
  const ClassInfo& classSummary) {

  const Eigen::VectorXd& x = testFeatureVector;
  const Eigen::VectorXd& mu = classSummary.mean;
  const double& sigmaDet = classSummary.covarianceDeterminant;
  const Eigen::MatrixXd& sigmaInv = classSummary.covarianceInverse;

  double scalingFactor = 1.0 / sqrt(pow(2.0 * PI, mu.size()) * sigmaDet);
  double exponent = -0.5 * (((x - mu).transpose() * sigmaInv).dot((x - mu)));

  return scalingFactor * exp(exponent);
}

unsigned
ClassifyObject(const ClassificationObject& object, const std::vector<ClassInfo>& classSummaries) {
  unsigned mostLikelyClass = 0;
  double highestProbability = classSummaries.front().label;

  for (const ClassInfo& classSummary : classSummaries) {
    double probability = GaussianPdf(object.features, classSummary) * classSummary.prior;
    if (probability > highestProbability) {
      mostLikelyClass = classSummary.label;
      highestProbability = probability;
    }
  }

  return mostLikelyClass;
}

#endif //_NAIVE_BAYES_


#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>

// 李群李代数 库
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

#include<stdio.h>
#include"mex.h"

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
    // nlhs represent the number of parameters of the output
    // plhs is a array of the mxarray pointers, each pointing to the output
    // nrhs represents the number of parameters of the input
    // prhs is a array of the mxarray pointers, each pointing to the input

    // prhs[0], 6x1 matrix
    // prhs[1], Mx1 cell, each cell with NX3 points
    // prhs[2], Mx1 cell, each cell with PXQ single matrix
    // prhs[3], 1x2, or 1x3, or 1x4, or 1x5 matrix
    // prhs[4], 3x3 matrix
    // prhs[5], 1x2 matrix

    if(nrhs < 1){
        mexErrMsgIdAndTxt( "euler2se3Mex:invalidNumInputs", "at least 1 input arguments required");
        return;
    }

    // get the euler transformation
    const size_t *dimArrayOfEulerTransformation = mxGetDimensions(prhs[0]);
    size_t sizeRowsEulerTransformation = *(dimArrayOfEulerTransformation + 0);
    size_t sizeColsEulerTransformation = *(dimArrayOfEulerTransformation + 1);
    if(sizeRowsEulerTransformation != 6 || sizeColsEulerTransformation != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 1st param should be 6x1");
        return;
    }
    double *ptrEulerTransformation = (double *)(mxGetPr(prhs[0]));
    Eigen::Matrix<double, 6, 1> eulerTransformation;
    for(int i = 0; i < 6; i++){
        eulerTransformation(i, 0) = *(ptrEulerTransformation + i);
    }

    // cout<<"eulerTransformation = "<<endl<<eulerTransformation<<endl;

    Eigen::Matrix<double,6,1> se3;

    Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_Z_vector ( eulerTransformation(0,0), Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 yaw
    Eigen::AngleAxisd rotation_Y_vector ( eulerTransformation(1,0), Eigen::Vector3d ( 0,1,0 ) );      //沿 Y 轴旋转 pitch
    Eigen::AngleAxisd rotation_X_vector ( eulerTransformation(2,0), Eigen::Vector3d ( 1,0,0 ) );      //沿 X 轴旋转 roll
    R = rotation_Z_vector.toRotationMatrix() * rotation_Y_vector.toRotationMatrix() * rotation_X_vector.toRotationMatrix();

    Eigen::Vector3d t(eulerTransformation(3,0), eulerTransformation(4,0), eulerTransformation(5,0));
    Sophus::SE3<double> SE3_Rt(R, t);           // 从R,t构造SE(3)

    // cout<<"SE3 = "<<endl<<SE3_Rt.matrix()<<endl;

    se3 = SE3_Rt.log();

    // cout<<"se3 = "<<endl<<se3.matrix()<<endl;

    // the se3 will be 6x1
    size_t dimArrayOfSe3[2] = { 6, 1 };
    plhs[0] = mxCreateNumericArray(2, dimArrayOfSe3, mxDOUBLE_CLASS, mxREAL);
    double *ptrSe3 = (double *)mxGetData(plhs[0]);
    for(int i = 0; i < 6; i++){
        ptrSe3[i] = se3(i, 0);
    }
}


#include <vector>

#include <NTL/ZZ.h>

#include "../src/elements/element.hpp"
#include "../src/algorithms/utils.hpp"

/* Example: using the remainder tree algorithm to search for Wolstenholme primes.
 * Wolstenholme's theorem states that choose(2*p - 1, p - 1) = 1 (mod p^3) for prime p.
 * A Wolstenholme prime satisfies the same identity modulo p^4.
 *
 * Suppose we want to use the remainder tree algorithm to aid our search for Wolstenholme primes.
 * Let w_p = choose(2*p - 1, p - 1). We wish to find w_p (mod p^4) for primes p. Define R_1 = 1
 *
 * R_n = 4*n - 2
 *
 * Observe that w_p = ((4*p - 2)/p)*w_{p-1}. Therefore,
 * (R_1 * R_2 ... * R_p)/p! will be w_p
 * Remember the goal is to find w_p (R_p)!/p! (mod p^4). Unfortunately,
 * p! contains a single factor of p which has no inverse modulo p^4.
 *
 * In general to find (n/d) (mod m) where d and m are not coprime, use the following.
 * Suppose q_0*m + r_0 = n/d. For some q_0 and 0 <= r_0 < m.
 * Then multiplying gives q_0*m*d + r_0*d = n
 * If we compute n modulo (m*d), we get q_1*m*d + r_1 = n. Since 0 <= r_0 < m implies 0 <= r_0*d < m*d,
 * then r_1 = r_0*d. The goal is to find r_0, and we do so by computing r_1 and dividing by d.
 *
 * In our case, we should find (R_p)! (mod p^5) and then divide the output by p.
 * Then, reduce modulo p^4 and multiply by the modular inverse of (p-1)!.
 *
 * We will need to do remainder tree twice. The first time, set the multiplicands to be
 * A_0 = 1, and A_n = R_n. Set the moduli to be m_n = n^5 if n is prime, and m_n is 1 otherwise.
 * Then, for each index n in the output, divide the remainder by n, and then reduce modulo (n^4).
 *
 * The second remainder tree, set the multiplicands to be A_0 = A_1 = 1, and A_n = n - 1 and set the
 * moduli to be m_n = n^4 if n is prime, and m_n is 1 otherwise. Then, for each index in the output,
 * set each remainder to be its modular inverse, then multiply by the corresponding index from the first
 * remainder tree. The final vector will contain w_p (mod p^4) for primes p, as desired.
 */

using std::vector;

// Like in the search for Wilson primes, we only need to be able to use integers. See
// wilson.hpp for more explanation on how Elt works and how to wrap types.
using NTL::ZZ;

//The convention here is to generate from lower bound---inclusive to upper---exclusive
vector<Elt<ZZ>> gen_wolstenholme_numerator(long lower, long upper) {
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; ++i) {
        if(i <= 1){
            output[i] = Elt<ZZ>(1);
        }
        else {
            //If you worry that the initializer (4*n - 2) in this case will already overflow,
            //Just initialize on i and do arithmetic there.
            output[i-lower] = Elt<ZZ> (4*i - 2);
        }
    }
    return output;
}

vector<Elt<ZZ>> gen_wolstenholme_denominator(long lower, long upper){
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; ++i) {
        if(i <= 1){
            output[i] = Elt<ZZ>(1);
        }
        else {
            output[i-lower] = Elt<ZZ> (i - 1);
        }
    }
    return output;
}

vector<Elt<ZZ>> gen_fourth_prime_power(long lower, long upper) {
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; i++){
        ZZ n(i);
        if(ProbPrime(n)) { //Technically a sieve is faster & more correct, but shouldn't make a big difference.
                            //This is just an example anyway.
            NTL::power(n, n, 4);
            output[i-lower] = Elt<ZZ>(n);
        }
        else{
            output[i-lower] = Elt<ZZ>(1);
        }
    }
    return output;
}

vector<Elt<ZZ>> gen_fifth_prime_power(long lower, long upper) {
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; i++){
        ZZ n(i);
        if(ProbPrime(n)) {
            power(n, n, 5);
            output[i-lower] = Elt<ZZ>(n);
        }
        else {
            output[i-lower] = Elt<ZZ>(1);
        }
    }
    return output;
}

//TODO: combine the above and actually write a search function that zips the outputs and finds XGCD etc.
//TODO: explain how to modify calculate_factorial and compute V


#include "SDL2/SDL.h"
#include <Eigen/Dense>
#include "stdio.h"
#include <iostream>
#include <vector>
#include <cmath>
// SDL_Window *window;

// lldb (print pow correctly): expr -l objective-c -- @import Darwin

class GlobalSettings
{
  public:
  const static int ScreenResolutionX = 640;
  const static int ScreenResolutionY = 480;
};

struct Ray
{
  Eigen::Vector3d origin;
  Eigen::Vector3d direction;
};

class Object;

struct RayHitResult
{
  Eigen::Vector3d hitPosition;
  bool hit;
  Object* hitObject;
};

class Object
{
  public:
  Eigen::Vector3d color;

  virtual RayHitResult raytrace(Ray ray) = 0;
  virtual const Eigen::Vector3d normalAt(const Eigen::Vector3d pos) = 0;
};

class Sphere : public Object
{
  float radii;
  Eigen::Vector3d position;

  public:
  Sphere (double pRadii, Eigen::Vector3d pPosition, Eigen::Vector3d pColor)
  {
    radii = pRadii; // in meters
    position = pPosition;
    color = pColor;
  }

  virtual const Eigen::Vector3d normalAt(const Eigen::Vector3d pPos)
  {
    return pPos - position;
  }

  virtual RayHitResult raytrace(Ray ray)
  {
    // print the closest colision point
    // see equation at https://en.wikipedia.org/wiki/Line%E2%80%93sphere_intersection
    // - (l * ( o - c) ) +- sqrt( (l * ( o - c))^2 - (o-c)^2 + r^2 )
    double l_o_c =
      ray.direction.x() * ( ray.origin.x() - position.x() ) +
      ray.direction.y() * ( ray.origin.y() - position.y() ) +
      ray.direction.z() * ( ray.origin.z() - position.z() );

    double sqrtValue =
      pow( l_o_c, 2)
      - ( pow(ray.origin.x() - position.x(), 2) +
          pow(ray.origin.y() - position.y(), 2) +
          pow(ray.origin.z() - position.z(), 2)
        )
      + pow(radii, 2);

    RayHitResult hitResult;

    if (sqrtValue < 0)
    {
      hitResult.hit = false;

      return hitResult; //  missed the sphere
    }
    else
    {
      double t = - l_o_c - sqrtValue;

      if (t < 0)
      {
        hitResult.hit = false;
        return hitResult;
      }

      // ray equation
      // R(t) = StartPos + Direction * t
      Eigen::Vector3d hitPosition = ray.origin + ray.direction * t;

      hitResult.hit = true;
      hitResult.hitPosition = hitPosition;
      return hitResult;
    }
  }
};

struct Light
{
  Eigen::Vector3d pos;
  Eigen::Vector3d color;
  double intensity = 70;
};

class World
{
  public:
  std::vector<Object*> sceneObjects;
  Light light;

  void spawnObject()
  {
    light.pos = Eigen::Vector3d(0, -2, 0);
    light.color = Eigen::Vector3d(255, 255, 255);

    sceneObjects.push_back(new Sphere(0.5, Eigen::Vector3d(0.5, 0.8, -8), Eigen::Vector3d(100, 100, 0)));
    sceneObjects.push_back(new Sphere(0.5, Eigen::Vector3d(1.9, 0.3, -9.8), Eigen::Vector3d(0, 100, 0)));
    sceneObjects.push_back(new Sphere(0.5, Eigen::Vector3d(0.9, 0.8, -7.5), Eigen::Vector3d(0, 100, 55)));
  }
};

class Camera
{
  public:
  Eigen::Vector3d pos;
  Eigen::Vector3d dir;
  double viewPlaceDist = -0.5;
  double viewPlaneXsize;  // the size of the rendering place, in meters
  double viewPlaneYsize;  // the size of the rendering place, in meters

  Camera(double pScreenWidth, double pScreenHeight)
  {
    viewPlaneYsize = 0.1;
    viewPlaneXsize = (pScreenWidth/pScreenHeight) * viewPlaneYsize;
  }

  Ray RayAtScreenSpace(double x, double y)
  {
    Ray ray;

    ray.origin = Eigen::Vector3d(0, 0, 0.5); // TODO change this to use the camera position
    ray.direction = Eigen::Vector3d(x * viewPlaneXsize, y * viewPlaneYsize, viewPlaceDist);
    ray.direction.normalize();

    return ray;
  }
};

class Renderer
{
  public:

  SDL_Window *window;
  int windowIndex;
  SDL_Renderer* sdl_renderer;

  World* world;

  Renderer(SDL_Window *window, World *pWorld)
  {
    this->window = window;
    world = pWorld;

    windowIndex = -1; // the index of the rendering driver to initialize, or -1 to initialize the first one supporting the requested flags
    int flags = 0;
    sdl_renderer = SDL_CreateRenderer(window, windowIndex, flags);
  }

  void render()
  {
    SDL_SetRenderDrawColor(sdl_renderer, 255, 0, 0, 1); // If something is FULL red on the screen, it means that pixel was not rendered
    SDL_RenderClear(sdl_renderer);
    SDL_RenderPresent( sdl_renderer );

    Camera camera(GlobalSettings::ScreenResolutionX, GlobalSettings::ScreenResolutionY);

    double screenSpaceXRatio = 1.0 / GlobalSettings::ScreenResolutionX;
    double screenSpaceYRatio = 1.0 / GlobalSettings::ScreenResolutionY;

    for (int y = 0; y < GlobalSettings::ScreenResolutionY; ++y)
    {
      for (int x = 0; x < GlobalSettings::ScreenResolutionX; ++x)
      {
        double screenSpaceX = screenSpaceXRatio * x;
        double screenSpaceY = screenSpaceYRatio * y;
        Ray ray = camera.RayAtScreenSpace(screenSpaceX, screenSpaceY);

        RayHitResult hitResult = findClosestHit(world->sceneObjects, ray);

        if (hitResult.hit)
        {
          // calculating pixel color (SHADER!)
          Eigen::Vector3d normal = hitResult.hitObject->normalAt(hitResult.hitPosition);

          Eigen::Vector3d lightNormalToHitPos = world->light.pos - hitResult.hitPosition;
          double distanceFromLight = lightNormalToHitPos.norm();
          double lightAttenuation = (1/ (1 + 0.1 * distanceFromLight + 0.1 * distanceFromLight * distanceFromLight ));

          lightNormalToHitPos.normalize();
          double lightAngle = lightNormalToHitPos.dot(normal);

          if (lightAngle > 0)
          {
            int r = hitResult.hitObject->color.x() * lightAngle * lightAttenuation * world->light.intensity;
            int g = hitResult.hitObject->color.y() * lightAngle * lightAttenuation * world->light.intensity;
            int b = hitResult.hitObject->color.z() * lightAngle * lightAttenuation * world->light.intensity;
            SDL_SetRenderDrawColor(sdl_renderer, r, g, b, 1);
          }
          else
          {
            SDL_SetRenderDrawColor(sdl_renderer, 0, 0, 0, 1);
          }
        }
        else
        {
          SDL_SetRenderDrawColor(sdl_renderer, 50, 50, 50, 1);
        }

        SDL_RenderDrawPoint(sdl_renderer, x, y);
      }
    }

    SDL_RenderPresent( sdl_renderer );
    std::cout << "done" << std::endl;
  }

  RayHitResult findClosestHit(std::vector<Object*> worldObjects, Ray ray)
  {
    RayHitResult closestHitResult;
    closestHitResult.hitPosition = Eigen::Vector3d(9999, 9999, 9999);
    closestHitResult.hit = false;
    closestHitResult.hitObject = nullptr;

    for (int i = 0; i < worldObjects.size(); ++i)
    {
      Object* sceneObject = world->sceneObjects[i];

      RayHitResult hitResult;
      hitResult = sceneObject->raytrace(ray);

      if (hitResult.hit && hitResult.hitPosition.norm() < closestHitResult.hitPosition.norm())
      {
        closestHitResult = hitResult;
        closestHitResult.hitObject = sceneObject;
      }
    }

    return closestHitResult;
  }
};

void waitUntilQuit()
{
  // A basic main loop to prevent blocking
  bool is_running = true;
  SDL_Event event;
  while (is_running) {
      while (SDL_PollEvent(&event)) {
          if (event.type == SDL_QUIT) {
              is_running = false;
          }
      }
      SDL_Delay(16);
  }
}

int main(int argc, char* argv[])
{
  SDL_Init(SDL_INIT_VIDEO);

  SDL_Window* window = SDL_CreateWindow(
      "Raytracer",
      SDL_WINDOWPOS_CENTERED,
      SDL_WINDOWPOS_CENTERED,
      GlobalSettings::ScreenResolutionX,
      GlobalSettings::ScreenResolutionY,
      SDL_WINDOW_MAXIMIZED | SDL_WINDOW_SHOWN
  );

  if (window == NULL)
  {
      printf("Could not create window: %s\n", SDL_GetError());
      return 1;
  }

  SDL_Delay(1);

  World world;
  world.spawnObject();

  Renderer render(window, &world);
  render.render();

  waitUntilQuit();

  SDL_DestroyWindow(window);
  SDL_Quit();

  return 0;
}


#include <iostream>
#include <iomanip>
#include <time.h>

#include <boost/multiprecision/cpp_dec_float.hpp>

#include "../include/calcPI.h"

#include <omp.h>

using namespace boost::multiprecision;
//namespace mp = boost::multiprecision; // mp::pow

const long long ACCUR = 100000; // Кол-во знаков после запятой
const size_t DIGNUM = ACCUR+10;

typedef number< cpp_dec_float<DIGNUM> > cpp_dec_float_DIGNUM;

/*
Неэффективная реализация этой формулы:
https://i.imgur.com/dE8klLQ.png
*/
void calcPI()
{
    const cpp_dec_float_DIGNUM ONE = 1;
    const cpp_dec_float_DIGNUM TWO = 2;
    cpp_dec_float_DIGNUM pi;
    cpp_dec_float_DIGNUM buff;
    cpp_dec_float_DIGNUM a;
    cpp_dec_float_DIGNUM b;
    cpp_dec_float_DIGNUM diff;
    cpp_dec_float_DIGNUM arith;
    cpp_dec_float_DIGNUM geom;
    cpp_dec_float_DIGNUM series;

    buff = 10;
    buff = pow(buff, -ACCUR);
    const cpp_dec_float_DIGNUM epsilon = buff;

    a = 1;

    buff = sqrt(TWO);
    b = ONE / buff;

    diff = a - b;
    series = 0;

    size_t n = 0;
    while(diff > epsilon)
    {
        ++n;
        arith = (a + b) / TWO;
        geom = sqrt(a*b);

        a = arith;
        b = geom;

        buff = pow(TWO, n+1);
        series += buff * (a*a - b*b);

        diff = a - b;
    }

    buff = 4;
    pi = (buff*a*a) / (ONE - series);

    //std::cout << "n = " << n << std::endl;
    //std::cout << std::setprecision(std::numeric_limits<number< cpp_dec_float<ACCUR> >>::max_digits10) << "pi = " << pi << std::endl;
}

/*
Неточное вычисление числа pi, с помощью этой формулы:
https://i.imgur.com/gAMAToc.png
*/
void calcPI_mul(const unsigned threadNum)
{
    const size_t N = 100000000;
    const size_t blocksize = 50000;
    const unsigned REP_NUM = 50;

    for(unsigned gi = 0; gi < REP_NUM; ++gi)
    {

        long double sum = 0;

        #pragma omp parallel reduction (+: sum) num_threads(threadNum)
        {
            #pragma omp for schedule(dynamic, blocksize) nowait
            for (size_t i = 0; i < N; ++i)
            {
                long double xi;
                xi = (i + 0.5); xi /= N;
                sum += (long double)4 / (1 + xi*xi);
            }
        }
        sum /= N;
        //std::cout << std::setprecision(80) << sum << std::endl;
    }
}


#include <iostream>
#include <boost/multiprecision/float128.hpp>

using namespace std;
using namespace boost::multiprecision;

class Fibonacci{
    float128 phi;
    float128 a;
    // Source : http://www.maths.surrey.ac.uk/hosted-sites/R.Knott/Fibonacci/fibFormula.html
    // Based on the source, we reduce the formula to on phi as 'V' contribute neglagible and its distance from zero keeps on increasing with sequence.

    float128 even_sum(int num){

        // Calculate the Gemotric Sum i.e. Sum = a * ((1+r**n)/(1-r))
        // n = (1 + r **n )
        float128 n = 1 - pow(phi,3*num);
        // d = (1 - r)
        float128 d = 1 - pow(phi,3);
        // Gm of even number = a * (n/d)
        float128 sum = a * (n/d);

        return sum;
    }

public:

    Fibonacci(){
        phi = (1+sqrt(5))/2;
        a = pow(phi,3)/sqrt(5);
    }

    unsigned long long int get_even_sum(int limit){
        int totalSum;
        if(limit <= 2){
            return 0;
        }
        totalSum = limit / 3;

        return static_cast<unsigned long long int>(even_sum(totalSum)+1);
    }
};

int main()
{

    Fibonacci obj;
    int m_input;
    cout<<"Enter the Fibonacci Range : ";
    cin>>m_input;
    cout<<obj.get_even_sum(m_input);
    return 0;
}


#include <Eigen/Dense>
#include <cassert>
#include <iostream>

// To disable assert*() calls, uncomment this line:
// #define NDEBUG

// This represents a triplet of indices as a column vector of nonnegative
// integers:
// [ i ]
// [ j ]
// [ k ]
typedef Eigen::Matrix<std::size_t, 3, 1> GridIndices;
//
// Returns the indices of the grid cell containing the point |p| for a grid with
// lower corner |lc| and grid cell width (spacing) |dx|.
//
// If |dx| <= 0 or |p|'s location relative to |lc| would result in negative
// indices being returned and assertions are on, then assertion failures will
// crash this program.
inline GridIndices floor(const Eigen::Vector3d& p, const Eigen::Vector3d& lc,
                         double dx) {
  // Ensure grid spacings are positive.
  assert(dx > 0.0);

  // Compute |p|'s location relative to |lc|.
  // Dividing by |dx| yields a 3D vector indicating the number of grid
  // cells (including fractions of grid cells, as the vector elements are
  // floating-point values) away from |lc| that |p| is located.
  Eigen::Vector3d p_lc_over_dx = (p - lc) / dx;

  // Ensure we won't end up with negative indices.
  assert(p_lc_over_dx[0] >= 0.0);
  assert(p_lc_over_dx[1] >= 0.0);
  assert(p_lc_over_dx[2] >= 0.0);

  // Indices are valid. Construct and return them.
  // This casts the elements of the vector above as nonnegative integers.
  return p_lc_over_dx.cast<std::size_t>();
}

void print(const GridIndices& indices) {
  std::cout << "Indices: " << std::endl;
  std::cout << indices << std::endl;
}

inline Eigen::Vector3d weights(const Eigen::Vector3d& p,
                               const Eigen::Vector3d& lc, double dx,
                               const GridIndices& indices) {
  return (p - lc) / dx - indices.cast<double>();
}

void PrintPointIndicesAndWeights(const Eigen::Vector3d& p,
                                 const Eigen::Vector3d& lc, double dx) {
  GridIndices p_indices = floor(p, lc, dx);
  print(p_indices);
  Eigen::Vector3d w = weights(p, lc, dx, p_indices);
  std::cout << "Weights = " << std::endl;
  std::cout << w << std::endl << std::endl;
}

int main(int argc, char** argv) {
  // Let's have our grid's lower corner be at (3, 4, 5) with a grid spacing of
  // 2. So, each grid cell will be a 2 x 2 x 2 cube.
  Eigen::Vector3d lc(3, 4, 5);
  double dx = 2.0;

  // Expect (i, j, k) == (0, 3, 4), (w0, w1, w2) == (0.5, 0, 0.5)
  Eigen::Vector3d p1(4, 10, 14);
  PrintPointIndicesAndWeights(p1, lc, dx);

  // Expect (i, j, k) == (3, 4, 16), (w0, w1, w2) == (0.5, 0.5, 0)
  Eigen::Vector3d p2(10, 13, 37);
  PrintPointIndicesAndWeights(p2, lc, dx);

  // Expect (i, j, k) == (0, 0, 0), (w0, w1, w2) == (0, 0, 0)
  Eigen::Vector3d p3(3, 4, 5);
  PrintPointIndicesAndWeights(p3, lc, dx);
  return 0;
}


//
//  GivensQR.hpp
//  IPC
//
//  Created by Minchen Li on 10/19/19.
//

#include "Types.hpp"

#include <Eigen/Eigen>

namespace IPC {

template <int dim>
class GivensQR {
public:
    static void compute(const Eigen::Matrix<double, dim, dim>& A,
        Eigen::Matrix<double, dim, dim>& Q,
        Eigen::Matrix<double, dim, dim>& R)
    {
        R = A;
        Q.setIdentity();
        for (int j = 0; j < dim; ++j) {
            for (int i = dim - 1; i > j; --i) {
                double a = R(i - 1, j), b = R(i, j);
                double d = a * a + b * b;
                double c = 1, s = 0;
                double sqrtd = std::sqrt(d);
                if (sqrtd) {
                    double t = 1.0 / sqrtd;
                    c = a * t;
                    s = -b * t;
                }

                for (int k = 0; k < dim; ++k) {
                    double tau1 = R(i - 1, k);
                    double tau2 = R(i, k);
                    R(i - 1, k) = c * tau1 - s * tau2;
                    R(i, k) = s * tau1 + c * tau2;
                }

                for (int k = 0; k < dim; ++k) {
                    double tau1 = Q(i - 1, k);
                    double tau2 = Q(i, k);
                    Q(i - 1, k) = c * tau1 - s * tau2;
                    Q(i, k) = s * tau1 + c * tau2;
                }
            }
        }
        Q.transposeInPlace();
    }
};

} // namespace IPC


/***************************************************************************
 * tiny.cpp    Blitz++ TinyVector<T,N> ray reflection example
 *
 * This example illustrates the TinyVector<T,N> class.  TinyVectors can be
 * used for small vectors whose sizes are known at compile time.  Most
 * operations on TinyVectors have their loops unravelled inline using template
 * metaprograms.
 *
 * The routine reflect(..) calculates the reflection of a monochrome ray
 * of light bouncing off a perfectly reflective, smooth surface.
 ****************************************************************************/

#include <blitz/array.h>
#include <blitz/tinyvec2.h>

BZ_USING_NAMESPACE(blitz)

void reflect(TinyVector<double,3>& reflection, const TinyVector<double,3>& ray,
    const TinyVector<double,3>& surfaceNormal)
{
    // The surface normal must be unit length to use this equation.

    reflection = ray - 2 * dot(ray,surfaceNormal) * surfaceNormal;
}

int main()
{
    TinyVector<double,3> x, y, z;

    // y will be the incident ray
    y[0] = 1;
    y[1] = 0;
    y[2] = -1;

    // z is the surface normal
    z[0] = 0;
    z[1] = 0;
    z[2] = 1;

    reflect(x, y, z);

    cout << "Reflected ray is: [ " << x[0] << " " << x[1] << " " << x[2]
         << " ]" << endl;
}

// Here's the assembly generated for reflect() using KCC +K3 -O2 on an
// IBM RS/6000:

// .reflect__(mangled-name)
//        lfd     fp0,16(r4)
//        lfd     fp1,16(r5)
//        lfd     fp3,8(r4)
//        fm      fp1,fp0,fp1
//        lfd     fp0,8(r5)
//        lfd     fp2,0(r4)
//        fma     fp0,fp3,fp0,fp1
//        lfd     fp1,0(r5)
//        fma     fp0,fp2,fp1,fp0
//        fa      fp0,fp0,fp0
//        fnms    fp1,fp1,fp0,fp2
//        stfd    fp1,0(r3)
//        lfd     fp1,8(r4)
//        lfd     fp2,8(r5)
//        fnms    fp1,fp0,fp2,fp1
//        stfd    fp1,8(r3)
//        lfd     fp1,16(r4)
//        lfd     fp2,16(r5)
//        fnms    fp0,fp0,fp2,fp1
//        stfd    fp0,16(r3)
//        bcr     BO_ALWAYS,CR0_LT


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

#include <boost/math/tools/fraction.hpp>
#include <iostream>
#include <complex>

//[golden_ratio_1
template <class T>
struct golden_ratio_fraction
{
   typedef T result_type;

   result_type operator()()
   {
      return 1;
   }
};
//]

//[cf_tan_fraction
template <class T>
struct tan_fraction
{
private:
   T a, b;
public:
   tan_fraction(T v)
      : a(-v * v), b(-1)
   {}

   typedef std::pair<T, T> result_type;

   std::pair<T, T> operator()()
   {
      b += 2;
      return std::make_pair(a, b);
   }
};
//]
//[cf_tan
template <class T>
T tan(T a)
{
   tan_fraction<T> fract(a);
   return a / continued_fraction_b(fract, std::numeric_limits<T>::epsilon());
}
//]
//[cf_expint_fraction
template <class T>
struct expint_fraction
{
   typedef std::pair<T, T> result_type;
   expint_fraction(unsigned n_, T z_) : b(z_ + T(n_)), i(-1), n(n_) {}
   std::pair<T, T> operator()()
   {
      std::pair<T, T> result = std::make_pair(-static_cast<T>((i + 1) * (n + i)), b);
      b += 2;
      ++i;
      return result;
   }
private:
   T b;
   int i;
   unsigned n;
};
//]
//[cf_expint
template <class T>
inline std::complex<T> expint_as_fraction(unsigned n, std::complex<T> const& z)
{
   boost::uintmax_t max_iter = 1000;
   expint_fraction<std::complex<T> > f(n, z);
   std::complex<T> result = boost::math::tools::continued_fraction_b(
      f,
      std::complex<T>(std::numeric_limits<T>::epsilon()),
      max_iter);
   result = exp(-z) / result;
   return result;
}
//]
//[cf_upper_gamma_fraction
template <class T>
struct upper_incomplete_gamma_fract
{
private:
   typedef typename T::value_type scalar_type;
   T z, a;
   int k;
public:
   typedef std::pair<T, T> result_type;

   upper_incomplete_gamma_fract(T a1, T z1)
      : z(z1 - a1 + scalar_type(1)), a(a1), k(0)
   {
   }

   result_type operator()()
   {
      ++k;
      z += scalar_type(2);
      return result_type(scalar_type(k) * (a - scalar_type(k)), z);
   }
};
//]
//[cf_gamma_Q
template <class T>
inline std::complex<T> gamma_Q_as_fraction(const std::complex<T>& a, const std::complex<T>& z)
{
   upper_incomplete_gamma_fract<std::complex<T> > f(a, z);
   std::complex<T> eps(std::numeric_limits<T>::epsilon());
   return pow(z, a) / (exp(z) *(z - a + T(1) + boost::math::tools::continued_fraction_a(f, eps)));
}
//]

int main()
{
   using namespace boost::math::tools;

   //[cf_gr
   golden_ratio_fraction<double> func;
   double gr = continued_fraction_a(
      func,
      std::numeric_limits<double>::epsilon());
   std::cout << "The golden ratio is: " << gr << std::endl;
   //]

   std::cout << tan(0.5) << std::endl;

   std::complex<double> arg(3, 2);
   std::cout << expint_as_fraction(5, arg) << std::endl;

   std::complex<double> a(3, 3), z(3, 2);
   std::cout << gamma_Q_as_fraction(a, z) << std::endl;

   return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "AlgorithmUtils.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>
#include <map>

namespace fluid {
namespace algorithm {

class DistanceFuncs
{

public:
  enum class Distance {
    kManhattan,
    kEuclidean,
    kSqEuclidean,
    kMax,
    kMin,
    kKL,
    kCosine
  };

  using ArrayXcd = Eigen::ArrayXcd;
  using ArrayXd = Eigen::ArrayXd;
  using MatrixXd = Eigen::MatrixXd;
  using DistanceFuncsMap =
      std::map<Distance, std::function<double(ArrayXd, ArrayXd)>>;

  static DistanceFuncsMap& map()
  {
    static DistanceFuncsMap _funcs = {
        {Distance::kManhattan,
         [](ArrayXd x, ArrayXd y) { return (x - y).abs().sum(); }},
        {Distance::kEuclidean,
         [](ArrayXd x, ArrayXd y) {
           return std::sqrt((x - y).square().sum());
         }},
        {Distance::kSqEuclidean,
         [](ArrayXd x, ArrayXd y) { return (x - y).square().sum(); }},
        {Distance::kMax,
         [](ArrayXd x, ArrayXd y) { return (x - y).abs().maxCoeff(); }},
        {Distance::kMin,
         [](ArrayXd x, ArrayXd y) { return (x - y).abs().minCoeff(); }},
        {Distance::kKL,
         [](ArrayXd x, ArrayXd y) {
           auto   logX = x.max(epsilon).log(), logY = y.max(epsilon).log();
           double d1 = (x * (logX - logY)).sum();
           double d2 = (y * (logY - logX)).sum();
           return d1 + d2;
         }},
        {Distance::kCosine, [](ArrayXd x, ArrayXd y) {
           double norm = x.matrix().norm() * y.matrix().norm();
           double dot = x.matrix().dot(y.matrix());
           return dot / norm;
         }}};
    return _funcs;
  }
};

Eigen::MatrixXd DistanceMatrix(Eigen::Ref<Eigen::MatrixXd> X, index distance)
{
  auto            dist = static_cast<DistanceFuncs::Distance>(distance);
  Eigen::MatrixXd D = Eigen::MatrixXd::Zero(X.rows(), X.rows());
  for (index i = 0; i < X.rows(); i++)
  {
    for (index j = 0; j < X.rows(); j++)
    {
      D(i, j) = DistanceFuncs::map()[dist](X.row(i).array(), X.row(j).array());
    }
  }
  return D;
}

template <typename Derived>
Eigen::MatrixXd DistanceMatrix(const Eigen::PlainObjectBase<Derived>& X,
                               const Eigen::PlainObjectBase<Derived>& Y,
                               index                                  distance)
{
  auto            dist = static_cast<DistanceFuncs::Distance>(distance);
  Eigen::MatrixXd D = Eigen::MatrixXd::Zero(X.rows(), Y.rows());
  for (index i = 0; i < X.rows(); i++)
  {
    for (index j = 0; j < Y.rows(); j++)
    {
      D(i, j) = DistanceFuncs::map()[dist](X.row(i).array(), Y.row(j).array());
    }
  }
  return D;
}

} // namespace algorithm
} // namespace fluid


//GlobalFunctions.cpp

//Modification Date: 6/22/15

#include <boost/math/distributions/normal.hpp>
#include <cmath>
#include <stdio.h>
#include <iostream>
#include <vector>
#include "GlobalFunctions.hpp"
using namespace std;

double CallPrice(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return (S * exp((b - r)*T) * N(d1)) - (K * exp(-r * T)* N(d2));
}

double CallPricePCP(double P, double S, double K, double T, double r, double b) {
	return P + S*exp((b-r)*T) - K*exp(-r*T);
}

double PutPrice(double S, double K, double T, double r, double sig, double b){
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return (K * exp(-r*T) * N(-d2)) - (S * exp((b - r) * T) * N(-d1));
}

double PutPricePCP(double C, double S, double K, double T, double r, double b) {
	return C - S*exp((b - r)*T) + K*exp(-r*T);
}

std::vector<double> MeshArray(double LowerLimit, double UpperLimit, int Num)			//N - amount of steps
{
	vector<double> mesh;
	mesh.reserve(Num+1);
	double h = (UpperLimit - LowerLimit) / Num;

	for (double x = LowerLimit; x <= UpperLimit; x += h)
	{
		mesh.push_back(x);
	}
	return mesh;
}

void PrintVector (const vector<double>& V)
{
	for (vector<double>::const_iterator it = V.begin(); it != V.end(); ++it)
		cout << *it << ", ";
}

double N(double x) {
	boost::math::normal_distribution<> myNormal(0.0, 1.0);
	return boost::math::cdf(myNormal, x);
}

double n(double x) {
	boost::math::normal_distribution<> myNormal(0.0, 1.0);
	return boost::math::pdf(myNormal, x);
}

double CallDelta(double S, double K, double T, double r, double sig, double b)
{
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;

	return exp((b - r)*T) * N(d1);
}

double PutDelta(double S, double K, double T, double r, double sig, double b)
{
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;

	return exp((b - r)*T) * (N(d1) - 1.0);
}

double GammaGF(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;

	return n(d1)*exp((b - r)*T) / (S*tmp);
}

double VegaGF(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;

	return S*sqrt(T)*exp((b - r)*T)*n(d1);
}

double CallTheta(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return -S*sig*exp((b - r)*T)*n(d1) / (2 * sqrt(T)) - (b - r)*S*exp((b - r)*T)*N(d1) - r*K*exp(-r*T)*N(d2);
}

double PutTheta(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return -S*sig*exp((b - r)*T)*n(d1) / (2 * sqrt(T)) - (b - r)*S*exp((b - r)*T)*N(-d1) + r*K*exp(-r*T)*N(-d2);
}

double CallRho(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return 0.01*K*T*exp(- r*T)*N(d2);
}

double PutRho(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return -0.01*K*T*exp(- r*T)*N(-d2);
}

double CallDelta(double S, double h, double K, double T, double r, double sig, double b) {		//Delta Aproximation  using divided difference
	return (CallPrice(S + h, K, T, r, sig, b) - CallPrice(S - h, K, T, r, sig, b)) / (2 * h);
}

double PutDelta(double S, double h, double K, double T, double r, double sig, double b) {		//Delta Aproximation  using divided difference
	return (PutPrice(S + h, K, T, r, sig, b) - PutPrice(S - h, K, T, r, sig, b)) / (2 * h);
}

double GammaGF(double S, double h, double K, double T, double r, double sig, double b) {		//Gamma Aproximation  using divided difference
	return (CallPrice(S + h, K, T, r, sig, b) - 2 * CallPrice(S, K, T, r, sig, b) + CallPrice(S - h, K, T, r, sig, b)) / (h * h);
}

double PerpetualCall(double S, double K, double r, double sig, double b)
{ // Dividend q = r - b

	double sig2 = sig*sig;								//sig to the second power defined for convenience
	double fac = b / sig2 - 0.5; fac *= fac;			//fac to the second power defined for convenience
	double y1 = 0.5 - b / sig2 + sqrt(fac + 2.0*r / sig2);

	if (1.0 == y1)								//no need to calculate the function if y1 equals to 1,
		return S;

	double fac2 = ((y1 - 1.0)*S) / (y1 * K);
	double c = K * pow(fac2, y1) / (y1 - 1.0);

	return c;
}

double PerpetualPut(double S, double K, double r, double sig, double b)
{
	double sig2 = sig*sig;
	double fac = b / sig2 - 0.5; fac *= fac;
	double y2 = 0.5 - b / sig2 - sqrt(fac + 2.0*r / sig2);

	if (0.0 == y2)
		return S;

	double fac2 = ((y2 - 1.0)*S) / (y2 * K);
	double p = K * pow(fac2, y2) / (1.0 - y2);

	return p;
}


#include <Eigen/Core>
#include <iostream>
#include <mathtoolbox/rbf-interpolation.hpp>
#include <random>
#include <vector>

using Eigen::MatrixXd;
using Eigen::Vector2d;
using Eigen::VectorXd;

namespace
{
    std::random_device                     seed;
    std::default_random_engine             engine(seed());
    std::uniform_real_distribution<double> uniform_dist(-1.0, 1.0);

    double CalcFunction(const Vector2d& x) { return std::sin(10.0 * x(0)) + std::sin(10.0 * x(1)); }
} // namespace

int main()
{
    // Generate scattered data (in this case, 500 data points in a 2-dimensional space)
    constexpr int    number_of_samples = 500;
    constexpr double noise_intensity   = 0.1;
    Eigen::MatrixXd  X(2, number_of_samples);
    Eigen::VectorXd  y(number_of_samples);
    for (int i = 0; i < number_of_samples; ++i)
    {
        X.col(i) = Vector2d(uniform_dist(engine), uniform_dist(engine));
        y(i)     = CalcFunction(X.col(i)) + noise_intensity * uniform_dist(engine);
    }

    // Define interpolation settings
    const auto     kernel             = mathtoolbox::ThinPlateSplineRbfKernel();
    constexpr bool use_regularization = true;

    // Instantiate an interpolator
    mathtoolbox::RbfInterpolator rbf_interpolator(kernel);

    // Set data
    rbf_interpolator.SetData(X, y);

    // Calculate internal weights with or without regularization
    rbf_interpolator.CalcWeights(use_regularization);

    // Calculate and print interpolated values on randomly sampled points in CSV format
    constexpr int number_of_test_samples = 100;
    std::cout << "x(0),x(1),y" << std::endl;
    for (int i = 0; i < number_of_test_samples; ++i)
    {
        const Vector2d x = Vector2d(uniform_dist(engine), uniform_dist(engine));
        const double   y = rbf_interpolator.CalcValue(x);

        std::cout << x(0) << "," << x(1) << "," << y << std::endl;
    }

    return 0;
}


/* Functions-3.hpp (exercise 1.5.3)
Description:
	* Functions for exercise 1.5.3.
Functions:
	*tuple<double,double,double,double,double> GetStatisticalPRoperties(const Container&): calculate the mean, mean deviation, range, variance and standard deviation of passed dataset stored in STL container.
	*Type median(const Container<Type, Alloc>&): calculate median of dataset stored in passed sorted STL container.
	*Type mode(const Container<Type, Alloc>&): calculate the mode of dataset stored in passed STL container. If multimodal, then return the smallest mode.
*/

#ifndef FUNCTIONS3_HPP
#define FUNCTIONS3_HPP

#ifndef _SILENCE_ALL_CXX17_DEPRECATION_WARNINGS
	#define _SILENCE_ALL_CXX17_DEPRECATION_WARNINGS
#endif

#include <boost/numeric/ublas/vector.hpp>
#include <boost/range/numeric.hpp>
#include <memory>
#include <numeric>
#include <set>
#include <tuple>
#include "Exception.hpp"
#include "SizeExcept.hpp"

/* 1.5.3: */
// a) Create a function that returns statistical properties of dataset implemented in a boost::vector:
template<typename Container>
std::tuple<double, double, double, double, double> GetStatisticalProperties(const Container &values)
{
	std::size_t numElements = values.size();
	double mean, meanDev = 0, range, var, stdDev;
	if (numElements > 0)
	{
		// Calculate mean:
		mean = std::accumulate(values.begin(), values.end(), 0.0L, [&](double first, double second) { return first + second; }) / (double)numElements;
		// Calculate mean deviation:
		std::for_each(values.begin(), values.end(), [&](double element) { meanDev += std::abs(element - mean) / (double)numElements; });
		// meanDev = std::accumulate(values.begin(), values.end(), 0.0L, [&](double first, double second) { return std::abs(first - mean) + second; }) / (double)numElements;
		// Calculate range:
		range = (double) *std::max_element(values.begin(), values.end()) - (double) *std::min_element(values.begin(), values.end());
		// Calculate the variance (E[(x - mu)^2] = E(x^2) - E(x)^2).
		var = boost::inner_product(values, values, 0.0L) / (double)numElements - mean * mean;
		// Calculate the standard deviation:
		stdDev = std::sqrt(var);
	}
	else
	{
		throw Exceptions::SizeExcept("There must be at least one element in the passed vector.");
	}
	return std::make_tuple<double, double, double, double, double>(std::move(mean), std::move(meanDev), std::move(range), std::move(var), std::move(stdDev));
}
// b) Create a function that returns median and mode of dataset stored in boost::vector:
template<template<typename T, typename> class Container, class Type, class Alloc = std::allocator<Type>>
Type median(const Container<Type, Alloc> &values)											/* Get median of elements in passed sorted container. */
{
	if (values.size() % 2 == 0)
	{
		return values[values.size() / 2]  / 2.0 + values[values.size() / 2 + 1] / 2.0;
	}
	else
	{
		return values[values.size() / 2];
	}
}

template<template<typename, typename> class Container, class Type, class Alloc = std::allocator<Type>>
Type mode(const Container<Type, Alloc> &values)												/* Get mode of elements in passed container. */
{
	std::map<Type, std::size_t> uniqueCounts;
	std::set<Type> modeSet;
	std::size_t modeIndex = 0, maxFreq = 0;
	for (auto elem = values.begin(); elem != values.end(); elem++)
	{
		// Add element to unique list if not present or increment frequency by 1:
		if (uniqueCounts.find(*elem) != uniqueCounts.end())
		{
			uniqueCounts[*elem]++;
		}
		else
		{
			uniqueCounts.insert(std::pair<Type, std::size_t>(*elem, 1));
		}
	}
	if (uniqueCounts.size())
	{
		// Find the maximum frequency:
		std::for_each(uniqueCounts.begin(), uniqueCounts.end(),
		[&](auto elem)
		{
			std::size_t currFreq = std::get<1>(elem);
			if (maxFreq < currFreq)
			{
				maxFreq = currFreq;
			}
		});
		// Put all uniques that share the maximum frequency into the mode-set:
		std::for_each(uniqueCounts.begin(), uniqueCounts.end(),
		[&](auto elem)
		{
			if (std::get<1>(elem) == maxFreq)
			{
				modeSet.emplace(std::get<0>(elem));
			}
		});
		// Return the first element in the set (std::set is in ascending order by default so first element is the smallest mode):
		return *modeSet.begin();
	}
	else
	{
		throw Exceptions::SizeExcept("There must be at least one element in container to calculate the mode. ");
	}
}

#endif

Advanced C++ Course/Benjamin Rutan HW 1 Submission/1.5/1.5/Functions-3.hpp


#include "Classes.h"
#include <Eigen/Dense>
#include <math.h>
using namespace Eigen;

int Wireframe::normalise() {
    double sumx = 0.0;
    double sumy = 0.0;
    double sumz = 0.0;
    for(int i = 0; i < this->vertices.size(); i++) {
        sumx += this->vertices[i].x;
        sumy += this->vertices[i].y;
        sumz += this->vertices[i].z;
    }
    sumx = sumx/(this->vertices.size());
    sumy = sumy/(this->vertices.size());
    sumz = sumz/(this->vertices.size());
    for(int i = 0; i < this->edges.size(); i++) {
        this->edges[i].p1.x -= sumx;
        this->edges[i].p1.y -= sumy;
        this->edges[i].p1.z -= sumz;
        this->edges[i].p2.x -= sumx;
        this->edges[i].p2.y -= sumy;
        this->edges[i].p2.z -= sumz;
    }
    return 0;
}

Wireframe* Wireframe::projectFrame() {
    ///
    /// Project the frame on xz plane
    ///
    double plane[4] = {0,0,1,0};
    Wireframe* projected;
    projected = new Wireframe;
    for(int i = 0; i < this->edges.size(); i++) {
        Edge edge;
        edge.p1 = edges[i].p1.projectPoint(plane);
        edge.p2 = edges[i].p2.projectPoint(plane);
        projected->edges.push_back(edge);
    }
    return projected;
}

int Wireframe::rotateFrame(int type) {
    ///
    /// Rotate the Wireframe by ten degrees
    ///
    Matrix3d rot;
    double theta = 0.2;
    double costheta = cos(theta);
    double sintheta = sin(theta);
    if(type==1) {
        rot << 1, 0, 0,
            0, costheta, sintheta,
            0, -sintheta, costheta;
    }
    if(type==2) {
        rot << costheta, 0, -sintheta,
            0, 1, 0,
            sintheta, 0, costheta;
    }
    if(type==3) {
        rot << 1, 0, 0,
            0, costheta, -sintheta,
            0, sintheta, costheta;
    }
    if(type==4) {
        rot << costheta, 0, sintheta,
            0, 1, 0,
            -sintheta, 0, costheta;
    }
    for(int i = 0; i < this->edges.size(); i++) {
        Vector3d p1(edges[i].p1.x,edges[i].p1.y,edges[i].p1.z);
        Vector3d p2(edges[i].p2.x,edges[i].p2.y,edges[i].p2.z);
        Vector3d p1new = rot*p1;
        Vector3d p2new = rot*p2;
        edges[i].p1.setCoordinates(p1new.x(),p1new.y(),p1new.z());
        edges[i].p2.setCoordinates(p2new.x(),p2new.y(),p2new.z());
    }
    return 0;
}


#pragma once

#include <iostream>
#include <Eigen/Dense>

class EKF {

public:

    EKF(const Eigen::MatrixXd& P0,
        const Eigen::MatrixXd& Q,
        const Eigen::MatrixXd& R,
        const float& dt ) : initialized(false), P_post(P0), P_prio(P0), Q(Q), R(R), dt(dt)
    {}

    void init(const Eigen::VectorXd& x0) {
        x_hat_prio = x0;
        x_hat_post = x0;

        I = Eigen::MatrixXd::Identity(x0.size(), x0.size());

        initialized = true;
    }

    void prediction_update(const Eigen::VectorXd& U) {
        if(!initialized) {
            throw std::runtime_error("Filter is not initialized!");
        }

        // Use the motion model to predict a-priori estimate
        this->x_hat_prio = this->motion_model(this->x_hat_post, U, this->dt);
        Eigen::MatrixXd jF = this->jacobian_F(this->x_hat_prio, U, this->dt);
        P_prio = jF * P_post * jF.transpose() + Q;
    }

    void innovation_update(const Eigen::VectorXd& Z) {
        Eigen::MatrixXd jH = this->jacobian_H(this->x_hat_prio);
        Eigen::VectorXd z_predict = this->observation_model(this->x_hat_prio);

        Eigen::MatrixXd K = P_prio * jH.transpose() * (jH * P_prio * jH.transpose() + R).inverse();
        this->x_hat_post = this->x_hat_prio + K * (Z - z_predict);
        P_post = (I - K * jH) * P_prio;
    }

    // Return the current state
    Eigen::VectorXd get_state() {return x_hat_post;};

    // Return the current state error covariance
    Eigen::MatrixXd get_P() {return P_post;};

    // is the filter initialized?
    bool initialized;

    // // Motion model (user defined)
    std::function<Eigen::VectorXd(const Eigen::VectorXd& x_hat_post,
                                  const Eigen::VectorXd& u_measured,
                                  const float& dt)> motion_model;

    // Observation model (user defined)
    std::function<Eigen::VectorXd(const Eigen::VectorXd&)> observation_model;

    // Jacobian of the motion model
    std::function<Eigen::MatrixXd(const Eigen::VectorXd& x_hat_prio,
                                  const Eigen::VectorXd& u_measured,
                                  const float& dt)> jacobian_F;

    // Jacobian of the observation model
    std::function<Eigen::MatrixXd(const Eigen::VectorXd& x_hat_prio)> jacobian_H;

private:
    // Estimated states
    Eigen::VectorXd x_hat_prio, x_hat_post;

    float dt;

    Eigen::MatrixXd P_post, P_prio, Q, R, I;

};


#pragma once

#include <armadillo>
#include "estructura_capas_red.cpp"

using namespace std;
using namespace arma;

namespace ic {

vec sigmoid(const vec& v)
{
	return 2 / (1 + exp(-0.5 * v)) - 1;
}

vector<vec> salidaMulticapa(const vector<mat>& pesos,
                            const vec& patron)
{
	// Este multicapa va a tener salida lineal sólo en la capa de salida.
	// En las demás capas se va a usar salida sigmoidea.
	vector<vec> ySalidas;

	// Calculo de la salida para la primer capa
	{
		vec v = pesos[0] * join_vert(vec{-1}, patron); // agrega entrada correspondiente al sesgo

		if (pesos.size() != 1) { // Si hay más de una capa, a la salida de la primera
			v = sigmoid(v);      // hay que aplicarle sigmoidea.
		}

		ySalidas.push_back(v);
	}

	// Calculo de las salidas para las demas capas
	for (unsigned int i = 1; i < pesos.size(); ++i) {
		vec v = pesos[i] * join_vert(vec{-1}, ySalidas[i - 1]); // agrega entrada correspondiente al sesgo

		if (i != pesos.size() - 1) { // Para todas las capas menos la última, usar salida sigmoidea.
			v = sigmoid(v);
		}

		ySalidas.push_back(v);
	}

	return ySalidas;
}

double errorCuadraticoMulticapa(const vector<mat>& pesos,
                                const mat& patrones,
                                const mat& salidaDeseada)
{
	double errorCuadraticoTotal = 0;

	for (unsigned int n = 0; n < patrones.n_rows; ++n) {
		vector<vec> ySalidas = salidaMulticapa(pesos, patrones.row(n).t());
		const vec salidaRed = ySalidas.back();

		const double errorCuadraticoPatron = sum(pow(salidaDeseada.row(n).t() - salidaRed, 2));
		errorCuadraticoTotal += errorCuadraticoPatron;
	}

	return errorCuadraticoTotal;
}

double errorRelativoPromedioMulticapa(const vector<mat>& pesos,
                                      const mat& patrones,
                                      const mat& salidaDeseada)
{
	double sumaErroresRelativos = 0;

	for (unsigned int n = 0; n < patrones.n_rows; ++n) {
		vector<vec> ySalidas = salidaMulticapa(pesos, patrones.row(n).t());
		const vec salidaRed = ySalidas.back();

		double sumaParcial = 0;

		for (unsigned int i = 0; i < salidaDeseada.n_cols; ++i) {
			const double errorRelativoPatron = abs(salidaRed(i) - salidaDeseada(n, i)) / abs(salidaDeseada(n, i));
			sumaParcial += errorRelativoPatron;
		}

		// Promedio del error absoluto a lo largo de todas las salidas
		// de este patrón.
		sumaErroresRelativos += sumaParcial / salidaDeseada.n_cols;
	}

	// Promedio del error a lo largo de todos los patrones
	return sumaErroresRelativos / patrones.n_rows * 100;
}

vector<mat> epocaMulticapa(const mat& patrones,
                           const mat& salidaDeseada,
                           double tasaAprendizaje,
                           double inercia,
                           vector<mat> pesos)
{
	//Entrenamiento
	vector<mat> deltaWOld;
	for (unsigned int i = 0; i < pesos.size(); ++i)
		deltaWOld.push_back(zeros(pesos[i].n_rows, pesos[i].n_cols));

	for (unsigned int n = 0; n < patrones.n_rows; ++n) {
		// Calcular las salidas para cada capa
		const vector<vec> ySalidas = salidaMulticapa(pesos, patrones.row(n).t());

		// Calculo del error
		const vec error = salidaDeseada.row(n).t() - ySalidas.back();

		// Calculo retropropagacion
		// Calculo de gradiente error local instantaneo
		vector<vec> delta;
		// Tenemos tantos vectores de deltas como capas
		delta.resize(ySalidas.size());

		// Delta de ultima capa
		delta[delta.size() - 1] = error;
		// Deltas de las capas anteriores
		for (int i = ySalidas.size() - 2; i >= 0; --i) {
			// No participan los pesos correspondientes al sesgo en el cálculo de los deltas
			const mat pesosAux = pesos[i + 1].tail_cols(pesos[i + 1].n_cols - 1);
			delta[i] = pesosAux.t() * delta[i + 1];
		}

		// Actualizacion de pesos de todas las capas menos la primera.
		// Si la red tiene una sola capa, no se entra acá.
		for (int i = pesos.size() - 1; i >= 1; --i) {
			const mat deltaWnuevo = tasaAprendizaje
			                            * delta[i]
			                            * join_horiz(vec{-1}, ySalidas[i - 1].t())
			                        + inercia * deltaWOld[i];
			pesos[i] += deltaWnuevo;
			deltaWOld[i] = deltaWnuevo;
		}

		// Actualización de pesos de la primer capa
		const mat deltaW = tasaAprendizaje
		                       * delta[0]
		                       * join_horiz(vec{-1}, patrones.row(n))
		                   + inercia * deltaWOld[0];
		pesos[0] += deltaW;
		deltaWOld[0] = deltaW;
	}

	return pesos;
} // fin funcion Epoca

tuple<vector<mat>, double, int> entrenarMulticapa(const EstructuraCapasRed& estructura,
                                                  const mat& datos,
                                                  int nEpocas,
                                                  double tasaAprendizaje,
                                                  double inercia,
                                                  double tolErrorRelativoPromedio,
                                                  bool monitoreo = false)
{
    mat partEntrenamiento;
    mat partMonitoreo;

    if (monitoreo) {
        const int nEntrenamiento = datos.n_rows * 0.9;
        partEntrenamiento = datos.head_rows(nEntrenamiento);
        partMonitoreo = datos.tail_rows(datos.n_rows - nEntrenamiento);
    }
    else {
        partEntrenamiento = datos;
    }

	const int nSalidas = estructura(estructura.n_elem - 1);
    const int nEntradas = partEntrenamiento.n_cols - nSalidas;
	const int nCapas = estructura.n_elem;
	// Vamos a tener tantas columnas en salidaDeseada
	// como neuronas en la capa de salida
    const mat salidaDeseada = partEntrenamiento.tail_cols(nSalidas);
	// Extender la matriz de patrones con la entrada correspondiente al umbral
    const mat patrones = partEntrenamiento.head_cols(nEntradas);

	// Inicializar pesos y tasa de error
	vector<mat> pesos;

	// La primer matriz matriz de pesos tiene tantas filas como neuronas en la primer capa
	// y tantas columnas como componentes tiene la entrada, más la entrada correspondiente
	// al sesgo.
	pesos.push_back(randu(estructura(0), nEntradas + 1) - 0.5);

	for (int i = 1; i < nCapas; ++i) {
		// Las siguientes matrices de pesos tienen tantas filas como neuronas en dicha capa
		// y tantas columnas como entradas a esa capa, que van a ser las salidas de
		// la capa anterior mas la entrada correspondiente al sesgo.
		// Las salidas de la capa anterior es igual al nro de neuronas en la capa anterior.
		pesos.push_back(randu(estructura(i), estructura(i - 1) + 1) - 0.5);
	}

	double errorRelativoPromedio;
    double menorErrorMonitoreo = numeric_limits<double>::max();
    vector<mat> mejoresPesos = pesos;
    //    int mejorEpoca = 0;

	// Ciclo de las epocas
	int epoca = 1;
	for (; epoca <= nEpocas; ++epoca) {
		// Ciclo para una época
		pesos = epocaMulticapa(patrones,
		                       salidaDeseada,
		                       tasaAprendizaje,
		                       inercia,
		                       pesos);

		errorRelativoPromedio = errorRelativoPromedioMulticapa(pesos, patrones, salidaDeseada);
        //        cout << epoca << " Error cuadratico entrenamiento: "
        //             << errorCuadraticoMulticapa(pesos, patrones, salidaDeseada) << endl;

        if (monitoreo) {
            const double errorMonitoreo = errorCuadraticoMulticapa(pesos,
                                                                   partMonitoreo.head_cols(nEntradas),
                                                                   partMonitoreo.tail_cols(nSalidas));
            //            cout << "Error cuadratico monitoreo: " << errorMonitoreo << endl;

            if (errorMonitoreo < menorErrorMonitoreo) {
                menorErrorMonitoreo = errorMonitoreo;
                mejoresPesos = pesos;
                //                mejorEpoca = epoca;
            }
        }

        if (errorRelativoPromedio <= tolErrorRelativoPromedio)
            break;
    }
    // Fin ciclo (epocas)

    // Si el bucle anterior no cortó por tolerancia de error,
    // el for va a incrementar la variable una vez de más.
    if (epoca > nEpocas)
        epoca = nEpocas;

    //    cout << "Mejor epoca: " << mejorEpoca << endl;

    if (monitoreo)
        return make_tuple(mejoresPesos, errorRelativoPromedio, epoca);
    else
        return make_tuple(pesos, errorRelativoPromedio, epoca);
}

struct ParametrosMulticapa {
	EstructuraCapasRed estructuraRed;
	int nEpocas;
	double tasaAprendizaje;
	double inercia;
	double toleranciaError;
};
}

istream& operator>>(istream& is, ic::ParametrosMulticapa& parametros)
{
	// Formato:
	// estructura: [3 2 1]
	// n_epocas: 200
	// tasa_entrenamiento: 0.1
	// inercia: 0.5
	// parametro_sigmoidea: 1
	// tolerancia_error: 5
	string str;

	// No chequeamos si la etiqueta de cada línea está bien o no. No nos importa
	is >> str >> parametros.estructuraRed
	    >> str >> parametros.nEpocas
	    >> str >> parametros.tasaAprendizaje
	    >> str >> parametros.inercia
	    >> str >> parametros.toleranciaError;

	// Control básico de valores de parámetros
	if (parametros.nEpocas <= 0
	    || parametros.tasaAprendizaje <= 0
	    || parametros.toleranciaError <= 0
	    || parametros.toleranciaError >= 100)
		is.clear(ios::failbit);

	return is;
}


/**
 * \file RobertBristowJohnsonFilter.hxx
 */

#include "RobertBristowJohnsonFilter.h"

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

namespace ATK
{
  template<typename DataType>
  RobertBristowJohnsonLowPassCoefficients<DataType>::RobertBristowJohnsonLowPassCoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonLowPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 - cosw0)/2 / (1 + alpha);
    coefficients_in[1] = (1 - cosw0) / (1 + alpha);
    coefficients_in[0] = (1 - cosw0) / 2 / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonLowPassCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonLowPassCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonLowPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonHighPassCoefficients<DataType>::RobertBristowJohnsonHighPassCoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonHighPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 + cosw0) / 2 / (1 + alpha);
    coefficients_in[1] = -(1 + cosw0) / (1 + alpha);
    coefficients_in[0] = (1 + cosw0) / 2 / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonHighPassCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonHighPassCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonHighPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandPassCoefficients<DataType>::RobertBristowJohnsonBandPassCoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = Q * alpha / (1 + alpha);
    coefficients_in[1] = 0;
    coefficients_in[0] = - Q * alpha / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPassCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandPassCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandPass2Coefficients<DataType>::RobertBristowJohnsonBandPass2Coefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandPass2Coefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = alpha / (1 + alpha);
    coefficients_in[1] = 0;
    coefficients_in[0] = -alpha / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPass2Coefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandPass2Coefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandPass2Coefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandStopCoefficients<DataType>::RobertBristowJohnsonBandStopCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandStopCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = 1 / (1 + alpha);
    coefficients_in[1] = -2 * cosw0 / (1 + alpha);
    coefficients_in[0] = 1 / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandStopCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandStopCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandStopCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonAllPassCoefficients<DataType>::RobertBristowJohnsonAllPassCoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonAllPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 - alpha) / (1 + alpha);
    coefficients_in[1] = -2 * cosw0 / (1 + alpha);
    coefficients_in[0] = 1;
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonAllPassCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonAllPassCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonAllPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandPassPeakCoefficients<DataType>::RobertBristowJohnsonBandPassPeakCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandPassPeakCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 + alpha * gain) / (1 + alpha / gain);
    coefficients_in[1] = -2 * cosw0 / (1 + alpha / gain);
    coefficients_in[0] = (1 - alpha * gain) / (1 + alpha / gain);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha / gain);
    coefficients_out[0] = (alpha / gain - 1) / (1 + alpha / gain);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    if (gain <= 0)
    {
      throw std::out_of_range("gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  RobertBristowJohnsonLowShelvingCoefficients<DataType>::RobertBristowJohnsonLowShelvingCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonLowShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);
    CoeffDataType d = (gain + 1) + (gain - 1) * cosw0 + 2 * std::sqrt(gain) * alpha;

    coefficients_in[2] = gain * ((gain + 1) - (gain - 1) * cosw0 + 2 * sqrt(gain) * alpha) / d;
    coefficients_in[1] = 2 * gain * ((gain - 1) - (gain + 1) * cosw0) / d;
    coefficients_in[0] = gain * ((gain + 1) - (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
    coefficients_out[1] = 2 * ((gain - 1) + (gain + 1) * cosw0) / d;
    coefficients_out[0] = -((gain + 1) + (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
  }

  template <typename DataType_>
  void RobertBristowJohnsonLowShelvingCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonLowShelvingCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonLowShelvingCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void RobertBristowJohnsonLowShelvingCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    if (gain <= 0)
    {
      throw std::out_of_range("gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonLowShelvingCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonLowShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  RobertBristowJohnsonHighShelvingCoefficients<DataType>::RobertBristowJohnsonHighShelvingCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonHighShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);
    CoeffDataType d = (gain + 1) - (gain - 1) * cosw0 + 2 * std::sqrt(gain) * alpha;

    coefficients_in[2] = gain * ((gain + 1) + (gain - 1) * cosw0 + 2 * sqrt(gain) * alpha) / d;
    coefficients_in[1] = -2 * gain * ((gain - 1) + (gain + 1) * cosw0) / d;
    coefficients_in[0] = gain * ((gain + 1) + (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
    coefficients_out[1] = -2 * ((gain - 1) - (gain + 1) * cosw0) / d;
    coefficients_out[0] = -((gain + 1) - (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
  }

  template <typename DataType_>
  void RobertBristowJohnsonHighShelvingCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonHighShelvingCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonHighShelvingCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void RobertBristowJohnsonHighShelvingCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    if (gain <= 0)
    {
      throw std::out_of_range("gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonHighShelvingCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonHighShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }
}


// MomentTools.cpp
//
// Breannan Smith
// Last updated: 09/14/2015

#include "MomentTools.h"

#include <iostream>

#include <Eigen/Eigenvalues>

static void diagonalizeInertiaTensor( const Matrix3s& I, Matrix3s& R0, Vector3s& I0 )
{
  // Inertia tensor should by symmetric
  assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  // Inertia tensor should have positive determinant
  assert( I.determinant() > 0.0 );

  // Compute the eigenvectors and eigenvalues of the input matrix
  const Eigen::SelfAdjointEigenSolver<Matrix3s> es{ I };

  // Check for errors
  if( es.info() == Eigen::NumericalIssue )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NumericalIssue" << std::endl;
  }
  else if( es.info() == Eigen::NoConvergence )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NoConvergence" << std::endl;
  }
  else if( es.info() == Eigen::InvalidInput )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::InvalidInput" << std::endl;
  }
  assert( es.info() == Eigen::Success );

  // Save the eigenvectors and eigenvalues
  I0 = es.eigenvalues();
  assert( ( I0.array() > 0.0 ).all() );
  assert( I0.x() <= I0.y() );
  assert( I0.y() <= I0.z() );
  R0 = es.eigenvectors();
  assert( fabs( fabs( R0.determinant() ) - 1.0 ) <= 1.0e-6 );

  // Ensure that we have an orientation preserving transform
  if( R0.determinant() < 0.0 )
  {
    R0.col( 0 ) *= -1.0;
  }
}

namespace MomentTools
{

// TODO: most of this function can be vectorized
void computeMoments( const Matrix3Xsc& vertices, const Matrix3Xuc& indices, scalar& mass, Vector3s& I, Vector3s& center, Matrix3s& R )
{
  assert( ( indices.array() < unsigned( vertices.cols() ) ).all() );

  constexpr scalar oneDiv6{ 1.0 / 6.0 };
  constexpr scalar oneDiv24{ 1.0 / 24.0 };
  constexpr scalar oneDiv60{ 1.0 / 60.0 };
  constexpr scalar oneDiv120{ 1.0 / 120.0 };

  // order:  1, x, y, z, x^2, y^2, z^2, xy, yz, zx
  VectorXs integral{ VectorXs::Zero( 10 ) };

  for( int i = 0; i < indices.cols(); ++i )
  {
    // Copy the vertices of triangle i
    const Vector3s v0{ vertices.col( indices( 0, i ) ) };
    const Vector3s v1{ vertices.col( indices( 1, i ) ) };
    const Vector3s v2{ vertices.col( indices( 2, i ) ) };

    // Compute a normal for the current triangle
    const Vector3s N{ ( v1 - v0 ).cross( v2 - v0 ) };

    // Compute the integral terms
    scalar tmp0{ v0.x() + v1.x() };
    scalar tmp1{ v0.x() * v0.x() };
    scalar tmp2{ tmp1 + v1.x() * tmp0 };
    const scalar f1x{ tmp0 + v2.x() };
    const scalar f2x{ tmp2 + v2.x() * f1x };
    const scalar f3x{ v0.x() * tmp1 + v1.x() * tmp2 + v2.x() * f2x };
    const scalar g0x{ f2x + v0.x() * ( f1x + v0.x() ) };
    const scalar g1x{ f2x + v1.x() * ( f1x + v1.x() ) };
    const scalar g2x{ f2x + v2.x() * ( f1x + v2.x() ) };

    tmp0 = v0.y() + v1.y();
    tmp1 = v0.y() * v0.y();
    tmp2 = tmp1 + v1.y() * tmp0;
    const scalar f1y{ tmp0 + v2.y() };
    const scalar f2y{ tmp2 + v2.y() * f1y };
    const scalar f3y{ v0.y() * tmp1 + v1.y() * tmp2 + v2.y() * f2y };
    const scalar g0y{ f2y + v0.y() * ( f1y + v0.y() ) };
    const scalar g1y{ f2y + v1.y() * ( f1y + v1.y() ) };
    const scalar g2y{ f2y + v2.y() * ( f1y + v2.y() ) };

    tmp0 = v0.z() + v1.z();
    tmp1 = v0.z()*v0.z();
    tmp2 = tmp1 + v1.z()*tmp0;
    const scalar f1z{ tmp0 + v2.z() };
    const scalar f2z{ tmp2 + v2.z() * f1z };
    const scalar f3z{ v0.z() * tmp1 + v1.z() * tmp2 + v2.z() * f2z };
    const scalar g0z{ f2z + v0.z() * ( f1z + v0.z() ) };
    const scalar g1z{ f2z + v1.z() * ( f1z + v1.z() ) };
    const scalar g2z{ f2z + v2.z() * ( f1z + v2.z() ) };

    // Update integrals
    integral(0) += N.x() * f1x;
    integral(1) += N.x() * f2x;
    integral(2) += N.y() * f2y;
    integral(3) += N.z() * f2z;
    integral(4) += N.x() * f3x;
    integral(5) += N.y() * f3y;
    integral(6) += N.z() * f3z;
    integral(7) += N.x() * ( v0.y() * g0x + v1.y() * g1x + v2.y() * g2x );
    integral(8) += N.y() * ( v0.z() * g0y + v1.z() * g1y + v2.z() * g2y );
    integral(9) += N.z() * ( v0.x() * g0z + v1.x() * g1z + v2.x() * g2z );
  }

  integral(0) *= oneDiv6;
  integral(1) *= oneDiv24;
  integral(2) *= oneDiv24;
  integral(3) *= oneDiv24;
  integral(4) *= oneDiv60;
  integral(5) *= oneDiv60;
  integral(6) *= oneDiv60;
  integral(7) *= oneDiv120;
  integral(8) *= oneDiv120;
  integral(9) *= oneDiv120;

  // Mass
  mass = integral(0);

  // Center of mass
  center = Vector3s{ integral(1), integral(2), integral(3) } / mass;

  // Inertia relative to world origin
  R(0,0) = integral(5) + integral(6);
  R(0,1) = -integral(7);
  R(0,2) = -integral(9);
  R(1,0) = R(0,1);
  R(1,1) = integral(4) + integral(6);
  R(1,2) = -integral(8);
  R(2,0) = R(0,2);
  R(2,1) = R(1,2);
  R(2,2) = integral(4) + integral(5);

  // Comptue the inertia relative to the center of mass
  R(0,0) -= mass * ( center.y() * center.y() + center.z() * center.z() );
  R(0,1) += mass * center.x() * center.y();
  R(0,2) += mass * center.z() * center.x();
  R(1,0) = R(0,1);
  R(1,1) -= mass * ( center.z() * center.z() + center.x() * center.x() );
  R(1,2) += mass * center.y() * center.z();
  R(2,0) = R(0,2);
  R(2,1) = R(1,2);
  R(2,2) -= mass * ( center.x() * center.x() + center.y() * center.y() );

  // Diagonalize the inertia tensor
  Matrix3s R0;
  diagonalizeInertiaTensor( R, R0, I );
  // Check that we actually diagonalized the inertia tensor
  assert( ( R0 * I.asDiagonal() * R0.transpose() - R ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
  assert( ( R0.transpose() * R * R0 - Matrix3s{ I.asDiagonal() } ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
  R = R0;

  // All inertias should be positive
  assert( ( I.array() > 0.0 ).all() );
  // Check that we have an orthonormal transformation
  assert( ( R * R.transpose() - Matrix3s::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
  assert( fabs( R.determinant() - 1.0 ) <= 1.0e-9 );
}

}


#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ceres/ceres.h"
#include "glog/logging.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;

const double DT = 1.0 / 18;
// const Eigen::Vector3d GRAVITY{0, 0, 0};
const Eigen::Vector3d GRAVITY{0, 0, -9.8};

struct State {
  Eigen::Vector3d pos = Eigen::Vector3d::Random(); // position
  Eigen::Vector3d vel = Eigen::Vector3d::Random(); // velocity
  Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom(); // pose Qwr

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct Measurement{
  Eigen::Matrix3d Rwr;
  Eigen::Quaterniond qwr;
  Eigen::Vector3d twr;
  Eigen::Vector3d acc;
  Eigen::Vector3d omega;

  Measurement(Eigen::Matrix3d Rwr,
              Eigen::Quaterniond qwr,
              Eigen::Vector3d twr,
              Eigen::Vector3d acc,
              Eigen::Vector3d omega)
    : Rwr(Rwr), qwr(qwr), twr(twr), acc(acc), omega(omega) {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct PositionError {
	PositionError(const Eigen::Vector3d& pos_measured)
		: pos_measured_(pos_measured) {}

	template <typename T>
	bool operator()(const T* const pos_hat_ptr,
									T* residuals_ptr) const {
		Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
		Eigen::Matrix<T, 3, 1> pos_delta = pos_hat - pos_measured_.template cast<T>();

    for (int i = 0; i < 3; i++) {
      residuals_ptr[i] = pos_delta[i];
    }
		return true;
	}

	static CostFunction* Create(const Eigen::Vector3d& pos_measured) {
		return new AutoDiffCostFunction<PositionError, 3, 3>(
			new PositionError(pos_measured)
		);
	}

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
	const Eigen::Vector3d pos_measured_;
};

struct PoseError {
  PoseError(const Eigen::Vector3d& pos_measured,
            const Eigen::Quaterniond& q_measured)
    : pos_measured_(pos_measured), q_measured_(q_measured) {}

  template <typename T>
  bool operator()(const T* const pos_hat_ptr,
                  const T* const q_hat_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 6, 1> residuals;

    Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
    Eigen::Matrix<T, 3, 1> pos_delta;
    residuals.template block<3, 1>(0, 0) = pos_hat - pos_measured_.template cast<T>();

    Eigen::Quaternion<T> q_hat(q_hat_ptr);
    Eigen::Quaternion<T> q_delta = q_measured_.conjugate().template cast<T>() * q_hat;
    residuals.template block<3, 1>(3, 0) = q_delta.vec();
    for (int i = 0; i < 6; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& pos_measured,
                              const Eigen::Quaterniond& q_measured) {
    return new AutoDiffCostFunction<PoseError, 6, 3, 4>(
      new PoseError(pos_measured, q_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d pos_measured_;
  const Eigen::Quaterniond q_measured_;
};

struct PredictionError{
  PredictionError(const Eigen::Vector3d& acc_measured,
                  const Eigen::Vector3d& omega_measured)
    : acc_measured_(acc_measured), omega_measured_(omega_measured) {}

  template <typename T>
  bool operator()(const T* const pos_b_ptr,
                  const T* const vel_b_ptr,
                  const T* const q_b_ptr,
                  const T* const pos_e_ptr,
                  const T* const vel_e_ptr,
                  const T* const q_e_ptr,
                  const T* const bias_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 9, 1> residuals;

    // quat error
    const Eigen::Quaternion<T> q_b(q_b_ptr);
    const Eigen::Quaternion<T> q_e(q_e_ptr);

    Eigen::Quaternion<T> q_new;
    Eigen::Quaternion<T> q_add;

    // // https://gamedev.stackexchange.com/questions/108920/applying-angular-velocity-to-quaternion
    // Eigen::Quaternion<T> q_omega;
    // // q_omega.w() = 0;
    // q_omega.vec() = omega_measured_.template cast<T>() * DT * 0.5;
    // q_add = q_omega * q_b;
    // q_new.w() = q_b.w() + q_add.w();
    // q_new.vec() = q_b.vec() + q_add.vec();

    Eigen::Vector3d rotated = omega_measured_ * DT;
    double angle = rotated.norm();
    Eigen::Vector3d axis = rotated.normalized();
    q_add = Eigen::AngleAxisd(angle, axis).template cast<T>();
    q_new = q_b * q_add;

    Eigen::Quaternion<T> q_delta = q_e.conjugate() * q_new;
    residuals.template block<3, 1>(6, 0) = q_delta.vec();

    // vel error
    const Eigen::Matrix<T, 3, 1> bias(bias_ptr);
    const Eigen::Matrix<T, 3, 1> vel_b(vel_b_ptr);
    const Eigen::Matrix<T, 3, 1> vel_e(vel_e_ptr);
    residuals.template block<3, 1>(3, 0) = vel_b + (q_b * (acc_measured_.template cast<T>() - bias) - GRAVITY) * DT - vel_e;

    // pos error
    const Eigen::Matrix<T, 3, 1> pos_b(pos_b_ptr);
    const Eigen::Matrix<T, 3, 1> pos_e(pos_e_ptr);
    residuals.template block<3, 1>(0, 0) = pos_b + vel_b * DT - pos_e;

    for (int i = 0; i < 9; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& acc_measured,
                              const Eigen::Vector3d& omega_measured) {
    return new AutoDiffCostFunction<PredictionError, 9, 3, 3, 4, 3, 3, 4, 3>(
      new PredictionError(acc_measured, omega_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d acc_measured_;
  const Eigen::Vector3d omega_measured_;
};

struct OriPredictionError{
  OriPredictionError(const Eigen::Vector3d& acc_measured,
                  const Eigen::Vector3d& omega_measured)
    : acc_measured_(acc_measured), omega_measured_(omega_measured) {}

  template <typename T>
  bool operator()(const T* const q_b_ptr,
                  const T* const q_e_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 3, 1> residuals;

    // quat error
    const Eigen::Quaternion<T> q_b(q_b_ptr);
    const Eigen::Quaternion<T> q_e(q_e_ptr);

    Eigen::Quaternion<T> q_new;
    Eigen::Quaternion<T> q_add;

    // // https://gamedev.stackexchange.com/questions/108920/applying-angular-velocity-to-quaternion
    // Eigen::Quaternion<T> q_omega;
    // // q_omega.w() = 0;
    // q_omega.vec() = omega_measured_.template cast<T>() * DT * 0.5;
    // q_add = q_omega * q_b;
    // q_new.w() = q_b.w() + q_add.w();
    // q_new.vec() = q_b.vec() + q_add.vec();

    Eigen::Vector3d rotated = omega_measured_ * DT;
    double angle = rotated.norm();
    Eigen::Vector3d axis = rotated.normalized();
    q_add = Eigen::AngleAxisd(angle, axis).template cast<T>();
    q_new = q_b * q_add;

    Eigen::Quaternion<T> q_delta = q_e.conjugate() * q_new;
    residuals.template block<3, 1>(0, 0) = T(2.0) * q_delta.vec();

    for (int i = 0; i < 3; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& acc_measured,
                              const Eigen::Vector3d& omega_measured) {
    return new AutoDiffCostFunction<OriPredictionError, 3, 4, 4>(
      new OriPredictionError(acc_measured, omega_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d acc_measured_;
  const Eigen::Vector3d omega_measured_;
};

std::vector<Measurement, Eigen::aligned_allocator<Measurement>> readSensorData(std::string path) {
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> ret;

  std::ifstream csvFile;
  csvFile.open(path);

  std::string line;
  while(std::getline(csvFile, line)) {
    std::vector<double> row;
    std::cout << "line:" << line << std::endl;
    std::istringstream s(line);
    std::string field;
    while (std::getline(s, field,',')) {
      // std::cout << "field: " << field << std::endl;
      row.push_back(std::stod(field));
    }
    Eigen::Matrix3d Rwr;
    Eigen::Quaterniond qwr;
    Eigen::Vector3d twr;
    Eigen::Vector3d acc;
    Eigen::Vector3d omega;
    Rwr << row[0], row[1], row[2],
          row[4], row[5], row[6],
          row[8], row[9], row[10];
    qwr = Rwr;
    twr << row[3], row[7], row[11];
    acc << row[16], row[17], row[18];
    omega << row[19], row[20], row[21];
    std::cout << "Rwr: " << Rwr << std::endl;
    std::cout << "qwr: " << qwr.w() << " " << qwr.vec() << std::endl;
    std::cout << "twr: " << twr << std::endl;
    std::cout << "acc: " << acc << std::endl;
    std::cout << "omega: " << omega << std::endl;

    ret.push_back(Measurement(Rwr, qwr, twr, acc, omega));
  }

  return ret;
}

void output_pose(const Eigen::Vector3d& pos,
								 const Eigen::Quaterniond& q) {
	Eigen::AngleAxisd ori(q);

	std::cout << "Location: " << pos << std::endl;
	std::cout << "Orientation: " << ori.angle() << " * " << std::endl << ori.axis() << std::endl;
}

void output_measurement(const Measurement& data) {
  std::cout << "\nData State: \n" << "R: \n" << data.Rwr << "\nt: \n" << data.twr \
            << "\nacc: \n" << data.acc << "\nomega: \n" << data.omega << std::endl;
}

void save_states(const std::string& filename,
                 std::vector<State, Eigen::aligned_allocator<State>>& states) {
  std::fstream outfile;
  outfile.open(filename.c_str(), std::istream::out);

  for (auto& state : states) {
    Eigen::Matrix3d rot = state.q.matrix();
    outfile << rot << "\n" << state.pos.transpose() << "\n" << state.vel.transpose() << "\n\n";
  }
}

double abs_pos_error(const std::vector<State, Eigen::aligned_allocator<State>>& states,
                     const std::vector<State, Eigen::aligned_allocator<State>>& gt_states) {
  double err = 0.0;
  std::cout << "abs_pos_err by step: ";
  for (int i = 0; i < states.size(); i++) {
    double step_err = (states[i].pos - gt_states[i].pos).norm();
    err += step_err;
    std::cout << step_err << " ";
  }
  std::cout << std::endl;
  return err;
}

int main(int argc, char** argv) {
  if(argc < 2) {
    std::cout << "missing arg for the csv file" << std::endl;
  }

  std::string path = argv[1];
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> data = readSensorData(path);

  if (false) {
    output_measurement(data[0]);
    output_measurement(data[1]);

    Eigen::Vector3d rotated = data[0].omega * DT;
    double angle = rotated.norm();
    Eigen::Vector3d axis = rotated.normalized();
    Eigen::Quaterniond q_add(Eigen::AngleAxisd(angle, axis));
    Eigen::Quaterniond q_new = data[0].qwr * q_add;
    Eigen::Matrix3d R_new = q_new.normalized().toRotationMatrix();

    std::cout << "R_new: \n" << R_new << std::endl;

    Eigen::Vector3d vel_add = data[0].qwr * data[0].acc * DT;
    std::cout << "vel_new: \n" << vel_add << std::endl;
    return 0;
  }

  int cnt = data.size();
  // int cnt = 3;

  // Eigen::Vector3d bias = Eigen::Vector3d::Random();
  Eigen::Vector3d bias;
  bias << 0, 0, 0;
  std::vector<State, Eigen::aligned_allocator<State>> states(cnt);
  std::vector<State, Eigen::aligned_allocator<State>> gt_states(cnt);
  std::cout << "states size: " << states.size() << std::endl;

  for (int i = 0; i < gt_states.size(); i++) {
    gt_states[i].pos = data[i].twr;
    gt_states[i].vel = Eigen::Vector3d::Zero();
    gt_states[i].q = data[i].qwr;
  }

  Problem problem;

  ceres::LossFunction* loss_function = nullptr;
  ceres::LocalParameterization* quaternion_local_parameterization =
      new ceres::EigenQuaternionParameterization;

  for (int i = 0; i < cnt; i++) {
    ceres::CostFunction* position_cost_function = PositionError::Create(data[i].twr);
    problem.AddResidualBlock(position_cost_function,
                             loss_function,
                             states[i].pos.data());
    // ceres::CostFunction* pos_cost_function = PoseError::Create(data[i].twr, data[i].qwr);
    // problem.AddResidualBlock(pos_cost_function,
    //                          loss_function,
    //                          states[i].pos.data(),
    //                          states[i].q.coeffs().data());
    // problem.SetParameterization(states[i].q.coeffs().data(),
    //                             quaternion_local_parameterization);
    if (i > 0) {
      ceres::CostFunction* pred_cost_function = PredictionError::Create(data[i].acc, data[i].omega);
      problem.AddResidualBlock(pred_cost_function,
                               loss_function,
                               states[i - 1].pos.data(),
                               states[i - 1].vel.data(),
                               states[i - 1].q.coeffs().data(),
                               states[i].pos.data(),
                               states[i].vel.data(),
                               states[i].q.coeffs().data(),
                               bias.data());
      // ceres::CostFunction* pred_cost_function = OriPredictionError::Create(data[i].acc, data[i].omega);
      // problem.AddResidualBlock(pred_cost_function,
      //                          loss_function,
      //                          states[i - 1].q.coeffs().data(),
      //                          states[i].q.coeffs().data());
      problem.SetParameterization(states[i - 1].q.coeffs().data(),
                                  quaternion_local_parameterization);
      problem.SetParameterization(states[i].q.coeffs().data(),
                                  quaternion_local_parameterization);
    }
  }

  // set bias to constant
  // problem.SetParameterBlockConstant(bias.data());
  // Eigen::Quaterniond q_noise(Eigen::AngleAxisd(0.1, Eigen::Vector3d::Random().normalized()));
  // states[0].q = data[0].qwr * q_noise;
  // states[0].q = data[0].qwr;
  // problem.SetParameterBlockConstant(states[0].q.coeffs().data());

  ceres::Solver::Options options;
	options.max_num_iterations = 200;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  // options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";

  if (false) {
    output_measurement(data[0]);
    output_measurement(data[1]);
    output_measurement(data[2]);

    for (int i = 0; i < states.size(); i++) {
      std::cout << "Estimated State:" << i << " \n" << "R: \n" << states[i].q.matrix()
                                              << "\nt: \n" << states[i].pos
                                              << "\nvel: \n" << states[i].vel << std::endl;
    }
  }

  std::string est_filename = "./results/est_no_ori_measure_nofixfirst_states.txt";
  // std::string est_filename = "./results/est_position_predic_ori_states.txt";
  save_states(est_filename, states);
  std::string gt_filename = "./results/gt_states.txt";
  save_states(gt_filename, gt_states);

  double err = abs_pos_error(states, gt_states);
  std::cout << "absolute position error: " << err << std::endl;
  return 0;
}


#include <iostream>

#include <Eigen/Dense>

#include <cppmath/matrix/PseudoInverseSVD.hpp>

using namespace Eigen;
using namespace std;

int main()
{
    const MatrixXd::Index rows = 42;
    const MatrixXd::Index cols = 7;

    // Generate system of linear equations
    MatrixXd A( rows, cols );
    A.setRandom();
    VectorXd x( cols );
    x.setRandom();
    VectorXd b = A * x;
    cout << "x:" << endl << x << endl;
    VectorXd diff;

    // Method call #1
    cppmath::PseudoInverseSVD< MatrixXd > pinv( A );
    MatrixXd Ainv;
    pinv.compute( &Ainv );
    VectorXd xs1 = Ainv * b;
    diff = x - xs1;
    cout << "xs1:" << endl << xs1 << endl;
    cout << "x - xs1: " << diff.squaredNorm() << endl;

    // Methof call #2
    const MatrixXd& Apinv = pinv.compute();
    VectorXd xs2 = Apinv * b;
    diff = x - xs2;
    cout << "xs2:" << endl << xs2 << endl;
    cout << "x - xs2: " << diff.squaredNorm() << endl;

    // Operator call
    VectorXd xs3 = pinv * b;
    diff = x - xs3;
    cout << "xs3:" << endl << xs3 << endl;
    cout << "x - xs3: " << diff.squaredNorm() << endl;

    return 0;
}