/*
 * Copyright (c) 2020-2021, Marco Sánchez Beeckman
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <array>
#include <chrono>
#include <iostream>
#include <limits>
#include <random>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <opencv2/core.hpp>

#include "Homography.h"

namespace tfg {

/**
 * Obtain weights that minimize the smooth truncated quadratic cost given by equation (2) in Szeliski's paper.
 * The formula for these weights is shown in equation (3).
 * @param residuals2 Squared track residuals, ordered by track.
 * @param tau2 Tau squared, an inlier threshold for equations (2) to (4) in Szeliski's paper.
 * @return The squared weights, ordered by track.
 */
std::vector<float> getWeights2(std::vector<float> &residuals2, float tau2) {
    std::vector<float> weights2;
    weights2.reserve(residuals2.size());
    for(unsigned int i = 0; i < residuals2.size(); i++) {
        float boundedResidual2 = residuals2[i] < tau2 ? residuals2[i] : tau2;
        float weight2 = 1 - boundedResidual2/tau2;
        weights2.push_back(weight2);
    }
    return weights2;
}

std::vector<float> getWeightsFromInliers(std::vector<std::vector<int>> &inliers, std::shared_ptr<tfg::TrackTable> &trackTable) {
    std::vector<float> weights(trackTable->numberOfTracks(), 0);
    std::vector<unsigned int> timesCountedAsInlier(trackTable->numberOfTracks(), 0);
    for(unsigned int f = 0; f < inliers.size(); f++) {
        std::vector<unsigned int> trajectories = trackTable->trajectoriesInFrame(f);
        for(unsigned int i = 0; i < inliers[f].size(); i++) {
            timesCountedAsInlier[trajectories[inliers[f][i]]]++;
        }
    }

    for(unsigned int t = 0; t < weights.size(); t++) {
        const unsigned int trackDuration = trackTable->durationOfTrack(t);
        // weights[t] = timesCountedAsInlier[t] == 0 ? 0 : 1;
        weights[t] = timesCountedAsInlier[t] == trackDuration - 1 ? 1 : 0;
    }

    return weights;
}

/**
 * Obtain the homography that fits a model the best given a set of origin points and another set of destinations, using RANSAC.
 * @param p0 Origin points.
 * @param p1 Destination points.
 * @param n Number of points.
 * @param niter Number of iterations for RANSAC.
 * @param tolerance Maximum distance in pixels to consider a point an inlier.
 * @param H Output homography.
 * @param inliers Output list of indexes that correspond to those of the tracks that are inliers.
 */
void computeHomographyRANSAC(const std::vector<cv::Vec2f> &p0, const std::vector<cv::Vec2f> &p1, int n, int niter, float tolerance, cv::Matx33f &H, std::vector<int> &inliers) {

    float tolerance2 = tolerance * tolerance;
    unsigned int maxInliers = 0;
    std::vector<int> bestInliers;

    std::array<int, 4> randomIndices;
    std::random_device randomDevice;
    std::mt19937 mersenneTwister(randomDevice());
    std::uniform_int_distribution<int> uniformIntDist(0, n-1);

    for(int iter = 0; iter < niter; iter++) {

        // Sample 4 different numbers between 0 and n-1
        for(int i = 0; i < 4; i++) {
            int randomNumber = uniformIntDist(mersenneTwister);
            while(std::find(randomIndices.begin(), randomIndices.end(), randomNumber) != randomIndices.end()) {
                randomNumber = uniformIntDist(mersenneTwister);
            }
            randomIndices[i] = randomNumber;
        }

        // Obtain the 4 points corresponding to the random indices
        std::vector<cv::Vec2f> pointsL(4);
        std::vector<cv::Vec2f> pointsR(4);
        for(int i = 0; i < 4; i++) {
            pointsL[i] = p0[randomIndices[i]];
            pointsR[i] = p1[randomIndices[i]];
        }

        // Compute homography using Least Squares method
        cv::Matx33f Haux;
        std::vector<unsigned int> zeros(4, 0);
        std::vector<float> ones(1, 1.0f);
        tfg::computeHomographyWLS(pointsL, pointsR, 4, zeros, ones, Haux);

        // Obtain the inliers for this iteration
        std::vector<int> iterationInliers;
        iterationInliers.reserve(n);
        for(int i = 0; i < n; i++) {
            cv::Vec3f pointOrigin;
            cv::Vec3f pointDestination;
            pointOrigin(0) = p0[i](0);      pointOrigin(1) = p0[i](1);      pointOrigin(2) = 1.0f;
            pointDestination(0) = p1[i](0); pointDestination(1) = p1[i](1); pointDestination(2) = 1.0f;

            cv::Vec3f pred = Haux * pointOrigin;

            if(pred(2) == 0.0f) continue;

            pred(0) = pred(0)/pred(2); pred(1) = pred(1)/pred(2); pred(2) = 1.0f;
            float reprojectionError2 = cv::norm(pointDestination - pred, cv::NORM_L2SQR);
            if(reprojectionError2 < tolerance2) {
                iterationInliers.push_back(i);
            }
        }

        // If the number of inliers has increased from the best iteration, update it along with the homography
        const unsigned int numInliers = iterationInliers.size();
        if(numInliers > maxInliers) {
            maxInliers = numInliers;
            bestInliers.swap(iterationInliers);

            H(0,0) = Haux(0,0); H(0,1) = Haux(0,1); H(0,2) = Haux(0,2);
            H(1,0) = Haux(1,0); H(1,1) = Haux(1,1); H(1,2) = Haux(1,2);
            H(2,0) = Haux(2,0); H(2,1) = Haux(2,1); H(2,2) = Haux(2,2);
        }

    }
    inliers.swap(bestInliers);
}

/**
 * Compute an homography using Weighted Least Squares, i.e. minimize ||WAh||, whose solution is the eigenvector with smallest eigenvalue of the matrix AtWtWA.
 * @param p0 Origin points.
 * @param p1 Destination points.
 * @param n Number of points.
 * @param trajectories The index of the trajectory corresponding to each one of the points.
 * @param weights2 The weights of each track, squared.
 * @param H Output homography.
 */
void computeHomographyWLS(const std::vector<cv::Vec2f> &p0, const std::vector<cv::Vec2f> &p1, int n, const std::vector<unsigned int> &trajectories, const std::vector<float> &weights2, cv::Matx33f &H) {

    // Normalize the left and right observations
    std::vector<cv::Vec2f> pnorm0;
    std::vector<cv::Vec2f> pnorm1;
    cv::Vec2f centerL(0, 0), centerR(0, 0);
    cv::Vec2f scaleL(0, 0), scaleR(0, 0);
    tfg::isotropicNormalization(p0, pnorm0, centerL, scaleL);
    tfg::isotropicNormalization(p1, pnorm1, centerR, scaleR);

    ////////////////////////// Minimization problem || Ah || /////////////////////////////

    Eigen::Matrix3d Tpl, Timinv;

    // Similarity transformation of the plane
    Tpl(0, 0) = scaleL(0); Tpl(0, 1) = 0.0;       Tpl(0, 2) = -scaleL(0)*centerL(0);
    Tpl(1, 0) = 0.0;       Tpl(1, 1) = scaleL(1); Tpl(1, 2) = -scaleL(1)*centerL(1);
    Tpl(2, 0) = 0.0;       Tpl(2, 1) = 0.0;       Tpl(2, 2) = 1.0;

    // Inverse similarity transformation of the image
    Timinv(0, 0) = 1.0/scaleR(0); Timinv(0, 1) = 0.0;           Timinv(0, 2) = centerR(0);
    Timinv(1, 0) = 0.0;           Timinv(1, 1) = 1.0/scaleR(1); Timinv(1, 2) = centerR(1);
    Timinv(2, 0) = 0.0;           Timinv(2, 1) = 0.0;           Timinv(2, 2) = 1.0;

    // Build At*Wt*W*A
    Eigen::Matrix<double, 9, 9> AtA = Eigen::Matrix<double, 9, 9>::Zero();
    for(int i = 0; i < n; i++) {
        float xpl = pnorm0[i](0), ypl = pnorm0[i](1),
                xim = pnorm1[i](0), yim = pnorm1[i](1);

        std::array<float, 9> row1 = {0.0f, 0.0f, 0.0f, -xpl, -ypl, -1.0f, yim * xpl, yim * ypl, yim};
        std::array<float, 9> row2 = {xpl, ypl, 1.0f, 0.0f, 0.0f, 0.0f, -xim * xpl, -xim * ypl, -xim};
        float w2 = weights2[trajectories[i]];

        // Lower half only, since SelfAdjointEigenSolver does not use the upper half
        for(int j = 0; j < 9; j++) {
            for(int k = j; k < 9; k++) {
                AtA(k, j) += w2 * row1[j] * row1[k] + w2 * row2[j] * row2[k];
            }
        }
    }

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 9, 9>> eigenSolver;
    eigenSolver.compute(AtA);
    Eigen::Matrix3d V(eigenSolver.eigenvectors().col(0).data()); V.transposeInPlace();

    // Denormalize H = Timinv * V.col(imin)* Tpl;
    Eigen::Matrix3d Vdenorm = Timinv * (V * Tpl);

    // Divide every entry by H(2, 2) so the bottom right entry is 1
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++) {
            H(i, j) = static_cast<float>(Vdenorm(i, j)/Vdenorm(2, 2));
        }
    }
}

/**
 * Normalize points such that their centroid is at the origin, and their average distance to it is sqrt(2).
 * @param points Vector of points.
 * @param normalizedPoints Output vector of normalized points.
 * @param center Output computed centroid of the points.
 * @param scale Output computed scaling factor.
 */
void isotropicNormalization(const std::vector<cv::Vec2f> &points, std::vector<cv::Vec2f> &normalizedPoints, cv::Vec2f &center, cv::Vec2f &scale) {
    const int NUMBER_OF_POINTS = points.size();
    normalizedPoints.clear();
    normalizedPoints.reserve(NUMBER_OF_POINTS);

    // Compute baricenter of the observations
    center(0) = 0.0f; center(1) = 0.0f;
    for(int i = 0; i < NUMBER_OF_POINTS; i++) {
        center = center + points[i];
    }
    center = center / NUMBER_OF_POINTS;

    // Scaling so that the average distance from the center is sqrt(2)
    scale(0) = 0.0f; scale(1) = 0.0f;
    for(int i = 0; i < NUMBER_OF_POINTS; i++) {
        const float obsDistance = cv::norm(points[i] - center);
        scale(0) = scale(0) + obsDistance;
        scale(1) = scale(1) + obsDistance;
    }
    scale(0) = (sqrtf(2.0f) * NUMBER_OF_POINTS) / scale(0);
    scale(1) = (sqrtf(2.0f) * NUMBER_OF_POINTS) / scale(1);

    // Normalize the observations
    for(int i = 0; i < NUMBER_OF_POINTS; i++) {
        normalizedPoints[i](0) = (points[i](0) - center(0)) * scale(0);
        normalizedPoints[i](1) = (points[i](1) - center(1)) * scale(1);
    }
}

/**
 * Perform Iteratively Reweighted Least Squares on a model with already computed initial homographies and weights.
 * @param model The initial model, to be updated on each iteration if an improvement is made.
 * @param trackTable The table with the previously computed tracks.
 * @param weights2 The squared weights of the tracks.
 * @param tau2 Tau squared, an inlier threshold for equations (2) to (4) in Szeliski's paper.
 */
void IRLS(std::shared_ptr<tfg::MotionModel> &model, std::shared_ptr<tfg::TrackTable> &trackTable, std::vector<float> &weights2, float tau2) {
    // std::cout << "First homography of the initial model:" << '\n';
    // model->printHomography(0);

    float s = 1.0f;
    unsigned int k = 0;
    float bestCost = -1;

    std::vector<float> initialWeights2(weights2.size(), 1.0f);
    std::vector<float> refinedWeights2(weights2.size(), 1.0f);
    std::vector<float> bestWeights2(weights2.size(), 1.0f);

    bool converged = false;
    unsigned int numIter = 0;
    initialWeights2.swap(weights2);
    std::shared_ptr<tfg::MotionModel> refinedModel;

    while(numIter < 50 && !converged) {
        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

        // Refine model with new weights
        refinedModel = std::make_shared<tfg::MotionModel>(trackTable, tau2);
        refinedModel->fitFromWeights(initialWeights2);

        // Compute residuals of the new model
        std::vector<float> residuals2 = refinedModel->getResiduals2();
        std::vector<float> currentWeights2 = tfg::getWeights2(residuals2, tau2);
        // Update weights based on the current best model and the new one, then get the cost of the model
        for(unsigned int i = 0; i < weights2.size(); i++) {
            refinedWeights2[i] = s*currentWeights2[i] + (1-s)*bestWeights2[i];
        }
        float cost = refinedModel->getCost();

        std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

        std::cout << "(" << k + 1 << ") Iteration cost: " << cost << '\n';
        std::cout << "(" << k + 1 << ") Cost margin: " << std::abs((cost - bestCost) / bestCost) << '\n';
        std::cout << "(" << k + 1 << ") Time: " << (std::chrono::duration_cast<std::chrono::microseconds>(end-begin).count())/1000000.0 << " seconds" << '\n';

        // If the cost has increased, reduce the step. If it has decreased, make the step bigger and update the best model to the new one
        if(cost > bestCost && bestCost >= 0) {
            s = s/4;
        } else {
            if(cost > 0 && std::abs((cost - bestCost) / bestCost) < 0.00001) converged = true;
            bestCost = cost;
            k = k + 1;
            s = 4*s < 1 ? 4*s : 1;
            for(unsigned int i = 0; i < bestWeights2.size(); i++) {
                bestWeights2[i] = initialWeights2[i];
            }
            model = refinedModel;
        }
        initialWeights2.swap(refinedWeights2);
        numIter++;
    }
    weights2.swap(bestWeights2);
}

} // namespace tfg


#include <algorithm>
#include <boost/filesystem.hpp>
#include <chrono>
#include <cmath>
#include <functional>
#include <list>
#include <map>
#include <memory>
#include <optional>
#include <random>
#include <set>
#include <sstream>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <vector>


#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE UnitTest

#include <string>
#include <fstream>
#include <boost/hana.hpp>
#include <boost/test/unit_test.hpp>
#include <rapidjson/rapidjson.h>
#include <rapidjson/reader.h>

#include "boilerplateCodeDoc.h"

namespace hana = boost::hana;
namespace boiler = boilerplateCodeDoc;

// make CTEST_OUTPUT_ON_FAILURE=1 test
// just logging something ( --log_level=message )

static constexpr const char* const CSS {R"(
table, th, td {
    border: 1px solid black;
    border-collapse: collapse;
}
th, td {
    padding: 5px;
    text-align: left;
}
table#t01 {
    width: 100%;
    background-color: #f1f1c1;
}
)"};

struct GlobalInit {
  GlobalInit() : argc(boost::unit_test_framework::framework::master_test_suite().argc),
		argv(boost::unit_test_framework::framework::master_test_suite().argv){}
  ~GlobalInit() {}
  int argc {};
  char **argv {nullptr};
};
BOOST_GLOBAL_FIXTURE( GlobalInit );

BOOST_AUTO_TEST_CASE( test000 ) {
   BOOST_TEST_MESSAGE( "\ntest000: BASIC USAGE of HANA lib" );

   using namespace hana::literals;

   struct Fish { std::string name; };
   struct Cat  { std::string name; };
   struct Dog  { std::string name; };

     // Sequences capable of holding heterogeneous objects, and algorithms
  // to manipulate them.
  auto animals = hana::make_tuple(Fish{"Nemo"}, Cat{"Garfield"}, Dog{"Snoopy"});
  auto names = hana::transform(animals, [](auto a) {
    return a.name;
  });
  BOOST_CHECK(hana::reverse(names) == hana::make_tuple("Snoopy", "Garfield", "Nemo"));

  // No compile-time information is lost: even if `animals` can't be a
  // constant expression because it contains strings, its length is constexpr.
  BOOST_CHECK(hana::length(animals) == 3u);

  // Computations on types can be performed with the same syntax as that of
  // normal C++. Believe it or not, everything is done at compile-time.
  auto animal_types = hana::make_tuple(hana::type_c<Fish*>, hana::type_c<Cat&>, hana::type_c<Dog*>);
  auto animal_ptrs = hana::filter(animal_types, [](auto a) {
    return hana::traits::is_pointer(a);
  });
  BOOST_CHECK(animal_ptrs == hana::make_tuple(hana::type_c<Fish*>, hana::type_c<Dog*>));

  // And many other goodies to make your life easier, including:
  // 1. Access to elements in a tuple with a sane syntax.
  static_assert(animal_ptrs[0_c] == hana::type_c<Fish*>, "");
  static_assert(animal_ptrs[1_c] == hana::type_c<Dog*>, "");

  // 2. Unroll loops at compile-time without hassle.
  std::string s;
  hana::int_c<10>.times([&]{ s += "x"; });
  // equivalent to s += "x"; s += "x"; ... s += "x";

  // 3. Easily check whether an expression is valid.
  //    This is usually achieved with complex SFINAE-based tricks.
  auto has_name = hana::is_valid([](auto&& x) -> decltype((void)x.name) { });
  BOOST_CHECK(has_name(animals[0_c]));
  BOOST_CHECK(!has_name(1));
}

BOOST_AUTO_TEST_CASE( test001 ) {
   BOOST_TEST_MESSAGE( "\ntest001: BASIC USAGE of RapidJSON lib" );

   using namespace rapidjson;
   using namespace std;

   struct MyHandler {
       bool Null() { BOOST_TEST_MESSAGE("Null()"); return true; }
       bool Bool(bool b) { BOOST_TEST_MESSAGE("Bool(" << boolalpha << b << ")"); return true; }
       bool Int(int i) { BOOST_TEST_MESSAGE("Int(" << i << ")"); return true; }
       bool Uint(unsigned u) { BOOST_TEST_MESSAGE("Uint(" << u << ")"); return true; }
       bool Int64(int64_t i) { BOOST_TEST_MESSAGE("Int64(" << i << ")"); return true; }
       bool Uint64(uint64_t u) { BOOST_TEST_MESSAGE("Uint64(" << u << ")"); return true; }
       bool Double(double d) { BOOST_TEST_MESSAGE("Double(" << d << ")"); return true; }
       bool RawNumber(const char* str, SizeType length, bool copy) {
	   BOOST_TEST_MESSAGE("Number(" << str << ", " << length << ", " << boolalpha << copy << ")");
	   return true;
       }
       bool String(const char* str, SizeType length, bool copy) {
	   BOOST_TEST_MESSAGE("String(" << str << ", " << length << ", " << boolalpha << copy << ")");
	   return true;
       }
       bool StartObject() { BOOST_TEST_MESSAGE("StartObject()"); return true; }
       bool Key(const char* str, SizeType length, bool copy) {
	   BOOST_TEST_MESSAGE("Key(" << str << ", " << length << ", " << boolalpha << copy << ")");
	   return true;
       }
       bool EndObject(SizeType memberCount) { BOOST_TEST_MESSAGE("EndObject(" << memberCount << ")"); return true; }
       bool StartArray() { BOOST_TEST_MESSAGE("StartArray()"); return true; }
       bool EndArray(SizeType elementCount) { BOOST_TEST_MESSAGE("EndArray(" << elementCount << ")"); return true; }
   };

   const char json[] = " { \"hello\" : \"world\", \"t\" : true , \"f\" : false, \"n\": null, \"i\":123, \"pi\": 3.1416, \"a\":[1, 2, 3, 4] } ";

   MyHandler handler;
   Reader reader;
   StringStream ss(json);

   BOOST_CHECK(kParseErrorNone == reader.Parse(ss, handler));
}

BOOST_AUTO_TEST_CASE( test002 ) {
   BOOST_TEST_MESSAGE( "\ntest002: Transform external Json Schema into HTML");

   // taken for granted that CMake copied default json schema file in the very directory where this test binary is generated
   std::string filename{"schema.json"};
   std::string binary{boost::unit_test::framework::master_test_suite().argv[0]};
   size_t found = binary.find_last_of("/\\");
   if( found > 0 ) {
	BOOST_TEST_MESSAGE( "found=" << found);
	filename = binary.substr(0,found+1) + filename;
   }

   int argc =boost::unit_test::framework::master_test_suite().argc;
   if( argc > 1) {
	filename = std::string(boost::unit_test::framework::master_test_suite().argv[1]);
   }
   BOOST_TEST_MESSAGE( "current binary=" << binary );
   BOOST_TEST_MESSAGE( "schema.json=" << filename );

   boiler::JsonSchema jsonSchema{filename};
   BOOST_TEST_MESSAGE( "Json Schema: " << jsonSchema.message);
   boiler::JsonSchema2HTML handler {};
   bool result = handler(jsonSchema);
   BOOST_TEST_MESSAGE( "JsonSchema2HTML: " << handler.message << "\n\n" << handler.filtered);
   BOOST_CHECK( result );
}

BOOST_AUTO_TEST_CASE( test003 ) {
   BOOST_TEST_MESSAGE( "\ntest003: Transform external Json Schema into C++ structure header");

   // taken for granted that CMake copied default json schema file in the very directory where this test binary is generated
   std::string filename{"schema.json"};
   std::string binary{boost::unit_test::framework::master_test_suite().argv[0]};
   size_t found = binary.find_last_of("/\\");
   if( found > 0 ) {
	BOOST_TEST_MESSAGE( "found=" << found);
	filename = binary.substr(0,found+1) + filename;
   }

   int argc =boost::unit_test::framework::master_test_suite().argc;
   if( argc > 1) {
	filename = std::string(boost::unit_test::framework::master_test_suite().argv[1]);
   }
   BOOST_TEST_MESSAGE( "current binary=" << binary );
   BOOST_TEST_MESSAGE( "schema.json=" << filename );

   boiler::JsonSchema jsonSchema{filename};
   BOOST_TEST_MESSAGE( "Json Schema: " << jsonSchema.message);
   boiler::JsonSchema2H handler {};
   bool result = handler(jsonSchema);
   BOOST_TEST_MESSAGE( "JsonSchema2H: " << handler.message << "\n\n" << handler.filtered);
   BOOST_CHECK( result );
}

BOOST_AUTO_TEST_CASE( test004 ) {
   BOOST_TEST_MESSAGE( "\ntest004: Transform external Json Schema into C++ structure file");

   // taken for granted that CMake copied default json schema file in the very directory where this test binary is generated
   std::string filename{"schema.json"};
   std::string binary{boost::unit_test::framework::master_test_suite().argv[0]};
   size_t found = binary.find_last_of("/\\");
   if( found > 0 ) {
	BOOST_TEST_MESSAGE( "found=" << found);
	filename = binary.substr(0,found+1) + filename;
   }

   int argc =boost::unit_test::framework::master_test_suite().argc;
   if( argc > 1) {
	filename = std::string(boost::unit_test::framework::master_test_suite().argv[1]);
   }
   BOOST_TEST_MESSAGE( "current binary=" << binary );
   BOOST_TEST_MESSAGE( "schema.json=" << filename );

   boiler::JsonSchema jsonSchema{filename};
   BOOST_TEST_MESSAGE( "Json Schema: " << jsonSchema.message);
   boiler::JsonSchema2CPP handler {};
   bool result = handler(jsonSchema);
   BOOST_TEST_MESSAGE( "JsonSchema2CPP: " << handler.message << "\n\n" << handler.filtered);
   BOOST_CHECK( result );
}


//N0673230
#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

BOOST_AUTO_TEST_SUITE( total_Height_Gain )

const bool isFileName = true;

// The elevation of ABCD.gpx is always 0. This means that the totalHeightGain is always going to be 0
//this test checks that the function returns 0 for the ABCD.gpx file.
BOOST_AUTO_TEST_CASE( ABCD_HeightGain )
{
   Route route = Route(LogFiles::GPXRoutesDir + "ABCD.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.totalHeightGain(), 0 );
}

// The elevation of NorthYorkMoors.gpx changes, and the total should be 285648.
//this test checks that the function returns 285648 for the NorthYorkMoors.gpx file.
BOOST_AUTO_TEST_CASE( North_HeightGain )
{
   Route route = Route(LogFiles::GPXRoutesDir + "NorthYorkMoors.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.totalHeightGain(), 285648, 0.01 );
}

//the minimum elevation possible is the same as the radius of the earth. If a value exceeds this it shouldn't be counted and an out_of_range error should be thrown.
// this test case uses ExtremeValues.gpx to see if the function throws an error for extreme elevation values.
//if the extreme value causes and error to be throw, then it passes the test.
BOOST_AUTO_TEST_CASE( ExtremeValues_HeightGain )
{
   Route route = Route(LogFiles::GPXRoutesDir + "ExtremeValues.gpx", isFileName);
   BOOST_CHECK_THROW( route.totalHeightGain(), std::out_of_range );
}

//As there is below sea level it is possible that the elevation may be negative.
//This means that the function needs to be able to work out negative elevations as well.
//this test case uses the BelowSeaLevel.gpx file to find out if the function can correctly calculate a negative total elevation for a route below sea level.
//if the elevation is calculated correctly the function passes the test.
//this also makes sure that the previous test case only works for extreme negative values, i.e. the ones greater than the radius of the earth.

BOOST_AUTO_TEST_CASE( BelowSeaLevel_HeightGain )
{
   Route route = Route(LogFiles::GPXRoutesDir + "BelowSeaLevel.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.totalHeightGain(), -300, 0.01);
}

//Although it is possible to go higher, the maximum elevation that a person can reach on foot is the summit of Mount Everest. Which is 8848 metres above sea level.
//this test case checks that the function accepts extreme positive elevation values.
//As it is technically possible to go above this level, the function doesn't limit the elevation. But this test tests for large values.
BOOST_AUTO_TEST_CASE( MountEverest_HeightGain )
{
   Route route = Route(LogFiles::GPXRoutesDir + "MountEverest.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.totalHeightGain(), 26524, 0.01);
}

BOOST_AUTO_TEST_SUITE_END()


#include <ros/ros.h>
#include <stdio.h>

#include <tf/transform_listener.h>
#include <Eigen/Dense>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_tf_listener");
  ros::NodeHandle node("~");
  //ros::NodeHandle node;

  tf::TransformListener listener;
  ros::Rate rate(1);
  float r_x = 0.0f;
  float r_y = 0.0f;
  float r_z = 0.0f;
  float r_w = 0.0f;

  float t_x = 0.0f;
  float t_y = 0.0f;
  float t_z = 0.0f;
  int count = 0;

  bool cali;
  int caliStep;
  node.param("calibration", cali, true);
  node.param("calibrationStep", caliStep, 1);
  // std::cout << "The value of calibration " << cali << std::endl;

  //while (node.ok())
  while (ros::ok())
  {
      // hold the transformation parameters between two coordinate frames
      //tf::StampedTransform result_cali;
      tf::StampedTransform transformPara;
      try
      {
        if (cali)
        {
          if (caliStep == 1)
          {
            // listener.waitForTransform("inter_robotBase_frame", "endEffectorI5_frame", ros::Time(0), ros::Duration(3.0));
            listener.waitForTransform("base_link", "camera_frame1", ros::Time(0), ros::Duration(4.0));
            // For calibration - calculate the transformation between the two frames
            listener.lookupTransform("camera_frame1", "base_link", ros::Time(0), transformPara);
            // listener.lookupTransform("base_link", "camera_frame1", ros::Time(0), transformPara);

            // get the value
            tf::Quaternion rotation = transformPara.getRotation();
            tf::Vector3 tfVect = transformPara.getOrigin();

            std::cout << "Calibration at Step 1 - Inverse Quaternion from Auto I5 Robot" << std::endl;
            ROS_INFO("Origin & Quat: %f %f %f %f %f %f %f", tfVect.x(), tfVect.y(), tfVect.z(), rotation.x(), rotation.y(), rotation.z(), rotation.w());

            tf::Matrix3x3 mat(rotation);
            tfScalar y, p, r;
            mat.getEulerYPR(y, p, r);

            ROS_INFO("Three Angles: %f %f %f ", r, p, y);

            // btQuaternion::btMatrix3x3 tfMatrix; // = transformPara.getBasis();

            // geometry_msgs::Pose robot_pose;
            Eigen::Matrix4f hete_matrix;

            // for translation
            hete_matrix(0,3) = tfVect.x();
            hete_matrix(1,3) = tfVect.y();
            hete_matrix(2,3) = tfVect.z();
            hete_matrix(3,3) = 1;
            // the last row
            hete_matrix(3,0) = 0;
            hete_matrix(3,1) = 0;
            hete_matrix(3,2) = 0;

            // for rotation part
            hete_matrix(0,0) = 1 - 2*pow(rotation.y(),2) - 2*pow(rotation.z(),2);
            hete_matrix(1,0) = 2*(rotation.x()*rotation.y() + rotation.z()*rotation.w());
            hete_matrix(2,0) = 2*(rotation.x()*rotation.z() - rotation.y()*rotation.w());

            hete_matrix(0,1) = 2*(rotation.x()*rotation.y() - rotation.z()*rotation.w());
            hete_matrix(1,1) = 1 - 2*pow(rotation.x(),2) - 2*pow(rotation.z(),2);
            hete_matrix(2,1) = 2*(rotation.x()*rotation.w() + rotation.y()*rotation.z());

            hete_matrix(0,2) = 2*(rotation.x()*rotation.z() + rotation.y()*rotation.w());
            hete_matrix(1,2) = 2*(rotation.y()*rotation.z() - rotation.x()*rotation.w());
            hete_matrix(2,2) = 1 - 2*pow(rotation.x(),2) - 2*pow(rotation.y(),2);

            ROS_INFO("Matrix: %f %f %f %f ", hete_matrix(0,0), hete_matrix(0,1), hete_matrix(0,2), hete_matrix(0,3));
            ROS_INFO("Matrix: %f %f %f %f ", hete_matrix(1,0), hete_matrix(1,1), hete_matrix(1,2), hete_matrix(1,3));
            ROS_INFO("Matrix: %f %f %f %f ", hete_matrix(2,0), hete_matrix(2,1), hete_matrix(2,2), hete_matrix(2,3));
            ROS_INFO("Matrix: %f %f %f %f ", hete_matrix(3,0), hete_matrix(3,1), hete_matrix(3,2), hete_matrix(3,3));

            // ROS_INFO("Matrix: %f %f %f ", tfMatrix.xz(), tfMatrix.yz(), tfMatrix.zz());
          }

          if (caliStep == 2)
          {
            count++;
            // wait until the two frames of camera and robot base are available
            listener.waitForTransform("camera_depth_optical_frame", "depend_robotBase_frame", ros::Time(0), ros::Duration(3.0));
            // For calibration - calculate the transformation between the two frames
            listener.lookupTransform("camera_depth_optical_frame", "depend_robotBase_frame", ros::Time(0), transformPara);

            // get the angle/quaternion
            tf::Quaternion rotation = transformPara.getRotation();
            // get the distance between the two frames
            tf::Vector3 tfVect = transformPara.getOrigin();

            r_x += rotation.x();
            r_y += rotation.y();
            r_z += rotation.z();
            r_w += rotation.w();
            t_x += tfVect.x();
            t_y += tfVect.y();
            t_z += tfVect.z();

            ROS_INFO("Gathering data %d", count);
            if (count == 100)
            {
                std::cout << "Calibration at Step 2 - Quaternion of robotBase frame comapring to camera_frame" << std::endl;
                //ROS_INFO("Quat: %f %f %f %f", );
                ROS_INFO("Origin & Quat: %f %f %f %f %f %f %f", t_x/float(count), t_y/float(count), t_z/float(count), r_x/float(count), r_y/float(count), r_z/float(count), r_w/float(count));
                count = 0;
                t_x = 0.0;
                t_y = 0.0;
                t_z = 0.0;
                r_x = 0.0;
                r_y = 0.0;
                r_z = 0.0;
                r_w = 0.0;
            }
          }
        }
        else
        {
          // For xtion_frame.txt & displaying axes in OpenRave
          // wait for two frames: estimate_robot_base_frame and grasp_aubo_frame available
          listener.waitForTransform("fix_robotBase_frame", "grasp_aubo_frame", ros::Time(0), ros::Duration(3.0));

          // transform the coordiantes from grasp_aubo_frame to estimate_robot_base_frame
          // For display grasp in OpenRave
          listener.lookupTransform("fix_robotBase_frame", "grasp_aubo_frame", ros::Time(0), transformPara);

          // get the value
          tf::Quaternion rotation = transformPara.getRotation();
          tf::Vector3 tfVect = transformPara.getOrigin();

          ROS_INFO("Origin & Quat: %f %f %f %f %f %f %f", tfVect.x(), tfVect.y(), tfVect.z(), rotation.x(), rotation.y(), rotation.z(), rotation.w());
        }
      }
      catch (tf::TransformException &ex)
      {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
        continue;
      }

      rate.sleep();
  }
  return 0;
}


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_READSEAMEDOBJ_HPP
#define MCL_READSEAMEDOBJ_HPP 1

#include <Eigen/Core>
#include <vector>

// TODO my own readOBJ to remove igl dependency
#include <igl/readOBJ.h>

namespace mcl
{

static inline bool read_seamed_obj(
	std::string filename,
	Eigen::MatrixXd &V3D, // 3D vertices
	Eigen::MatrixXd &VTC, // 2D UV tex init, if found
	Eigen::MatrixXi &F)
{
	using namespace Eigen;
	MatrixXd V3D_, vN_unused;
	MatrixXi F3D_, fN_unused;
	bool read_success = igl::readOBJ(
		filename,
		V3D_, VTC, vN_unused,
		F3D_, F, fN_unused);

	if (!read_success) { return false; }

	if (VTC.rows() > 0 && F.rows() > 0)
	{
		// Create 3D verts from F so face meshes match
		std::vector<bool> filled(VTC.rows(),false);
		V3D = MatrixXd::Zero(VTC.rows(),3);
		if(F3D_.rows() != F.rows()){ return false; }
		int nf = F3D_.rows();
		for(int i=0; i<nf; i++)
		{
			for(int j:{0,1,2})
			{
				int f = F3D_(i,j);
				int f_uv = F(i,j);
				if(!filled[f_uv])
				{
					V3D.row(f_uv) = V3D_.row(f);
					filled[f_uv] = true;
				}
			}
		}
	}
	else
	{
		F = F3D_;
		VTC = V3D_.block(0,0,V3D_.rows(),2);
		V3D = V3D_;
	}

	if( F.minCoeff() > 0 ){ F.array() -= 1; }
	return true;

}

} // end ns mcl

#endif


/*
    Copyright (C) 2008 Wei Dong <wdong@princeton.edu>. All Rights Reserved.

    This file is part of LSHKIT.

    LSHKIT is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    LSHKIT is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with LSHKIT.  If not, see <http://www.gnu.org/licenses/>.
*/

/**
  * \file lsh-run.cpp
  * \brief Example of using RandomThresholding LSH index for L1 distance.
  *
  * This program is an example of using LSH index.  The LSH used is
  * Tail<RepeatHash<ThresholdingLsh> >.  The ThresholdingLSH is repeated
  * M times and then randomly hashed to a integer within the range of
  * [0, H).  The size of one hash table is H.  The LSH family approximates
  * L1 distance.
  *
  * The program reconstruct the LSH index by default.  You can give the
  * --index option to make the program save the LSH index.  The next
  * time you run the program with the same --index option, the program
  * will try to load the previously saved index.  When a saved index is
  * used, you need to make sure that the dataset and other parameters match
  * the previous run.  However, the benchmark file, Q and K can be different.
  *
\verbatim
Allowed options:
  -h [ --help ]            produce help message.
  -M [ -- ] arg (=20)
  -L [ -- ] arg (=1)       number of hash tables
  -Q [ -- ] arg (=100)     number of queries to use
  -K [ -- ] arg (=50)      number of nearest neighbors to retrieve
  -D [ --data ] arg        dataset path
  -B [ --benchmark ] arg   benchmark path
  --index arg              index file
  -H [ -- ] arg (=1017881) hash table size, use the default.
\endverbatim
  *
  */

#include <boost/program_options.hpp>
#include <boost/progress.hpp>
#include <boost/format.hpp>
#include <boost/timer.hpp>
#include <lshkit.h>

using namespace std;
using namespace lshkit;
namespace po = boost::program_options;

int main (int argc, char *argv[])
{
    string data_file;
    string benchmark;
    string index_file;

    float R;
    unsigned M, L, H;
    unsigned Q, K;
    bool do_benchmark = true;
    bool use_index = false; // load the index from a file

    boost::timer timer;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message.")
        (",M", po::value<unsigned>(&M)->default_value(20), "")
        (",L", po::value<unsigned>(&L)->default_value(1), "number of hash tables")
        (",Q", po::value<unsigned>(&Q)->default_value(100), "number of queries to use")
        (",K", po::value<unsigned>(&K)->default_value(50), "number of nearest neighbors to retrieve")
        (",R", po::value<float>(&R)->default_value(numeric_limits<float>::max()), "R-NN distance range")
        ("data,D", po::value<string>(&data_file), "dataset path")
        ("benchmark,B", po::value<string>(&benchmark), "benchmark path")
        ("index", po::value<string>(&index_file), "index file")
        (",H", po::value<unsigned>(&H)->default_value(1017881), "hash table size, use the default.")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") || (vm.count("data") < 1))
    {
        cout << desc;
        return 0;
    }

    if ((Q == 0) || (vm.count("benchmark") == 0)) {
        do_benchmark = false;
    }

    if (vm.count("index") == 1) {
        use_index = true;
    }

    cout << "LOADING DATA..." << endl;
    timer.restart();
    FloatMatrix data(data_file);
    cout << boost::format("LOAD TIME: %1%s.") % timer.elapsed() << endl;

    //typedef Tail<RepeatHash<CauchyLsh> > MyLsh;

    typedef Tail<RepeatHash<ThresholdingLsh> > MyLsh;
    typedef LshIndex<MyLsh, unsigned> Index;

    metric::l1<float> l1(data.getDim());
    FloatMatrix::Accessor accessor(data);
    Index index;

    bool index_loaded = false;

    if (use_index) {
        ifstream is(index_file.c_str(), ios_base::binary);
        if (is) {
            is.exceptions(ios_base::eofbit | ios_base::failbit | ios_base::badbit);
            cout << "LOADING INDEX..." << endl;
            timer.restart();
            index.load(is);
            BOOST_VERIFY(is);
            cout << boost::format("LOAD TIME: %1%s.") % timer.elapsed() << endl;
            index_loaded = true;
        }
    }

    if (!index_loaded) {
        // We define a short name for the MPLSH index.
        float min = numeric_limits<float>::max();
        float max = -numeric_limits<float>::max();

        for (int i = 0; i < data.getSize(); ++i) {
            for (int j = 0; j < data.getDim(); ++j) {
                if (data[i][j] > max) max = data[i][j];
                if (data[i][j] < min) min = data[i][j];
            }
        }

        Index::Parameter param;

        // Setup the parameters.  Note that L is not provided here.
        param.range = H;
        param.repeat = M;
        param.min = min;
        param.max = max;
        param.dim = data.getDim();
        DefaultRng rng;

        index.init(param, rng, L);
        // The accessor.

        // Initialize the index structure.  Note L is passed here.
        cout << "CONSTRUCTING INDEX..." << endl;

        timer.restart();
        {
            boost::progress_display progress(data.getSize());
            for (int i = 0; i < data.getSize(); ++i)
            {
                // Insert an item to the hash table.
                // Note that only the key is passed in here.
                // MPLSH will get the feature from the accessor.
                index.insert(i, data[i]);
                ++progress;
            }
        }
        cout << boost::format("CONSTRUCTION TIME: %1%s.") % timer.elapsed() << endl;

        if (use_index) {
            timer.restart();
            cout << "SAVING INDEX..." << endl;
            {
                ofstream os(index_file.c_str(), ios_base::binary);
                os.exceptions(ios_base::eofbit | ios_base::failbit | ios_base::badbit);
                index.save(os);
                BOOST_VERIFY(os);
            }
            cout << boost::format("SAVING TIME: %1%s") % timer.elapsed() << endl;
        }
    }

    if (do_benchmark) {

        Benchmark<> bench;
        cout << "LOADING BENCHMARK..." << endl;
        bench.load(benchmark);
        bench.resize(Q, K);
        cout << "DONE." << endl;

        for (unsigned i = 0; i < Q; ++i)
        {
            for (unsigned j = 0; j < K; ++j)
            {
                assert(bench.getAnswer(i)[j].key < data.getSize());
            }
        }

        cout << "RUNNING QUERIES..." << endl;

        Stat recall;
        Stat cost;

        timer.restart();
        {
            TopkScanner<FloatMatrix::Accessor, metric::l1<float> > query(accessor, l1, K, R);
            boost::progress_display progress(Q);
            for (unsigned i = 0; i < Q; ++i)
            {
                query.reset(data[bench.getQuery(i)]);
                index.query(data[bench.getQuery(i)], query);
                recall << bench.getAnswer(i).recall(query.topk());
                cost << double(query.cnt())/double(data.getSize());
                ++progress;
            }
        }
        cout << boost::format("QUERY TIME: %1%s.") % timer.elapsed() << endl;

        cout << "[RECALL] " << recall.getAvg() << " +/- " << recall.getStd() << endl;
        cout << "[COST] " << cost.getAvg() << " +/- " << cost.getStd() << endl;

    }

    return 0;
}


#include <shg/specfunc.h>
#include <boost/math/distributions/fisher_f.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <shg/mconsts.h>
#include <shg/utils.h>
#include "testing.h"

namespace SHG::Testing {

BOOST_AUTO_TEST_SUITE(specfunc_test)

BOOST_AUTO_TEST_CASE(normal_integral_at_zero_test) {
     using boost::math::normal_distribution;
     const double y = normal_integral(0.0);
     BOOST_CHECK(faeq(y, 0.5, 1e-12));
}

namespace bdata = boost::unit_test::data;

BOOST_DATA_TEST_CASE(normal_integral_test, bdata::xrange(61), xr1) {
     using boost::math::normal_distribution;
     const double x = -3.0 + xr1 * 0.1;
     const double y = normal_integral(x);
     normal_distribution normal;
     const double z = cdf(normal, x);
     BOOST_CHECK(faeq(y, z, 1e-12));
}

BOOST_DATA_TEST_CASE(ppnd_test, bdata::xrange(199), xr1) {
     using boost::math::normal_distribution;
     const int i = xr1;
     const double x = 0.005 * (i + 1);
     const double y = ppnd7(x);
     normal_distribution normal;
     const double z = quantile(normal, x);
     BOOST_CHECK(faeq(y, z, 3e-7));
}

BOOST_DATA_TEST_CASE(gammad_test,
                     bdata::xrange(101) * bdata::xrange(100), xr1,
                     xr2) {
     using boost::math::gamma_p;
     const double x = xr1 * 0.1;
     const double p = (xr2 + 1) * 0.1;
     const double y = gammad(x, p);
     const double z = gamma_p(p, x);
     BOOST_CHECK(faeq(y, z, 1e-8));
}

BOOST_DATA_TEST_CASE(probst_test,
                     bdata::xrange(18) * bdata::xrange(81), xr1,
                     xr2) {
     using boost::math::students_t_distribution;
     const int df = xr1 + 1;
     const double x = xr2 * 0.1;
     const double y = probst(x, df);
     students_t_distribution t(narrow_cast<double, int>(df));
     const double z = cdf(t, x);
     BOOST_CHECK(faeq(y, z, 1e-15));
}

BOOST_DATA_TEST_CASE(betain_test,
                     bdata::xrange(9) * bdata::xrange(10) *
                          bdata::xrange(10),
                     xr1, xr2, xr3) {
     using boost::math::ibeta;
     const double x = xr1 * 0.1;
     const double p = xr2 + 1;
     const double q = xr3 + 1;
     const double y = betain(x, p, q);
     const double z = ibeta(p, q, x);
     BOOST_CHECK(faeq(y, z, 1e-10));
}

BOOST_DATA_TEST_CASE(cdffdist_test,
                     bdata::xrange(10) * bdata::xrange(10) *
                          bdata::xrange(10),
                     xr1, xr2, xr3) {
     using boost::math::fisher_f_distribution;
     const int m = xr1 + 1;
     const int n = xr2 + 1;
     const double x = xr3 + 1;

     const double y = cdffdist(m, n, x);
     fisher_f_distribution f(narrow_cast<double, int>(m),
                             narrow_cast<double, int>(n));
     const double z = cdf(f, x);
     BOOST_CHECK(faeq(y, z, 3e-8));
}

BOOST_AUTO_TEST_CASE(digamma_at_zero_test) {
     BOOST_CHECK_THROW(digamma(0.0), std::invalid_argument);
}

// digamma(0.5) = -2 ln(2) - gamma
BOOST_AUTO_TEST_CASE(digamma_at_half_test) {
     using Constants::gamma;
     BOOST_CHECK(faeq<double>(
          digamma(0.5), -2.0 * std::log(2.0) - gamma<double>, 9e-7));
}

// digamma(n) = -gamma + \sum_{k = 1}^{n - 1} (1 / k) for n >= 2
BOOST_DATA_TEST_CASE(digamma_at_int_test, bdata::xrange(19), xr1) {
     using Constants::gamma;
     const int n = xr1 + 2;
     double s = 0.0;
     for (int k = n - 1; k > 0; k--)
          s += 1.0 / k;
     s -= gamma<double>;
     BOOST_CHECK(faeq<double>(digamma(n), s, 3e-7));
}

BOOST_DATA_TEST_CASE(digamma_test, bdata::xrange(100), xr1) {
     const float x = (xr1 + 1) * 0.1;
     const float y = digamma(x);
     const float z = boost::math::digamma(x);
     BOOST_CHECK(faeq(y, z, 1e-6f));
}

BOOST_AUTO_TEST_SUITE_END()

}  // namespace SHG::Testing


/*
 *  parameter_test.cpp
 *
 *  Created by Ania M. Kedzierska on 11/11/11.
 *  Copyright 2011 Politecnic University of Catalonia, Center for Genomic Regulation.  This is program can be redistributed, modified or else as given by the terms of the GNU General Public License.
 *
 */
// as in parameter.cpp, this file contains additional functions I used for testing and getting a feel of the problem.

#include "parameter_tests.h"
#include "em.h"
#include "sampling.h"
#include "parameters.h"
#include "miscelania.h"
#include "random.h"
#include "alignment.h"
#include "fisher.h"
#include <tuple>

#include <vector>
#include <algorithm>
#include "boost/math/distributions/chi_squared.hpp"
#include "boost/math/special_functions/fpclassify.hpp"
#include "boost/math/special_functions/erf.hpp"
#include <boost/math/special_functions/beta.hpp>

#include <cmath>
#include<iostream>
#include <fstream>
#include <list>

#include <stdexcept>

// Those values should be read from a file, here they were calculated prior to the simulations
double get_scale_constant(Model &Mod) {
  if (Mod.m == JC) return 11.8877007085245;
  else if (Mod.m == K80) return 14.3316208140274;
  else if (Mod.m == K81) return 17.3950207294731;
  else if (Mod.m == SSM) return 17.3738070524995;
  else return 0;
}

// multinomial sampling

// p: vector with probabilities
// N: length of the sampled vector
// x: output vector with a single multinomial sample of length p.size(),
// the entry x[s] is the count for the state s and these entries add up to N.

void multinomial_sample(std::vector<double> &p, long N, std::vector<double> &x) {
	unsigned long i;
	long s;
	x.resize(p.size(), 0);   // counts for every possible state. Initially set to 0.
	for (i=0; i < p.size(); i++) {
		x[i] = 0;
	}

	for(long j=0; j < N; j++) {
		s = discrete(p);
		//std::cout << "sample: " << s << "             \n";

		x[s]++;
	}

}

// sum of KL's for the first row of the transition matrix.
void KL_divergence_edges(Tree &T, Model &Mod, Parameters &Par1, Parameters &Par2, std::vector<double> &KL) {
  long e, i, r, rr;
  double d, dacc;
  r = 0;
  rr = 1;

	//if unimplemented modelwas called
  if (Mod.m != JC && Mod.m != K80  && Mod.m != K81  && Mod.m != SSM) {
    throw std::range_error( "ERROR: KL_divergence_edges not implemented for this model.");
  }

  KL.resize(T.nedges);
  for (e=0; e < T.nedges; e++) {
    dacc = 0;
    for (i=0; i < T.nalpha; i++) {
      if (Par1.tm[e][r][i] > 0) {
        d = Par1.tm[e][r][i]*log(Par1.tm[e][r][i]/Par2.tm[e][r][i]);
      } else {
        d = 0;
      }
      dacc = dacc + d;
    }

    if (Mod.m == SSM) {
      for (i=0; i < T.nalpha; i++) {
        if (Par1.tm[e][rr][i] > 0) {
          d = Par1.tm[e][rr][i]*log(Par1.tm[e][rr][i]/Par2.tm[e][rr][i]);
        } else {
          d = 0;
        }
        dacc = dacc + d;
      }
    }
    KL[e] = dacc;
  }
}

// Computes a chi^2 statistic. Covbr is the inverse covariance matrix for a branch.
double chi2_mult(std::vector<double> &mu, std::vector<double> &x, Array2 &Covbr){
  double X = 0.0;
  for(unsigned int i=0; i < mu.size(); i++) {
    for(unsigned int j=0; j < mu.size(); j++) {
      X = X + Covbr[j][i]*(x[i] - mu[i])*(x[j] - mu[j]);
    }
  }
  return X;
}

bool double_pointer_comparison(const double *a, const double *b) {
  return *a < *b;
}

// version 2:
void BH(std::vector<double> &pvals, std::vector<double> &qvals) {
	long i, n;

        // Copies pvals to qvals
        for(unsigned long j=0; j < pvals.size(); j++) {
	  qvals[j] = pvals[j];
        }

     	std::vector<double*> pvals_p;        // the vector of pointers
	n = (long) pvals.size();

        pvals_p.resize(n);
        for(long j=0; j < n; j++) {
	     pvals_p[j] = &qvals[j];
        }

        // Sorts the vector of pointers only.
	std::sort(pvals_p.begin(), pvals_p.end(), double_pointer_comparison);

	     // Here corrects p-values using the sorted vector of pointers pval_p.
        // *(pvals_p[i]) is the value of the i-th smallest number in qvals,
        // moreover, modifying its value here, changes it in the vector qvals.

	for(i=n-2; i >= 0; i--) {
	  *(pvals_p[i]) = *(pvals_p[i]) * ((double)n/(double)(i+1));
	  if (*(pvals_p[i]) > *(pvals_p[i+1])) *(pvals_p[i]) = *(pvals_p[i+1]);
	}
}

// Computes the p-value of X for a chi^2 distribution with a given df
double pvalue_chi2 (double X, int df) {
	if (boost::math::isinf(X)) {
		return 0.;
	} else {
		boost::math::chi_squared chi2(df);
		return 1 - boost::math::cdf(chi2, X);
	}
}

// Uses Fischer's method to combine the p-values
double Fisher_combined_pvalue(std::vector<double> &pvals) {
  int i;
  long n;
  double X2;
  n = pvals.size();

  // We compute the Fisher's statistic
  X2 = 0;
  for (i=0; i < n; i++) {
    X2 = X2 + -2*log(pvals[i]);
  }

  // Return the p-value of Fisher's statistc
  return pvalue_chi2(X2, 2*n);
}

// Normalizes a vector of data.
void normalize(std::vector<double> &x) {
  unsigned long i;
  double mean, std;
  mean = 0.;
  for (i = 0; i < x.size(); i++) {
    mean = mean + x[i];
  }
  mean = mean / (double)x.size();

  std = 0.;
  for (i=0; i < x.size(); i++) {
    std = std + (x[i] - mean)*(x[i] - mean);
  }
  std = sqrt(std);

  for (i=0; i < x.size(); i++) {
    x[i] = (x[i] - mean)/std;
  }
}

// Combines p-values using Z-score.
double Zscore_combined_pvalue(std::vector<double> &pvals) {

  unsigned long i;
  double z, zcomb, pcomb;

  zcomb = 0.;
  for (i=0; i < pvals.size(); i++) {

    // If some p-value is 0, the score will be 0.
    if (pvals[i] <= 0) {
      return 0;
    }

    // If some p-value is 1, the score will be 1.
    if (pvals[i] >= 1) {
      return 1;
    }

    z = sqrt(2)*boost::math::erf_inv(2*(1-pvals[i]) - 1);       // Sure ???
    zcomb = zcomb + z;
  }
  zcomb = zcomb / sqrt((double) pvals.size());
  pcomb = 1 - 0.5*(1+erf(zcomb/sqrt(2)));
  return pcomb;
}

// Performs N repetitions of the EM algorithm and computes combined p-values for every edge.
// Parameters:
// Nrep:    Number of repetitions
// length:  Length of the alignment of simulated data
// pvals:   Vector of p-values indexed by the edges in T
// data_prefix: If different from != "" stores the output data

void parameter_test(Tree &T, Model &Mod, long Nrep, long length, double eps, std::vector<double> &pvals, std::string data_prefix, bool save_mc_exact){

  long iter;
  long i, r;

  double df, C;
  double distance, KL;
	KL=0;
	distance=0;
  double likel;

  Parameters Parsim, Par, Par_noperm;
  Alignment align;
  Counts data;

  double eps_pseudo = 0.001;     // Amount added to compute the pseudo-counts.

  StateList sl;

  bool save_data = (data_prefix != "");

  std::string output_filename;
  std::stringstream output_index;
  std::ofstream logfile;
  std::ofstream logdistfile;

  std::ofstream out_chi2;
  std::ofstream out_br;
  std::ofstream out_brPerc;

  std::ofstream out_pvals;
  std::ofstream out_pvals_noperm;
  std::ofstream out_qvals;
  std::ofstream out_bound;
  std::ofstream out_variances;
  std::ofstream out_qvalsComb;
  std::ofstream out_qvalsCombzscore;
  std::ofstream out_covmatrix;

  std::ofstream out_parest;
  std::ofstream out_parsim;

  std::vector<double> KLe;
  std::vector<std::vector<double> > chi2_array; // an array of chi2 for every edge.
  std::vector<std::vector<double> > mult_array; // an array of mult for every edge.
  std::vector<std::vector<double> > br_array; // an array of br. length for every edge.
  std::vector<std::vector<double> > br_arrayPerc; // an array of br. length for every edge.

  std::vector<std::vector<double> > cota_array; // an array of upper bounds of the diff in lengths for every edge.
  std::vector<std::vector<double> > pval_array; // an array of pvals for every edge.
  std::vector<std::vector<double> > pval_noperm_array;
  std::vector<std::vector<double> > qval_array; // an array of qvalues for every edge.
  std::vector<std::vector<double> > variances_array; // an array of theoretical variances.
  std::vector<std::vector<double> > parest_array; // array of estimated parameters
  std::vector<std::vector<double> > parsim_array; // array of simulation parameters

	//  ci_binom ci_bin; // condfidence interval
  std::vector<std::vector<ci_binom> > CIbinomial ; //  	vector of CIs

  std::list<long> produced_nan;

  long npars = T.nedges*Mod.df + Mod.rdf;

  // Initializing pvals
  pvals.resize(T.nedges);

  // Initialize the parameters for simulation of K81 data for testing
  Par = create_parameters(T);
  Parsim = create_parameters(T);

  // Initializing data structures
  KLe.resize(T.nedges);

	pval_array.resize(T.nedges);
        pval_noperm_array.resize(T.nedges);
        qval_array.resize(T.nedges);
	chi2_array.resize(T.nedges);
	mult_array.resize(T.nedges);
	br_array.resize(T.nedges);
        br_arrayPerc.resize(T.nedges);
	cota_array.resize(T.nedges);
        variances_array.resize(npars);
        parest_array.resize(npars);
        parsim_array.resize(npars);

	// initialize to 0's
  for (i=0; i < T.nedges; i++) {
      pval_array[i].resize(Nrep, 0);
      pval_noperm_array[i].resize(Nrep, 0);
      qval_array[i].resize(Nrep, 0);
          chi2_array[i].resize(Nrep, 0);
	  mult_array[i].resize(Nrep, 0);
	  br_array[i].resize(Nrep, 0);
          br_arrayPerc[i].resize(Nrep, 0);
	  cota_array[i].resize(Nrep, 0);
  }

  for(i=0; i < npars; i++) {
    variances_array[i].resize(Nrep, 0);
    parest_array[i].resize(Nrep, 0);
    parsim_array[i].resize(Nrep, 0);
  }

  // Information about the chi^2.
  df = Mod.df;
  C = get_scale_constant(Mod);

  if (save_data) {
    logfile.open((data_prefix + ".log").c_str(), std::ios::out);
    logfile << "model:  " << Mod.name << std::endl;
    logfile << "length: " << length << std::endl;
    logfile << "eps:    " << eps << std::endl;
    logfile << "nalpha: " << T.nalpha << std::endl;
    logfile << "leaves: " << T.nleaves << std::endl;
    logfile << "tree:   " << T.tree_name << std::endl;
    logfile << std::endl;
    logdistfile.open((data_prefix + ".dist.log").c_str(), std::ios::out);

    out_chi2.open(("out_chi2-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_br.open(("out_br-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_brPerc.open(("out_brPerc-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_pvals.open(("out_pvals-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_pvals_noperm.open(("out_pvals_noperm-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_qvals.open(("out_qvals-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_variances.open(("out_variances-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_parest.open(("out_params-est-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_parsim.open(("out_params-sim-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_bound.open(("out_bound-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_qvalsComb.open(("out_qvalsComb-" + data_prefix + ".txt").c_str(), std::ios::out);
    out_qvalsCombzscore.open(("out_qvalsCombzscore-" + data_prefix + ".txt").c_str(), std::ios::out);

    out_parsim.precision(15);
    out_parest.precision(15);
    out_variances.precision(15);
  }

	// uncomment the 2 following lines if want to fix the parameters
	// random_parameters_length(T, Mod, Parsim);
	 //random_data(T, Mod, Parsim, length, align);

  for (iter=0; iter < Nrep; iter++) {
    std::cout << "iteration: " << iter << "             \n";

    // Produces an alignment from random parameters
    random_parameters_length(T, Mod, Parsim);

    random_data(T, Mod, Parsim, length, align);
    get_counts(align, data);
    add_pseudocounts(eps_pseudo, data);

    // Saving data
    if (save_data) {
      output_index.str("");
      output_index << iter;
      output_filename = data_prefix + "-" + output_index.str();
      save_alignment(align, output_filename + ".fa");
      save_parameters(Parsim, output_filename + ".sim.dat");
    }

    // Runs the EM
    std::tie(likel, iter) = EMalgorithm(T, Mod, Par, data, eps);

    // If algorithm returns NaN skip this iteration.
    if (boost::math::isnan(likel)) {
      produced_nan.push_back(iter);
      continue;
    }
    copy_parameters(Par, Par_noperm);

    // Chooses the best permutation.
    guess_permutation(T, Mod, Par);

    distance = parameters_distance(Parsim, Par);

      // estimated counts: Par ; original: Parsim
      std::vector<double> counts_est;
      counts_est.resize(T.nalpha, 0);

      // calculate the cov matrix
      std::vector<std::vector<double> > Cov;
      Array2 Cov_br;

      full_MLE_covariance_matrix(T, Mod, Parsim, length, Cov);

      if(save_data) {
          save_matrix(Cov, output_filename + ".cov.dat");
      }

      // Save the covariances in an array
      std::vector<double> param;
      std::vector<double> param_sim;

      param.resize(npars);
      param_sim.resize(npars);

      get_free_param_vector(T, Mod, Par, param);
      get_free_param_vector(T, Mod, Parsim, param_sim);

      for(i=0; i < npars; i++) {
	variances_array[i][iter] = Cov[i][i];
        parsim_array[i][iter] = param_sim[i];
        parest_array[i][iter] = param[i];
      }

      std::vector<double> xbranca, xbranca_noperm, mubranca;
      double chi2_noperm;
      xbranca.resize(Mod.df);
      xbranca_noperm.resize(Mod.df);
      mubranca.resize(Mod.df);
      for (i=0; i < T.nedges; i++) {
		  r = 0; // row to be fixed
		  // Extracts the covariance matrix, 1 edge

				  branch_inverted_covariance_matrix(Mod, Cov, i, Cov_br);
                  get_branch_free_param_vector(T, Mod, Parsim, i, mubranca);
                  get_branch_free_param_vector(T, Mod, Par, i, xbranca);
                  get_branch_free_param_vector(T, Mod, Par_noperm, i, xbranca_noperm);

			      chi2_array[i][iter] = chi2_mult(mubranca, xbranca, Cov_br);
                  chi2_noperm = chi2_mult(mubranca, xbranca_noperm, Cov_br);

                  pval_array[i][iter] =  pvalue_chi2(chi2_array[i][iter], Mod.df);
                  pval_noperm_array[i][iter] = pvalue_chi2(chi2_noperm, Mod.df);

			      br_array[i][iter] = T.edges[i].br - branch_length(Par.tm[i], T.nalpha);
				  br_arrayPerc[i][iter] = branch_length(Par.tm[i], T.nalpha)/T.edges[i].br;

		// Upper bound on the parameter distance using multinomial:
		//  cota_array[i][iter] = bound_mult(Parsim.tm[i], Xm, length);
	    // and using the  L2 bound
		  cota_array[i][iter] = branch_length_error_bound_mult(Parsim.tm[i], Par.tm[i]);
		  out_br <<  br_array[i][iter]  << " ";
		  out_brPerc <<  br_arrayPerc[i][iter]  << " ";

		  out_bound  <<  cota_array[i][iter] << " ";
		  out_chi2 << chi2_array[i][iter] << " ";

			}
         out_chi2 << std::endl;
		 out_bound  <<  std::endl;
		 out_br << std::endl;
		 out_brPerc << std::endl;

    // Saves more data.
    if (save_data) {
      logfile << iter << ": " << distance << "   " << KL << std::endl;
      save_parameters(Par, output_filename + ".est.dat");

      logdistfile << iter << ": ";
      logdistfile << parameters_distance_root(Par, Parsim) << " ";
      for(int j=0; j < T.nedges; j++) {
        logdistfile << parameters_distance_edge(Par, Parsim, j) << " ";
      }
      logdistfile << std::endl;
    }

} // close iter loop here

  // Correct the p-values
  for(i=0; i < T.nedges; i++) {
       BH(pval_array[i], qval_array[i]);
	//save them
  }

  if (save_mc_exact) {
    for(long iter=0; iter < Nrep; iter++) {
      for(long i=0; i < T.nedges; i++) {
        out_pvals << pval_array[i][iter] << "  ";
        out_pvals_noperm << pval_noperm_array[i][iter] << "  ";
        out_qvals << qval_array[i][iter] << "  ";
      }
      out_pvals  << std::endl;
      out_pvals_noperm << std::endl;
      out_qvals  << std::endl;

      for(long i=0; i < npars; i++) {
        out_variances << variances_array[i][iter] << "  ";
        out_parsim << parsim_array[i][iter] << "  ";
        out_parest << parest_array[i][iter] << "  ";
      }
      out_variances << std::endl;
      out_parsim << std::endl;
      out_parest << std::endl;
    }
  }

	// now combine the pvalues
   for(i=0; i < T.nedges; i++) {
	pvals[i] = Fisher_combined_pvalue(pval_array[i]);
    //using the Zscore it goes like this: pvals[i] = Zscore_combined_pvalue(pval_array[i]);
	if (save_mc_exact) {	   out_qvalsComb <<  pvals[i] << "  " ;
	out_qvalsCombzscore << Zscore_combined_pvalue(pval_array[i]) << " ";
	}
  }

  // Close files
  if (save_data) {
    logdistfile.close();
    logfile.close();
  }

if (save_mc_exact) {
	out_chi2.close();
	out_bound.close();
        out_variances.close();
        out_parest.close();
        out_parsim.close();
	out_br.close();
	out_brPerc.close();

	out_pvals.close();
        out_qvals.close();
	out_qvalsComb.close();
	out_qvalsCombzscore.close();
        out_covmatrix.close();
	}

  // Warn if some EM's produced NaN.
  if (produced_nan.size() > 0) {
    std::cout << std::endl;
    std::cout << "WARNING: Some iterations produced NaN." << std::endl;
    std::list<long>::iterator it;
    for (it = produced_nan.begin(); it != produced_nan.end(); it++) {
      std::cout << *it << ", ";
    }
    std::cout << std::endl;
  }
}

// below we test the fit the fit of the theoretical distribution to the data, i.e. we simulate data with all visible and check it per node,
// that is what was used to produce the histograms in the paper

void parameter_cloud(Tree &T, Model &Mod, long Nrep, long length, double eps, Parameters &Parsim){

  long iter;

  float likel;

  Parameters Par;
  Alignment align;
  Counts data;

  double eps_pseudo = 0.001;     // Amount added to compute the pseudo-counts.

  // Initialize the parameters for simulation of K81 data for testing
  Par = create_parameters(T);

  // Obtaining the distribution of estimated parameters with EM

  std::ofstream estpar;
  estpar.open("est-par.dat", std::ios::out);
  estpar.precision(15);

  std::vector<double> param;
  for (iter=0; iter < Nrep; iter++) {
    random_data(T, Mod, Parsim, length, align);
    get_counts(align, data);
    add_pseudocounts(eps_pseudo, data);

    // Runs EM
    std::tie(likel, iter)= EMalgorithm(T, Mod, Par, data, eps);

    // Choses the best permutation.
    guess_permutation(T, Mod, Par);

    get_free_param_vector(T, Mod, Par, param);

    for (unsigned long k=0; k < param.size(); k++) {
      estpar << param[k] << "  ";
    }
    estpar << std::endl;
  }

}


#include "Line2D.h"
#include "Angle.h"
#include "Pose2D.h"
#include "Geometry.h"

#include <cmath>
#include <boost/concept_check.hpp>
#include <limits>

using namespace A2O;
using namespace Eigen;

Line2D::Line2D()
      : _start(0, 0), _extension(1, 0)
{
}

Line2D::Line2D(const double& m,
	       const double& b)
      : _start(0, b), _extension(1, m)
{
}

Line2D::Line2D(const double& xStart,
	       const double& yStart,
	       const Angle& alpha,
	       const double& length)
      : _start(xStart, yStart), _extension(alpha.getVector(length))
{
  if (_extension(0) == 0 && _extension(1) == 0) {
    _extension = Vector2d(1, 0);
  }
}

Line2D::Line2D(const Eigen::Vector2d& start,
	       const Angle& alpha,
	       const double& length)
      : _start(start), _extension(alpha.getVector(length))
{
  validate();
}

Line2D::Line2D(const Vector2d& start,
	       const Vector2d& end)
      : _start(start), _extension(end - start)
{
  validate();
}

Line2D::Line2D(const double& xStart,
	       const double& yStart,
	       const double& xEnd,
	       const double& yEnd)
      : _start(xStart, yStart), _extension(xEnd - xStart, yEnd - yStart)
{
  validate();
}

Line2D::~Line2D()
{
}

void Line2D::validate()
{
  if (_extension(0) == 0 && _extension(1) == 0) {
    _extension = Vector2d(0, 1);
  }
}

const Eigen::Vector2d& Line2D::getStart() const
{
  return _start;
}

const Eigen::Vector2d& Line2D::getExtensionVector() const
{
  return _extension;
}

const Eigen::Vector2d Line2D::getEnd() const
{
  return _start + _extension;
}

const Angle Line2D::getAngle() const
{
  return Angle::to(_extension);
}

const double Line2D::m() const
{
  if (_extension(0) == 0) {
    if (_extension(1) > 0) {
      return std::numeric_limits<double>::max();
    } else {
      return std::numeric_limits<double>::lowest();
    }
  }

  return _extension(1) / _extension(0);
}

const double Line2D::b() const
{
  if (_extension(0) == 0) {
    return 0;
  }

  return _start(1) - ((_extension(1) / _extension(0)) * _start(0));
}

const double Line2D::yValue(const double& x) const
{
  if (_extension(0) == 0) {
    // this line is y-parallel
    return 0;
  }

  return (_extension(1) / _extension(0)) * (x - _start(0)) + _start(1);
}

const double Line2D::xValue(const double& y) const
{
  if (_extension(1) == 0) {
    // this line is x-parallel
    return _start(0);
  }

  return (_extension(0) / _extension(1)) * (y - _start(1)) + _start(0);
}

Pose2D Line2D::getClosestPose(const Vector2d& point)
{

  Angle lineAngle(Angle::to(_extension));
  Angle pointAngle(Angle::to(point - _start));

  Angle offsetAngle (pointAngle - lineAngle);
  double hypotenuse = Geometry::getDistance<double, 2>(point, _start);
  double factor = cos(offsetAngle.rad()) * hypotenuse;
  Vector2d closestPoint (_start + _extension / Geometry::getNorm<double, 2>(_extension) * factor);

  return Pose2D(closestPoint, Angle::to(_extension));
}

std::vector< Vector2d > Line2D::getTrail(const Vector2d& start, const Vector2d& end)
{
  Pose2D poseStart = getClosestPose(start);
  Pose2D poseEnd = getClosestPose(end);
  double distance = poseStart.getDistanceTo(poseEnd);
  Vector2d offset = poseStart.getAngle().getVector(0.1 * distance);
  Vector2d point = poseStart.getPosition();
  std::vector<Vector2d> points;
  int i = 0;
  do{
    point += offset;
    points.push_back(point);
  } while(++i < 10);
  return points;
}

bool Line2D::operator==(const Line2D& other) const
{
  return _start == other._start && _extension == other._extension;
}

bool Line2D::operator!=(const Line2D& other) const
{
  return !(*this == other);
}


#include <trajopt_utils/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <cstdio>
#include <gtest/gtest.h>
#include <Eigen/Core>
#include <iostream>
#include <sstream>
#include <vector>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_sco/solver_interface.hpp>
#include <trajopt_sco/solver_utils.hpp>
#include <trajopt_utils/logging.hpp>
#include <trajopt_utils/stl_to_string.hpp>

using namespace sco;

TEST(solver_utils, exprToEigen)
{
  int n_vars = 2;
  std::vector<VarRepPtr> x_info;
  VarVector x;
  for (int i = 0; i < n_vars; ++i)
  {
    std::stringstream var_name;
    var_name << "x_" << i;
    VarRepPtr x_el(new VarRep(i, var_name.str(), nullptr));
    x_info.push_back(x_el);
    x.push_back(Var(x_el.get()));
  }

  // x_affine = [3, 2]*x + 1
  AffExpr x_affine;
  x_affine.vars = x;
  x_affine.coeffs = DblVec{ 3, 2 };
  x_affine.constant = 1;

  std::cout << "x_affine=  " << x_affine << std::endl;
  std::cout << "expecting A = [3, 2];" << std::endl << "          u = [1]" << std::endl;
  Eigen::MatrixXd m_A_expected(1, n_vars);
  m_A_expected << 3, 2;
  Eigen::VectorXd v_u_expected(1);
  v_u_expected << -1;

  Eigen::SparseVector<double> v_A;
  exprToEigen(x_affine, v_A, n_vars);
  ASSERT_EQ(v_A.size(), m_A_expected.cols());
  Eigen::VectorXd v_A_d = v_A;
  Eigen::VectorXd m_A_r = m_A_expected.row(0);
  EXPECT_TRUE(v_A_d.isApprox(m_A_r)) << "error converting x_affine"
                                     << " to Eigen::SparseVector. "
                                     << "v_A :" << std::endl
                                     << v_A << std::endl;

  Eigen::SparseMatrix<double> m_A;
  Eigen::VectorXd v_u;
  AffExprVector x_affine_vector(1, x_affine);
  exprToEigen(x_affine_vector, m_A, v_u, n_vars);
  ASSERT_EQ(v_u_expected.size(), v_u.size());
  EXPECT_TRUE(v_u_expected == v_u) << "v_u_expected != v_u" << std::endl << "v_u:" << std::endl << v_u << std::endl;
  EXPECT_EQ(m_A.nonZeros(), 2) << "m_A.nonZeros() != 2" << std::endl;
  ASSERT_EQ(m_A.rows(), m_A_expected.rows());
  ASSERT_EQ(m_A.cols(), m_A_expected.cols());
  EXPECT_TRUE(m_A.isApprox(m_A_expected)) << "error converting x_affine to "
                                          << "Eigen::SparseMatrix. m_A :" << std::endl
                                          << m_A << std::endl;

  QuadExpr x_squared = exprSquare(x_affine);
  std::cout << "x_squared= " << x_squared << std::endl;
  std::cout << "expecting Q = [9, 6;" << std::endl
            << "               6, 4]" << std::endl
            << "          q = [6, 4]" << std::endl;
  Eigen::MatrixXd m_Q_expected(2, 2);
  m_Q_expected << 9, 6, 6, 4;
  DblVec v_q_expected{ 6, 4 };

  Eigen::SparseMatrix<double> m_Q;
  Eigen::VectorXd v_q;
  exprToEigen(x_squared, m_Q, v_q, n_vars);
  {
    Eigen::Map<Eigen::VectorXd, Eigen::Unaligned> v_q_e_eig(v_q_expected.data(), v_q_expected.size());
    ASSERT_EQ(v_q.size(), v_q_e_eig.size());
    EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << std::endl
                                         << "v_q:" << std::endl
                                         << v_q << std::endl;
  }
  EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to "
                                          << "Eigen::SparseMatrix. m_Q :" << std::endl
                                          << m_Q << std::endl;
  EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << std::endl;

  exprToEigen(x_squared, m_Q, v_q, n_vars, true);
  {
    Eigen::Map<Eigen::VectorXd, Eigen::Unaligned> v_q_e_eig(v_q_expected.data(), v_q_expected.size());
    EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << std::endl
                                         << "v_q:" << std::endl
                                         << v_q << std::endl;
  }
  EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to"
                                              << " Eigen::SparseMatrix. m_Q :" << std::endl
                                              << m_Q << std::endl;
  EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << std::endl;

  x_affine.coeffs = DblVec{ 0, 2 };
  std::cout << "x_affine=  " << x_affine << std::endl;
  std::cout << "expecting A = [0, 2];" << std::endl;
  x_squared = exprSquare(x_affine);
  std::cout << "x_squared= " << x_squared << std::endl;
  std::cout << "expecting Q = [0, 0;" << std::endl << "               0, 4]" << std::endl;
  m_Q_expected.setZero();
  m_Q_expected << 0, 0, 0, 4;
  exprToEigen(x_squared, m_Q, v_q, n_vars, false, false);
  EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to "
                                          << "Eigen::SparseMatrix. m_Q :" << std::endl
                                          << m_Q << std::endl;
  EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << std::endl;

  exprToEigen(x_squared, m_Q, v_q, n_vars, true, false);
  EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to"
                                              << " Eigen::SparseMatrix. m_Q :" << std::endl
                                              << m_Q << std::endl;
  EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << std::endl;

  exprToEigen(x_squared, m_Q, v_q, n_vars, false, true);
  EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to "
                                          << "Eigen::SparseMatrix. m_Q :" << std::endl
                                          << m_Q << std::endl;
  EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << std::endl;

  exprToEigen(x_squared, m_Q, v_q, n_vars, true, true);
  EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to"
                                              << "Eigen::SparseMatrix. m_Q :" << std::endl
                                              << m_Q << std::endl;
  EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << std::endl;
}

TEST(solver_utils, eigenToTriplets)
{
  Eigen::MatrixXd m_Q(2, 2);
  m_Q << 9, 0, 6, 4;
  Eigen::SparseMatrix<double> m_Q_sparse_expected = m_Q.sparseView();
  IntVec m_Q_i, m_Q_j;
  DblVec m_Q_ij;
  eigenToTriplets(m_Q_sparse_expected, m_Q_i, m_Q_j, m_Q_ij);
  Eigen::SparseMatrix<double> m_Q_sparse(2, 2);
  tripletsToEigen(m_Q_i, m_Q_j, m_Q_ij, m_Q_sparse);
  EXPECT_TRUE(m_Q_sparse.isApprox(m_Q)) << "m_Q != m_Q_sparse when converting "
                                        << "m_Q -> triplets -> m_Q_sparse. m_Q:" << std::endl
                                        << m_Q << std::endl;
  EXPECT_EQ(m_Q_sparse.nonZeros(), 3) << "m_Q.nonZeros() != 3" << std::endl;
}

TEST(solver_utils, eigenToCSC)
{
  DblVec P;
  IntVec rows_i;
  IntVec cols_p;
  {
    /*
     * M = [ 1, 2, 3,
     *       1, 0, 9,
     *       1, 8, 0]
     */
    Eigen::MatrixXd M(3, 3);
    M << 1, 2, 3, 1, 0, 9, 1, 8, 0;
    Eigen::SparseMatrix<double> Ms = M.sparseView();

    eigenToCSC(Ms, rows_i, cols_p, P);

    EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()";
    EXPECT_TRUE((P == DblVec{ 1, 1, 1, 2, 8, 3, 9 })) << "bad P:\n" << CSTR(P);
    EXPECT_TRUE((rows_i == IntVec{ 0, 1, 2, 0, 2, 0, 1 })) << "bad rows_i:\n" << CSTR(rows_i);
    EXPECT_TRUE((cols_p == IntVec{ 0, 3, 5, 7 })) << "cols_p not in "
                                                  << "CRC form:\n"
                                                  << CSTR(cols_p);
  }
  {
    /*
     * M = [ 0, 2, 0,
     *       7, 0, 0,
     *       0, 0, 0]
     */
    Eigen::SparseMatrix<double> M(3, 3);
    M.coeffRef(0, 1) = 2;
    M.coeffRef(1, 0) = 7;

    eigenToCSC(M, rows_i, cols_p, P);

    EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()";
    EXPECT_TRUE((P == DblVec{ 7, 2 })) << "bad P:\n" << CSTR(P);
    EXPECT_TRUE((rows_i == IntVec{ 1, 0 })) << "rows_i != data_j:\n"
                                            << CSTR(rows_i) << " vs\n"
                                            << CSTR((IntVec{ 1, 0 }));
    EXPECT_TRUE((cols_p == IntVec{ 0, 1, 2, 2 })) << "cols_p not in "
                                                  << "CRC form:\n"
                                                  << CSTR(cols_p);

    std::vector<long long int> rows_i_ll, cols_p_ll;
    std::vector<long long int> rows_i_ll_exp{ 1, 0 };
    std::vector<long long int> cols_p_ll_exp{ 0, 1, 2, 2 };

    eigenToCSC(M, rows_i_ll, cols_p_ll, P);
    EXPECT_TRUE(rows_i_ll.size() == P.size()) << "rows_i_ll.size() != P.size()";
    EXPECT_TRUE((rows_i_ll == rows_i_ll_exp));
    EXPECT_TRUE((cols_p_ll == cols_p_ll_exp));

    std::vector<unsigned long long int> rows_i_ull, cols_p_ull;
    std::vector<unsigned long long int> rows_i_ull_exp{ 1, 0 };
    std::vector<unsigned long long int> cols_p_ull_exp{ 0, 1, 2, 2 };
    eigenToCSC(M, rows_i_ull, cols_p_ull, P);
    EXPECT_TRUE(rows_i_ull.size() == P.size()) << "rows_i_ll.size() != P.size()";
    EXPECT_TRUE((rows_i_ull == rows_i_ull_exp));
    EXPECT_TRUE((cols_p_ull == cols_p_ull_exp));
  }
  {
    /*
     * M = [ 0, 0, 0,
     *       0, 0, 0,
     *       0, 6, 0]
     */
    Eigen::SparseMatrix<double> M(3, 3);
    M.coeffRef(2, 1) = 6;

    eigenToCSC(M, rows_i, cols_p, P);

    EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()";
    EXPECT_TRUE((P == DblVec{ 6 })) << "bad P:\n" << CSTR(P);
    EXPECT_TRUE((rows_i == IntVec{ 2 })) << "rows_i != data_j:\n" << CSTR(rows_i) << " vs\n" << CSTR((IntVec{ 1, 0 }));
    EXPECT_TRUE((cols_p == IntVec{ 0, 0, 1, 1 })) << "cols_p not in "
                                                  << "CRC form:\n"
                                                  << CSTR(cols_p);
  }
}

TEST(solver_utils, eigenToCSC_upper_triangular)
{
  /*
   * M = [ 1, 2, 0,
   *       2, 4, 0,
   *       0, 0, 9]
   */
  Eigen::MatrixXd M(3, 3);
  M << 1, 2, 0, 2, 4, 0, 0, 0, 9;
  Eigen::SparseMatrix<double> Ms = M.sparseView();

  DblVec P;
  IntVec rows_i;
  IntVec cols_p;
  eigenToCSC<Eigen::Upper>(Ms, rows_i, cols_p, P);

  EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()";
  EXPECT_TRUE((P == DblVec{ 1, 2, 4, 9 })) << "bad P:\n" << CSTR(P);
  EXPECT_TRUE((rows_i == IntVec{ 0, 0, 1, 2 })) << "rows_i != expected"
                                                << ":\n"
                                                << CSTR(rows_i) << " vs\n"
                                                << CSTR((IntVec{ 0, 0, 1, 2 }));
  EXPECT_TRUE((cols_p == IntVec{ 0, 1, 3, 4 })) << "cols_p not in "
                                                << "CRC form:\n"
                                                << CSTR(cols_p);
}


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

#define MTL_WITH_DEVELOPMENT
#define MTL_VERBOSE_TEST

#include <string>
#include <iostream>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/matrix/sparse_banded.hpp>

template <typename Matrix>
void laplacian_test(Matrix& A, unsigned dim1, unsigned dim2, const char* name)
{
    mtl::io::tout << "\n" << name << "\n";
    laplacian_setup(A, dim1, dim2);
    mtl::io::tout << "Laplacian A:\n" << A << std::endl;
    if (dim1 > 1 && dim2 > 1) {
	typename Matrix::value_type four(4.0), minus_one(-1.0), zero(0.0);
	MTL_THROW_IF(A[0][0] != four, mtl::runtime_error("wrong diagonal"));
	MTL_THROW_IF(A[0][1] != minus_one, mtl::runtime_error("wrong east neighbor"));
	MTL_THROW_IF(A[0][dim2] != minus_one, mtl::runtime_error("wrong south neighbor"));
	MTL_THROW_IF(dim2 > 2 && A[0][2] != zero, mtl::runtime_error("wrong zero-element"));
	MTL_THROW_IF(A[1][0] != minus_one, mtl::runtime_error("wrong west neighbor"));
	MTL_THROW_IF(A[dim2][0] != minus_one, mtl::runtime_error("wrong north neighbor"));
	MTL_THROW_IF(dim2 > 2 && A[2][0] != zero, mtl::runtime_error("wrong zero-element"));
    }
}

template <typename Matrix>
void rectangle_test(Matrix& A, const char* name)
{
    {
	mtl::mat::inserter<Matrix> ins(A);
	int i= 1;
	unsigned nc= num_cols(A);
	for (unsigned r= 0; r < num_rows(A); r++) {
	    if (r < nc - 4) ins(r, r + 4) << i++;
	    if (r < nc) ins(r, r) << i++;
	    if (r >= 2 && r < nc + 2) ins(r, r - 2) << i++;
	    if (r >= 4 && r < nc + 4) ins[r][r - 4] << i++;
	}
    }
    mtl::io::tout << name << ": A=\n" << A << '\n';
}

template <typename Matrix, typename Tag>
void two_d_iteration(const Matrix & A, Tag)
{
    namespace traits = mtl::traits;

    typename traits::row<Matrix>::type                                 row(A);
    typename traits::col<Matrix>::type                                 col(A);
    typename traits::const_value<Matrix>::type                         value(A);
    typedef typename traits::range_generator<Tag, Matrix>::type        cursor_type;
    for (cursor_type cursor = mtl::begin<Tag>(A), cend = mtl::end<Tag>(A); cursor != cend; ++cursor) {
	typedef mtl::tag::nz     inner_tag;
	mtl::io::tout << "---\n";
	typedef typename traits::range_generator<inner_tag, cursor_type>::type icursor_type;
	for (icursor_type icursor = mtl::begin<inner_tag>(cursor), icend = mtl::end<inner_tag>(cursor); icursor != icend; ++icursor)
	    mtl::io::tout << "A[" << row(*icursor) << ", " << col(*icursor) << "] = " << value(*icursor) << '\n';
    }
    mtl::io::tout << "===\n\n";
}

template <typename Matrix>
void mat_vec_mult_test(const Matrix& A, const char* name)
{
    typedef typename Matrix::value_type  value_type;
    mtl::io::tout << name << " " << num_rows(A) << " by " << num_cols(A) << '\n' << A;

    mtl::dense_vector<value_type> v, w(num_cols(A), 3.0), v2;
    v= A * w;
    mtl::io::tout << "A * v =    " << v << '\n';

    mtl::compressed2D<value_type> B(A);
    v2= B * w;
    mtl::io::tout << "Should be: " << v2 << "\n\n";
    v2-= v;
    MTL_THROW_IF(two_norm(v2) > 0.001,
		 mtl::runtime_error("wrong result for sparse banded times vector"));
}

int main(int, char**)
{
    using namespace mtl;
#ifdef MTL_WITH_DEVELOPMENT
    unsigned dim1= 3, dim2= 4;
    mat::sparse_banded<double>  dr, dr2(6, 11), dr3(11, 6), dr4(6, 5);

    laplacian_test(dr, dim1, dim2, "Dense row major");
    rectangle_test(dr2, "Dense row major");
    rectangle_test(dr3, "Dense row major");
    rectangle_test(dr4, "Dense row major");

    mat::compressed2D<double> C;
    laplacian_setup(C, dim1, dim2);

    mat::sparse_banded<double>  D;
    D= C;
    mtl::io::tout << "D is\n" << D << '\n';

    two_d_iteration(D, mtl::tag::row());

    mat::compressed2D<double> E;
    E= D;
    mtl::io::tout << "E is\n" << E << '\n';

    mat::sparse_banded<double>  dr5(5, 5), dr6(5, 5);
    {
	mtl::mat::inserter<mat::sparse_banded<double> > ins5(dr5), ins6(dr6);
	ins5[2][0] << 1; ins5[3][1] << 2; ins5[4][2] << 3; ins5[4][0] << 4;
	ins6[0][2] << 1; ins6[1][3] << 2; ins6[2][4] << 3; ins6[0][4] << 4;
    }

    mat_vec_mult_test(dr2, "Dense row major");
    mat_vec_mult_test(dr3, "Dense row major");
    mat_vec_mult_test(dr4, "Dense row major");
    mat_vec_mult_test(dr5, "Dense row major");
    mat_vec_mult_test(dr6, "Dense row major");

    mat_vec_mult_test(dr, "Dense row major");
#endif

    return 0;
}


/// @file raster_example2.cc
/// @brief raster example
/// @author Jeff Perry <jeffsp@gmail.com>
/// @version 1.0
/// @date 2013-01-14

#include "jack_rabbit/jack_rabbit.h"
#include <algorithm>
#include <boost/static_assert.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

using namespace jack_rabbit;
using namespace std;

// Our video display mode dimensions.
const size_t WIDTH = 320;
const size_t HEIGHT = 240;

// An RGB pixel
struct Pixel
{
    char rgb[3];
};

// Imagine that some sort of a display class, like this one,
// provides access to the video display buffer.  We can
// interface with the video buffer using a raster if we
// utilize a simple custom allocator, defined below.
template <
    size_t W,
    size_t H
>
class Display
{
    public:
    Display () : p_ (W * H) { }
    Pixel *Pixels () { return &p_[0]; }
    size_t Width () const { return W; }
    size_t Height () const { return H; }
    private:
    vector<Pixel> p_;
};
Display<WIDTH,HEIGHT> display;

// Our custom allocator does not allocate any memory, but
// instead it returns an address to the video display
// memory.  The container that uses this allocator might not
// ask for *exactly* the number of bytes specified in the
// raster constructor.  If this happens, allocate will fail.
template<class T>
class Alloc : public allocator<T>
{
    public:
    T *allocate (size_t n, const void *hint = 0)
    {
        if (n != display.Width () * display.Height ())
            throw runtime_error ("allocate failed");
        return display.Pixels ();
    }
    void deallocate (Pixel *p, size_t n) { }
    private:
};

// Convert from YUV colorspace to RGB colorspace
template<typename Ty>
inline Ty YUVR (Ty y, Ty  , Ty v) { return  y + 1.14 * v; }
template<typename Ty>
inline Ty YUVG (Ty y, Ty u, Ty v) { return  y - 0.40 * u - 0.58 * v; }
template<typename Ty>
inline Ty YUVB (Ty y, Ty u, Ty  ) { return  y + 2.03 * u; }
template<typename Ty>
inline Ty CLIP (Ty x) { return x > 1.0 ? 1.0 : (x < 0.0 ? 0.0 : x); }

// Our subscript function object writes a chromaticity
// diagram of YUV values.  The diagram is white in the
// center and smoothly changes color from green to red, then
// purple, blue, and back to green as it goes around the
// edge of the diagram.
template<typename T>
struct Func
{
    Func (size_t /*r*/, size_t /*c*/) :
        MAXD (sqrt (static_cast<double> (CX * CX + CY * CY)))
    { }
    T operator() (size_t r, size_t c)
    {
        // Luminance is determined by distance from center
        double dx = static_cast<double> (c) - CX;
        double dy = static_cast<double> (r) - CY;
        double l = 1.0 - sqrt (dx * dx + dy * dy) / MAXD;
        // Color is determined by coordinate
        double v = static_cast<double> (c) / W - 0.5;
        double u = static_cast<double> (r) / H - 0.5;
        Pixel p;
        // Convert YUV to RGB
        p.rgb[0] = static_cast<char> (CLIP (YUVR (l, u, v)) * 255);
        p.rgb[1] = static_cast<char> (CLIP (YUVG (l, u, v)) * 255);
        p.rgb[2] = static_cast<char> (CLIP (YUVB (l, u, v)) * 255);
        return p;
    }
    static const int W = WIDTH;
    static const int H = HEIGHT;
    static const int CX = (W - 1) / 2;
    static const int CY = (H - 1) / 2;
    const double MAXD;
};

// PPM file writer helper
template<class T>
void WritePPM (const T &m, ofstream &ofs)
{
    // Write a ppm header
    ofs << "P6\n"
        << "# Raster Example\n"
        << m.cols () << ' ' << m.rows () << '\n'
        << "255\n";

    // Write the ppm pixels
    const std::streamsize sz =
        static_cast<std::streamsize> (m.size () * 3);
    ofs.write (reinterpret_cast<const char *> (&m[0]), sz);
}

int main ()
{
    try
    {
        BOOST_STATIC_ASSERT (sizeof (Pixel) == 3);

        // Access the display through a raster
        raster<Pixel> pixels (display.Height (), display.Width (), Pixel (), Alloc<Pixel> ());

        // Note that functor expects rows X cols
        subscript_generator<Pixel,Func> f (display.Height (), display.Width ());

        // Make a colorful display
        generate (pixels.begin (), pixels.end (), f);

        // Save the image to a file
        const string fn ("raster_example2.ppm");
        clog << "Writing image to " << fn << "..." << endl;

        ofstream ofs (fn.c_str ());
        if (!ofs)
            throw std::runtime_error ("Could not open file for writing");

        WritePPM (pixels, ofs);

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


// MIT License
//
// MEII - MAHI Exo-II Library
// Copyright (c) 2020 Mechatronics and Haptic Interfaces Lab - Rice University
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// Author(s): Craig McDonald (craig.g.mcdonald@gmail.com)

#pragma once

#include <vector>
#include <Mahi/Util/Math/Integrator.hpp>
#include <Mahi/Util/Timing/Time.hpp>
#include <Mahi/Robo/Trajectories/Trajectory.hpp>
#include <Mahi/Robo/Trajectories/WayPoint.hpp>
#include <Eigen/Dense>

namespace mahi {
namespace robo {

    class DynamicMotionPrimitive {

    public:

		/// Constructor without nonlinear function
		DynamicMotionPrimitive(const mahi::util::Time &sample_period, const WayPoint &start, const WayPoint &goal, double gamma = 25.0 / 3.0);

		/// Returns the trajectory generated by the DMP
        const Trajectory& trajectory();

		/// Updates the trajectory of the DMP based on the new value of theta
		const Trajectory& update(const std::vector<double> &theta);

		/// Clears the DMP memory
        void clear();

		/// Sets the start point and regenerates the trajectory. Returns true if successful.
		bool set_start(const WayPoint &start);

		/// Sets the goal point and regenerates the trajectory. Returns true if successful.
		bool set_goal(const WayPoint &goal);

		/// Sets the start point and goal point and regenerates the trajectory. Returns true if successful.
		bool set_endpoints(const WayPoint &start, const WayPoint &goal);

		/// Sets the interp_method and max_diff properties of the trajectory.
		void set_trajectory_params(Trajectory::Interp interp_method = Trajectory::Interp::Linear, const std::vector<double> &max_diff = { mahi::util::INF });

		/// Returns the value of the parameter gamma
		double get_gamma() const;

		/// Returns the value of the parameter tau
		double get_tau() const;

    private:

		/// Checks that input parameters start, goal, K, and D all have dimensions that are consistent
        bool check_param_dim();

		/// Sets the parameter tau based on given waypoints and generates a vector of waypoint times for the trajectory
		void set_timing_parameters();

		/// Generate trajectory from given parameters
		void generate_trajectory();

    private:

		mahi::util::Time Ts_; // sample period
        WayPoint q_0_; // starting point
		WayPoint g_; // goal point
        Eigen::MatrixXd K_; // stiffness matrix
        Eigen::MatrixXd D_; // damping matrix

		double gamma_; // rate parameter for decay of nonlinear vector field
        double tau_; // temporal scaling factor ensuring arrival at the goal
        double s_; // phase variable that monotonically decreases from one to zero

        std::size_t path_dim_; // dimensionality of the trajectory
        std::size_t path_size_; // number of waypoints in the trajectory
        std::vector<double> times_; // vector of times associated with trajectory waypoints
		std::size_t current_time_idx_; // index for tracking generation of trajectory
		std::vector<mahi::util::Integrator> integrator_; // vector of integrators for integrating state equations

        Eigen::VectorXd q_0_mat_; // matrix for storing starting point position
		Eigen::VectorXd g_mat_; // matrix for storing goal point position
        Eigen::VectorXd q_mat_; // matrix for storing current states
        Eigen::VectorXd q_dot_mat_; // matrix for storing current first time derivative of states
		Eigen::VectorXd q_ddot_mat_; // matrix for storing current second time derivative of states
		Eigen::VectorXd theta_mat_; // matrix for storing current feature weighting vector

        Trajectory trajectory_; // trajectory generated upon construction or update of feature weighting vector theta

    };

}  // namespace robo
}  // namespace mahi


#define BOOST_TEST_MODULE ea_solver_unit_tests

#include <boost/test/included/unit_test.hpp>
#include <armadillo>
#include "../src/numerical/evolutionary_algorithm/ea_solver.h"
#include "test_utils.cpp"
#include "../src/logging/easylogging++.h"

INITIALIZE_EASYLOGGINGPP

using namespace arma;

mat signals = create_signals();
auto solver = EASolver(SIGNALS, 200, 200, 0.0001, 100, 50);

BOOST_AUTO_TEST_CASE(good_initial_guess_should_converge_quickly)
{
    mat weights = {1, 1, 2};
    mat guess = {0.5, 1.5, 2.1};
    solver.set_initial_guess(guess);
    mat signal = sum_signal(weights, signals);
    mat result = solver.solve(signal);
    BOOST_CHECK(is_equal(weights, result, 0.1));
}

BOOST_AUTO_TEST_CASE(bad_initial_guess_should_not_converge)
{
    mat weights = {1, 1, 2};
    mat guess = {-50000, 50000, 500};
    solver.set_initial_guess(guess);
    mat signal = sum_signal(weights, signals);
    mat result = solver.solve(signal);
    BOOST_CHECK(!is_equal(weights, result, 0.1));       // Note: !is_equal = not equal
}


#ifndef __MOTIONNLP_HPP__
#define __MOTIONNLP_HPP__

#include "IpTNLP.hpp"
#include <math.h>
#include <Eigen/Dense>
#include <iostream>

#include "motion_planner/Agent.hpp"
#include <motion_planner/NumericalIntegrationConstraints.hpp>

using namespace Ipopt;

class MotionNLP: public TNLP
{

public:

  Agent* p_agent;

  /** default constructor */
  MotionNLP();
  MotionNLP(Agent* p_agent);

  /** default destructor */
  virtual ~MotionNLP();

     /**@name Overloaded from TNLP */
     //@{
     /** Method to return some info about the nlp */
     virtual bool get_nlp_info(
        Index&          n,
        Index&          m,
        Index&          nnz_jac_g,
        Index&          nnz_h_lag,
        IndexStyleEnum& index_style
     );

     /** Method to return the bounds for my problem */
     virtual bool get_bounds_info(
        Index   n,
        Number* x_l,
        Number* x_u,
        Index   m,
        Number* g_l,
        Number* g_u
     );

     /** Method to return the starting point for the algorithm */
     virtual bool get_starting_point(
        Index   n,
        bool    init_x,
        Number* x,
        bool    init_z,
        Number* z_L,
        Number* z_U,
        Index   m,
        bool    init_lambda,
        Number* lambda
     );

     /** Method to return the objective value */
     virtual bool eval_f(
        Index         n,
        const Number* x,
        bool          new_x,
        Number&       obj_value
     );

     /** Method to return the gradient of the objective */
     virtual bool eval_grad_f(
        Index         n,
        const Number* x,
        bool          new_x,
        Number*       grad_f
     );

     /** Method to return the constraint residuals */
     virtual bool eval_g(
        Index         n,
        const Number* x,
        bool          new_x,
        Index         m,
        Number*       g
     );

     /** Method to return:
      *   1) The structure of the Jacobian (if "values" is NULL)
      *   2) The values of the Jacobian (if "values" is not NULL)
      */
     virtual bool eval_jac_g(
        Index         n,
        const Number* x,
        bool          new_x,
        Index         m,
        Index         nele_jac,
        Index*        iRow,
        Index*        jCol,
        Number*       values
     );

     /** Method to return:
      *   1) The structure of the Hessian of the Lagrangian (if "values" is NULL)
      *   2) The values of the Hessian of the Lagrangian (if "values" is not NULL)
      */
     virtual bool eval_h(
        Index         n,
        const Number* x,
        bool          new_x,
        Number        obj_factor,
        Index         m,
        const Number* lambda,
        bool          new_lambda,
        Index         nele_hess,
        Index*        iRow,
        Index*        jCol,
        Number*       values
     );

     /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
     virtual void finalize_solution(
        SolverReturn               status,
        Index                      n,
        const Number*              x,
        const Number*              z_L,
        const Number*              z_U,
        Index                      m,
        const Number*              g,
        const Number*              lambda,
        Number                     obj_value,
        const IpoptData*           ip_data,
        IpoptCalculatedQuantities* ip_cq
     );
     //@}
     double getQuadraticCost(const Eigen::Vector3d & final_state,
                                                         const Eigen::Vector3d & target,
                                                         Eigen::Vector3d & gradient_f);
  private:
     /**@name Methods to block default compiler methods.
      *
      * The compiler automatically generates the following three methods.
      *  Since the default compiler implementation is generally not what
      *  you want (for all but the most simple classes), we usually
      *  put the declarations of these methods in the private section
      *  and never implement them. This prevents the compiler from
      *  implementing an incorrect "default" behavior without us
      *  knowing. (See Scott Meyers book, "Effective C++")
      */
     //@{
     MotionNLP(
        const MotionNLP&
     );

     MotionNLP& operator=(
        const MotionNLP&
     );
     //@}
     NumericalIntegrationConstraints integration_constr;

  };

  #endif


/*************************************************
 * @Description: file content
 * @Author: yuanquan
 * @Email: yuanquan2011@qq.com
 * @Date: 2021-05-17 21:20:13
 * @LastEditors: yuanquan
 * @LastEditTime: 2021-07-24 12:20:47
 * @copyright: Copyright (c) yuanquan
 *************************************************/
#include <iostream>
#include <vector>
#include <set>
#include <string>
#include <gtest/gtest.h>
#include <boost/array.hpp>
#include <tuple>

using namespace std;
using namespace boost;
using namespace testing;

int removeElement(vector<int>& nums, int val) {
	int len = 0;
	for(int i=0; i < nums.size(); ++i)
	{
		if(nums[i] != val)
		{
			nums[len] = nums[i];
			++len;
		}
	}
	return len;
}

TEST(removeDuplicatesTest, test)
{
	using TestData = tuple<vector<int>, int, vector<int>>;
	vector<TestData> datas{
		make_tuple(vector<int>{3,2,2,3}, 3, vector<int>{2, 2}),
		make_tuple(vector<int>{0, 1, 2, 2, 3, 0, 4, 2}, 2, vector<int>{0, 1, 3, 0, 4}),

	};
	for(auto& dat : datas)
	{
		auto& nums = get<0>(dat);
		auto& val = get<1>(dat);
		auto& result = get<2>(dat);
		nums.resize(removeElement(nums, val));
		sort(result.begin(), result.end());
		sort(nums.begin(), nums.end());
		EXPECT_EQ(result, nums);
	}
}