#include <cstddef>
#include <sys/time.h>
#include "FHE.h"
#include "EncryptedArray.h"
#include <NTL/ZZX.h>
#include <NTL/ZZ.h>
#include <gmp.h>
#include <omp.h>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <iostream>
#include "Ctxt.h"
#include "polyEval.h"
#include <algorithm>
#include <math.h>

//define how many numbers you want to compare at VECTOR_COUNT
#define VECTOR_COUNT 2
#define VECTOR_SIZE 1

// Simple class to measure time for each method
class Timer
{
public:
    void start() { m_start = my_clock(); }
    void stop() { m_stop = my_clock(); }
    double elapsed_time() const {
        return m_stop - m_start;
    }

private:
    double m_start, m_stop;
    double my_clock() const {
        struct timeval tv;
        gettimeofday(&tv, NULL);
        return tv.tv_sec + tv.tv_usec * 1e-6;
    }
};

//Equality recursive function at page 4
Ctxt z(int i,int j, std::vector<Ctxt> x,std::vector<Ctxt> y, Ctxt enc1){
    if (j==1){
        Ctxt g=x[i];
        g+=y[i];
        g+=enc1;
        return g;
    }
    else {
        int l=ceil(j/2);
        Ctxt f= z(i+l,j-l,x,y, enc1);
        f*=z(i,l,x,y,enc1);
        return f;
    }

}

//Inequality recursive function at page 4
Ctxt t(int i,int j, std::vector<Ctxt> x,std::vector<Ctxt> y,  Ctxt enc1){
    if (j==1){
    Ctxt  H=x[i];
    H*=y[i];
    H+=x[i];
    return H;
    }
    else {
        int l1=ceil(j/2);
        Ctxt we=z(i+l1,j-l1,x,y,enc1);
        we*=t(i,l1,x,y,enc1);
        we+=t(i+l1,j-l1,x,y,enc1);
        return we;
    }
 }

//selection  function with input two numbers (extracted to bits) with output the minimum of two numbers
std::vector<Ctxt> sel(std::vector<Ctxt> digits, std::vector<Ctxt> digits1,std::vector<Ctxt> digitsenc1,std::vector<Ctxt> digitsqw){
    for (int i=0;i<digits1.size();i++){
        Ctxt reset=digitsenc1[0];
        digits1[i]*=digitsqw[0];
        digitsenc1[0]-=digitsqw[0];
        digits[i]*=digitsenc1[0];
        digits1[i]+=digits[i];
        digitsenc1[0]=reset;
    }
    return digits1;

}

//function with input a list of encrypted numbers and output the minimum of them
std::vector<Ctxt> minimum(std::vector<Ctxt> list, Ctxt enc1){
    Ctxt mini=list[0];

    std::vector<Ctxt> digits1;
    extractDigits(digits1, mini);

    for (int i=1;i<list.size();i++){

         std::vector<Ctxt> digitsenc1;
        extractDigits(digitsenc1,enc1);

        std::vector<Ctxt> digitsi;
        extractDigits(digitsi, list[i]);

        Ctxt compute_t=t(0,digits1.size(),digits1,digitsi, enc1);

        std::vector<Ctxt> digitsqw;
        extractDigits(digitsqw, compute_t);
        std::vector<Ctxt> mi=sel( digits1, digitsi, digitsenc1, digitsqw);
        digits1=mi;
    }
    return digits1;

}

int main(int argc, char **argv)
{
    /*** BEGIN INITIALIZATION ***/
    long m = 0;                   // Specific modulus
    long p = 2;                 // Plaintext base [default=2], should be a prime number
    long r = 8;                   // Lifting [default=1]
    long L = 21;                  // Number of levels in the modulus chain [default=heuristic]
    long c = 2;                   // Number of columns in key-switching matrix [default=2]
    long w = 5;                  // Hamming weight of secret key
    long d = 1;                   // Degree of the field extension [default=1]
    long k = 80;                  // Security parameter [default=80]
    long s = 0;                   // Minimum number of slots [default=0]

    Timer tInit;
    tInit.start();

    std::cout << "Finding m... " << std::flush;
    m = FindM(k, L, c, p, d, s, 0);           // Find a value for m given the specified values

    std::cout << "m = " << m << std::endl;

    std::cout << "Initializing context... " << std::flush;
    FHEcontext context(m, p, r); 	          // Initialize context
    buildModChain(context, L, c);             // Modify the context, adding primes to the modulus chain
    std::cout << "OK!" << std::endl;

    std::cout << "Generating keys... " << std::flush;

    fstream pubKeyFile("pk.txt", fstream::out|fstream::trunc);
    assert(pubKeyFile.is_open());
    writeContextBase(pubKeyFile,context);
    pubKeyFile << context << std::endl;

    FHESecKey sk(context);                    // Construct a secret key structure
    const FHEPubKey& pk = sk;                 // An "upcast": FHESecKey is a subclass of FHEPubKey
    sk.GenSecKey(w);                          // Actually generate a secret key with Hamming weight
    //addSome1DMatrices(sk);                    // Extra information for relinearization
    std::cout << "OK!" << std::endl;

    pubKeyFile << pk << std::endl;
    pubKeyFile.close();

    /****INITIALIZATION END****/

    std::ifstream infile("message1.txt");

   //open the message.txt file each line of this txt is a vector

    std::vector< std::vector<int> > e;
    e.resize(VECTOR_COUNT);
    for (int i=0; i<VECTOR_COUNT; i++){
        e[i].resize(VECTOR_SIZE);
    }

    for (int i=0; i<VECTOR_COUNT; i++){
        for (int j=0; j<VECTOR_SIZE ;j++){
            infile >> e[i][j];
        }
    }

    std::cout << "starting"<< std::endl;

    //put the first line vector to u and the second line vector to v

    /*******************************/
    /************CLIENT*************/
    /*******************************/
    long int u,v,y,z;
    u=e[0][0];
    v=e[1][0];
    //y=e[2][0];
    //z=e[3][0];
    std::cout << "u:" << u << std::endl;
    std::cout << "v:" << v << std::endl;
    //std::cout << "y:" << y << std::endl;
    //std::cout << "z:" << z << std::endl;
    std::cout << "encryption of two number from the file message1.txt" << std::endl;

    Ctxt encU(pk),encV(pk),encG(pk),enc1(pk),enc11(pk),enc0(pk),encY(pk),encZ(pk);

    pk.Encrypt(encU,to_ZZX(u));
    pk.Encrypt(encV,to_ZZX(v));
    pk.Encrypt(enc0,to_ZZX(0));
     pk.Encrypt(enc1,to_ZZX(1));
     //pk.Encrypt(encY,to_ZZX(y));
     pk.Encrypt(enc11,to_ZZX(1));
     //pk.Encrypt(encZ,to_ZZX(z));
     std::vector<Ctxt> digits0;
     extractDigits(digits0, enc0);

    //extractdigits of encU and store them at vector:digits
    std::vector<Ctxt> digitsU;
    extractDigits(digitsU, encU);

  	 /*************************************************************/
    /***decrypt each digit of encU to see if extractDigits work*****/
    /***************************************************************/

    long res[digitsU.size()];
    for (int i=0;i<digitsU.size();i++){
    ZZX result;
    sk.Decrypt(result,digitsU[i]);
    if (result[0]>(pow(p,r))/2){ result[0]=result[0]-pow(p,r);}
    res[i]=conv<long>(result[0]);
    }

    std::cout<< "U:";
    size_t res_size = sizeof(res)/sizeof(res[0]);
    std::reverse(res, res + res_size);
    for (int i=0;i<digitsU.size();i++){
        std::cout << res[i] << "," ;
    }
    std::cout << std::endl;

    /****CORRECT******/

    //extractdigits of encV and store them at vector:digitsV
    std::vector<Ctxt> digitsV;
    extractDigits(digitsV, encV);

    std::vector<Ctxt> digitsenc1;
    extractDigits(digitsenc1, enc1);

    /*************************************************************/
    /***decrypt each digit of encV to see if extractDigits work*****/
    /***************************************************************/

    long res1[digitsV.size()];
    for (int i=0;i<digitsV.size();i++){
    ZZX result;
    sk.Decrypt(result,digitsV[i]);
    if (result[0]>(pow(p,r))/2){ result[0]=result[0]-pow(p,r);}
    res1[i]=conv<long>(result[0]);
    }
    size_t res1_size = sizeof(res1)/sizeof(res1[0]);
    std::reverse(res1, res1 + res1_size);
    std::cout<< "V:";
    for (int i=0;i<digitsV.size();i++){
        std::cout << res1[i] << ",";
    }
    std::cout<< std::endl;
    /*******CORRECT******/

     //extractdigits of encV and store them at vector:digitsY
    //td::vector<Ctxt> digitsY;
    //extractDigits(digitsY, encY);

    /*************************************************************/
    /***decrypt each digit of encY to see if extractDigits work*****/
    /***************************************************************/

    /*long dig[digitsY.size()];
    for (int i=0;i<digitsY.size();i++){
     ZZX resultY;
    sk.Decrypt(resultY,digitsY[i]);
    if (resultY[0]>(pow(p,r))/2){ resultY[0]=resultY[0]-pow(p,r);}
    dig[i]=conv<long>(resultY[0]);
    }
    size_t dig_size = sizeof(dig)/sizeof(dig[0]);
    std::reverse(dig, dig + dig_size);
    std::cout<< "Y:";
    for (int i=0;i<digitsY.size();i++){
        std::cout << dig[i] << ",";
    }
    std::cout<< std::endl;
    */

    //input all encrypted numbers on list
    std::vector<Ctxt> list;

        list.push_back(encU);
        list.push_back(encV);
        //list.push_back(encY);
        //list.push_back(encZ);

    Timer timecompare;
    timecompare.start();

   	//calculate the encrypted minimum of the list
    std::vector<Ctxt> kappa=minimum( list, enc1);

     timecompare.stop();

     //decryption of the minimum
    long res2[digitsV.size()];
    for (int i=0;i<digitsV.size();i++){

        ZZX result2;
        sk.Decrypt(result2,kappa[i]);

        res2[i]=conv<long>(result2[0]);
    }
    //the number
    std::cout<< "the minimum number is:";
    int min=0,flow;
    for (int i=0;i<digitsV.size();i++){
        flow=pow(2,i)*res2[i];
        min+=flow;
    }
    std::cout << min << std::endl;
    std::cout << "and his binary form is:" ;
    //the binary form of the number
    size_t res2_size = sizeof(res2)/sizeof(res2[0]);
    std::reverse(res2, res2 + res2_size);
    for (int i=0;i<digitsU.size();i++){
        std::cout << res2[i] << "," ;
    }
    std::cout<< std::endl;
    std::cout << "the time to compute minimum is:" << timecompare.elapsed_time() << std::endl;

    std::cout << std::endl;

     return 0;

}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>
#include <adept_source.h>
#include <adept.h>
using adept::adouble;

template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#include "fft.h"

void foobar(double* data, unsigned len) {
  fft(data, len);
  ifft(data, len);
}

void afoobar(aVector& data, unsigned len) {
  fft(data, len);
  ifft(data, len);
}

extern "C" {
  int enzyme_dupnoneed;
}

static double foobar_and_gradient(unsigned len) {
    double *inp = new double[2*len];
    for(int i=0; i<2*len; i++) inp[i] = 2.0;
    double *dinp = new double[2*len];
    for(int i=0; i<2*len; i++) dinp[i] = 1.0;
    __enzyme_autodiff<void>(foobar, enzyme_dupnoneed, inp, dinp, len);
    double res = dinp[0];
    delete[] dinp;
    delete[] inp;
    return res;
}

static double afoobar_and_gradient(unsigned len) {
    adept::Stack stack;

    aVector x(2*len);
    for(int i=0; i<2*len; i++) x(i) = 2.0;
    stack.new_recording();
    afoobar(x, len);
    for(int i=0; i<2*len; i++)
      x(i).set_gradient(1.0);
    stack.compute_adjoint();

    double *dinp = new double[2*len];
    for(int i=0; i<2*len; i++)
      dinp[i] = x(i).get_gradient();
    double res = dinp[0];
    delete[] dinp;
    return res;
}

static double tfoobar_and_gradient(unsigned len) {
    double *inp = new double[2*len];
    for(int i=0; i<2*len; i++) inp[i] = 2.0;
    double *dinp = new double[2*len];
    for(int i=0; i<2*len; i++) dinp[i] = 1.0;
    foobar_b(inp, dinp, len);
    double res = dinp[0];
    delete[] dinp;
    delete[] inp;
    return res;
}

static void adept_sincos(double inp, unsigned len) {
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Adept real %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector x(2*len);
  for(int i=0; i<2*len; i++) x[i] = 2.0;
 // stack.new_recording();
  afoobar(x, len);
  double res = x(0).value();

  gettimeofday(&end, NULL);
  printf("Adept forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = afoobar_and_gradient(len);

  gettimeofday(&end, NULL);
  printf("Adept combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

static void tapenade_sincos(double inp, unsigned len) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Tapenade real %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double* x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Tapenade forward %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = tfoobar_and_gradient(len);

  gettimeofday(&end, NULL);
  printf("Tapenade combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

static void enzyme_sincos(double inp, unsigned len) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Enzyme real %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Enzyme forward %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = foobar_and_gradient(len);

  gettimeofday(&end, NULL);
  printf("Enzyme combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

/* Function to check if x is power of 2*/
bool isPowerOfTwo (int x)
{
    /* First x in the below expression is for the case when x is 0 */
    return x && (!(x&(x-1)));
}

unsigned max(unsigned A, unsigned B){
  if (A>B) return A;
  return B;
}

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

  if (argc < 2) {
    printf("usage %s n [must be power of 2]\n", argv[0]);
    return 1;
  }
  unsigned N = atoi(argv[1]);
  if (!isPowerOfTwo(N)) {
    printf("usage %s n [must be power of 2]\n", argv[0]);
    return 1;
  }
  double inp = -2.1;

  for(unsigned iters=max(1, N>>5); iters <= N; iters*=2) {
    printf("iters=%d\n", iters);
    adept_sincos(inp, iters);
    tapenade_sincos(inp, iters);
    enzyme_sincos(inp, iters);
  }
}


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

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Basics/timeType.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_time_type )

using namespace mathematical_constants;

//! Test if Time objects cast to the expected precision
BOOST_AUTO_TEST_CASE( testTimeBasicCasts )
{
    Time testTime( 2, LONG_PI );

    //Test if Time casts to double/long double at the expected precision
    {

        BOOST_CHECK_CLOSE_FRACTION( testTime.getSeconds< long double >( ), 2.0L * TIME_NORMALIZATION_TERM + LONG_PI,
                                    std::numeric_limits< long double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( testTime.getSeconds< double >( ), 2.0 * TIME_NORMALIZATION_TERM + PI,
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( static_cast< long double >( testTime ), 2.0L * TIME_NORMALIZATION_TERM + LONG_PI,
                                    std::numeric_limits< long double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( static_cast< double >( testTime ),  2.0 * TIME_NORMALIZATION_TERM + PI,
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test if Time pre-/post-multiplies Eigen vectors at expected level of precision
    {
        Eigen::Vector3d testVector = ( Eigen::Vector3d(  ) << 4.5, 4.5, 4.5 ).finished( );
        Eigen::Matrix< long double, 3, 1 > testVectorLong = ( Eigen::Matrix< long double, 3, 1 >(  ) << 4.5L, 4.5L, 4.5L ).finished( );

        Eigen::Vector3d multipliedTestVector = testTime * testVector;
        Eigen::Matrix< long double, 3, 1 > multipliedTestVectorLong = testTime * testVectorLong;

        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_CLOSE_FRACTION( multipliedTestVector( i ), multipliedTestVectorLong( i ),
                                        std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION( 4.5L * ( 2.0L * TIME_NORMALIZATION_TERM + LONG_PI ), multipliedTestVectorLong( i ),
                                        std::numeric_limits< long double >::epsilon( ) );
        }

        multipliedTestVector = testVector * testTime;
        multipliedTestVectorLong = testVectorLong * testTime;

        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_CLOSE_FRACTION( multipliedTestVector( i ), multipliedTestVectorLong( i ),
                                        std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION( 4.5L * ( 2.0L * TIME_NORMALIZATION_TERM + LONG_PI ), multipliedTestVectorLong( i ),
                                        std::numeric_limits< long double >::epsilon( ) );
        }
    }
}

//! Test if time saves/retrieves entries at the expected level of precision.
BOOST_AUTO_TEST_CASE( testTimeContentsPrecision )
{
    int numberOfDays = 759;
    long double numberOfSeconds = 2.0L * TIME_NORMALIZATION_TERM + LONG_PI;

    Time testTime( numberOfDays, numberOfSeconds );
    BOOST_CHECK_EQUAL( testTime.getFullPeriods( ), numberOfDays + 2 );
    BOOST_CHECK_CLOSE_FRACTION( testTime.getSecondsIntoFullPeriod( ), LONG_PI,
                                TIME_NORMALIZATION_TERM * std::numeric_limits< long double >::epsilon( ) );

    numberOfSeconds = -2.0L * TIME_NORMALIZATION_TERM + LONG_PI;

    Time testTime2 = Time( numberOfDays, numberOfSeconds );
    BOOST_CHECK_EQUAL( testTime2.getFullPeriods( ), numberOfDays - 2 );
    BOOST_CHECK_CLOSE_FRACTION( testTime2.getSecondsIntoFullPeriod( ), LONG_PI,
                                TIME_NORMALIZATION_TERM * std::numeric_limits< long double >::epsilon( ) );

    Time testTime3 = testTime2;
    BOOST_CHECK_EQUAL( testTime2.getSecondsIntoFullPeriod( ), testTime3.getSecondsIntoFullPeriod( ) );
    BOOST_CHECK_EQUAL( testTime2.getFullPeriods( ), testTime3.getFullPeriods( ) );

    numberOfSeconds = -2.0L * TIME_NORMALIZATION_TERM - LONG_PI;

    Time testTime4 = Time( numberOfDays, numberOfSeconds );
    BOOST_CHECK_EQUAL( testTime4.getFullPeriods( ), numberOfDays - 3 );
    BOOST_CHECK_CLOSE_FRACTION( testTime4.getSecondsIntoFullPeriod( ), TIME_NORMALIZATION_TERM - LONG_PI,
                                TIME_NORMALIZATION_TERM * std::numeric_limits< long double >::epsilon( ) );

}

//! Test basic arithmetic operations of time object
BOOST_AUTO_TEST_CASE( testArithmeticOperations )
{
    {
        // Define Time test values
        int numberOfDays1 = 759;
        long double numberOfSeconds1 = 2566.8309405984728595902;
        Time inputTime1( numberOfDays1, numberOfSeconds1 );

        int numberOfDays2 = 2;
        long double numberOfSeconds2 = 1432.48492385475949349;
        Time inputTime2( numberOfDays2, numberOfSeconds2 );

        Time outputTime;

        // Test Time additions
        {
            outputTime = inputTime1 + inputTime2;
            BOOST_CHECK_EQUAL( numberOfDays1 + numberOfDays2 + 1, outputTime.getFullPeriods( ) );
            BOOST_CHECK_CLOSE_FRACTION( numberOfSeconds1 + numberOfSeconds2 - TIME_NORMALIZATION_TERM,
                                        outputTime.getSecondsIntoFullPeriod( ),
                                        std::numeric_limits< long double >::epsilon( ) );
            outputTime = inputTime1;
            outputTime += inputTime2;
            BOOST_CHECK_EQUAL( numberOfDays1 + numberOfDays2 + 1, outputTime.getFullPeriods( ) );
            BOOST_CHECK_CLOSE_FRACTION( numberOfSeconds1 + numberOfSeconds2 - TIME_NORMALIZATION_TERM,
                                        outputTime.getSecondsIntoFullPeriod( ),
                                        std::numeric_limits< long double >::epsilon( ) );
        }

        // Test addition between doubles and Time
        {
            outputTime = inputTime1 + numberOfSeconds2;
            BOOST_CHECK_EQUAL( numberOfDays1 + 1, outputTime.getFullPeriods( ) );
            BOOST_CHECK_CLOSE_FRACTION( numberOfSeconds1 + numberOfSeconds2 - TIME_NORMALIZATION_TERM,
                                        outputTime.getSecondsIntoFullPeriod( ),
                                        std::numeric_limits< long double >::epsilon( ) );

            outputTime = inputTime1;
            outputTime += numberOfSeconds2;
            BOOST_CHECK_EQUAL( numberOfDays1 + 1, outputTime.getFullPeriods( ) );
            BOOST_CHECK_CLOSE_FRACTION( numberOfSeconds1 + numberOfSeconds2 - TIME_NORMALIZATION_TERM,
                                        outputTime.getSecondsIntoFullPeriod( ),
                                        std::numeric_limits< long double >::epsilon( ) );

            outputTime = numberOfSeconds2 + inputTime1;
            BOOST_CHECK_EQUAL( numberOfDays1 + 1, outputTime.getFullPeriods( ) );
            BOOST_CHECK_CLOSE_FRACTION( numberOfSeconds1 + numberOfSeconds2 - TIME_NORMALIZATION_TERM,
                                        outputTime.getSecondsIntoFullPeriod( ),
                                        std::numeric_limits< long double >::epsilon( ) );
        }

        // Test subtractions of Time objects
        {
            outputTime = inputTime2 - inputTime1;
            BOOST_CHECK_EQUAL( numberOfDays2 - numberOfDays1 - 1, outputTime.getFullPeriods( ) );
            BOOST_CHECK_CLOSE_FRACTION( numberOfSeconds2 - numberOfSeconds1 + TIME_NORMALIZATION_TERM,
                                        outputTime.getSecondsIntoFullPeriod( ),
                                        std::numeric_limits< long double >::epsilon( ) );
            outputTime = inputTime2;
            outputTime -= inputTime1;
            BOOST_CHECK_EQUAL( numberOfDays2 - numberOfDays1 - 1, outputTime.getFullPeriods( ) );
            BOOST_CHECK_CLOSE_FRACTION( numberOfSeconds2 - numberOfSeconds1 + TIME_NORMALIZATION_TERM,
                                        outputTime.getSecondsIntoFullPeriod( ),
                                        std::numeric_limits< long double >::epsilon( ) );

            outputTime = inputTime2 - numberOfSeconds1;
            BOOST_CHECK_EQUAL( numberOfDays2 - 1, outputTime.getFullPeriods( ) );
            BOOST_CHECK_CLOSE_FRACTION( numberOfSeconds2 - numberOfSeconds1 + TIME_NORMALIZATION_TERM,
                                        outputTime.getSecondsIntoFullPeriod( ),
                                        std::numeric_limits< long double >::epsilon( ) );

            outputTime = numberOfSeconds2 - inputTime1;
            BOOST_CHECK_EQUAL( -numberOfDays1 - 1, outputTime.getFullPeriods( ) );
            BOOST_CHECK_CLOSE_FRACTION( numberOfSeconds2 - numberOfSeconds1 + TIME_NORMALIZATION_TERM,
                                        outputTime.getSecondsIntoFullPeriod( ),
                                        std::numeric_limits< long double >::epsilon( ) );
        }

    }

    // Test division of Time by double/long double values
    {
        // Define Time test values
        Time testTime( 2, LONG_PI );

        Time dividedTime = testTime / 2.0L;
        BOOST_CHECK_EQUAL( dividedTime.getFullPeriods( ), 1 );
        BOOST_CHECK_CLOSE_FRACTION( dividedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI / 2.0L,
                                    2.0 * std::numeric_limits< long double >::epsilon( ) );

        dividedTime = testTime / 2.0;
        BOOST_CHECK_EQUAL( dividedTime.getFullPeriods( ), 1 );
        BOOST_CHECK_CLOSE_FRACTION( dividedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI / 2.0L,
                                    2.0 * std::numeric_limits< double >::epsilon( ) );

        dividedTime = testTime / 3.0L;
        BOOST_CHECK_EQUAL( dividedTime.getFullPeriods( ), 0 );
        BOOST_CHECK_CLOSE_FRACTION( dividedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI / 3.0L + TIME_NORMALIZATION_TERM * 2.0L / 3.0L,
                                    2.0 *std::numeric_limits< long double >::epsilon( ) );
        dividedTime = testTime / 3.0;
        BOOST_CHECK_EQUAL( dividedTime.getFullPeriods( ), 0 );
        BOOST_CHECK_CLOSE_FRACTION( dividedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI / 3.0L + TIME_NORMALIZATION_TERM * 2.0L / 3.0L,
                                    2.0 *std::numeric_limits< double >::epsilon( ) );

        testTime = Time( 9, 8.0 * LONG_PI );
        dividedTime = testTime / 2.0L;
        BOOST_CHECK_EQUAL( dividedTime.getFullPeriods( ), 4 );
        BOOST_CHECK_CLOSE_FRACTION( dividedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI * 4.0L + TIME_NORMALIZATION_TERM / 2.0L,
                                    2.0 * std::numeric_limits< long double >::epsilon( ) );
        dividedTime = testTime / 2.0;
        BOOST_CHECK_EQUAL( dividedTime.getFullPeriods( ), 4 );
        BOOST_CHECK_CLOSE_FRACTION( dividedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI * 4.0L + TIME_NORMALIZATION_TERM / 2.0L,
                                    2.0 * std::numeric_limits< double >::epsilon( ) );

        dividedTime = testTime / 3.0L;
        BOOST_CHECK_EQUAL( dividedTime.getFullPeriods( ), 3 );
        BOOST_CHECK_CLOSE_FRACTION( dividedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI * 8.0L / 3.0L,
                                    2.0 * std::numeric_limits< long double >::epsilon( ) );
        dividedTime = testTime / 3.0;
        BOOST_CHECK_EQUAL( dividedTime.getFullPeriods( ), 3 );
        BOOST_CHECK_CLOSE_FRACTION( dividedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI * 8.0L / 3.0L,
                                    2.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Test multiplication of Time by double/long double values
    {
        Time testTime( 5, 1200.0L + LONG_PI );
        Time multipliedTime = testTime * 2.0L;
        BOOST_CHECK_EQUAL( multipliedTime.getFullPeriods( ), 10 );
        BOOST_CHECK_CLOSE_FRACTION( multipliedTime.getSecondsIntoFullPeriod( ),
                                    2400.0L + LONG_PI * 2.0L,
                                    2.0 * std::numeric_limits< long double >::epsilon( ) );

        multipliedTime = testTime * 3.0L;
        BOOST_CHECK_EQUAL( multipliedTime.getFullPeriods( ), 16 );
        BOOST_CHECK_CLOSE_FRACTION( multipliedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI * 3.0L,
                                    200.0 * std::numeric_limits< long double >::epsilon( ) );

        multipliedTime = 2.0L * testTime;
        BOOST_CHECK_EQUAL( multipliedTime.getFullPeriods( ), 10 );
        BOOST_CHECK_CLOSE_FRACTION( multipliedTime.getSecondsIntoFullPeriod( ),
                                    2400.0L + LONG_PI * 2.0L,
                                    2.0 * std::numeric_limits< long double >::epsilon( ) );

        multipliedTime = 3.0L * testTime;
        BOOST_CHECK_EQUAL( multipliedTime.getFullPeriods( ), 16 );
        BOOST_CHECK_CLOSE_FRACTION( multipliedTime.getSecondsIntoFullPeriod( ),
                                    LONG_PI * 3.0L,
                                    200.0 * std::numeric_limits< long double >::epsilon( ) );

        multipliedTime = testTime * 2.0;
        BOOST_CHECK_EQUAL( multipliedTime.getFullPeriods( ), 10 );
        BOOST_CHECK_CLOSE_FRACTION( multipliedTime.getSecondsIntoFullPeriod( ),
                                    2400.0 + LONG_PI * 2.0,
                                    2.0 * std::numeric_limits< double >::epsilon( ) );

        multipliedTime = testTime * 3.0;
        BOOST_CHECK_EQUAL( multipliedTime.getFullPeriods( ), 16 );
        BOOST_CHECK_CLOSE_FRACTION( multipliedTime.getSecondsIntoFullPeriod( ),
                                    PI * 3.0,
                                    2.0 * std::numeric_limits< double >::epsilon( ) );

        multipliedTime = 2.0 * testTime;
        BOOST_CHECK_EQUAL( multipliedTime.getFullPeriods( ), 10 );
        BOOST_CHECK_CLOSE_FRACTION( multipliedTime.getSecondsIntoFullPeriod( ),
                                    2400.0 + LONG_PI * 2.0,
                                    2.0 * std::numeric_limits< double >::epsilon( ) );

        multipliedTime = 3.0 * testTime;
        BOOST_CHECK_EQUAL( multipliedTime.getFullPeriods( ), 16 );
        BOOST_CHECK_CLOSE_FRACTION( multipliedTime.getSecondsIntoFullPeriod( ),
                                    PI * 3.0,
                                    2.0 * std::numeric_limits< double >::epsilon( ) );

    }
}

//! Test if the comparison operators defined for the Time object function correctly
BOOST_AUTO_TEST_CASE( testComparisonOperators )
{
    for( unsigned int i = 0; i < 4; i++ )
    {
        // Define test time (same for each test)
        int numberOfDays = 759;
        long double numberOfSeconds = 2.0L * TIME_NORMALIZATION_TERM + LONG_PI;

        Time testTime( numberOfDays, numberOfSeconds );
        double testTimeDouble = testTime.getSeconds< double >( );
        long double testTimeLongDouble = testTime.getSeconds< long double >( );

        // Define test time (different values for each test)
        Time testTime2 = TUDAT_NAN;
        double testTimeDouble2 = TUDAT_NAN;
        long double testTimeLongDouble2 = TUDAT_NAN;

        // Check comparison if only seconds is larger
        if( i == 0 )
        {
            testTime2 = Time( numberOfDays, numberOfSeconds + 1.0L );
            testTimeDouble2 = testTime.getSeconds< double >( ) + 1.0;
            testTimeLongDouble2 = testTime.getSeconds< long double >( ) + 1.0L;
        }
        // Check comparison if only hours is larger
        else if( i == 1 )
        {
            testTime2 = Time( numberOfDays + 3, numberOfSeconds );
            testTimeDouble2 = testTime.getSeconds< double >( ) + 3.0 * TIME_NORMALIZATION_TERM;
            testTimeLongDouble2 = testTime.getSeconds< long double >( ) + 3.0L * TIME_NORMALIZATION_TERM;
        }
        // Check comparison if hours is larger and seconds is larger
        else if( i == 2 )
        {
            testTime2 = Time( numberOfDays + 3, numberOfSeconds + 1.0L );
            testTimeDouble2 = testTime.getSeconds< double >( ) + 3.0 * TIME_NORMALIZATION_TERM + 1.0;
            testTimeLongDouble2 = testTime.getSeconds< long double >( ) + 3.0L * TIME_NORMALIZATION_TERM + 1.0L;
        }
        // Check comparison if hours is larger and seconds is smaller
        else if( i == 3 )
        {
            testTime2 = Time( numberOfDays + 3, numberOfSeconds - 1.0L );
            testTimeDouble2 = testTime.getSeconds< double >( ) + 3.0 * TIME_NORMALIZATION_TERM  - 1.0;
            testTimeLongDouble2 = testTime.getSeconds< long double >( ) + 3.0L * TIME_NORMALIZATION_TERM - 1.0L;
        }

        // Check equals comparison
        BOOST_CHECK( testTime == testTimeDouble );
        BOOST_CHECK( testTime == testTimeLongDouble );
        BOOST_CHECK( testTime == testTime );

        BOOST_CHECK( testTimeDouble  == testTime );
        BOOST_CHECK( testTimeLongDouble  == testTime );
        BOOST_CHECK( testTimeLongDouble  == testTime );

        // Check not-equals comparison
        BOOST_CHECK( testTime2 != testTimeDouble );
        BOOST_CHECK( testTime2 != testTimeLongDouble );
        BOOST_CHECK( testTime2 != testTime );

        BOOST_CHECK( testTimeDouble  != testTime2 );
        BOOST_CHECK( testTimeLongDouble  != testTime2 );

        // Check (strict) greater/less than
        BOOST_CHECK( testTime2 > testTimeDouble );
        BOOST_CHECK( testTime2 > testTimeLongDouble );
        BOOST_CHECK( testTime2 > testTime );

        BOOST_CHECK( testTimeDouble < testTime2 );
        BOOST_CHECK( testTimeLongDouble  < testTime2 );

        BOOST_CHECK( testTime2 >= testTimeDouble );
        BOOST_CHECK( testTime2 >= testTimeLongDouble );
        BOOST_CHECK( testTime2 >= testTime );

        BOOST_CHECK( testTime >= testTime );

        BOOST_CHECK( testTimeDouble <= testTime2 );
        BOOST_CHECK( testTimeLongDouble  <= testTime2 );

        // Check (strict) greater/less than (opposite direction)
        BOOST_CHECK( testTime < testTimeDouble2 );
        BOOST_CHECK( testTime < testTimeLongDouble2 );
        BOOST_CHECK( testTime < testTime2 );

        BOOST_CHECK( testTimeDouble2 > testTime );
        BOOST_CHECK( testTimeLongDouble2  > testTime );

        BOOST_CHECK( testTime <= testTimeDouble2 );
        BOOST_CHECK( testTime <= testTimeLongDouble2 );
        BOOST_CHECK( testTime <= testTime2 );

        BOOST_CHECK( testTimeDouble2 >= testTime );
        BOOST_CHECK( testTimeLongDouble2  >= testTime );

        // Check if comparison picks up small differences
        long double currentFullSeconds = testTime2.getSecondsIntoFullPeriod( );
        BOOST_CHECK( testTime2 != testTime2 + currentFullSeconds * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK( testTime2 < testTime2 + currentFullSeconds * std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK( testTime2 != testTime2 - currentFullSeconds * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK( testTime2 > testTime2 - currentFullSeconds * std::numeric_limits< double >::epsilon( ) );

        long double testTimeLongDouble2Rounded = testTimeLongDouble2 *
                ( 1.0L + 2.0L * std::numeric_limits< long double >::epsilon( ) );
        BOOST_CHECK( testTime2 != testTimeLongDouble2Rounded );
        BOOST_CHECK( testTime2 < testTimeLongDouble2Rounded );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-euler
    This function object computes an approwimation of the gamma function
    using the Stirling formula:
  \f$\sqrt{2 \pi} x^{x-\frac12} e^{-x} ( 1 + \frac1{x} P(\frac1{x}))\f$,
    where \f$P\f$ is a polynomial.

    @see gamma, gammaln

    @par Header <boost/simd/function/stirling.hpp>

    @par Example:

      @snippet stirling.cpp stirling

    @par Possible output:

      @snippet stirling.txt stirling

  **/
  Value stirling(Value const& x);
} }
#endif

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

#endif


#pragma once

#include <Eigen/Dense>
#include <robotics/common.hpp>

namespace Robotics::Model {

    /**
     * @brief A class for implemeting a generic dynamical system. Mostly serves to describe an
     * interface.
     */
    template <int StateSize, int InputSize, int OutputSize>
    class SystemBase {
        static_assert(StateSize > 0);
        static_assert(InputSize > 0);
        static_assert(OutputSize > 0);

      public:
        using State = ColumnVector<StateSize>;
        using Input = ColumnVector<InputSize>;
        using Output = ColumnVector<OutputSize>;

        using StateMatrix = SquareMatrix<StateSize>;
        using InputMatrix = Matrix<StateSize, InputSize>;
        using OutputMatrix = Matrix<OutputSize, StateSize>;
        using FeedthroughMatrix = Matrix<OutputSize, StateSize>;

        /**
         * @brief Propagates the state for one time step
         * @param u system input
         */
        virtual void PropagateDynamics(const Input& u, double dt) = 0;

        /**
         * @brief Updates the internal state of the system, which will become the new initial
         * condition
         * @param state new state
         */
        void SetInitialState(State state) { x = state; }

        /**
         * @brief Gets the latest state computed
         * @return the latest state
         */
        State GetState() const { return x; }

        /**
         * @brief Gets the current output of the system
         * @return the current output
         */
        Output GetOutput()
        {
            y = C * x;
            return y;
        }

        /**
         * @brief Gets the system's output matrix
         * @return theoutput matrix
         */
        OutputMatrix GetOutputMatrix() const { return C; }

        /**
         * @brief Gets a noisy output reading
         * @param noise noise statistic for the output reading
         * @return the noisy output reading
         */
        Output GetNoisyMeasurement(const SquareMatrix<OutputSize>& noise)
        {
            return C * x + noise * random.GetColumnVector<OutputSize>();
        }

      protected:
        State x{State::Zero()};
        Output y{Output::Zero()};

        StateMatrix A;
        InputMatrix B;
        OutputMatrix C;
        FeedthroughMatrix D;

        Robotics::NormalDistributionRandomGenerator random;
    };

}  // namespace Robotics::Model


/**
 *          Copyright Matthias Walter 2010.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 **/

#ifndef R10_HPP_
#define R10_HPP_

#include <boost/graph/isomorphism.hpp>

namespace unimod
{

  /**
   * Singleton class to test a bipartite graph to be ismorphic to the graphs representing R10.
   */

  class bipartite_r10_graphs
  {
  public:
    typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::undirectedS> graph_t;

  private:

    /**
     * Constructs the R10 graph class.
     */

    bipartite_r10_graphs()
    {
      typedef std::pair <int, int> E;

      E g1_edges[] =
      { E(0, 5), E(0, 8), E(0, 9), E(1, 5), E(1, 6), E(1, 9), E(2, 6), E(2, 7), E(2, 9), E(3, 7), E(3, 8), E(3, 9), E(4, 5), E(4, 6), E(4, 7),
          E(4, 8), E(4, 9) };
      g1 = graph_t(&g1_edges[0], &g1_edges[0] + sizeof(g1_edges) / sizeof(E), 10);

      E g2_edges[] =
      { E(0, 5), E(0, 8), E(0, 9), E(1, 5), E(1, 6), E(1, 8), E(2, 6), E(2, 7), E(2, 9), E(3, 7), E(3, 8), E(3, 9), E(4, 5), E(4, 6), E(4, 7) };
      g2 = graph_t(&g2_edges[0], &g2_edges[0] + sizeof(g2_edges) / sizeof(E), 10);
    }

    /**
     * Singleton instance function.
     *
     * @return The unique instance
     */

    static bipartite_r10_graphs& instance()
    {
      static bipartite_r10_graphs* instance = NULL;
      if (instance == NULL)
        instance = new bipartite_r10_graphs();
      return *instance;
    }

  public:

    /**
     * Checks a given graph to be isomorphic to the ones stored.
     *
     * @param graph Given graph
     * @return true if and only if it is isomorphic to any of the two stored ones.
     */

    template <typename Graph>
    static bool is_r10_graph(const Graph& graph)
    {
      return boost::isomorphism(graph, instance().g1) || boost::isomorphism(graph, instance().g2);
    }

  private:
    graph_t g1, g2;
  };

  /**
   * Tests a given matrix to be matrix-isomorphic to one of the R10-representing matrices
   * by examining the corresponding bipartite graphs.
   *
   * @param matrix A given matrix
   * @return true if and only if this matrix is a represenation matrix for R10
   */

  template <typename MatrixType>
  inline bool is_r10(MatrixType matrix)
  {
    if (matrix.size1() != 5 || matrix.size2() != 5)
      return false;

    bipartite_r10_graphs::graph_t graph(10);

    for (size_t row = 0; row < 5; ++row)
    {
      for (size_t column = 0; column < 5; ++column)
      {
        if (matrix(row, column))
        {
          boost::add_edge(boost::vertex(row, graph), boost::vertex(5 + column, graph), graph);
        }
      }
    }

    return bipartite_r10_graphs::is_r10_graph(graph);
  }
}

#endif /* R10_HPP_ */


#include "aslam/triangulation/triangulation.h"

#include <Eigen/QR>
#include <glog/logging.h>

namespace aslam {

TriangulationResult::Status TriangulationResult::SUCCESSFUL =
    TriangulationResult::Status::kSuccessful;
TriangulationResult::Status TriangulationResult::TOO_FEW_MEASUREMENTS =
    TriangulationResult::Status::kTooFewMeasurments;
TriangulationResult::Status TriangulationResult::UNOBSERVABLE =
    TriangulationResult::Status::kUnobservable;
TriangulationResult::Status TriangulationResult::UNINITIALIZED =
    TriangulationResult::Status::kUninitialized;

TriangulationResult linearTriangulateFromNViews(
    const Aligned<std::vector, Eigen::Vector2d>& measurements_normalized,
    const aslam::TransformationVector& T_G_B,
    const aslam::Transformation& T_B_C, Eigen::Vector3d* G_point) {
  CHECK_NOTNULL(G_point);
  CHECK_EQ(measurements_normalized.size(), T_G_B.size());
  if (measurements_normalized.size() < 2u) {
    return TriangulationResult(TriangulationResult::TOO_FEW_MEASUREMENTS);
  }

  VLOG(200) << "Triangulating from " << T_G_B.size() << " views.";

  const size_t rows = 3 * measurements_normalized.size();
  const size_t cols = 3 + measurements_normalized.size();
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(rows, cols);
  Eigen::VectorXd b = Eigen::VectorXd::Zero(rows);

  const Eigen::Matrix3d R_B_C = T_B_C.getRotationMatrix();

  // Fill in A and b.
  for (size_t i = 0; i < measurements_normalized.size(); ++i) {
    Eigen::Vector3d v(measurements_normalized[i](0),
        measurements_normalized[i](1), 1.);
    Eigen::Matrix3d R_G_B = T_G_B[i].getRotationMatrix();
    const Eigen::Vector3d& p_G_B = T_G_B[i].getPosition();
    A.block<3, 3>(3 * i, 0) = Eigen::Matrix3d::Identity();
    A.block<3, 1>(3 * i, 3 + i) = -R_G_B * R_B_C * v;
    b.segment<3>(3 * i) = p_G_B + R_G_B * T_B_C.getPosition();
  }

  Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr = A.colPivHouseholderQr();
  static constexpr double kRankLossTolerance = 0.001;
  qr.setThreshold(kRankLossTolerance);
  const size_t rank = qr.rank();

  if ((rank - measurements_normalized.size()) < 3) {
    return TriangulationResult(TriangulationResult::UNOBSERVABLE);
  }

  *G_point = qr.solve(b).head<3>();

  return TriangulationResult(TriangulationResult::SUCCESSFUL);
}

TriangulationResult linearTriangulateFromNViews(
    const Eigen::Matrix3Xd& t_G_bv,
    const Eigen::Matrix3Xd& p_G_C,
    Eigen::Vector3d* p_G_P) {
  CHECK_NOTNULL(p_G_P);

  const int num_measurements = t_G_bv.cols();
  if (num_measurements < 2) {
    return TriangulationResult(TriangulationResult::TOO_FEW_MEASUREMENTS);
  }

  // 1.) Formulate the geometrical problem
  // p_G_P + alpha[i] * t_G_bv[i] = p_G_C[i]      (+ alpha intended)
  // as linear system Ax = b, where
  // x = [p_G_P; alpha[0]; alpha[1]; ... ] and b = [p_G_C[0]; p_G_C[1]; ...]
  //
  // 2.) Apply the approximation AtAx = Atb
  // AtA happens to be composed of mostly more convenient blocks than A:
  // - Top left = N * Eigen::Matrix3d::Identity()
  // - Top right and bottom left = t_G_bv
  // - Bottom right = t_G_bv.colwise().squaredNorm().asDiagonal()

  // - Atb.head(3) = p_G_C.rowwise().sum()
  // - Atb.tail(N) = columnwise dot products between t_G_bv and p_G_C
  //               = t_G_bv.cwiseProduct(p_G_C).colwise().sum().transpose()
  //
  // 3.) Apply the Schur complement to solve after p_G_P only
  // AtA = [E B; C D] (same blocks as above) ->
  // (E - B * D.inverse() * C) * p_G_P = Atb.head(3) - B * D.inverse() * Atb.tail(N)

  const Eigen::MatrixXd BiD = t_G_bv *
      t_G_bv.colwise().squaredNorm().asDiagonal().inverse();
  const Eigen::Matrix3d AxtAx = num_measurements * Eigen::Matrix3d::Identity() -
      BiD * t_G_bv.transpose();
  const Eigen::Vector3d Axtbx = p_G_C.rowwise().sum() - BiD *
      t_G_bv.cwiseProduct(p_G_C).colwise().sum().transpose();

  Eigen::ColPivHouseholderQR<Eigen::Matrix3d> qr = AxtAx.colPivHouseholderQr();
  static constexpr double kRankLossTolerance = 1e-5;
  qr.setThreshold(kRankLossTolerance);
  const size_t rank = qr.rank();
  if (rank < 3) {
    return TriangulationResult(TriangulationResult::UNOBSERVABLE);
  }

  *p_G_P = qr.solve(Axtbx);
  return TriangulationResult(TriangulationResult::SUCCESSFUL);
}

TriangulationResult linearTriangulateFromNViewsMultiCam(
    const Aligned<std::vector, Eigen::Vector2d>& measurements_normalized,
    const std::vector<size_t>& measurement_camera_indices,
    const Aligned<std::vector, aslam::Transformation>& T_G_B,
    const Aligned<std::vector, aslam::Transformation>& T_B_C,
    Eigen::Vector3d* G_point) {
  CHECK_NOTNULL(G_point);
  CHECK_EQ(measurements_normalized.size(), T_G_B.size());
  CHECK_EQ(measurements_normalized.size(), measurement_camera_indices.size());
  if (measurements_normalized.size() < 2u) {
    return TriangulationResult(TriangulationResult::TOO_FEW_MEASUREMENTS);
  }

  const size_t rows = 3 * measurements_normalized.size();
  const size_t cols = 3 + measurements_normalized.size();
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(rows, cols);
  Eigen::VectorXd b = Eigen::VectorXd::Zero(rows);

  // Fill in A and b.
  for (size_t i = 0; i < measurements_normalized.size(); ++i) {
    size_t cam_index = measurement_camera_indices[i];
    CHECK_LT(cam_index, T_B_C.size());
    Eigen::Vector3d v(measurements_normalized[i](0),
        measurements_normalized[i](1), 1.);
    const Eigen::Vector3d& t_B_C = T_B_C[cam_index].getPosition();

    A.block<3, 3>(3 * i, 0) = Eigen::Matrix3d::Identity();
    A.block<3, 1>(3 * i, 3 + i) = -1.0 * T_G_B[i].getRotation().rotate(
        T_B_C[cam_index].getRotation().rotate(v));
    b.segment<3>(3 * i) = T_G_B[i] * t_B_C;
  }

  Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr = A.colPivHouseholderQr();
  static constexpr double kRankLossTolerance = 0.001;
  qr.setThreshold(kRankLossTolerance);
  const size_t rank = qr.rank();
  if ((rank - measurements_normalized.size()) < 3) {
    return TriangulationResult(TriangulationResult::UNOBSERVABLE);
  }

  *G_point = qr.solve(b).head<3>();
  return TriangulationResult(TriangulationResult::SUCCESSFUL);
}

// [1]  A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman filter
// "for vision-aided inertial navigation,” in Proc. IEEE Int. Conf. on Robotics
// and Automation, pp. 10–14, 2007.
// [2] T. Hinzmann, "Robust Vision-Based Navigation for Micro Air Vehicles", 2014.
TriangulationResult iterativeGaussNewtonTriangulateFromNViews(
    const Aligned<std::vector, Eigen::Vector2d>& measurements_normalized,
    const Aligned<std::vector, aslam::Transformation>& T_G_B,
    const aslam::Transformation& T_B_C, Eigen::Vector3d* G_point) {
  CHECK_NOTNULL(G_point);
  CHECK_EQ(measurements_normalized.size(), T_G_B.size());
  if (measurements_normalized.size() < 2u) {
    return TriangulationResult(TriangulationResult::TOO_FEW_MEASUREMENTS);
  }

  const double kPrecision = 1.0e-5;
  const size_t kIterMax = 10;

  // Initialize minimization variables.
  double alpha = 0.0;
  double beta = 0.0;
  double rho = 0.0;

  double residual_norm_last = 1000.0;
  double residual_norm = 100.0;

  // Camera frame n: camera frame in which feature was observed for the first time.
  const size_t n = 0;

  // Rotation and position of n-th camera in i-th camera frame.
  const aslam::Transformation T_Cn_G = T_B_C.inverse() * T_G_B[n].inverse();
  const Eigen::Matrix3d& R_Cn_G = T_Cn_G.getRotationMatrix();
  const Eigen::Vector3d& p_G_Cn = T_Cn_G.getPosition();

  // Cache matrices that are constant for every iteration.
  Aligned<std::vector, Eigen::Matrix3d> R_Ci_Cn;
  Aligned<std::vector, Eigen::Vector3d> p_Ci_Cn;
  for (size_t i = 0; i < measurements_normalized.size(); ++i) {
    const aslam::Transformation T_Ci_G = T_B_C.inverse() * T_G_B[i].inverse();
    // Rotation from first camera to current camera.
    const Eigen::Matrix3d R_Ci_G = T_Ci_G.getRotationMatrix();
    R_Ci_Cn.emplace_back(R_Ci_G * R_Cn_G.transpose());
    // Translation from first camera to current camera.
    const Eigen::Vector3d p_G_Ci = T_Ci_G.inverse().getPosition();
    p_Ci_Cn.emplace_back(R_Ci_G * p_G_Cn - R_Ci_G * p_G_Ci);
  }

  // [1.] Loop over iterations.
  // Loop while delta residual too large or number of maximum iterations reached.
  size_t iter = 0;
  while (residual_norm_last - residual_norm > kPrecision && iter < kIterMax) {
    const size_t num_measurements = 2 * measurements_normalized.size();
    Eigen::VectorXd residuals(num_measurements);
    Eigen::MatrixXd jacobian(num_measurements, 3);
    residuals.setZero(num_measurements);
    jacobian.setZero(num_measurements, 3);

    // [2.] Loop over camera frames / measurements.
    for (size_t i = 0; i < measurements_normalized.size(); ++i) {
      // Current measurement.
      const Eigen::Vector2d& h_meas = measurements_normalized[i];

      // Predicted measurement.
      const Eigen::Vector3d h_i = R_Ci_Cn[i] *
	  (Eigen::Matrix<double, 3, 1>() << alpha, beta, 1.0).finished() + rho * p_Ci_Cn[i];
      // Normalized predicted measurement.
      const Eigen::Vector2d h  = h_i.head<2>() / h_i(2);

      // Calculate residuals.
      residuals.segment<2>(i * 2) = h_meas - h;

      // Calculate jacobians.
      Eigen::Matrix<double, 2, 3> jacobian_perspective;
      jacobian_perspective << -1.0 / h_i(2), 0.0, h_i(0) / (h_i(2) * h_i(2)),
          0.0, -1.0 / h_i(2), h_i(1) / (h_i(2) * h_i(2));

      const Eigen::Matrix<double, 3, 1> jacobian_alpha =
          R_Ci_Cn[i] * (Eigen::Matrix<double, 3, 1>() << 1.0, 0.0, 0.0).finished();
      const Eigen::Matrix<double, 3, 1> jacobian_beta =
          R_Ci_Cn[i] * (Eigen::Matrix<double, 3, 1>() << 0.0, 1.0, 0.0).finished();
      const Eigen::Matrix<double, 3, 1> jacobian_rho = p_Ci_Cn[i];

      const Eigen::Matrix<double, 2, 1> jacobian_A = jacobian_perspective * jacobian_alpha;
      const Eigen::Matrix<double, 2, 1> jacobian_B = jacobian_perspective * jacobian_beta;
      const Eigen::Matrix<double, 2, 1> jacobian_C = jacobian_perspective * jacobian_rho;

      jacobian.block<1, 3>(i * 2, 0) =
          (Eigen::Matrix<double, 1, 3>() << jacobian_A(0), jacobian_B(0), jacobian_C(0)).finished();
      jacobian.block<1, 3>(i * 2 + 1, 0) =
          (Eigen::Matrix<double, 1, 3>() << jacobian_A(1), jacobian_B(1), jacobian_C(1)).finished();
    }  // Measurement loop.

    // Calculate update using LDLT decomposition.
    Eigen::Vector3d delta = (jacobian.transpose() * jacobian)
	.ldlt().solve(jacobian.transpose() * residuals);

    alpha = alpha - delta(0);
    beta = beta - delta(1);
    rho = rho - delta(2);

    residual_norm_last = residual_norm;
    residual_norm = residuals.squaredNorm();
    ++iter;
  } // Iteration loop.

  // Coordinate of feature in global frame.
  *G_point = 1.0 / rho * R_Cn_G.transpose() *
      (Eigen::Matrix<double, 3, 1>() << alpha, beta, 1.0).finished() + p_G_Cn;
  return TriangulationResult(TriangulationResult::SUCCESSFUL);
}

TriangulationResult triangulateFeatureTrack(
    const aslam::FeatureTrack& track,
    const aslam::TransformationVector& T_W_Bs,
    Eigen::Vector3d* W_landmark) {
  CHECK_NOTNULL(W_landmark);
  size_t track_length = track.getTrackLength();
  CHECK_GT(track_length, 1u);
  CHECK_EQ(track_length, T_W_Bs.size());

  VLOG(200) << "Triangulating track of length " << track_length;

  const aslam::Camera::ConstPtr& camera = track.getFirstKeypointIdentifier().getCamera();
  CHECK(camera);
  const aslam::CameraId track_camera_id = camera->getId();
  aslam::Transformation T_B_C = track.getFirstKeypointIdentifier().get_T_C_B().inverse();

  // Get the normalized measurements for all observations on the track.
  Aligned<std::vector, Eigen::Vector2d> normalized_measurements;
  normalized_measurements.reserve(track_length);

  size_t index = 0u;
  for (const aslam::KeypointIdentifier& keypoint_on_track : track.getKeypointIdentifiers()) {
    const aslam::Camera::ConstPtr& camera = keypoint_on_track.getCamera();
    CHECK(camera) << "Missing camera for keypoint on track with frame index: "
        << keypoint_on_track.getFrameIndex();

    // Obtain the normalized keypoint measurements.
    const Eigen::Vector2d& keypoint_measurement = keypoint_on_track.getKeypointMeasurement();
    Eigen::Vector3d C_ray;
    camera->backProject3(keypoint_measurement, &C_ray);
    Eigen::Vector2d normalized_measurement = C_ray.head<2>() / C_ray[2];
    normalized_measurements.push_back(normalized_measurement);
  }

  VLOG(200) << "Assembled triangulation data.";

  // Triangulate the landmark.
  CHECK_EQ(track_length, normalized_measurements.size());
  CHECK_EQ(track_length, T_W_Bs.size());
  aslam::TriangulationResult triangulation_result = linearTriangulateFromNViews(
                                                        normalized_measurements,
                                                        T_W_Bs,
                                                        T_B_C,
                                                        W_landmark);

  VLOG(200) << "Triangulation returned the following result:" << std::endl
          << triangulation_result;

  return triangulation_result;
}

TriangulationResult fastTriangulateFeatureTrack(
    const aslam::FeatureTrack& track,
    const aslam::TransformationVector& T_W_Bs,
    Eigen::Vector3d* W_landmark) {
  CHECK_NOTNULL(W_landmark);
  size_t track_length = track.getTrackLength();
  CHECK_GT(track_length, 1u);
  CHECK_EQ(track_length, T_W_Bs.size());

  VLOG(200) << "Triangulating track of length " << track_length;

  const aslam::Camera::ConstPtr& camera = track.getFirstKeypointIdentifier().getCamera();
  CHECK(camera);
  const aslam::CameraId track_camera_id = camera->getId();
  aslam::Transformation T_B_C = track.getFirstKeypointIdentifier().get_T_C_B().inverse();

  Eigen::Matrix3Xd G_bearing_vectors;
  Eigen::Matrix3Xd p_G_C_vector;

  G_bearing_vectors.resize(Eigen::NoChange, track_length);
  p_G_C_vector.resize(Eigen::NoChange, track_length);

  size_t index = 0u;
  for (const aslam::KeypointIdentifier& keypoint_on_track : track.getKeypointIdentifiers()) {
    const aslam::Camera::ConstPtr& camera = keypoint_on_track.getCamera();
    CHECK(camera) << "Missing camera for keypoint on track with frame index: "
        << keypoint_on_track.getFrameIndex();

    // Obtain the normalized keypoint measurements.
    const Eigen::Vector2d& keypoint_measurement = keypoint_on_track.getKeypointMeasurement();
    Eigen::Vector3d C_ray;
    camera->backProject3(keypoint_measurement, &C_ray);

    aslam::Transformation T_W_C = T_W_Bs[index] * keypoint_on_track.get_T_C_B().inverse();

    G_bearing_vectors.col(index) = T_W_C.getRotationMatrix() * C_ray;
    p_G_C_vector.col(index) = T_W_C.getPosition();
  }

  VLOG(200) << "Assembled triangulation data.";

  // Triangulate the landmark.
  aslam::TriangulationResult triangulation_result = linearTriangulateFromNViews(
      G_bearing_vectors, p_G_C_vector, W_landmark);

  VLOG(200) << "Triangulation returned the following result:" << std::endl
          << triangulation_result;

  return triangulation_result;
}

}  // namespace aslam


//============================================================================
// Name         : dnatemplatematrixfuncs.hpp
// Author       : Roger Fraser
// Contributors :
// Version      : 1.00
// Copyright    : Copyright 2017 Geoscience Australia
//
//                Licensed under the Apache License, Version 2.0 (the "License");
//                you may not use this file except in compliance with the License.
//                You may obtain a copy of the License at
//
//                http ://www.apache.org/licenses/LICENSE-2.0
//
//                Unless required by applicable law or agreed to in writing, software
//                distributed under the License is distributed on an "AS IS" BASIS,
//                WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//                See the License for the specific language governing permissions and
//                limitations under the License.
//
// Description  : Common functions involving matrix_2d operations
//============================================================================

#ifndef DNATEMPLATEMATRIXFUNCS_H_
#define DNATEMPLATEMATRIXFUNCS_H_

#if defined(_MSC_VER)
	#if defined(LIST_INCLUDES_ON_BUILD)
		#pragma message("  " __FILE__)
	#endif
#endif

#include <algorithm>
#include <functional>
#include <sstream>
#include <string>
#include <vector>
#include <stdlib.h>
#include <math.h>
#include <iostream>

#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>

#include <include/parameters/dnaellipsoid.hpp>
#include <include/config/dnatypes.hpp>
#include <include/math/dnamatrix_contiguous.hpp>
#include <include/measurement_types/dnameasurement.hpp>

using namespace std;
using namespace boost;

using namespace dynadjust::datum_parameters;
using namespace dynadjust::math;

// msr_t_Iterator = vector<measurement_t>::iterator
template<typename msr_t_Iterator>
// Fills upper triangle
void GetDirectionsVarianceMatrix(msr_t_Iterator begin, matrix_2d* vmat)
{
	msr_t_Iterator bmsRecord(begin);
	UINT32 a, angle_count(bmsRecord->vectorCount1 - 1);		// number of directions excluding the RO

	vmat->zero();
	vmat->redim(angle_count, angle_count);

	bmsRecord++;

	for (a=0; a<angle_count; ++a)
	{
		vmat->put(a, a, bmsRecord->scale2);				// derived angle variance
		if (a+1 < angle_count)
			vmat->put(a, a+1, bmsRecord->scale3);		// derived angle covariance
		bmsRecord++;
	}
}

// M = measurement_t, U = matrix_2d
// Fills upper triangle
template<typename msr_t_Iterator>
void GetGPSVarianceMatrix(const msr_t_Iterator begin, matrix_2d* vmat)
{
	msr_t_Iterator bmsRecord(begin);
	UINT32 variance_dim(bmsRecord->vectorCount1 * 3), covariance_dim, cov;
	vmat->zero();
	vmat->redim(variance_dim, variance_dim);

	for (UINT32 var(0), cov_elem; var<variance_dim; var+=3)
	{
		covariance_dim = bmsRecord->vectorCount2 * 3;

		vmat->put(var, var, (bmsRecord++)->term2);				// XX

		vmat->put(var, var+1, bmsRecord->term2);				// XY
		vmat->put(var+1, var+1, (bmsRecord++)->term3);			// YY

		vmat->put(var, var+2, bmsRecord->term2);				// XZ
		vmat->put(var+1, var+2, bmsRecord->term3);				// YZ
		vmat->put(var+2, var+2, (bmsRecord++)->term4);			// ZZ

		for (cov_elem=0; cov_elem<covariance_dim; cov_elem+=3)
		{
			cov = var + 3 + cov_elem;
			vmat->put(var, cov, bmsRecord->term1);				// m11
			vmat->put(var, cov+1, bmsRecord->term2);			// m12
			vmat->put(var, cov+2, (bmsRecord++)->term3);		// m13

			vmat->put(var+1, cov, bmsRecord->term1);			// m21
			vmat->put(var+1, cov+1, bmsRecord->term2);			// m22
			vmat->put(var+1, cov+2, (bmsRecord++)->term3);		// m23

			vmat->put(var+2, cov, bmsRecord->term1);			// m31
			vmat->put(var+2, cov+1, bmsRecord->term2);			// m32
			vmat->put(var+2, cov+2, (bmsRecord++)->term3);		// m33
		}
	}
}

// msr_t_Iterator = vector<measurement_t>::iterator
template<typename msr_t_Iterator>
// Sets values based on upper triangle
void SetDirectionsVarianceMatrix(msr_t_Iterator begin, const matrix_2d& vmat)
{
	msr_t_Iterator bmsRecord(begin);
	UINT32 a, angle_count(bmsRecord->vectorCount1 - 1);		// number of directions excluding the RO

	bmsRecord->scale2 = 0.;	// variance (angle)
	bmsRecord->scale3 = 0.;	// covariance (angle)

	bmsRecord++;

	for (a=0; a<angle_count; ++a)
	{
		bmsRecord->scale2 = vmat.get(a, a);				// derived angle variance
		if (a+1 < angle_count)
			bmsRecord->scale3 = vmat.get(a, a+1);		// derived angle covariance
		else
			bmsRecord->scale3 = 0.;						// not necessary, but in the interest of
														// preventing confusion, set to zero
		bmsRecord++;
	}
}

// msr_t_Iterator = vector<measurement_t>::iterator
template<typename msr_t_Iterator>
// Sets values based on upper triangle
void SetGPSVarianceMatrix(msr_t_Iterator begin, const matrix_2d& vmat)
{
	msr_t_Iterator bmsRecord(begin);
	UINT32 variance_dim(bmsRecord->vectorCount1 * 3), covariance_dim, cov;

	for (UINT32 var(0), cov_elem; var<variance_dim; var+=3)
	{
		covariance_dim = bmsRecord->vectorCount2 * 3;

		(bmsRecord++)->term2 = vmat.get(var, var);				// XX

		bmsRecord->term2 = vmat.get(var, var+1);				// XY
		(bmsRecord++)->term3 = vmat.get(var+1, var+1);			// YY

		bmsRecord->term2 = vmat.get(var, var+2);				// XZ
		bmsRecord->term3 = vmat.get(var+1, var+2);				// YZ
		(bmsRecord++)->term4 = vmat.get(var+2, var+2);			// ZZ

		for (cov_elem=0; cov_elem<covariance_dim; cov_elem+=3)
		{
			cov = var + 3 + cov_elem;
			bmsRecord->term1 = vmat.get(var, cov);				// m11
			bmsRecord->term2 = vmat.get(var, cov+1);			// m12
			(bmsRecord++)->term3 = vmat.get(var, cov+2);		// m13

			bmsRecord->term1 = vmat.get(var+1, cov);			// m21
			bmsRecord->term2 = vmat.get(var+1, cov+1);			// m22
			(bmsRecord++)->term3 = vmat.get(var+1, cov+2);		// m23

			bmsRecord->term1 = vmat.get(var+2, cov);			// m31
			bmsRecord->term2 = vmat.get(var+2, cov+1);			// m32
			(bmsRecord++)->term3 = vmat.get(var+2, cov+2);		// m33
		}
	}
}

template <class T>
void FormCarttoGeoRotationMatrix(const T& latitude, const T& longitude, const T& height,
	matrix_2d& mrotations, const CDnaEllipsoid* ellipsoid, bool CLUSTER=false, const UINT32& n=0)
{
	if (!CLUSTER)
		mrotations.redim(3, 3);

	T coslat(cos(latitude));
	T sinlat(sin(latitude));
	T coslon(cos(longitude));
	T sinlon(sin(longitude));

	T term1_a(ellipsoid->GetSemiMajor() * ellipsoid->GetE1sqd());
	T one_minus_esq(1. - ellipsoid->GetE1sqd());

	T nu_plus_h(primeVertical(ellipsoid, latitude) + height);
	T nu_1minuse2_plus_h((primeVertical(ellipsoid, latitude) * (one_minus_esq) + height));
	T term1_b(term1_a * sinlat * coslat);
	T term1_c(pow((1. - ellipsoid->GetE1sqd() * sinlat * sinlat), 1.5));

	// set up Rotation matrix (geo to cart)
	mrotations.put(n,   n,   (term1_b * coslat * coslon / term1_c) - ((nu_plus_h) * sinlat * coslon));
	mrotations.put(n,   n+1, -(nu_plus_h) * coslat * sinlon);
	mrotations.put(n,   n+2, coslat * coslon);
	mrotations.put(n+1, n,   (term1_b * coslat * sinlon / term1_c) - ((nu_plus_h) * sinlat * sinlon));
	mrotations.put(n+1, n+1, (nu_plus_h) * coslat * coslon);
	mrotations.put(n+1, n+2, coslat * sinlon);
	mrotations.put(n+2, n,   (term1_b * one_minus_esq * sinlat / term1_c) + (nu_1minuse2_plus_h * coslat));
	mrotations.put(n+2, n+1, 0.);
	mrotations.put(n+2, n+2, sinlat);
}

template <class T>
void FormCarttoGeoRotationMatrix_Cluster(const matrix_2d& mpositions, matrix_2d& mrotations,
									  const CDnaEllipsoid* ellipsoid)
{
	mrotations.redim(mpositions.rows(), mpositions.rows());

	for (UINT32 i(0); i<mpositions.rows(); i+=3)
	{
		FormCarttoGeoRotationMatrix<T>(
			mpositions.get(i, 0),
			mpositions.get(i+1, 0),
			mpositions.get(i+2, 0),
			mrotations, ellipsoid, true, i);
	}
}

// Assumes design elements for station 1 and station 2 are always
// -1 and 1 respectively.
template <class T>
void Precision_Adjusted_GNSS_bsl(const matrix_2d& mvariances,
	const UINT32& stn1, const UINT32& stn2,
	matrix_2d* mvariances_mod, bool FILLLOWER=true)
{
	matrix_2d tmp(3, 6);
	mvariances_mod->zero();
	UINT32 i, j, k;

	// 1. Form A * V
	for (i=0, j=0; i<3; ++i, ++j)
	{
		// variance-covariance
		for (j=0; j<3; ++j)
		{
			// stn 11 variance
			tmp.elementadd(i, j, -mvariances.get(stn1+i, stn1+j));
			// stn 21 covariance
			tmp.elementadd(i, j, mvariances.get(stn2+i, stn1+j));
		}
		k=j;
		// variance-covariance
		for (j=0; j<3; ++j, ++k)
		{
			// stn 12 covariance
			tmp.elementadd(i, k, -mvariances.get(stn1+i, stn2+j));
			// stn 22 variance
			tmp.elementadd(i, k, mvariances.get(stn2+i, stn2+j));
		}
	}

	//tmp.trace("A x V", "%.16G ");

	// 2. Form AV * AT (upper triangular variance-covariance)
	for (i=0; i<3; ++i)
		for (j=i; j<3; ++j)
			// Sum the variances & covariances
			mvariances_mod->put(i, j, tmp.get(i, j+3) - tmp.get(i, j));

	if (FILLLOWER)
		mvariances_mod->filllower();

	//mvariances_mod->trace("AV X At", "%.16G ");
}

template <class T>
void Prpagate_Variances_Geo_Cart(const matrix_2d& mvariances, matrix_2d mrotations, matrix_2d* mvariances_mod, bool FORWARD=true)
{
	// the rotation matrix is in the direction geo to cart (forward)
	// so to go cart to geo, perform inverse
	if (!FORWARD)
		mrotations = mrotations.sweepinverse();		// allows negative diagonal terms

	matrix_2d mV(mrotations);
	//mV.multiply(mvariances);		// original variance matrix
	mV.multiply_mkl("N", mvariances, "N");		// original variance matrix
	//mvariances_mod->multiply_square_t(mV, mrotations);
	mvariances_mod->multiply_mkl(mV, "N", mrotations, "T");
}

template <class T>
void PropagateVariances_GeoCart(const matrix_2d mvariances, matrix_2d* mvariances_mod,
								const T& latitude, const T& longitude, const T& height,
								matrix_2d& mrotations,
								const CDnaEllipsoid* ellipsoid, bool GEO_TO_CART, bool CALCULATE_ROTATIONS)
{

	if (CALCULATE_ROTATIONS)
		FormCarttoGeoRotationMatrix<T>(latitude, longitude, height, mrotations, ellipsoid);

	Prpagate_Variances_Geo_Cart<T>(mvariances, mrotations, mvariances_mod, GEO_TO_CART);
}

template <class T>
void PropagateVariances_GeoCart(const matrix_2d mvariances, matrix_2d* mvariances_mod,
								const T& latitude, const T& longitude, const T& height,
								const CDnaEllipsoid* ellipsoid, bool GEO_TO_CART)
{

	matrix_2d mrotations;
	PropagateVariances_GeoCart<T>(mvariances, mvariances_mod,
		latitude, longitude, height,
		mrotations,
		ellipsoid, GEO_TO_CART,
		true);		// Calculate rotations
}

template <class T>
void PropagateVariances_GeoCart_Cluster(const matrix_2d& mvariances, matrix_2d* mvariances_mod,
								const matrix_2d& mpositions, matrix_2d& mrotations,
								const CDnaEllipsoid* ellipsoid, bool GEO_TO_CART, bool CALCULATE_ROTATIONS)
{
	if (CALCULATE_ROTATIONS)
		FormCarttoGeoRotationMatrix_Cluster<T>(mpositions, mrotations, ellipsoid);

	Prpagate_Variances_Geo_Cart<T>(mvariances, mrotations, mvariances_mod, GEO_TO_CART);
}

template <class T>
void PropagateVariances_GeoCart_Cluster(const matrix_2d& mvariances, matrix_2d* mvariances_mod,
								const matrix_2d& mpositions_rad,
								const CDnaEllipsoid* ellipsoid, bool GEO_TO_CART)
{
	matrix_2d mrotations;
	FormCarttoGeoRotationMatrix_Cluster<T>(mpositions_rad, mrotations, ellipsoid);

	Prpagate_Variances_Geo_Cart<T>(mvariances, mrotations, mvariances_mod, GEO_TO_CART);
}

template <class T>
void ScaleMatrix(const matrix_2d mvariances, matrix_2d* mvariances_mod, const matrix_2d& scalars)
{
	//matrix_2d mV(mvariances_mod->multiply(scalars, mvariances));
	matrix_2d mV(mvariances_mod->multiply_mkl(scalars, "N", mvariances, "N"));
	//mvariances_mod->multiply_square_t(mV, scalars);
	mvariances_mod->multiply_mkl(mV, "N", scalars, "T");
}

template <class T>
void ScaleGPSVCV(const matrix_2d& mvariances, matrix_2d* mvariances_mod,
						const T& latitude, const T& longitude, const T& height,
						const CDnaEllipsoid* ellipsoid,
						const T& pScale, const T& lScale, const T& hScale)
{
	matrix_2d mrotations(3, 3);
	PropagateVariances_GeoCart<T>(mvariances, mvariances_mod,
		latitude, longitude, height, mrotations, ellipsoid,
		false, 		// Geographic -> Cartesian ?
		true);		// create the rotation matrix

	matrix_2d var_scalars(3, 3);
	var_scalars.put(0, 0, sqrt(pScale));
	var_scalars.put(1, 1, sqrt(lScale));
	var_scalars.put(2, 2, sqrt(hScale));
	ScaleMatrix<T>(*mvariances_mod, mvariances_mod, var_scalars);

	PropagateVariances_GeoCart<T>(*mvariances_mod, mvariances_mod,
		latitude, longitude, height, mrotations, ellipsoid,
		true,		// Geographic -> Cartesian ?
		false);		// don't create a rotation matrix
}

template <class T>
// coordType is passed so that ScaleGPSVCV_Cluster knows whether
// mvariances needs to be propagated to geographic first.  Hence,
// if coordType == LLH_type_i, no propagation is undertaken
void ScaleGPSVCV_Cluster(const matrix_2d& mvariances, matrix_2d* mvariances_mod,
						const matrix_2d& mpositions, const CDnaEllipsoid* ellipsoid,
						const T& pScale, const T& lScale, const T& hScale, _COORD_TYPE_ coordType=XYZ_type_i)
{
	matrix_2d mrotations;
	// form rotation matrix
	FormCarttoGeoRotationMatrix_Cluster<T>(mpositions, mrotations, ellipsoid);

	// Don't propagate if already in geographic
	if (coordType == XYZ_type_i)
		// propagate variances in cartesian system to geographic
		PropagateVariances_GeoCart_Cluster<T>(mvariances, mvariances_mod,
			mpositions, mrotations, ellipsoid,
			false, 		// Cartesian -> Geographic
			false);		// don't create a rotation matrix

	// scale matrix
	matrix_2d var_scalars(mvariances.rows(), mvariances.columns());

	for (UINT32 r(0); r<var_scalars.rows(); r+=3)
	{
		var_scalars.put(r, r, sqrt(pScale));
		var_scalars.put(r+1, r+1, sqrt(lScale));
		var_scalars.put(r+2, r+2, sqrt(hScale));
	}

	// perform the scaling
	ScaleMatrix<T>(*mvariances_mod, mvariances_mod, var_scalars);

	// propagate variances in geographic system to cartesian
	PropagateVariances_GeoCart_Cluster<T>(*mvariances_mod, mvariances_mod,
		mpositions, mrotations, ellipsoid,
		true, 		// Geographic -> Cartesian
		false);		// don't create a rotation matrix
}

template <class T>
void FormLocaltoCartRotationMatrix(const T& latitude, const T& longitude,
	matrix_2d& mrotations, bool LOCAL_TO_CART=true)
{
	mrotations.redim(3, 3);

	T coslat(cos(latitude));
	T sinlat(sin(latitude));
	T coslon(cos(longitude));
	T sinlon(sin(longitude));

	if (LOCAL_TO_CART)
	{
		// set up Rotation matrix for local to cart
		mrotations.put(0, 0, -sinlon);
		mrotations.put(0, 1, -sinlat*coslon);
		mrotations.put(0, 2, coslat*coslon);
		mrotations.put(1, 0, coslon);
		mrotations.put(1, 1, -sinlat*sinlon);
		mrotations.put(1, 2, coslat*sinlon);
		mrotations.put(2, 0, 0.);
		mrotations.put(2, 1, coslat);
		mrotations.put(2, 2, sinlat);
	}
	else
	{
		// set up Rotation matrix for cart to local, which is
		// transpose of local to cart!
		mrotations.put(0, 0, -sinlon);
		mrotations.put(0, 1, coslon);
		mrotations.put(0, 2, 0.);
		mrotations.put(1, 0, -sinlat*coslon);
		mrotations.put(1, 1, -sinlat*sinlon);
		mrotations.put(1, 2, coslat);
		mrotations.put(2, 0, coslat*coslon);
		mrotations.put(2, 1, coslat*sinlon);
		mrotations.put(2, 2, sinlat);
	}
}

template <class T>
void FormLocaltoPolarRotationMatrix(const T& azimuth, const T& elevation, const T& distance,
	matrix_2d& mrotations, bool LOCAL_TO_POLAR=true)
{
	mrotations.redim(3, 3);

	T cos_azimuth(cos(azimuth));
	T sin_azimuth(sin(azimuth));
	T cos_elevation(cos(elevation));
	T sin_elevation(sin(elevation));

	if (LOCAL_TO_POLAR)
	{
		// set up Jacobian matrix for local to polar
		mrotations.put(0, 0, cos_azimuth/distance);
		mrotations.put(0, 1, -sin_azimuth/distance);
		mrotations.put(0, 2, 0.);
		mrotations.put(1, 0, -sin_azimuth*sin_elevation/distance);
		mrotations.put(1, 1, -cos_azimuth*sin_elevation/distance);
		mrotations.put(1, 2, cos_elevation/distance);
		mrotations.put(2, 0, sin_azimuth*cos_elevation);
		mrotations.put(2, 1, cos_azimuth*cos_elevation);
		mrotations.put(2, 2, sin_elevation);
	}
	else
	{
		// Set up Jacobian matrix for polar to local, which is
		// transpose of local to polar!
		mrotations.put(0, 0, cos_azimuth/distance);
		mrotations.put(0, 1, -sin_azimuth*sin_elevation/distance);
		mrotations.put(0, 2, sin_azimuth*cos_elevation);
		mrotations.put(1, 0, -sin_azimuth/distance);
		mrotations.put(1, 1, -cos_azimuth*sin_elevation/distance);
		mrotations.put(1, 2, cos_azimuth*cos_elevation);
		mrotations.put(2, 0, 0.);
		mrotations.put(2, 1, cos_elevation/distance);
		mrotations.put(2, 2, sin_elevation);
	}
}

template <class T>
void PropagateVariances_CartLocal_Diagonal(const matrix_2d& mvariances, matrix_2d& mvariances_mod,
								  const T& latitude, const T& longitude,
								  matrix_2d& mrotations, bool CALCULATE_ROTATIONS=false)
{
	if (CALCULATE_ROTATIONS)
		FormLocaltoCartRotationMatrix<T>(latitude, longitude, mrotations);

	//matrix_2d mrotations_T(mrotations.rows(), mrotations.columns());
	matrix_2d rtv(mrotations.rows(), mrotations.columns());

	//mrotations_T.transpose(mrotations);

	mvariances_mod.redim(3, 3);

	// RtV
	//rtv.multiply(mrotations_T, mvariances);
	rtv.multiply_mkl(mrotations, "T", mvariances, "N");

	UINT32 row, col, i;

	// RtVR
	for (row=0; row<3; ++row) {
		for (col=0; col<3; ++col) {
			mvariances_mod.put(row, col, 0.0);
			// diagonals only
			if (row != col)
				continue;
			for (i=0; i<3; ++i)
				mvariances_mod.elementadd(row, col, rtv.get(row, i) * mrotations.get(i, col));
		}
	}
}

template <class T>
void PropagateVariances_LocalPolar_Diagonal(const matrix_2d& mvariances, matrix_2d& mvariances_mod,
								  const T& azimuth, const T& elevation, const T& distance,
								  matrix_2d& mrotations, bool CALCULATE_ROTATIONS=false)
{
	if (CALCULATE_ROTATIONS)
		FormLocaltoPolarRotationMatrix<T>(azimuth, elevation, distance, mrotations);

	matrix_2d mrotations_T(mrotations.rows(), mrotations.columns());
	matrix_2d rtv(mrotations.rows(), mrotations.columns());

	mrotations_T.transpose(mrotations);

	mvariances_mod.redim(3, 3);

	// RtV
	//rtv.multiply(mrotations, mvariances);
	rtv.multiply_mkl(mrotations, "N", mvariances, "N");

	UINT32 row, col, i;

	// RtVR
	for (row=0; row<3; ++row) {
		for (col=0; col<3; ++col) {
			mvariances_mod.put(row, col, 0.0);
			// diagonals only
			if (row != col)
				continue;
			for (i=0; i<3; ++i)
				mvariances_mod.elementadd(row, col, rtv.get(row, i) * mrotations_T.get(i, col));
		}
	}
}

template <class T>
void PropagateVariances_LocalCart(const matrix_2d& mvariances, matrix_2d& mvariances_mod,
								  const T& latitude, const T& longitude, bool LOCAL_TO_CART,
								  matrix_2d& mrotations, bool CALCULATE_ROTATIONS=false)
{
	if (CALCULATE_ROTATIONS)
		FormLocaltoCartRotationMatrix<T>(latitude, longitude, mrotations);

	// form transpose, from either passed in matrix or newly formed matrix
	matrix_2d mrotations_T(mrotations.rows(), mrotations.columns());
	mrotations_T.transpose(mrotations);

	// the rotation matrix is in the direction local to cart (forward)
	if (LOCAL_TO_CART)
	{
		// Vc = R * Vl * RT
		//matrix_2d mV(mvariances_mod.multiply(mrotations, mvariances));
		matrix_2d mV(mvariances_mod.multiply_mkl(mrotations, "N", mvariances, "N"));
		//mvariances_mod.multiply_square(mV, mrotations_T);
		mvariances_mod.multiply_mkl(mV, "N", mrotations, "T");
	}
	else
	{
		// Vc = R-1 * Vc * [R-1]T
		//    = RT * Vc * R (since R is orthogonal)
		//matrix_2d mV(mvariances_mod.multiply(mrotations_T, mvariances));
		matrix_2d mV(mvariances_mod.multiply_mkl(mrotations, "T", mvariances, "N"));
		//mvariances_mod.multiply_square(mV, mrotations);
		mvariances_mod.multiply_mkl(mV, "N", mrotations, "N");
	}
}

template <class T>
void PropagateVariances_LocalCart(const matrix_2d& mvariances, matrix_2d& mvariances_mod,
								  const T& latitude, const T& longitude, bool LOCAL_TO_CART)
{
	matrix_2d mrotations;

	PropagateVariances_LocalCart<T>(mvariances, mvariances_mod,
		latitude, longitude, LOCAL_TO_CART,
		mrotations, true);	// calculate rotations
}

template <class T>
void Rotate_LocalCart(const matrix_2d mvector, matrix_2d* mvector_mod,
								  const T& latitude, const T& longitude)
{
	matrix_2d mrotations(3, 3);
	FormLocaltoCartRotationMatrix<T>(latitude, longitude, mrotations);

	mvector_mod->redim(3, 1);
	//mvector_mod->multiply(mrotations, mvector);
	mvector_mod->multiply_mkl(mrotations, "N", mvector, "N");
}

template <class T>
void Rotate_CartLocal(const matrix_2d mvector, matrix_2d* mvector_mod,
								  const T& latitude, const T& longitude)
{
	// Helps
	T sin_lat(sin(latitude));
	T cos_lat(cos(latitude));
	T sin_long(sin(longitude));
	T cos_long(cos(longitude));

	mvector_mod->redim(3, 1);

	mvector_mod->put(0, 0, -sin_long * mvector.get(0,0) + cos_long * mvector.get(1,0));
	mvector_mod->put(1, 0, -sin_lat * cos_long * mvector.get(0,0) -
		sin_lat * sin_long * mvector.get(1,0) +
		cos_lat * mvector.get(2,0));
	mvector_mod->put(2, 0, cos_lat * cos_long * mvector.get(0,0) +
		cos_lat * sin_long * mvector.get(1,0) +
		sin_lat * mvector.get(2,0));
}

template <class T>
void Rotate_LocalPolar(const matrix_2d mvector, matrix_2d* mvector_mod,
								  const T& azimuth, const T& elevation, const T& distance)
{
	// Helps
	T sin_azimuth(sin(azimuth));
	T cos_azimuth(cos(azimuth));
	T sin_elevation(sin(elevation));
	T cos_elevation(cos(elevation));

	mvector_mod->redim(3, 1);

	mvector_mod->put(0, 0, -cos_azimuth * mvector.get(0,0) - sin_azimuth * mvector.get(1,0));
	mvector_mod->put(1, 0, -sin_elevation * sin_azimuth * mvector.get(0,0) -
		sin_elevation * cos_azimuth * mvector.get(1,0) +
		cos_elevation * mvector.get(2,0));
	mvector_mod->put(2, 0, cos_elevation * sin_azimuth * mvector.get(0,0) +
		cos_elevation * cos_azimuth * mvector.get(1,0) +
		sin_elevation * mvector.get(2,0));
}

template <class T>
// rotx is x rotation in radians
// roty is y rotation in radians
// rotz is z rotation in radians
void FormHelmertRotationMatrix(const T& rotx, const T& roty, const T& rotz, matrix_2d& mrotations, bool RIGOROUS=false)
{
	mrotations.redim(3, 3);

	// Which rotation matrix is required?
	// Convert to seconds for the test
	if (RIGOROUS)
	{
		// rigorous formula for large (> 10 seconds) rotations
		mrotations.put(0, 0, cos(roty) * cos(rotz));
		mrotations.put(0, 1, cos(roty) * sin(rotz));
		mrotations.put(0, 2, -sin(roty));
		mrotations.put(1, 0, (sin(rotx) * sin(roty) * cos(rotz)) - (cos(rotx) * sin(rotz)));
		mrotations.put(1, 1, (sin(rotx) * sin(roty) * sin(rotz)) + (cos(rotx) * cos(rotz)));
		mrotations.put(1, 2, sin(rotx) * cos(roty));
		mrotations.put(2, 0, (cos(rotx) * sin(roty) * cos(rotz)) + (sin(rotx) * sin(rotz)));
		mrotations.put(2, 1, (cos(rotx) * sin(roty) * sin(rotz)) - (sin(rotx) * cos(rotz)));
		mrotations.put(2, 2, cos(rotx) * cos(roty));
	}
	else
	{
		mrotations.put(0, 0, 1.);
		mrotations.put(0, 1, rotz);
		mrotations.put(0, 2, -roty);
		mrotations.put(1, 0, -rotz);
		mrotations.put(1, 1, 1.);
		mrotations.put(1, 2, rotx);
		mrotations.put(2, 0, roty);
		mrotations.put(2, 1, -rotx);
		mrotations.put(2, 2, 1.);
	}
}

template <class T>
void ReduceParameters(const T* parameters, T* reduced_parameters, const T& elapsedTime, bool DYNAMIC=true)
{
	// translations (reduce to metres)
	reduced_parameters[0] = parameters[0] / 1000.;
	reduced_parameters[1] = parameters[1] / 1000.;
	reduced_parameters[2] = parameters[2] / 1000.;
	// scale
	reduced_parameters[3] = parameters[3] / 1E9;
	// rotations
	reduced_parameters[4] = parameters[4];
	reduced_parameters[5] = parameters[5];
	reduced_parameters[6] = parameters[6];

	if (DYNAMIC)
	{
		// apply rates to translations
		reduced_parameters[0] += parameters[7] / 1000. * elapsedTime;
		reduced_parameters[1] += parameters[8] / 1000. * elapsedTime;
		reduced_parameters[2] += parameters[9] / 1000. * elapsedTime;
		// apply rate to scale
		reduced_parameters[3] += parameters[10] / 1E9 * elapsedTime;
		// apply rates to rotations
		reduced_parameters[4] += parameters[11] * elapsedTime;
		reduced_parameters[5] += parameters[12] * elapsedTime;
		reduced_parameters[6] += parameters[13] * elapsedTime;
	}

	// reduce rotations from milli-arc-seconds to radians
	reduced_parameters[4] = SecondstoRadians(reduced_parameters[4]) / 1000.;
	reduced_parameters[5] = SecondstoRadians(reduced_parameters[5]) / 1000.;
	reduced_parameters[6] = SecondstoRadians(reduced_parameters[6]) / 1000.;
}

template <class T>
// No check or safe guard in place to test if mcoordinates_mod is a reference to mcoordinates
void TransformCartesian(const matrix_2d& mcoordinates, matrix_2d& mcoordinates_mod,
	const matrix_2d& parameters, const matrix_2d& mrotations)
{
	// Add rotation and scale contributions
	for (UINT16 i(0), j; i<3; ++i)
	{
		// Initialise 'to datum' matrix
		mcoordinates_mod.put(i, 0, 0.0);

		for (j=0; j<3; ++j)		// For each column in the row
			mcoordinates_mod.elementadd(i, 0, mrotations.get(i, j) * mcoordinates.get(j, 0));	// Sum partial products

		// Apply scale
		mcoordinates_mod.elementmultiply(i, 0, 1.0 + parameters.get(3, 0));

		// Add translations
		mcoordinates_mod.elementadd(i, 0, parameters.get(i, 0));
	}
}

template <class T>
void Transform_7parameter(const matrix_2d& mcoordinates, matrix_2d& mcoordinates_mod, const T parameters[])
{
	mcoordinates_mod.redim(3, 1);

	bool RIGOROUS(false);
	if (parameters[4] > 10. || parameters[5] > 10. || parameters[6] > 10.)
		RIGOROUS = true;

	// Form rotation matrix
	matrix_2d mrotations(3, 3);
	FormHelmertRotationMatrix<T>(parameters[4], parameters[5], parameters[6], mrotations, RIGOROUS);

	// Put translations and scale into reducedParameters
	matrix_2d mtrans_scale(4, 1, parameters, 4);

	// Transform
	TransformCartesian<T>(mcoordinates, mcoordinates_mod,
		mtrans_scale, mrotations);

}

template <typename T>
void PositionalUncertainty(const T& semimajor, const T& semiminor, const T& azimuth, const T& sdHt,
						   T& hzPosU_Radius, T& vtPosU_Radius)
{
	hzPosU_Radius = vtPosU_Radius = -1.;
	if (semimajor < 0.0)
		return;
	if (semiminor < 0.0)
		return;

	// Horizontal
	T c(semiminor / semimajor);
	T K(HPOS_UNCERT_Q0 + (HPOS_UNCERT_Q1 * c) + (HPOS_UNCERT_Q2 * (c * c)) + (HPOS_UNCERT_Q3 * (c * c * c)));
	T R(semimajor * K);

	hzPosU_Radius = R;

	// Vertical
	vtPosU_Radius = sdHt * 1.96;
}

template <typename T>
T PedalVariance(const matrix_2d& mvariance, const T& direction)
{
	T cos_theta(cos(direction));
	T sin_theta(sin(direction));
	return mvariance.get(0, 0) * cos_theta * cos_theta +
		mvariance.get(1, 1) * sin_theta * sin_theta +
		2. * mvariance.get(0, 1) * cos_theta * sin_theta;
}

template <typename T>
void ErrorEllipseParameters(const matrix_2d& mvariance, T& semimajor, T& semiminor, T& azimuth)
{
	semimajor = semiminor = azimuth = -1.;

	if (mvariance.rows() < 2)
		return;
	if (mvariance.columns() < 2)
		return;

	T e2(mvariance.get(0, 0));
	T n2(mvariance.get(1, 1));
	T en(mvariance.get(0, 1));
	T e2_plus_n2(e2 + n2);
	T e2_minus_n2(e2 - n2);
	T n2_minus_e2(n2 - e2);

	T W((e2_minus_n2 * e2_minus_n2) + (4. * en * en));

	if (W < 0.0)
	{
		if (fabs(W) > PRECISION_1E15)
			return;								// temp term cannot be negative!!!
		else
			W = 0.0;
	}

	T a_sqd(0.5 * (e2_plus_n2 + sqrt(W)));		// semi-major
	T b_sqd(0.5 * (e2_plus_n2 - sqrt(W)));		// semi-minor

	if (a_sqd < 0.0)
		return;									// lamda2 term cannot be negative!!!
	if (b_sqd < 0.0)
		return;									// lamda2 term cannot be negative!!!

	semimajor = sqrt(a_sqd);
	semiminor = sqrt(b_sqd);

	// Compute the azimuth of the semi-major axis
	if (fabs(e2 - n2) < PRECISION_1E25)
	{
		if (en < PRECISION_1E25)
			azimuth = 0.;			// ellipse is a circle
		else
			azimuth = PI / 4.;		// azimuth = 45
	}
	else
	{
		T x(en + en);
		azimuth = 0.5 * atan_2(x, n2_minus_e2);
	}
}

#endif /* DNATEMPLATEMATRIXFUNCS_H_ */


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <Eigen/Core>
#include "gtest/gtest.h"

#include "modules/geometry/polygon.hpp"
#include "modules/geometry/line.hpp"
#include "modules/geometry/commons.hpp"
#include "modules/models/dynamic/single_track.hpp"
#include "modules/models/dynamic/integration.hpp"

TEST(single_track_model, dynamic_test) {
  using namespace std;
  using namespace modules::geometry;
  using namespace modules::models::dynamic;

  State x(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  x << 0, 0, 0, 0, 5;

  Input u(2);
  u << 0, 0;

  DynamicModel *m;
  SingleTrackModel single_track_model;
  m = &single_track_model;

  float dt = 0.1;
  for (int i = 0; i < 10; i++) {
    x = euler_int(m, x, u, dt);
    cout << x << endl;
  }
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include <iostream>
#include <Eigen/Eigenvalues>
#include <Eigen/Dense>
#include <vector>
#include <math.h>
#include <cmath>
#include <fstream>
#include <string>
#include <vector>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/fpfh.h>

#include <Config.hpp>

int main(int argc, char *argv[])
{
    // *Parse Config
    Config config;
    std::string input_pcd = config.input_pcd;
    std::string output_pcd = config.output_pcd;

    // *Read point cloud
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::io::loadPCDFile(input_pcd, *cloud);
    std::cout << "Number of points in the input cloud is:" << cloud->points.size() << std::endl;

    // *Create the filtering object
    // pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
    // sor.setInputCloud(cloud);
    // sor.setLeafSize(0.1f, 0.1f, 0.1f);
    // sor.filter(*cloud);

    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Normals(new pcl::PointCloud<pcl::PointXYZRGBA>);
    Normals->resize(cloud->size());

    // *K nearest neighbor search
    int KNumbersNeighbor = 10; // numbers of neighbors 7 , 120
    std::vector<int> NeighborsKNSearch(KNumbersNeighbor);
    std::vector<float> NeighborsKNSquaredDistance(KNumbersNeighbor);

    int *NumbersNeighbor = new int[cloud->points.size()];
    pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
    kdtree.setInputCloud(cloud);
    pcl::PointXYZRGBA searchPoint;

    double *SmallestEigen = new double[cloud->points.size()];
    double *MiddleEigen = new double[cloud->points.size()];
    double *LargestEigen = new double[cloud->points.size()];

    double *DLS = new double[cloud->points.size()];
    double *DLM = new double[cloud->points.size()];
    double *DMS = new double[cloud->points.size()];
    double *Sigma = new double[cloud->points.size()];

    //  ************ All the Points of the cloud *******************
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        searchPoint.x = cloud->points[i].x;
        searchPoint.y = cloud->points[i].y;
        searchPoint.z = cloud->points[i].z;

        if (kdtree.nearestKSearch(searchPoint, KNumbersNeighbor, NeighborsKNSearch, NeighborsKNSquaredDistance) > 0)
        {
            NumbersNeighbor[i] = NeighborsKNSearch.size();
        }
        else
        {
            NumbersNeighbor[i] = 0;
        }

        float Xmean;
        float Ymean;
        float Zmean;
        float sum = 0.00;
        // *Computing Covariance Matrix
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += cloud->points[NeighborsKNSearch[ii]].x;
        }
        Xmean = sum / NumbersNeighbor[i];
        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += cloud->points[NeighborsKNSearch[ii]].y;
        }
        Ymean = sum / NumbersNeighbor[i];
        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += cloud->points[NeighborsKNSearch[ii]].z;
        }
        Zmean = sum / NumbersNeighbor[i];

        float CovXX;
        float CovXY;
        float CovXZ;
        float CovYX;
        float CovYY;
        float CovYZ;
        float CovZX;
        float CovZY;
        float CovZZ;

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].x - Xmean) * (cloud->points[NeighborsKNSearch[ii]].x - Xmean));
        }
        CovXX = sum / (NumbersNeighbor[i] - 1);

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].x - Xmean) * (cloud->points[NeighborsKNSearch[ii]].y - Ymean));
        }
        CovXY = sum / (NumbersNeighbor[i] - 1);

        CovYX = CovXY;

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].x - Xmean) * (cloud->points[NeighborsKNSearch[ii]].z - Zmean));
        }
        CovXZ = sum / (NumbersNeighbor[i] - 1);

        CovZX = CovXZ;

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].y - Ymean) * (cloud->points[NeighborsKNSearch[ii]].y - Ymean));
        }
        CovYY = sum / (NumbersNeighbor[i] - 1);

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].y - Ymean) * (cloud->points[NeighborsKNSearch[ii]].z - Zmean));
        }
        CovYZ = sum / (NumbersNeighbor[i] - 1);

        CovZY = CovYZ;

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].z - Zmean) * (cloud->points[NeighborsKNSearch[ii]].z - Zmean));
        }
        CovZZ = sum / (NumbersNeighbor[i] - 1);

        // *Computing Eigenvalue and EigenVector
        Eigen::Matrix3f Cov;
        Cov << CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(Cov);
        if (eigensolver.info() != Eigen::Success)
            abort();

        double EigenValue1 = eigensolver.eigenvalues()[0];
        double EigenValue2 = eigensolver.eigenvalues()[1];
        double EigenValue3 = eigensolver.eigenvalues()[2];

        double Smallest = 0.00;
        double Middle = 0.00;
        double Largest = 0.00;
        if (EigenValue1 < EigenValue2)
        {
            Smallest = EigenValue1;
        }
        else
        {
            Smallest = EigenValue2;
        }
        if (EigenValue3 < Smallest)
        {
            Smallest = EigenValue3;
        }

        if (EigenValue1 <= EigenValue2 && EigenValue1 <= EigenValue3)
        {
            Smallest = EigenValue1;
            if (EigenValue2 <= EigenValue3)
            {
                Middle = EigenValue2;
                Largest = EigenValue3;
            }
            else
            {
                Middle = EigenValue3;
                Largest = EigenValue2;
            }
        }

        if (EigenValue1 >= EigenValue2 && EigenValue1 >= EigenValue3)
        {
            Largest = EigenValue1;
            if (EigenValue2 <= EigenValue3)
            {
                Smallest = EigenValue2;
                Middle = EigenValue3;
            }
            else
            {
                Smallest = EigenValue3;
                Middle = EigenValue2;
            }
        }

        if ((EigenValue1 >= EigenValue2 && EigenValue1 <= EigenValue3) || (EigenValue1 <= EigenValue2 && EigenValue1 >= EigenValue3))
        {
            Middle = EigenValue1;
            if (EigenValue2 >= EigenValue3)
            {
                Largest = EigenValue2;
                Smallest = EigenValue3;
            }
            else
            {
                Largest = EigenValue3;
                Smallest = EigenValue2;
            }
        }

        SmallestEigen[i] = Smallest;
        MiddleEigen[i] = Middle;
        LargestEigen[i] = Largest;

        DLS[i] = std::abs(SmallestEigen[i] / LargestEigen[i]); // std::abs ( LargestEigen[i] -  SmallestEigen[i] ) ;
        DLM[i] = std::abs(MiddleEigen[i] / LargestEigen[i]);   // std::abs (  LargestEigen[i] - MiddleEigen[i] ) ;
        DMS[i] = std::abs(SmallestEigen[i] / MiddleEigen[i]);  // std::abs ( MiddleEigen[i] -  SmallestEigen[i] ) ;
        Sigma[i] = (SmallestEigen[i]) / (SmallestEigen[i] + MiddleEigen[i] + LargestEigen[i]);
    } // For each point of the cloud

    std::cout << "Computing Sigma is Done! " << std::endl;
    // *Color Map For the difference of the eigen values

    double MaxD = 0.00;
    double MinD = cloud->points.size();

    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        if (Sigma[i] < MinD)
            MinD = Sigma[i];
        if (Sigma[i] > MaxD)
            MaxD = Sigma[i];
    }

    std::cout << "Minimum is :" << MinD << std::endl;
    std::cout << "Maximum  is :" << MaxD << std::endl;

    //   *****************************************
    // *Red and white (khaki)

    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        cloud->points[i].r = 240;
        cloud->points[i].g = 230;
        cloud->points[i].b = 140;
    }

    float step = ((MaxD - MinD) / 100);
    std::vector<Eigen::Vector3f> Edge;
    int EdgeNum = 0;
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        if (Sigma[i] > (MinD + (10 * step)))
        {
            // *Original: 6 * step
            cloud->points[i].r = 255;
            cloud->points[i].g = 0;
            cloud->points[i].b = 0;
            Eigen::Vector3f temp(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
            Edge.push_back(temp);
            EdgeNum++;
        }
    }

    Eigen::Vector3f mu(0.0, 0.0, 0.0);
    float mu_distance = 0.0;
    float mean_distance = 0.0;
    const int N = Edge.size();
    for(size_t i = 0; i < N; i++) {
        mu = mu + Edge[i];
        mean_distance = mean_distance + sqrt(Edge[i].dot(Edge[i]));
    }
    mu = mu / N;
    mu_distance = sqrt(mu.dot(mu));
    mean_distance = mean_distance / N;
    std::cout << "Edge Points Center: " << mu(0) << ' ' << mu(1) << ' ' << mu(2) << std::endl;
    std::cout << "Distance of Edge Points Center: " << mu_distance << std::endl;
    std::cout << "Mean Distance of Edge Points: " << mean_distance << std::endl;

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

    std::cout << "Number of Edge points  is :" << EdgeNum << std::endl;

    pcl::io::savePCDFileASCII(output_pcd, *cloud);
    std::cerr << "Saved " << cloud->size() << " data points to test_pcd.pcd." << std::endl;

    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped())
    {
    }

    return 0;
}