#include "network.hpp"
#include "fc_layer.hpp"
#include "relu_layer.hpp"
#include "tanh_layer.hpp"

#include <iostream>
#include <regex>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

// Short alias for this namespace
namespace pt = boost::property_tree;

namespace net
{
Network& Network::add_layer_imp( layer_t layer )
{
	mLayers.push_back( std::move(layer) );
	return *this;
}

/*ComputationNode Network::forward(Vector input) const
{
	ComputationNode node( std::move(input) );

	for(auto& layer : mLayers)
	{
		node = layer->forward( std::move(node) );
	}
	return node;
}

ComputationNode Network::operator()( Vector input ) const
{
	return forward( std::move(input) );
}
*/
void Network::update(Solver& solver)
{
    for(auto& l : mLayers)
	{
        l->update(solver);
	}
}

Network Network::clone() const
{
	Network newnet;
	for(const auto& layer : mLayers)
	{
		newnet.mLayers.push_back( layer->clone() );
	}
	return newnet;
}

bool Network::save (std::string filename)
{
	std::ofstream file;

	file.open(filename.c_str());

	for (layer_t layer : mLayers) {
		layer->save (file);
		file << "\n";
	}
	file << "END";

	file.close ();
	return true;
}

bool Network::load (std::string filename)
{
	std::ifstream input(filename.c_str());
	if (input.fail()) {
		std::cerr << "ERROR. Cannot Read a fc_layer" << std::endl;
		return false;
	}

	std::string line;
	while (getline(input, line))
	{
		Matrix m;
		if (line.compare ("FC") == 0) {
			loadLayer (input, m);
			(*this) << FcLayer (m);
		}
		else if (line.compare ("RELU") == 0) {
			loadLayer (input, m);
			(*this) << ReLULayer (m);
		}
		else if (line.compare ("TANH") == 0) {
			loadLayer (input, m);
			(*this) << TanhLayer (m);
		}
		else if (line.compare ("END") == 0) {
			break;
		}
		else {
			input.close ();
			return false;
		}
	}
	input.close ();
	return true;
}

std::string fix_json_numbers(const std::string& json_str)
{
    std::regex re("\\\"(-?[0-9]+\\.{0,1}[0-9]*(e-?[0-9]+)?)\\\"");
    return std::regex_replace(json_str, re, "$1");
}

bool Network::saveJson (std::string filename)
{

	std::vector<std::vector<std::vector<number_t>>> weights;
	std::vector<std::vector<std::vector<number_t>>> biases;

	for (layer_t layer : mLayers) {
		layer->saveJson(weights, biases);
	}

	pt::ptree root;

	root.put("cost", "CrossEntropyCost");
	pt::ptree content;
	for (int i = 0; i < weights.size(); i++)
	{
    	pt::ptree matrix_node;
    	const std::vector<std::vector<number_t>>& matrix = weights[i];

    	for (int j = 0; j < matrix.size(); j++)
    	{
    		pt::ptree row;
    		const std::vector<number_t> matrix_row = matrix[j];
    		for (int k = 0; k < matrix_row.size(); k++) {
        		pt::ptree cell;
        		cell.put_value(matrix_row[k]);
        		row.push_back(std::make_pair("", cell));
        	}
    		matrix_node.push_back(std::make_pair("", row));
    	}
    	content.push_back(std::make_pair("", matrix_node));
	}
	root.add_child("weights", content);

	pt::ptree content2;
	for (int i = 0; i < biases.size(); i++)
	{
    	pt::ptree matrix_node;
    	const std::vector<std::vector<number_t>>& matrix = biases[i];

    	for (int j = 0; j < matrix.size(); j++)
    	{
    		pt::ptree row;
    		const std::vector<number_t> matrix_row = matrix[j];
    		for (int k = 0; k < matrix_row.size(); k++) {
        		pt::ptree cell;
        		cell.put_value(matrix_row[k]);
        		row.push_back(std::make_pair("", cell));
        	}
    		matrix_node.push_back(std::make_pair("", row));
    	}
    	content2.push_back(std::make_pair("", matrix_node));
	}
	root.add_child("biases", content2);

	pt::ptree size;
	std::vector<int> sizes;

	int pre_m = -1, pre_n = -1, m, n;
	for (int i = 0; i < weights.size(); i++) {
		n = weights[i].size();
		m = weights[i][0].size();
		if (pre_n != m)
			sizes.push_back(m);
		sizes.push_back(n);

		pre_m = m;
		pre_n = n;
	}

	for (int s : sizes) {
    	pt::ptree size_node;
    	size_node.put("", s);

    	size.push_back(std::make_pair("", size_node));
	}
	root.add_child("sizes", size);

	std::ostringstream buf;
	pt::write_json (buf, root);
	std::string treetext = fix_json_numbers(buf.str());

	std::ofstream file;
	file.open(filename.c_str());
	file << treetext;
	file.close();

	return true;
}
}


#include <iostream>
#include "PhaseSpace.h"
#include <math.h>
#include <vector>
#include "statistics.h"
//#include "data_processing.h"
//#include "using_gnuplot.h"
//#include "cpp_dec_float.hpp"
//#include "gnuplot_i.hpp"
//#include "gnuplot-iostream.h"
//#include <boost/tuple/tuple.hpp>
using namespace std;

PhaseSpace::PhaseSpace(double hWidthC, double hHeightC, double VzDistC, double zDistC, double chirpC, double bC, double pulseEnergyC, double intensityMultiplierC,
					   double hDepthC, double hDepthVelC, double VxDistC, double xDistC, double chirpTC, double bTC, double VzCC, double zCC, double VxCC, double xCC)
	: hWidth(hWidthC), hHeight(hHeightC), VzDist(VzDistC), zDist(zDistC), chirp(chirpC), b(bC), pulseEnergy(pulseEnergyC), intensityMultiplier(intensityMultiplierC),
	  hDepth(hDepthC), hDepthVel(hDepthVelC), VxDist(VxDistC), xDist(xDistC), chirpT(chirpTC), bT(bTC), VzC(VzCC), zC(zCC), VxC(VxCC), xC(zCC) {}

PhaseSpace::PhaseSpace(vector<PhaseSpace> spaces) : hWidth(0), hHeight(0), VzDist(0), zDist(0), chirp(0), b(0), pulseEnergy(0), intensityMultiplier(0), hDepth(0), hDepthVel(0), VxDist(0), xDist(0), chirpT(0), bT(0), VzC(0), zC(0), VxC(0), xC(0)
{
	for (int i = 0; i < splitNumber; i++)
	{
		VzC += spaces[i].VzC * spaces[i].intensityMultiplier;
		//DEBUGGING cout << spaces[i].intensityMultiplier << endl;
		zC += spaces[i].zC * spaces[i].intensityMultiplier;
		xC += spaces[i].xC * spaces[i].intensityMultiplier;
		intensityMultiplier += spaces[i].intensityMultiplier;
		pulseEnergy += spaces[i].pulseEnergy;
		//spaces[i].print();
	}
	//this.VzC = taskPool.reduce!"a + b"(0.0, std.algorithm.map!"a.VzC"(spaces))*this.intensityRatio;
	//int maxSize = sizeof(spaces[]);

	//for (PhaseSpace space : spaces[splitNumber]){
	//	VzC += space.VzC*space.intensityMultiplier;
	//	zC += space.zC*space.intensityMultiplier;
	//	xC += space.xC*space.intensityMultiplier;
	//}
	//this.zC = taskPool.reduce!"a + b"(0.0, std.algorithm.map!"a.zC"(spaces));

	//Recalculation from single variable change
	//HWIDTH BASED RECALCULATION
	//hWidth = spaces[0].hWidth; (Shown not to be accurate/gives wrong results)
	hWidth = originalHWidth + (spaces[0].hWidth - originalHWidth) * splitNumber;
	hHeight = spaces[0].hHeight * splitNumber; //If you add a * 1/chirp here sometimes it doesn't process it for some reason, a 1/chirp isn't needed here anyway but its an odd mystery why its only sometimes processed
	zDist = spaces[0].zDist;
	VzDist = zDist * hHeight / hWidth;
	chirp = VzDist * sqrt((1 / pow(zDist, 2)) - (1 / pow(hWidth, 2)));
	b = chirp * pow(zDist / VzDist, 2);
	hDepth = spaces[0].hDepth;
	hDepthVel = spaces[0].hDepthVel;
	VxDist = spaces[0].VxDist;
	xDist = spaces[0].xDist;
	chirpT = spaces[0].chirpT;
	bT = spaces[0].bT;

	//pulseEnergy = taskPool.reduce!"a + b"(0.0, std.algorithm.map!"a.totalPulseEnergy"(spaces));
	//intensityMultiplier = taskPool.reduce!"a + b"(0.0, std.algorithm.map!"a.intensityRatio"(spaces));
}

vector<PhaseSpace> PhaseSpace::split()
{
	vector<PhaseSpace> splitSpaces;
	originalHWidth = hWidth;
	//phaseSpaces.length = to!int(spaces);
	double intensityMultipliers[splitNumber];
	for (int i = 0; i < splitNumber; i++)
	{
		intensityMultipliers[i] = get_intensity(splitNumber, i + 1);
	}
	double splitHHeight = 0;
	double splitVzDist = 0;
	double splitB = 0;
	double splitZDist = 0;
	double splitChirp = 0;
	splitHHeight = hHeight / splitNumber;
	splitVzDist = VzDist / splitNumber;
	//b = chirp * pow(zDist / VzDist, 2);
	splitB = zDist * sqrt((1 / pow(splitVzDist, 2)) - (1 / pow(splitHHeight, 2)));
	splitZDist = splitB / (sqrt((1 / pow(splitVzDist, 2)) - (1 / pow(splitHHeight, 2))));
	splitChirp = splitVzDist * sqrt((1 / pow(zDist, 2)) - (1 / pow(hWidth, 2)));
	//i, ref elem; phaseSpaces

	for (int j = 0; j < splitNumber; j++)
	{
		splitSpaces.push_back(PhaseSpace(hWidth, splitHHeight, splitVzDist, splitZDist, splitChirp, splitB, pulseEnergy * intensityMultipliers[j], intensityMultipliers[j],
										 hDepth, hDepthVel, VxDist, xDist, chirpT, bT, hHeight - (hHeight * 2 / splitNumber) * (double(j) + 0.5), (hHeight - (hHeight * 2 / splitNumber) * (double(j) + 0.5)) / chirp, VxC, xC));
	}
	phaseSpaces += splitNumber;
	return splitSpaces;
}

vector<PhaseSpace> PhaseSpace::shatter(vector<vector<double>> spectroTable)
{
	int spaces = spectroTable.size();
	vector<PhaseSpace> shatteredPulses;
	double newVzDist = VzDist / spaces;
	double newChirp = newVzDist * sqrt((1 / pow(zDist, 2)) - (1 / pow(hWidth, 2)));
	double newB = newChirp * pow(zDist / newVzDist, 2);
	for (int i = 0; i < spaces; i++)
	{
		//cout << spectroTable[i][1] << endl;
		shatteredPulses.push_back(PhaseSpace(hWidth, hHeight / spaces, newVzDist, zDist, newChirp, newB, pulseEnergy * spectroTable[i][1 / baseTotal], intensityMultiplier * spectroTable[i][1],
											 hDepth, hDepthVel, VxDist, xDist, chirpT, bT, hHeight + (spectroTable[i][0] / 1117), zC, VxC, xC));
	}
	return shatteredPulses;
}

double PhaseSpace::intensity(double x, double y)
{
	double negTwohWidthsq = -2 * hWidth * hWidth;
	double twoVzIntDistsq = 2 * VzDist * VzDist;
	double twoPIhWidthVzIntDist = 2 * M_PI * (hWidth * VzDist);
	return exp((x * x / (negTwohWidthsq)) - ((y - chirp * x) * (y - chirp * x) / (twoVzIntDistsq))) / (twoPIhWidthVzIntDist);
}

double PhaseSpace::get_intensity(double numSections, double sectionNum)
{
	//Gets intensity % proportionally to 1 (like if its gets .5 its 50% of total intensity)
	//search with xSearch & ySearch = +- 5.803*hWidth or hHeight to get the total intensity of the phase space (equal to 1)
	double ySearchLB = -catchFactor * hHeight + ((catchFactor * hHeight * 2.0 / numSections) * (sectionNum - 1));
	double ySearchUB = catchFactor * hHeight - (catchFactor * hHeight * 2.0 / numSections) * (numSections - sectionNum);
	double xSearchLB = -catchFactor * hWidth;
	double xSearchUB = catchFactor * hWidth;

	//Convert to intensity_integration parameters
	double xHalfRange = (xSearchUB - xSearchLB) / 2;
	double yHalfRange = (ySearchUB - ySearchLB) / 2;

	double xOffset = (xSearchUB + xSearchLB) / 2;
	double yOffset = (ySearchUB + ySearchLB) / 2;

	return intensity_integration(xHalfRange, yHalfRange, xOffset, yOffset);
}

double PhaseSpace::x_integration(double xLeftLim, double xRightLim)
{
	double accuracyY = 2 * hHeight / 199;
	double accuracyX = (xRightLim - xLeftLim) / 199;
	double x = xLeftLim;
	double y = -hHeight + accuracyY / 2;

	double intensityValue = 0;

	while (y < hHeight)
	{
		while (x < xRightLim)
		{
			intensityValue += accuracyX * accuracyY * intensity(x - xC, y);
			x += accuracyX;
		}
		y += accuracyY;
		double x = xLeftLim;
	}
	return intensityValue;
}

double PhaseSpace::intensity_integration(double xHalfRange, double yHalfRange, double xOffset, double yOffset)
{
	double accuracyY = 2 * yHalfRange / 199;
	double accuracyX = 2 * xHalfRange / 199;
	double x = -xHalfRange + xOffset + accuracyX / 2;
	double y = -yHalfRange + yOffset + accuracyY / 2;

	double intensityValue = 0;

	while (y < yHalfRange + yOffset)
	{
		while (x < xHalfRange + xOffset)
		{
			intensityValue += accuracyX * accuracyY * intensity(x, y);
			x += accuracyX;
		}
		y += accuracyY;
		x = -xHalfRange + xOffset + accuracyX / 2;
	}
	return intensityValue;
}

void PhaseSpace::grid_integration(double xHalfRange, double yHalfRange, double xOffset, double yOffset, double grid[modelingXRange][modelingYRange], double xGridHalfRange, double yGridHalfRange)
{
	double accuracyY = 2 * yHalfRange / 199;
	double accuracyX = 2 * xHalfRange / 199;
	double x = -xHalfRange + xOffset + accuracyX / 2;
	double y = -yHalfRange + yOffset + accuracyY / 2;

	double intensityValue = 0;

	while (y < yHalfRange + yOffset)
	{
		while (x < xHalfRange + xOffset)
		{
			grid[int(map(x, -xGridHalfRange, xGridHalfRange, 0, double(modelingXRange) - 1) + 0.5)][int(map(y, -yGridHalfRange, yGridHalfRange, 0, double(modelingYRange) - 1) + 0.5)] += accuracyX * accuracyY * intensity(x, y);
			x += accuracyX;
		}
		y += accuracyY;
		x = -xHalfRange + xOffset + accuracyX / 2;
	}
}

PhaseSpace PhaseSpace::evolution(double dist)
{ //--To deal with processing we might need to make our own math functions. (less/more digits of accuracy)
	//A divided by 1E6 was found in D code. Reason is unknown.
	double postTime = dist / 164.35;
	b += postTime;
	bT += postTime;

	if (chirp > 0)
	{
		VzDist = sqrt(1 / ((1 / pow(hHeight, 2)) + pow((b / zDist), 2)));
	}
	if (chirpT > 0)
	{
		VxDist = sqrt(1 / ((1 / pow(hDepthVel, 2)) + pow((bT / xDist), 2)));
	}
	if (chirp < 0)
	{
		zDist = b / (sqrt((1 / pow(VzDist, 2)) - (1 / pow(hHeight, 2))));
	}
	if (chirpT < 0)
	{
		xDist = bT / (sqrt((1 / pow(VxDist, 2)) - (1 / pow(hDepthVel, 2))));
	}

	//VzDist = sqrt(1/((1/pow(hHeight,2))+pow((b/zDist),2)));
	//VxDist = sqrt(1/((1/pow(hDepthVel,2))+pow((bT/xDist),2)));

	chirp = b * pow(VzDist / zDist, 2);
	chirpT = bT * pow(VxDist / xDist, 2);
	hWidth = sqrt(1 / ((1 / pow(zDist, 2)) - pow(chirp / VzDist, 2)));
	hDepth = sqrt(1 / ((1 / pow(xDist, 2)) - pow(chirpT / VxDist, 2)));

	zC += VzC * postTime;
	xC += hDepthVel * postTime;
	return *this;
}

PhaseSpace PhaseSpace::RFLens(double power)
{
	double tempSlope = VzC / zC;
	tempSlope -= sqrt(power) * RF_LENS_COEFFICIENT;
	VzC = tempSlope * zC;

	chirp -= sqrt(power) * RF_LENS_COEFFICIENT;
	zDist = sqrt(1 / ((1 / pow(hWidth, 2)) + pow(chirp / VzDist, 2)));
	b = chirp * pow(zDist / VzDist, 2);
	hHeight = sqrt(1 / ((1 / pow(VzDist, 2)) - pow(b / zDist, 2)));
	return *this;
}

PhaseSpace PhaseSpace::mag_lens(double power)
{
	//A divided by 1E12 for power was found in D code. Reason is unknown.
	//double tempSlope = VxC / xC;
	//tempSlope -= pow(power, 2) * MAG_LENS_COEFFICIENT;
	//VxC = tempSlope * xC;
	//xC = VxC / tempSlope;

	chirpT -= pow(power, 2) * MAG_LENS_COEFFICIENT;
	xDist = sqrt(1 / ((1 / pow(hDepth, 2)) + pow(chirpT / VxDist, 2)));
	bT = chirpT * pow(xDist / VxDist, 2);
	hDepthVel = sqrt(1 / ((1 / pow(VxDist, 2)) - pow(bT / xDist, 2)));
	return *this;
}

PhaseSpace PhaseSpace::spectroscopy_function()
{
	xC = xC + 7172.99042634 * VzC;
	return *this;
}

//Accessor methods
double PhaseSpace::getHWidth() { return hWidth; }
double PhaseSpace::getHHeight() { return hHeight; }
double PhaseSpace::getVzDist() { return VzDist; }
double PhaseSpace::getZDist() { return zDist; }
double PhaseSpace::getChirp() { return chirp; }
double PhaseSpace::getB() { return b; }
double PhaseSpace::getIntensityMultiplier() { return intensityMultiplier; }
double PhaseSpace::getVzC() { return VzC; }
double PhaseSpace::getZC() { return zC; }

//Longitudinal accessor methods
double PhaseSpace::getHDepth() { return hDepth; }
double PhaseSpace::getHDepthVel() { return hDepthVel; }
double PhaseSpace::getVxDist() { return VxDist; }
double PhaseSpace::getXDist() { return xDist; }
double PhaseSpace::getChirpT() { return chirpT; }
double PhaseSpace::getBT() { return bT; }
double PhaseSpace::getVxC() { return VxC; }
double PhaseSpace::getXC() { return xC; }


#include <vector>
#include "Drone.h"
#include <math.h>
#include <boost/asio/io_service.hpp>
#include <Fullnavdata.h>
#include <gnuplot_iostream.h>
#include <deque>

/*
 * PRIVATE HEADER
 */

#define DRONE_IP                    "10.42.0.10"
#define DRONE_MAX_ALTITUDE          1.0
#define DRONE_MAX_HORIZONTAL_SPEED  0.3
#define DRONE_MAX_VERTICAL_SPEED    0.3
#define LAND_AFTER_LAST_WAYPOINT    true
#define CALIBRATION_FILE            "res/calib_bd2.xml"
#define HULLPROTECTIONON            true
#define LOOK_FOR_CHESSBOARD         false

/*
 * UTILITY FUNCTIONS
 */
/**
 * Returns:
 *      value if value is inside [min; max],
 *      min if value < min,
 *      max if value > max
 * @param value
 * @param min
 * @param max
 * @return
 */
int interval(int value, int min, int max){
    if( min < value) {
        if( value < max){
            return value;
        }else{
            return max;
        }
    }else{
        return min;
    }
}

/**
 * Returns value if value is greater than +epsilon or lower than -epsilon. Otherwise, 0 is returned
 * @param value
 * @param epsilon
 * @return
 */
float valueIfAboveEpsilon(float value, float epsilon){
    if(0 < value and value < epsilon){
        return 0;
    }
    if( -epsilon < value and value < 0){
        return 0;
    }
    return value;

}

/*
 * MAIN
 */
int main(){

    /// ******************************************************************************************** CONFIGURE WAYPOINTS
    std::vector<cv::Point3d> steps = {
            cv::Point3d(-1000, 0, 1500),
            cv::Point3d(1000, 0, 1500),
            cv::Point3d(-500, 0, 2000),
            cv::Point3d(500, 0, 2000),
            cv::Point3d(0, 0, 1500),
            cv::Point3d(0, 0, 1000)
    };

    int stepCurrent= 0;
    int stepsCount = (int) steps.size();

    cv::Point3d wishedPosition(steps[0]);

    /// ***************************************************************************** CONNECT TO AND CONFIGURE THE DRONE
    Drone d;//("10.42.0.11");
    assert(d.connect());

    // Waiting for the drone to be ready
    while(!d.isRunning()){ sleep(1); }

    std::string droneIp = d.getIpAddress();

    assert(d.setMaxAltitude(DRONE_MAX_ALTITUDE));
    assert(d.setMaxHorizontalSpeed(DRONE_MAX_HORIZONTAL_SPEED));
    assert(d.setMaxVerticalSpeed(DRONE_MAX_VERTICAL_SPEED));

    std::cout << "INITIALISATION IS OK" << std::endl;

    assert(d.blockingStartStreaming());
    std::cout << "STREAMING IS OK" << std::endl;

    // Flat trim at start. The drone MUST be on the ground at that time
    if(d.blockingFlatTrim()) {
        std::cout << "FLAT TRIM IS OK" << std::endl;
    }else{
        std::cerr << "FLAT TRIM NOT OK" << std::endl;
        return 1;
    }

    assert(d.setHullPresence(HULLPROTECTIONON));
    assert(d.setVideoAutorecord(false));

    // Initialising the openCV camera
    d.blockingInitCam();

    /// *********************************** VIDEO ANALYSIS INITIALISATION. TODO SHOULD BE INSIDE A METHOD OF DRONE CLASS

    // Camera calibration file
    cv::FileStorage fs(CALIBRATION_FILE, cv::FileStorage::READ);

    if(!fs.isOpened()){
        std::cout << "Can't open calibration file!" << std::endl;
        exit(1);
    }

    cv::Mat camMatrix, distCoeff;
    float unit;

    fs["camera_matrix"] >> camMatrix;
    fs["distortion_coefficients"] >> distCoeff;
    fs["square_size"] >> unit;
    fs.release();

    // Chessboard specifications
    int chess_x = 9;
    int chess_y = 6;
    int ref_pt = 22;

    cv::Mat model_pts = cv::Mat::zeros(chess_x * chess_y, 3, CV_32FC1);

    int i = 0;
    for(int y = -(ref_pt/chess_x); y < chess_y - (ref_pt/chess_x); ++y){
        for(int x = -(ref_pt%chess_x); x < chess_x - ref_pt%chess_x; ++x){
            model_pts.at<float>(i, 0) = x;
            model_pts.at<float>(i, 1) = y;
            ++i;
        }
    }

    // Axis to be displayed on the video (it helps to see if the calibration is ~correct or not)
    std::vector<cv::Point3d> axis;
    axis.push_back(cv::Point3d(3, 0, 0));
    axis.push_back(cv::Point3d(0, 3, 0));
    axis.push_back(cv::Point3d(0, 0, -3));

    cv::Mat rvec, tvec;

    cv::Point3d distFromWished(0,0,0);
    cv::Point3d dist(0,0,0);

    cv::Mat frame;

    bool FOLLOW = false;    // if TRUE, the drone will follow the chessboard
    bool proceed = true;    // if TRUE, the program will interact with the drone
    bool first_time = true; // if TRUE, the program will use the previous chessboard location as a hint

    double camTilt, prevCamTilt = INT_MAX/2;

    cv::namedWindow(droneIp);

    /** **/
    std::cout << "GO NAVDATA" << std::endl;
    assert(d.useFullNavdata());
    sleep(1);
    assert(d.isUsingFullNavdata());
    std::cout << "YATA" << std::endl;

    Gnuplot gp;
    gp << "set title \"RAW\"\n";
    std::deque<float> lolx;
    std::deque<float> loly;
    std::deque<float> lolz;
    Eigen::Vector3f vec;

    Gnuplot gp2;
    gp2 << "set title \"CORRECTED\"\n";
    std::deque<float> lolx2;
    std::deque<float> loly2;
    std::deque<float> lolz2;
    Eigen::Vector3f vec2;
    /*
    while(1){
        if(lolx.size() > 150){
            lolx.pop_front();
            loly.pop_front();
            lolz.pop_front();
        }

        vec = d._navdata.get_gyroscope_raw();

        lolx.push_back(vec(0));
        loly.push_back(vec(1));
        lolz.push_back(vec(2));

        gp << "plot '-' binary" << gp.binFmt1d(lolx, "array") << "with lines title \"acce_x\",";
        gp << "'-' binary" << gp.binFmt1d(loly, "array") << "with lines title \"acce_y\",";
        gp << "'-' binary" << gp.binFmt1d(lolz, "array") << "with lines title \"acce_z\"\n";
        gp.sendBinary1d(lolx);
        gp.sendBinary1d(loly);
        gp.sendBinary1d(lolz);
        gp.flush();

        usleep(15000);
    }
     */

/*
    while(1){
        sleep(1);
    }
    return 0;
*/
    /** **/
    while(proceed)
    {
        frame = d.retrieveLastFrame();
        if(frame.data != NULL) {

            std::vector<cv::Point2d> corners;
            bool chess_board = false;

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

            if (lolx.size() > 150) {
                lolx.pop_front();
                loly.pop_front();
                lolz.pop_front();
            }
/*
            if (lolx2.size() > 150) {
                lolx2.pop_front();
                loly2.pop_front();
                lolz2.pop_front();
            }
*/
        //    d._navdata.lock();
            vec = d._navdata->get_gyroscope_filt();
      //      vec2 = d._navdata.get_body_speed();
    //        d._navdata.release();

            lolx.push_back(vec(0));
            loly.push_back(vec(1));
            lolz.push_back(vec(2));

/*
            lolx2.push_back(vec2(0));
            loly2.push_back(vec2(1));
            lolz2.push_back(vec2(2));
*/

            gp << "plot '-' binary" << gp.binFmt1d(lolx, "array") << "with lines title \"body x\",";
            gp << "'-' binary" << gp.binFmt1d(loly, "array") << "with lines title \"body y\",";
            gp << "'-' binary" << gp.binFmt1d(lolz, "array") << "with lines title \"body z\"\n";/*,";
            gp << "'-' binary" << gp.binFmt1d(lolx2, "array") << "with lines title \"NED x\",";
            gp << "'-' binary" << gp.binFmt1d(loly2, "array") << "with lines title \"NED y\",";
            gp << "'-' binary" << gp.binFmt1d(lolz2, "array") << "with lines title \"NED z\"\n";
            */
            gp.sendBinary1d(lolx);
            gp.sendBinary1d(loly);
            gp.sendBinary1d(lolz);
            /*
            gp.sendBinary1d(lolx2);
            gp.sendBinary1d(loly2);
            gp.sendBinary1d(lolz2);
            */
            gp.flush();

/*
            gp2 << "plot '-' binary" << gp2.binFmt1d(lolx2, "array") << "with lines title \"x\",";
            gp2 << "'-' binary" << gp2.binFmt1d(loly2, "array") << "with lines title \"y\",";
            gp2 << "'-' binary" << gp2.binFmt1d(lolz2, "array") << "with lines title \"z\"\n";
            gp2.sendBinary1d(lolx2);
            gp2.sendBinary1d(loly2);
            gp2.sendBinary1d(lolz2);
            gp2.flush();
*/
            /***********************************************************************************************/

            if (LOOK_FOR_CHESSBOARD) {
                cv::findChessboardCorners(frame, cv::Size(chess_x, chess_y), corners);
                //cv::cornerSubPix(frame, corners, cv::Size(11,11), cv::Size(-1,-1), cv::TermCriteria)

                if (chess_board) {
                    /// **************************************************************************** DECORATIONS FOR THE IHM
                    /*for(int i = 0; i < corners.size(); ++i){
                        cv::circle(frame, corners[i], 5, cv::Scalar(0, 0, 255));
                    }*/

                    cv::circle(frame, corners[ref_pt], 20, cv::Scalar(0, 255, 0));

                    cv::solvePnP(model_pts, corners, camMatrix, distCoeff, rvec, tvec, !first_time);

                    cv::Mat rotM;
                    std::vector<cv::Point2d> imgPts;
                    cv::projectPoints(axis, rvec, tvec, camMatrix, distCoeff, imgPts);

                    cv::line(frame, corners[ref_pt], imgPts[0], cv::Scalar(255, 0, 0), 5);
                    cv::line(frame, corners[ref_pt], imgPts[1], cv::Scalar(0, 255, 0), 5);
                    cv::line(frame, corners[ref_pt], imgPts[2], cv::Scalar(0, 0, 255), 5);

                    /// ****************************************************************** GETTING THE POSITION OF THE DRONE
                    cv::Rodrigues(rvec, rotM);

                    cv::Mat camPos = rotM.t() * tvec;
                    camPos *= unit;

                    dist.x = camPos.at<double>(0, 0);
                    dist.y = camPos.at<double>(1, 0);
                    dist.z = camPos.at<double>(2, 0);

                    distFromWished = dist - wishedPosition;

                    cv::putText(frame, "Dist from chessboard", cv::Point(10, 30), cv::QT_FONT_NORMAL, 1,
                                cv::Scalar(0, 0, 0), 2, 8);
                    cv::putText(frame, std::to_string((int) dist.x), cv::Point(10, 80), cv::QT_FONT_NORMAL, 1,
                                cv::Scalar(255, 0, 0), 2, 8);
                    cv::putText(frame, std::to_string((int) dist.y), cv::Point(10, 130), cv::QT_FONT_NORMAL, 1,
                                cv::Scalar(0, 255, 0), 2, 8);
                    cv::putText(frame, std::to_string((int) dist.z), cv::Point(10, 180), cv::QT_FONT_NORMAL, 1,
                                cv::Scalar(0, 0, 255), 2, 8);

                    if (FOLLOW) {

                        cv::putText(frame, "Dist from wished", cv::Point(10, 230), cv::QT_FONT_NORMAL, 1,
                                    cv::Scalar(0, 0, 0), 2, 8);
                        cv::putText(frame, std::to_string((int) distFromWished.x), cv::Point(10, 280), cv::QT_FONT_NORMAL,
                                    1, cv::Scalar(255, 0, 0), 2, 8);
                        cv::putText(frame, std::to_string((int) distFromWished.y), cv::Point(10, 330), cv::QT_FONT_NORMAL,
                                    1, cv::Scalar(0, 255, 0), 2, 8);
                        cv::putText(frame, std::to_string((int) distFromWished.z), cv::Point(10, 380), cv::QT_FONT_NORMAL,
                                    1, cv::Scalar(0, 0, 255), 2, 8);

                        // AskedDx, y, z are values according to the drone axis
                        float askedDx = (float) (distFromWished.z / 1000);
                        float askedDy = -(float) (distFromWished.x / 1000);
                        float askedDz = -(float) (distFromWished.y / 1000);

                        askedDx = valueIfAboveEpsilon(askedDx, 0.1);
                        askedDy = valueIfAboveEpsilon(askedDy, 0.1);
                        askedDz = valueIfAboveEpsilon(askedDz, 0.1);

                        cv::putText(frame, "Asked Dy", cv::Point(100, 280), cv::QT_FONT_NORMAL, 1, cv::Scalar(0, 0, 0), 2,
                                    8); //x
                        cv::putText(frame, "Asked Dz", cv::Point(100, 330), cv::QT_FONT_NORMAL, 1, cv::Scalar(0, 0, 0), 2,
                                    8); //y
                        cv::putText(frame, "Asked Dx", cv::Point(100, 380), cv::QT_FONT_NORMAL, 1, cv::Scalar(0, 0, 0), 2,
                                    8); //z

                        cv::putText(frame, std::to_string(askedDy), cv::Point(300, 280), cv::QT_FONT_NORMAL, 1,
                                    cv::Scalar(255, 0, 0), 2, 8);
                        cv::putText(frame, std::to_string(askedDz), cv::Point(300, 330), cv::QT_FONT_NORMAL, 1,
                                    cv::Scalar(0, 255, 0), 2, 8);
                        cv::putText(frame, std::to_string(askedDx), cv::Point(300, 380), cv::QT_FONT_NORMAL, 1,
                                    cv::Scalar(0, 0, 255), 2, 8);

                        /// **************************************************** TILT OF THE CAMERA TO FOLLOW THE CHESSBOARD
                        double rotate = atan(tvec.at<double>(0, 0) / tvec.at<double>(2, 0));
                        cv::Point center(frame.cols / 2, frame.rows / 2);

                        cv::line(frame, center - cv::Point(0, 100), center + cv::Point(0, 100), cv::Scalar(0, 0, 0), 2);
                        cv::line(frame, center - cv::Point(0, 100), center + cv::Point(0, 100), cv::Scalar(255, 255, 255),
                                 1);

                        cv::line(frame, center - cv::Point(100, 0), center + cv::Point(100, 0), cv::Scalar(0, 0, 0), 2);
                        cv::line(frame, center - cv::Point(100, 0), center + cv::Point(100, 0), cv::Scalar(255, 255, 255),
                                 1);

                        //rotate = valueIfAboveEpsilon(rotate, 0.01);

                        camTilt = atan(tvec.at<double>(1, 0) / tvec.at<double>(2, 0));

                        camTilt *= -57.9;

                        camTilt -= 13; // DEFAULT

                        if (abs((int) (camTilt - prevCamTilt)) > 1) {
                            std::cout << "NEW TILT " << camTilt << std::endl;
                            //TODO Changing camera tilt probably reduces position precision since it's numerical
                            // stabilisation with a fish eye and not optical stabilisation
                            d.rotateCamera((float) camTilt, 0);
                            prevCamTilt = camTilt;
                        }
                        /// ************************************************************************* PROPER MOVE BY COMMAND

                        d.moveBy(askedDx, askedDy, askedDz, (float) rotate);

                        /// ************************** IF THE DRONE REACHED THE CURRENT WAYPOINT, GO TO THE NEXT ONE OR LAND

                        if (askedDx == 0 and askedDy == 0) {
                            ++stepCurrent;
                            if (stepCurrent >= stepsCount) {
                                std::cout << "ALL WAYPOINTS REACHED" << std::endl;
                                if (LAND_AFTER_LAST_WAYPOINT) {
                                    std::cout << "ALL WAYPOINTS LAND" << std::endl;
                                    d.land();
                                }
                            } else {
                                std::cout << "WAYPOINT " << stepCurrent << "/" << stepsCount << " REACHED" << std::endl;
                                wishedPosition = steps[stepCurrent];
                            }
                        }
                    }

                    first_time = false;
                } else {
                    /// ***************************************************************************** NO CHESSBOARD DETECTED
                    cv::putText(frame,
                                "STOP EVERYTHIG, NO CHESSBOARD !",
                                cv::Point(10, 300),
                                cv::QT_FONT_NORMAL,
                                1,
                                cv::Scalar(0, 0, 255),
                                3,
                                8
                    );
                    d.moveBy(0, 0, 0, 0);
                    // Better without. TODO look around to find the board if it's not in the sight of the drone
                    //d.rotateCamera(-13.0f, 0);
                    first_time = true;
                }
            }
            /// ********************************************************************************************** IHM STUFF

            std::string help("T: takeoff, L: land, F: follow");
            help.append(FOLLOW ? " (on)" : " (off)");
            help.append(", Q: emergency, S: stop");
            cv::putText(frame, help, cv::Point(10,465), cv::QT_FONT_NORMAL, 1, cv::Scalar(255,255,255), 2);
            cv::putText(frame, help, cv::Point(10,465), cv::QT_FONT_NORMAL, 1, cv::Scalar(0,0,0), 1);

            cv::putText(frame, std::to_string(d.getBatteryLvl()), cv::Point(800, 30),
                        cv::QT_FONT_NORMAL, 1, cv::Scalar(255, 0, 255), 3, 8);

            cv::imshow(droneIp, frame);
        }

        /// ****************************************************************************************** IHM KEY REACTIONS

        char k = (char)cv::waitKey(10);
        switch(k){
            case 'l':
                d.land();
                break;
            case 'q':
                d.emergency();
                proceed = false;
                break;
            case 's':
                std::cout << "STOP" << std::endl;
                d.modifyYaw(0);
                d.modifyRoll(0);
                d.modifyPitch(0);
                d.modifyAltitude(0);
                assert(d.moveBy(0, 0, 0, 0));
                FOLLOW = false;
                break;
            case 'f':
                FOLLOW = !FOLLOW;
                if(!FOLLOW){
                    assert(d.moveBy(0,0,0,0));
                }
                break;
            case 't':
                d.takeOff();
                break;
                /// *********************************************************************************** IHM "CHEAT" KEYS
            case 'u':
                std::cout << "move 0.5m up" << std::endl;
                assert(d.moveBy(0, 0, -0.5f, 0));
                break;
            case 'd':
                std::cout << "move 0.5m down" << std::endl;
                assert(d.moveBy(0, 0, 0.5f, 0));
                break;
            case 'y':
                std::cout << "move 0.5m forward" << std::endl;
                assert(d.moveBy(0.5f, 0, 0, 0));
                break;
            case 'h':
                std::cout << "move 0.5m backward" << std::endl;
                assert(d.moveBy(-0.5f, 0, 0, 0));
                break;
            case 'j':
                std::cout << "move 0.5m right" << std::endl;
                assert(d.moveBy(0, 0.5f, 0, 0));
                break;
            case 'g':
                std::cout << "move 0.5m left" << std::endl;
                assert(d.moveBy(0, -0.5f, 0, 0));
                break;
            case '+':
                std::cout << "turn 90° right" << std::endl;
                assert(d.moveBy(0, 0, 0, 1.57f));
                break;
            case '-':
                std::cout << "turn 90° left" << std::endl;
                assert(d.moveBy(0, 0, 0, -1.57f));
                break;

            case 'w':
                d._navdata->lock();
                std::cout << "data locked" << std::endl;
                break;
            case 'x':
                d._navdata->release();
                std::cout << "data released" << std::endl;
                break;
            default:
                break;
        }
    }

    return 0;
}


// Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, ETH Zurich, Switzerland
// Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland
// Copyright (c) 2016, Luc Oth
// Copyright (C) 2016 ETH Zurich, Wyss Zurich, Zurich Eye
// All rights reserved.
//
// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Derived from https://github.com/ethz-asl/kalibr/ (2016)

#include <ze/splines/bspline.hpp>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/QR>
#include <boost/tuple/tuple.hpp>

namespace ze {

BSpline::BSpline(int spline_order)
  : spline_order_(spline_order)
{
  CHECK_GE(spline_order_, 2)
      << "The B-spline order must be greater than or equal to 2";
}

BSpline::~BSpline()
{
}

int BSpline::spline_order() const
{
  return spline_order_;
}

int BSpline::dimension() const
{
  return coefficients_.rows();
}

int BSpline::polynomialDegree() const
{
  return spline_order_ - 1;
}

void BSpline::setKnotsAndCoefficients(const std::vector<real_t>& knots,
                                      const MatrixX& coefficients)
{
  //std::cout << "setting " << knots.size() << " knots\n";
  // This will throw an exception if it is an invalid knot sequence.
  verifyKnotSequence(knots);

  // Check if the number of coefficients matches the number of knots.
  CHECK_EQ(numCoefficientsRequired(numValidTimeSegments(knots.size())), coefficients.cols())
       <<  "A B-spline of order " << spline_order_ << " requires "
       << numCoefficientsRequired(numValidTimeSegments(knots.size()))
       << " coefficients for the " << numValidTimeSegments(knots.size())
       << " time segments defined by " << knots.size() << " knots";

  knots_ = knots;
  coefficients_ = coefficients;

  initializeBasisMatrices();
}

void BSpline::initializeBasisMatrices()
{
  basis_matrices_.resize(numValidTimeSegments());

  for(unsigned i = 0; i < basis_matrices_.size(); i++)
  {
    basis_matrices_[i] = M(spline_order_,i + spline_order_ - 1);
  }
}

MatrixX BSpline::M(int k, int i)
{
  CHECK_GE(k, 1) << "The parameter k must be greater than or equal to 1";
  // \todo: redo these checks.
  CHECK_GE(i, 0) << "The parameter i must be greater than or equal to 0";
  CHECK_LT(i, (int)knots_.size())
      << "The parameter i must be less than the number of time segments";
  if(k == 1)
  {
    // The base-case for recursion.
    MatrixX M(1,1);
    M(0,0) = 1;
    return M;
  }
  else
  {
    MatrixX M_km1 = M(k-1,i);
    // The recursive equation for M
    // M_k = [ M_km1 ] A  + [  0^T  ] B
    //       [  0^T  ]      [ M_km1 ]
    //        -------        -------
    //         =: M1          =: M2
    //
    //     = M1 A + M2 B
    MatrixX M1 = MatrixX::Zero(M_km1.rows() + 1, M_km1.cols());
    MatrixX M2 = MatrixX::Zero(M_km1.rows() + 1, M_km1.cols());

    M1.topRightCorner(M_km1.rows(),M_km1.cols()) = M_km1;
    M2.bottomRightCorner(M_km1.rows(),M_km1.cols()) = M_km1;

    MatrixX A = MatrixX::Zero(k-1, k);
    for(int idx = 0; idx < A.rows(); idx++)
    {
      int j = i - k + 2 + idx;
      real_t d0 = d_0(k, i, j);
      A(idx, idx  ) = 1.0 - d0;
      A(idx, idx+1) = d0;
    }

    MatrixX B = MatrixX::Zero(k-1, k);
    for(int idx = 0; idx < B.rows(); idx++)
    {
      int j = i - k + 2 + idx;
      real_t d1 = d_1(k, i, j);
      B(idx, idx  ) = -d1;
      B(idx, idx+1) = d1;
    }

    MatrixX M_k;

    return M_k = M1 * A + M2 * B;
  }
}

real_t BSpline::d_0(int k, int i, int j)
{
  CHECK_LE(j+k-1.0, (int)knots_.size()) <<  "Index out of range with k=" << k
                                        << ", i=" << i << ", and j=" << j;
  CHECK_LT(0, (int)knots_.size()) <<  "Index out of range with k=" << k
                                  << ", i=" << i << ", and j=" << j;
  CHECK_LE(j, (int)knots_.size()) <<  "Index out of range with k=" << k
                                  << ", i=" << i << ", and j=" << j;
  CHECK_LE(i, (int)knots_.size()) <<  "Index out of range with k=" << k
                                  << ", i=" << i << ", and j=" << j;

  real_t denom = knots_[j+k-1] - knots_[j];
  if(denom <= 0.0)
  {
    return 0.0;
  }

  real_t numerator = knots_[i] - knots_[j];

  return numerator/denom;
}

real_t BSpline::d_1(int k, int i, int j)
{
  CHECK_LE(j+k-1.0, (int)knots_.size()) <<  "Index out of range with k="
                                        << k << ", i=" << i << ", and j=" << j;
  CHECK_LT(0, (int)knots_.size()) <<  "Index out of range with k="
                                  << k << ", i=" << i << ", and j=" << j;
  CHECK_LE(j, (int)knots_.size()) <<  "Index out of range with k="
                                  << k << ", i=" << i << ", and j=" << j;
  CHECK_LE(i, (int)knots_.size()) <<  "Index out of range with k="
                                  << k << ", i=" << i << ", and j=" << j;
  real_t denom = knots_[j+k-1] - knots_[j];
  if(denom <= 0.0)
  {
    return 0.0;
  }

  real_t numerator = knots_[i+1] - knots_[i];

  return numerator/denom;
}

void BSpline::setKnotVectorAndCoefficients(const VectorX& knots,
                                           const MatrixX& coefficients)
{
  std::vector<real_t> k(knots.size());
  for(unsigned i = 0; i < k.size(); i++)
  {
    k[i] = knots(i);
  }

  setKnotsAndCoefficients(k, coefficients);
}

const std::vector<real_t> BSpline::knots() const
{
  return knots_;
}

VectorX BSpline::knotVector() const
{
  VectorX k(knots_.size());
  for(unsigned i = 0; i < knots_.size(); i++)
  {
    k(i) = knots_[i];
  }

  return k;
}

const MatrixX& BSpline::coefficients() const
{
  return coefficients_;
}

void BSpline::verifyKnotSequence(const std::vector<real_t>& knots)
{
  CHECK_GE((int)knots.size(), minimumKnotsRequired())
      << "The sequence does not contain enough knots to define an active time sequence "
      << "for a B-spline of order " << spline_order_
      << ". At least " << minimumKnotsRequired()
      << " knots are required";

  for(unsigned i = 1; i < knots_.size(); i++)
  {
    CHECK_LE(knots[i-1], knots[i])
        << "The knot sequence must be nondecreasing. Knot " << i
        << " was not greater than or equal to knot " << (i-1);
  }
}

int BSpline::numValidTimeSegments(int numKnots) const
{
  int nv = numKnots - 2*spline_order_ + 1;
  return std::max(nv,0);
}

int BSpline::numValidTimeSegments() const
{
  return numValidTimeSegments(knots_.size());
}

int BSpline::minimumKnotsRequired() const
{
  return numKnotsRequired(1);
}

int BSpline::numCoefficientsRequired(int num_time_segments) const
{
  return num_time_segments + spline_order_ - 1;
}

int BSpline::numKnotsRequired(int num_time_segments) const
{
  return numCoefficientsRequired(num_time_segments) + spline_order_;
}

real_t BSpline::t_min() const
{
  CHECK_GE((int)knots_.size(), minimumKnotsRequired())
      << "The B-spline is not well initialized";
  return knots_[spline_order_ - 1];
}

real_t BSpline::t_max() const
{
  CHECK_GE((int)knots_.size(), minimumKnotsRequired())
      << "The B-spline is not well initialized";
  return knots_[knots_.size() - spline_order_];
}

std::pair<real_t,int> BSpline::computeTIndex(real_t t) const
{
  CHECK_GE(t, t_min()) << "The time is out of range by " << (t - t_min());

  //// HACK - avoids numerical problems on initialisation
  if (std::abs(t_max() - t) < 1e-10)
  {
    t = t_max();
  }
  //// \HACK

  CHECK_LE(t, t_max())
      << "The time is out of range by " << (t_max() - t);
  std::vector<real_t>::const_iterator i;
  if(t == t_max())
  {
    // This is a special case to allow us to evaluate the spline at the boundary of the
    // interval. This is not stricly correct but it will be useful when we start doing
    // estimation and defining knots at our measurement times.
    i = knots_.end() - spline_order_;
  }
  else
  {
    i = std::upper_bound(knots_.begin(), knots_.end(), t);
  }
  //CHECK_NE(i, knots_.end()) << "Something very bad has happened in computeTIndex(" << t << ")";

  // Returns the index of the knot segment this time lies on and the width of this knot segment.
  return std::make_pair(*i - *(i-1),(i - knots_.begin()) - 1);

}

std::pair<real_t,int> BSpline::computeUAndTIndex(real_t t) const
{
  std::pair<real_t,int> ui = computeTIndex(t);

  int index = ui.second;
  real_t denom = ui.first;

  if(denom <= 0.0)
  {
    // The case of duplicate knots.
    //std::cout << "Duplicate knots\n";
    return std::make_pair(0, index);
  }
  else
  {
    real_t u = (t - knots_[index])/denom;

    return std::make_pair(u, index);
  }
}

int dmul(int i, int derivative_order)
{
  if(derivative_order == 0)
  {
    return 1;
  }
  else if(derivative_order == 1)
  {
    return i;
  }
  else
  {
    return i * dmul(i-1,derivative_order-1) ;
  }
}

VectorX BSpline::computeU(real_t uval,
                          int segmentIndex,
                          int derivativeOrder) const
{
  VectorX u = VectorX::Zero(spline_order_);
  real_t delta_t = knots_[segmentIndex+1] - knots_[segmentIndex];
  real_t multiplier = 0.0;
  if(delta_t > 0.0)
  {
    multiplier = 1.0/pow(delta_t, derivativeOrder);
  }

  real_t uu = 1.0;
  for(int i = derivativeOrder; i < spline_order_; i++)
  {
    u(i) = multiplier * uu * dmul(i,derivativeOrder) ;
    uu = uu * uval;
  }

  return u;
}

VectorX BSpline::eval(real_t t) const
{
  return evalD(t,0);
}

const MatrixX& BSpline::basisMatrixFromKnotIndex(int knot_index) const
{
  return basis_matrices_[basisMatrixIndexFromStartingKnotIndex(knot_index)];
}

VectorX BSpline::evalD(real_t t, int derivative_order) const
{
  CHECK_GE(derivative_order, 0) << "To integrate, use the integral function";
  // Returns the normalized u value and the lower-bound time index.
  std::pair<real_t,int> ui = computeUAndTIndex(t);
  VectorX u = computeU(ui.first, ui.second, derivative_order);

  int bidx = ui.second - spline_order_ + 1;

  // Evaluate the spline (or derivative) in matrix form.
  //
  // [c_0 c_1 c_2 c_3] * B^T * u
  // spline coefficients

  VectorX rv = coefficients_.block(0,bidx,coefficients_.rows(),spline_order_)
               * basis_matrices_[bidx].transpose() * u;

  return rv;
}

VectorX BSpline::evalDAndJacobian(real_t t,
                                  int derivative_order,
                                  MatrixX* Jacobian,
                                  VectorXi* coefficient_indices) const
{
  CHECK_GE(derivative_order, 0) << "To integrate, use the integral function";
  // Returns the normalized u value and the lower-bound time index.
  std::pair<real_t,int> ui = computeUAndTIndex(t);
  VectorX u = computeU(ui.first, ui.second, derivative_order);

  int bidx = ui.second - spline_order_ + 1;

  // Evaluate the spline (or derivative) in matrix form.
  //
  // [c_0 c_1 c_2 c_3] * B^T * u
  // spline coefficients

  // The spline value
  VectorX Bt_u = basis_matrices_[bidx].transpose() * u;
  VectorX v = coefficients_.block(0,bidx,coefficients_.rows(),spline_order_) * Bt_u;

  if(Jacobian)
  {
    // The Jacobian
    Jacobian->resize(coefficients_.rows(), Bt_u.size() * coefficients_.rows());
    MatrixX one = MatrixX::Identity(coefficients_.rows(), coefficients_.rows());
    for(int i = 0; i < Bt_u.size(); i++)
    {
      Jacobian->block(0, i*coefficients_.rows(),
                      coefficients_.rows(),
                      coefficients_.rows()) = one * Bt_u[i];
    }
  }

  if(coefficient_indices)
  {
    int D = coefficients_.rows();
    *coefficient_indices = VectorXi::LinSpaced(spline_order_ * D,
                                              bidx * D,
                                              (bidx + spline_order_) * D - 1);
  }

  return v;
}

std::pair<VectorX, MatrixX> BSpline::evalDAndJacobian(real_t t,
                                                      int derivative_order) const
{
  std::pair<VectorX, MatrixX> rv;

  rv.first = evalDAndJacobian(t, derivative_order, &rv.second, NULL);

  return rv;
}

MatrixX BSpline::localBasisMatrix(real_t t, int derivative_order) const
{
  return Phi(t,derivative_order);
}

MatrixX BSpline::localCoefficientMatrix(real_t t) const
{
  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;

  return coefficients_.block(0,bidx,coefficients_.rows(),spline_order_);
}

VectorX BSpline::localCoefficientVector(real_t t) const
{

  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;
  VectorX c(spline_order_ * coefficients_.rows());
  for(int i = 0; i < spline_order_; i++)
  {
    c.segment(i*coefficients_.rows(), coefficients_.rows()) = coefficients_.col(i + bidx);
  }

  return c;
}

VectorXi BSpline::localCoefficientVectorIndices(real_t t) const
{
  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;
  int D = coefficients_.rows();

  return VectorXi::LinSpaced(spline_order_*D,bidx*D,(bidx + spline_order_)*D - 1);
}

VectorXi BSpline::localVvCoefficientVectorIndices(real_t t) const
{
  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;

  return VectorXi::LinSpaced(spline_order_,bidx,(bidx + spline_order_) - 1);
}

MatrixX BSpline::Phi(real_t t, int derivative_order) const
{
  CHECK_GE(derivative_order, 0) << "To integrate, use the integral function";
  std::pair<real_t,int> ui = computeUAndTIndex(t);

  VectorX u = computeU(ui.first, ui.second, derivative_order);

  int bidx = ui.second - spline_order_ + 1;

  u = basis_matrices_[bidx].transpose() * u;

  MatrixX Phi = MatrixX::Zero(coefficients_.rows(),
                              spline_order_*coefficients_.rows());
  MatrixX one = MatrixX::Identity(Phi.rows(), Phi.rows());
  for(int i = 0; i < spline_order_; i++)
  {
    Phi.block(0,Phi.rows()*i,Phi.rows(),Phi.rows()) = one * u(i);
  }

  return Phi;
}

void BSpline::setCoefficientVector(const VectorX& c)
{
  CHECK_GE(c.size(), coefficients_.rows() * coefficients_.cols())
      << "The coefficient vector is the wrong size. The vector must contain all vector-valued coefficients stacked up into one column.";
  for(int i = 0; i < coefficients_.cols(); i++)
  {
    coefficients_.col(i) = c.segment(i * coefficients_.rows(),coefficients_.rows());
  }
}

VectorX BSpline::coefficientVector()
{
  VectorX c(coefficients_.rows() * coefficients_.cols());
  for(int i = 0; i < coefficients_.cols(); i++)
  {
    c.segment(i * coefficients_.rows(),coefficients_.rows()) = coefficients_.col(i);
  }
  return c;
}

void BSpline::setCoefficientMatrix(const MatrixX& coefficients)
{
  CHECK_EQ(coefficients_.rows(), coefficients.rows())
      << "The new coefficient matrix must match the size of the existing coefficient matrix";
  CHECK_EQ(coefficients_.cols(), coefficients.cols())
      << "The new coefficient matrix must match the size of the existing coefficient matrix";
  coefficients_ = coefficients;
}

const MatrixX& BSpline::basisMatrix(int i) const
{
  CHECK_LE(i, numValidTimeSegments()) << "index out of range";
  CHECK_LE(0, numValidTimeSegments()) << "index out of range";
  return basis_matrices_[i];
}

std::pair<real_t,real_t> BSpline::timeInterval() const
{
  return std::make_pair(t_min(), t_max());
}

std::pair<real_t,real_t> BSpline::timeInterval(int i) const
{
  CHECK_GE((int)knots_.size(), minimumKnotsRequired()) << "The B-spline is not well initialized";
  CHECK_LE(i, numValidTimeSegments()) << "index out of range";
  CHECK_LT(0, numValidTimeSegments()) << "index out of range";
  return std::make_pair(knots_[spline_order_ + i - 1],knots_[spline_order_ + i]);
}

void BSpline::initSpline(real_t t_0, real_t t_1,
                         const VectorX& p_0,
                         const VectorX& p_1)
{
  CHECK_EQ(p_0.size(), p_1.size())
      << "The coefficient vectors should be the same size";
  CHECK_GT(t_1, t_0) << "Time must be increasing from t_0 to t_1";

  // Initialize the spline so that it interpolates the two points
  // and moves between them with a constant velocity.

  // How many knots are required for one time segment?
  int K = numKnotsRequired(1);
  // How many coefficients are required for one time segment?
  int C = numCoefficientsRequired(1);
  // What is the vector coefficient dimension
  int D = p_0.size();

  // Initialize a uniform knot sequence
  real_t dt = t_1 - t_0;
  std::vector<real_t> knots(K);
  for(int i = 0; i < K; i++)
  {
    knots[i] = t_0 + (i - spline_order_ + 1) * dt;
  }
  // Set the knots and zero the coefficients
  setKnotsAndCoefficients(knots, MatrixX::Zero(D,C));

  // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
  int coefficientDim = C * D;
  // We always need an even number of constraints.
  int constraintsRequired = C + (C & 0x1);
  int constraintSize = constraintsRequired * D;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::Zero(constraintSize);

  // Add the position constraints.
  int brow = 0;
  int bcol = 0;
  A.block(brow,bcol,D,coefficientDim) = Phi(t_min(),0);
  b.segment(brow,D) = p_0;
  brow += D;
  A.block(brow,bcol,D,coefficientDim) = Phi(t_max(),0);
  b.segment(brow,D) = p_1;
  brow += D;

  if(spline_order_ > 2)
  {
    // At the very minimum we have to add velocity constraints.
    VectorX v = (p_1 - p_0)/dt;
    A.block(brow,bcol,D,coefficientDim) = Phi(t_min(),1);
    b.segment(brow,D) = v;
    brow += D;
    A.block(brow,bcol,D,coefficientDim) = Phi(t_max(),1);
    b.segment(brow,D) = v;
    brow += D;

    if(spline_order_ > 4)
    {
      // Now we add the constraint that all higher-order derivatives are zero.
      int derivativeOrder = 2;
      VectorX z = VectorX::Zero(D);
      while(brow < A.rows())
      {
        A.block(brow,bcol,D,coefficientDim) = Phi(t_min(),derivativeOrder);
        b.segment(brow,D) = z;
        brow += D;
        A.block(brow,bcol,D,coefficientDim) = Phi(t_max(),derivativeOrder);
        b.segment(brow,D) = z;
        brow += D;
        ++derivativeOrder;
      }
    }
  }

  // Now we solve the Ax=b system
  if(A.rows() != A.cols())
  {
    // The system is over constrained. This happens for odd ordered splines.
    b = (A.transpose() * b).eval();
    A = (A.transpose() * A).eval();
  }

  // Solve for the coefficient vector.
  VectorX c = A.householderQr().solve(b);
  // ldlt doesn't work for this problem. It may be because the ldlt decomposition
  // requires the matrix to be positive or negative semidefinite
  // http://eigen.tuxfamily.org/dox-devel/TutorialLinearAlgebra.html#TutorialLinAlgRankRevealing
  // which may imply that it is symmetric. Our A matrix is only symmetric in the over-constrained case.
  //VectorX c = A.ldlt().solve(b);
  setCoefficientVector(c);
}

void BSpline::addCurveSegment(real_t t, const VectorX& p_1)
{
  CHECK_GT(t, t_max())
      << "The new time must be past the end of the last valid segment";
  CHECK_EQ(p_1.size(), coefficients_.rows()) << "Invalid coefficient vector size";

  // Get the final valid time interval.
  int NT = numValidTimeSegments();
  std::pair<real_t, real_t> interval_km1 = timeInterval(NT-1);

  VectorX p_0;

  // Store the position of the spline at the  end of the interval.
  // We will use these as constraints as we don't want them to change.
  p_0 = eval(interval_km1.second);

  // Retool the knot vector.
  real_t du;
  int km1;
  std::tie(du,km1) = computeTIndex(interval_km1.first);

  // leave knots km1 and k alone but retool the other knots.
  real_t dt = t - knots_[km1 + 1];
  real_t kt = t;

  // add another knot.
  std::vector<real_t> knots(knots_);
  knots.push_back(0.0);
  // space the further knots uniformly.
  for(unsigned k = km1 + 2; k < knots.size(); k++)
  {
    knots[k] = kt;
    kt += dt;
  }
  // Tack on an new, uninitialized coefficient column.
  MatrixX c(coefficients_.rows(), coefficients_.cols() + 1);
  c.topLeftCorner(coefficients_.rows(), coefficients_.cols()) = coefficients_;
  setKnotsAndCoefficients(knots,c);

  // Now, regardless of the order of the spline, we should only have to add
  // a single knot and coefficient vector.
  // In this case, we should solve for the last two coefficient vectors
  // (i.e., the new one and the one before the new one).

  // Get the time interval of the new time segment.
  real_t t_0, t_1;
  std::tie(t_0,t_1) = timeInterval(NT);

  // what is the coefficient dimension?
  int D = coefficients_.rows();
  // How many vector-valued coefficients are required? In this case, 2.
  // We will leave the others fixed.
  int C = 2;
  // Now we have to solve an Ax = b linear system to determine the
  // correct coefficient vectors.
  int coefficientDim = C * D;
  // We always need an even number of constraints.
  int constraintsRequired = 2;
  int constraintSize = constraintsRequired * D;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::Zero(constraintSize);      // Build the A matrix.

  int phiBlockColumnOffset = D * std::max(0,(spline_order_ - 2));
  VectorX fixedCoefficients = localCoefficientVector(t_0).segment(0,
                                                                  phiBlockColumnOffset);

  // Add the position constraints.
  int brow = 0;
  int bcol = 0;
  MatrixX P;
  P = Phi(t_0,0);
  A.block(brow,bcol,D,coefficientDim) = P.block(0, phiBlockColumnOffset,
                                                D, coefficientDim);
  b.segment(brow,D) = p_0 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;
  brow += D;

  P = Phi(t_1,0);
  A.block(brow,bcol,D,coefficientDim) = P.block(0,phiBlockColumnOffset,
                                                D, coefficientDim);
  b.segment(brow,D) = p_1 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;;
  brow += D;

  // Add regularization constraints (keep the coefficients small)
  //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-4 * MatrixX::Identity(coefficientDim, coefficientDim);
  //b.segment(brow,coefficientDim) = VectorX::Zero(coefficientDim);
  //brow += coefficientDim;

  // Now we solve the Ax=b system
  if(A.rows() != A.cols())
  {
    // The system is over constrained. This happens for odd ordered splines.
    b = (A.transpose() * b).eval();
    A = (A.transpose() * A).eval();
  }

  VectorX cstar = A.householderQr().solve(b);
  coefficients_.col(coefficients_.cols() - 2) = cstar.head(D);
  coefficients_.col(coefficients_.cols() - 1) = cstar.tail(D);
}

void BSpline::removeCurveSegment()
{
  if(knots_.size() > 0 && coefficients_.cols() > 0)
  {
    knots_.erase(knots_.begin());
    coefficients_ = coefficients_.block(0,
                                        1,
                                        coefficients_.rows(),
                                        coefficients_.cols() - 1).eval();
  }
}

void BSpline::setLocalCoefficientVector(real_t t, const VectorX& c)
{
  CHECK_EQ(c.size(), spline_order_ * coefficients_.rows())
      << "The local coefficient vector is the wrong size";
  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;
  for(int i = 0; i < spline_order_; i++)
  {
    coefficients_.col(i + bidx) = c.segment(i * coefficients_.rows(),
                                            coefficients_.rows());
  }

}

void BSpline::initSpline2(const VectorX& times,
                          const MatrixX& interpolation_points,
                          int num_segments,
                          real_t lambda)
{
  CHECK_EQ(times.size(), interpolation_points.cols())
      << "The number of times and the number of interpolation points must be equal";
  CHECK_GE(times.size(), 2) << "There must be at least two times";
  CHECK_GE(num_segments, 1) << "There must be at least one time segment";
  for(int i = 1; i < times.size(); i++)
  {
    CHECK_LE(times[i-1], times[i])
        << "The time sequence must be nondecreasing. time " << i
        << " was not greater than or equal to time " << (i-1);
  }

  // Initialize the spline so that it interpolates the N points

  // How many knots are required for one time segment?
  int K = numKnotsRequired(num_segments);
  // How many coefficients are required for one time segment?
  int C = numCoefficientsRequired(num_segments);
  // What is the vector coefficient dimension
  int D = interpolation_points.rows();

  // Initialize a uniform knot sequence
  real_t dt = (times[times.size() - 1] - times[0]) / num_segments;
  std::vector<real_t> knots(K);
  for(int i = 0; i < K; i++)
  {
    knots[i] = times[0] + (i - spline_order_ + 1) * dt;
  }
  // Set the knots and zero the coefficients
  setKnotsAndCoefficients(knots, MatrixX::Zero(D,C));

  // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
  int coefficientDim = C * D;

  int numConstraints = (knots.size() - 2 * spline_order_ + 2) + interpolation_points.cols();
  int constraintSize = numConstraints * D;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::Zero(constraintSize);

  int brow = 0;
  //int bcol = 0;
  // Now add the regularization constraint.
  //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-1* MatrixX::Identity(coefficientDim, coefficientDim);
  //b.segment(brow,coefficientDim) = VectorX::Zero(coefficientDim);
  //brow += coefficientDim;
  for(int i = spline_order_ - 1; i < (int)knots.size() - spline_order_ + 1; i++)
  {
    VectorXi coeffIndices = localCoefficientVectorIndices(knots[i]);

    A.block(brow,coeffIndices[0],D,coeffIndices.size()) = lambda * Phi(knots[i],2);
    b.segment(brow,D) = VectorX::Zero(D);
    brow += D;
  }

  // Add the position constraints.
  for(int i = 0; i < interpolation_points.cols(); i++)
  {
    VectorXi coeffIndices = localCoefficientVectorIndices(times[i]);
    A.block(brow,coeffIndices[0],D,coeffIndices.size()) = Phi(times[i],0);

    b.segment(brow,D) = interpolation_points.col(i);
    brow += D;
  }

  // Now we solve the Ax=b system
  //if(A.rows() != A.cols())
  //  {
  // The system is over constrained. This happens for odd ordered splines.
  b = (A.transpose() * b).eval();
  A = (A.transpose() * A).eval();
  //  }

  // Solve for the coefficient vector.
  VectorX c = A.ldlt().solve(b);
  // ldlt doesn't work for this problem. It may be because the ldlt decomposition
  // requires the matrix to be positive or negative semidefinite
  // http://eigen.tuxfamily.org/dox-devel/TutorialLinearAlgebra.html#TutorialLinAlgRankRevealing
  // which may imply that it is symmetric. Our A matrix is only symmetric in the over-constrained case.
  // VectorX c = A.ldlt().solve(b);
  setCoefficientVector(c);
}

void BSpline::initSpline3(const VectorX& times,
                          const MatrixX& interpolation_points,
                          int num_segments,
                          real_t lambda)
{
  CHECK_EQ(times.size(), interpolation_points.cols())
      << "The number of times and the number of interpolation points must be equal";
  CHECK_GE(times.size(), 2) << "There must be at least two times";
  CHECK_GE(num_segments, 1) << "There must be at least one time segment";
  for(int i = 1; i < times.size(); i++)
  {
    CHECK_LE(times[i-1], times[i])
        <<  "The time sequence must be nondecreasing. time " << i
        << " was not greater than or equal to time " << (i-1);
  }

  // How many knots are required for one time segment?
  int K = numKnotsRequired(num_segments);
  // How many coefficients are required for one time segment?
  int C = numCoefficientsRequired(num_segments);
  // What is the vector coefficient dimension
  int D = interpolation_points.rows();

  // Initialize a uniform knot sequence
  real_t dt = (times[times.size() - 1] - times[0]) / num_segments;
  std::vector<real_t> knots(K);
  for(int i = 0; i < K; i++)
  {
    knots[i] = times[0] + (i - spline_order_ + 1) * dt;
  }

  setKnotsAndCoefficients(knots, MatrixX::Zero(D,C));

  // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
  int coefficientDim = C * D;

  int numConstraints = interpolation_points.cols();
  int constraintSize = numConstraints * D;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::Zero(constraintSize);

  int brow = 0;
  // Add the position constraints.
  for(int i = 0; i < interpolation_points.cols(); i++)
  {
    VectorXi coeffIndices = localCoefficientVectorIndices(times[i]);

    A.block(brow,coeffIndices[0],D,coeffIndices.size()) = Phi(times[i],0);

    b.segment(brow,D) = interpolation_points.col(i);
    brow += D;
  }

  b = (A.transpose() * b).eval();
  A = (A.transpose() * A).eval();

  // Add the motion constraint.
  VectorX W = VectorX::Constant(D,lambda);

  A += curveQuadraticIntegralDiag(W, 2);

  VectorX c = A.ldlt().solve(b);
  setCoefficientVector(c);

}

void BSpline::addCurveSegment2(real_t t,
                               const VectorX& p_1,
                               real_t lambda)
{
  CHECK_GT(t, t_max())
      << "The new time must be past the end of the last valid segment";
  CHECK_EQ(p_1.size(), coefficients_.rows())
      << "Invalid coefficient vector size";

  // Get the final valid time interval.
  int NT = numValidTimeSegments();
  std::pair<real_t, real_t> interval_km1 = timeInterval(NT-1);

  VectorX p_0;

  // Store the position of the spline at the  end of the interval.
  // We will use these as constraints as we don't want them to change.
  p_0 = eval(interval_km1.second);

  // Retool the knot vector.
  real_t du;
  int km1;
  std::tie(du,km1) = computeTIndex(interval_km1.first);

  // leave knots km1 and k alone but retool the other knots.
  real_t dt = t - knots_[km1 + 1];
  real_t kt = t;

  // add another knot.
  std::vector<real_t> knots(knots_);
  knots.push_back(0.0);
  // space the further knots uniformly.
  for(unsigned k = km1 + 2; k < knots.size(); k++)
  {
    knots[k] = kt;
    kt += dt;
  }
  // Tack on an new, uninitialized coefficient column.
  MatrixX c(coefficients_.rows(), coefficients_.cols() + 1);
  c.topLeftCorner(coefficients_.rows(), coefficients_.cols()) = coefficients_;
  setKnotsAndCoefficients(knots,c);

  // Now, regardless of the order of the spline, we should only have to
  // add a single knot and coefficient vector.
  // In this case, we should solve for the last two coefficient
  // vectors (i.e., the new one and the one before the
  // new one).

  // Get the time interval of the new time segment.
  real_t t_0, t_1;
  std::tie(t_0,t_1) = timeInterval(NT);

  // what is the coefficient dimension?
  int D = coefficients_.rows();
  // How many vector-valued coefficients are required? In this case, 2.
  // We will leave the others fixed.
  int C = 2;
  // Now we have to solve an Ax = b linear system to determine the correct
  // coefficient vectors.
  int coefficientDim = C * D;
  // We always need an even number of constraints.
  int constraintsRequired = 2 + 2;
  int constraintSize = constraintsRequired * D;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::Zero(constraintSize);      // Build the A matrix.

  int phiBlockColumnOffset = D * std::max(0,(spline_order_ - 2));
  VectorX fixedCoefficients = localCoefficientVector(t_0).segment(0,
                                                                  phiBlockColumnOffset);

  // Add the position constraints.
  int brow = 0;
  int bcol = 0;
  MatrixX P;
  P = Phi(t_0,0);
  A.block(brow,bcol,D,coefficientDim) = P.block(0, phiBlockColumnOffset,
                                                D, coefficientDim);
  b.segment(brow,D) = p_0 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;
  brow += D;

  P = Phi(t_1,0);
  A.block(brow,bcol,D,coefficientDim) = P.block(0, phiBlockColumnOffset,
                                                D, coefficientDim);
  b.segment(brow,D) = p_1 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;;
  brow += D;

  // Add regularization constraints (keep the acceleration small)
  P = Phi(t_0,2);
  A.block(brow,bcol,D,coefficientDim) = lambda * P.block(0, phiBlockColumnOffset,
                                                         D, coefficientDim);
  b.segment(brow,D) = VectorX::Zero(D);
  brow += D;

  P = Phi(t_1,2);
  A.block(brow,bcol,D,coefficientDim) = lambda * P.block(0, phiBlockColumnOffset,
                                                         D, coefficientDim);
  b.segment(brow,D) = VectorX::Zero(D);
  brow += D;

  //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-4 * MatrixX::Identity(coefficientDim, coefficientDim);
  //b.segment(brow,coefficientDim) = VectorX::Zero(coefficientDim);
  //brow += coefficientDim;

  // Now we solve the Ax=b system
  if(A.rows() != A.cols())
  {
    // The system is over constrained. This happens for odd ordered splines.
    b = (A.transpose() * b).eval();
    A = (A.transpose() * A).eval();
  }

  VectorX cstar = A.householderQr().solve(b);
  coefficients_.col(coefficients_.cols() - 2) = cstar.head(D);
  coefficients_.col(coefficients_.cols() - 1) = cstar.tail(D);
}

MatrixX BSpline::Vi(int segment_index) const
{
  CHECK_LT(segment_index, numValidTimeSegments())
      << "Segment index out of bounds";
  CHECK_LT(0, numValidTimeSegments())
      << "Segment index out of bounds";

  VectorX vals(spline_order_*2);
  for (int i = 0; i < vals.size(); ++i)
  {
    vals[i] = 1.0/(i + 1.0);
  }

  MatrixX V(spline_order_,spline_order_);
  for(int r = 0; r < V.rows(); r++)
  {
    for(int c = 0; c < V.cols(); c++)
    {
      V(r,c) = vals[r + c];
    }
  }

  real_t t_0,t_1;
  std::tie(t_0,t_1) = timeInterval(segment_index);

  V *= t_1 - t_0;

  return V;
}

VectorX BSpline::evalIntegral(real_t t1, real_t t2) const
{
  if(t1 > t2)
  {
    return -evalIntegral(t2,t1);
  }

  std::pair<real_t,int> u1 = computeTIndex(t1);
  std::pair<real_t,int> u2 = computeTIndex(t2);

  VectorX integral = VectorX::Zero(coefficients_.rows());

  // LHS remainder.
  real_t lhs_remainder = t1 - knots_[u1.second];
  if(lhs_remainder > 1e-16 && u1.first > 1e-16)
  {
    lhs_remainder /= u1.first;
    VectorX v(spline_order_);
    real_t du = lhs_remainder;
    for(int i = 0; i < spline_order_; i++)
    {
      v(i) = du/(i + 1.0);
      du *= lhs_remainder;
    }
    int bidx = basisMatrixIndexFromStartingKnotIndex(u1.second);
    integral -= u1.first * coefficients_.block(0,
                                               bidx,coefficients_.rows(),
                                               spline_order_)
                * basis_matrices_[bidx].transpose() * v;
  }

  // central time segments.
  VectorX v = VectorX::Zero(spline_order_);
  for(int i = 0; i < spline_order_; i++)
  {
    v(i) = 1.0/(i + 1.0);
  }

  for(int s = u1.second; s < u2.second; s++)
  {
    int bidx = basisMatrixIndexFromStartingKnotIndex(s);
    integral += (knots_[s+1] - knots_[s])
        * coefficients_.block(0, bidx, coefficients_.rows(), spline_order_)
        * basis_matrices_[bidx].transpose() * v;
  }

  // RHS remainder.
  real_t rhs_remainder = t2 - knots_[u2.second];
  if(rhs_remainder > 1e-16 && u2.first > 1e-16)
  {
    rhs_remainder /= u2.first;

    VectorX v(spline_order_);
    real_t du = rhs_remainder;
    for(int i = 0; i < spline_order_; i++)
    {
      v(i) = du / (i + 1.0);
      du *= rhs_remainder;
    }

    int bidx = basisMatrixIndexFromStartingKnotIndex(u2.second);
    integral += u2.first
                * coefficients_.block(0,bidx,coefficients_.rows(),spline_order_)
                * basis_matrices_[bidx].transpose()
                * v;
  }

  return integral;
}

int BSpline::basisMatrixIndexFromStartingKnotIndex(int starting_knot_index) const
{
  return starting_knot_index - spline_order_ + 1;
}

int BSpline::startingKnotIndexFromBasisMatrixIndex(int basis_matrix_index) const
{
  return spline_order_ + basis_matrix_index - 1;
}

MatrixX BSpline::Bij(int segment_index, int column_index) const
{
  CHECK_LE(segment_index, (int)basis_matrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  CHECK_LE(column_index, spline_order_) << "Out of range";
  CHECK_LT(0, spline_order_) << "Out of range";
  int D = coefficients_.rows();
  MatrixX B = MatrixX::Zero(spline_order_*D,D);
  for(int i = 0; i < D; i++)
  {
    B.block(i*spline_order_,i,spline_order_,1) = basis_matrices_[segment_index].col(column_index);
  }

  return B;
}

MatrixX BSpline::Mi(int segment_index) const
{
  CHECK_LE(segment_index, (int)basis_matrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  int D = coefficients_.rows();
  MatrixX M = MatrixX::Zero(spline_order_*D,spline_order_*D);

  for(int j = 0; j < spline_order_; j++)
  {
    M.block(0,j*D,D*spline_order_, D) = Bij(segment_index,j);
  }

  return M;
}

VectorX BSpline::getLocalBiVector(real_t t, int derivative_order) const
{
  VectorX ret = VectorX::Zero(spline_order_);
  getLocalBiInto(t, ret, derivative_order);

  return ret;
}

void BSpline::getLocalBiInto(real_t t, VectorX& ret, int derivative_order) const
{
  int si = segmentIndex(t);
  VectorX lu = u(t, derivative_order);
  for(int j = 0; j < spline_order_; j++) {
    ret[j] = lu.dot(basis_matrices_[si].col(j));
  }
}

VectorX BSpline::getLocalCumulativeBiVector(real_t t, int derivative_order) const
{
  VectorX bi = getLocalBiVector(t, derivative_order);
  int maxIndex = bi.rows() - 1;
  // tildeB(i) = np.sum(bi[i+1:]) :
  for(int i = 1; i <= maxIndex; i ++)
  {
    real_t sum = 0;
    for(int j = maxIndex; j > i; j--)
    {
      sum += bi[j];
    }
    bi[i] += sum;
  }
  if (derivative_order == 0)
  {
    bi[0] = 1; // the sum of k successive spline basis functions is always 1
  }
  else
  {
    bi[0] = 0;
  }
  return bi;
}

int BSpline::segmentIndex(real_t t) const
{
  std::pair<real_t,int> ui = computeTIndex(t);

  return basisMatrixIndexFromStartingKnotIndex(ui.second);
}

MatrixX BSpline::U(real_t t, int derivative_order) const
{
  VectorX uvec = u(t,derivative_order);
  int D = coefficients_.rows();
  MatrixX Umat = MatrixX::Zero(spline_order_ * D, D);

  for(int i = 0; i < D; i++)
  {
    Umat.block(i*spline_order_,i,spline_order_,1) = uvec;
  }

  return Umat;
}

VectorX BSpline::u(real_t t, int derivative_order) const
{
  std::pair<real_t,int> ui = computeUAndTIndex(t);

  return computeU(ui.first, ui.second, derivative_order);
}

MatrixX BSpline::Di(int segment_index) const
{
  int D = coefficients_.rows();
  MatrixX fullD = MatrixX::Zero(spline_order_*D, spline_order_*D);

  MatrixX subD = Dii(segment_index);

  for(int d = 0; d < D; d++)
  {
    fullD.block(d*spline_order_,d*spline_order_,spline_order_,spline_order_) = subD;
  }

  return fullD;
}

MatrixX BSpline::Dii(int segment_index) const
{
  CHECK_LE(segment_index, (int)basis_matrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  real_t t_0,t_1;
  std::tie(t_0,t_1) = timeInterval(segment_index);
  real_t dt = t_1 - t_0;

  real_t recip_dt = 0.0;
  if(dt > 0)
  {
    recip_dt = 1.0/dt;
  }
  MatrixX D = MatrixX::Zero(spline_order_,spline_order_);
  for(int i = 0; i < spline_order_ - 1; i++)
  {
    D(i,i+1) = (i+1.0) * recip_dt;
  }

  return D;
}

MatrixX BSpline::segmentQuadraticIntegral(const MatrixX& W,
                                          int segment_idx,
                                          int derivative_order) const
{
  int D = coefficients_.rows();
  //CHECK_GE(segmentIndex, (int)basisMatrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  CHECK_EQ(W.rows(), D)
      <<"W must be a square matrix the size of a single vector-valued coefficient";
  CHECK_EQ(W.cols(), D)
      << "W must be a square matrix the size of a single vector-valued coefficient";

  int N = D * spline_order_;
  MatrixX Q;// = MatrixX::Zero(N,N);
  MatrixX Dm = Dii(segment_idx);
  MatrixX V = Vi(segment_idx);
  MatrixX M = Mi(segment_idx);

  // Calculate the appropriate derivative version of V
  // using the matrix multiplication version of the derivative.
  for(int i = 0; i < derivative_order; i++)
  {
    V = (Dm.transpose() * V * Dm).eval();
  }

  MatrixX WV = MatrixX::Zero(N,N);

  for(int r = 0; r < D; r++)
  {
    for(int c = 0; c < D; c++)
    {
      CHECK_GE(1e-14, std::abs(W(r,c) - W(c,r))) << "W must be symmetric";
      //std::cout << "Size WV: " << WV.rows() << ", " << WV.cols() << std::endl;
      //std::cout << "Size V: " << V.rows() << ", " << V.cols() << std::endl;
      WV.block(spline_order_*r,
               spline_order_*c,
               spline_order_,
               spline_order_) = W(r,c) * V;
    }
  }

  Q = M.transpose() * WV * M;

  return Q;
}

MatrixX BSpline::segmentQuadraticIntegralDiag(const VectorX& Wdiag,
                                              int segment_idx,
                                              int derivative_order) const
{
  int D = coefficients_.rows();
  //CHECK_GE(segmentIndex, (int)basisMatrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  CHECK_EQ(Wdiag.size(), D) << "Wdiag must be the length of a single vector-valued coefficient";

  int N = D * spline_order_;
  MatrixX Q;// = MatrixX::Zero(N,N);
  MatrixX Dm = Dii(segment_idx);
  MatrixX V = Vi(segment_idx);
  MatrixX M = Mi(segment_idx);

  // Calculate the appropriate derivative version of V
  // using the matrix multiplication version of the derivative.
  for(int i = 0; i < derivative_order; i++)
  {
    V = (Dm.transpose() * V * Dm).eval();
  }

  MatrixX WV = MatrixX::Zero(N,N);

  for(int d = 0; d < D; d++)
  {
    //std::cout << "Size WV: " << WV.rows() << ", " << WV.cols() << std::endl;
    //std::cout << "Size V: " << V.rows() << ", " << V.cols() << std::endl;
    WV.block(spline_order_*d, spline_order_*d,spline_order_,spline_order_) = Wdiag(d) * V;
  }

  Q = M.transpose() * WV * M;

  return Q;
}

MatrixX BSpline::curveQuadraticIntegral(const MatrixX& W,
                                        int derivative_order) const
{
  int D = coefficients_.rows();
  CHECK_EQ(W.rows(), D)
      << "W must be a square matrix the size of a single vector-valued coefficient";
  CHECK_EQ(W.cols(), D)
      << "W must be a square matrix the size of a single vector-valued coefficient";
  int N = coefficients_.cols();

  MatrixX Q = MatrixX::Zero(D*N, D*N);

  int QiSize = spline_order_ * D;
  for(int s = 0; s < numValidTimeSegments(); s++)
  {
    Q.block(s*D,s*D,QiSize,QiSize) += segmentQuadraticIntegral(W, s, derivative_order);
  }

  return Q;
}

MatrixX BSpline::curveQuadraticIntegralDiag(const VectorX& Wdiag,
                                            int derivative_order) const
{
  int D = coefficients_.rows();
  CHECK_EQ(Wdiag.size(), D)
      << "Wdiag must be the length of a single vector-valued coefficient";
  int N = coefficients_.cols();

  MatrixX Q = MatrixX::Zero(D*N, D*N);

  int QiSize = spline_order_ * D;
  for(int s = 0; s < numValidTimeSegments(); s++)
  {
    Q.block(s*D,s*D,QiSize,QiSize) += segmentQuadraticIntegralDiag(Wdiag,
                                                                   s,
                                                                   derivative_order);
  }

  return Q;
}

int BSpline::coefficientVectorLength() const
{
  return coefficients_.rows() * coefficients_.cols();
}

void BSpline::initConstantSpline(real_t t_min,
                                 real_t t_max,
                                 int num_segments,
                                 const VectorX& constant)
{
  CHECK_GT(t_max, t_min) << "The max time is less than the min time";
  CHECK_GE(num_segments, 1) << "There must be at least one segment";
  CHECK_GE(constant.size(), 1)
      << "The constant vector must be of at least length 1";

  int K = numKnotsRequired(num_segments);
  int C = numCoefficientsRequired(num_segments);
  real_t dt = (t_max - t_min) / (real_t)num_segments;

  real_t minTime = t_min - (spline_order_ - 1)*dt;
  real_t maxTime = t_max + (spline_order_ - 1)*dt;
  VectorX knotVector = VectorX::LinSpaced(K,minTime,maxTime);
  // std::cout << "K: " << K << std::endl;
  // std::cout << "S: " << numSegments << std::endl;
  // std::cout << "segTime: " << t_min << ", " << t_max << std::endl;
  // std::cout << "dt: " << dt << std::endl;
  // std::cout << "time: " << minTime << ", " << maxTime << std::endl;
  // std::cout << "order: " << splineOrder_ << std::endl;
  // std::cout << knotVector.transpose() << std::endl;
  MatrixX coeff(constant.size(),C);
  for(int i = 0; i < C; i++)
  {
    coeff.col(i) = constant;
  }

  setKnotVectorAndCoefficients(knotVector,coeff);
}

int BSpline::numCoefficients() const
{
  return coefficients_.rows() * coefficients_.cols();
}

Eigen::Map<VectorX> BSpline::vvCoefficientVector(int i)
{
  CHECK_LE(i, coefficients_.cols()) << "Index out of range";
  CHECK_LT(0, coefficients_.cols()) << "Index out of range";
  return Eigen::Map<VectorX>(&coefficients_(0,i),coefficients_.rows());
}

Eigen::Map<const VectorX> BSpline::vvCoefficientVector(int i) const
{
  CHECK_LE(i, coefficients_.cols()) << "Index out of range";
  CHECK_LT(0, coefficients_.cols()) << "Index out of range";
  return Eigen::Map<const VectorX>(&coefficients_(0,i),coefficients_.rows());
}

int BSpline::numVvCoefficients() const
{
  return coefficients_.cols();
}

}  // namespace ze


#pragma once

#include <pcl/point_types.h>
#include <Eigen/Core>

inline Eigen::Vector3d pcl2eig(const pcl::PointXYZRGBNormal& pt)
{
    return Eigen::Vector3d(pt.x, pt.y, pt.z);
}

inline pcl::PointXYZRGBNormal eig2pcl(const Eigen::Vector3d& vec)
{
    pcl::PointXYZRGBNormal pt;
    pt.x = vec(0);
    pt.y = vec(1);
    pt.z = vec(2);
    return pt;
}

inline bool pointsOnSameSideOfLine(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, const Eigen::Vector3d& a, const Eigen::Vector3d& b)
{
    Eigen::Vector3d n1 = (b - a).cross(p1 - a);
    Eigen::Vector3d n2 = (b - a).cross(p2 - a);
    return n1.dot(n2) >= 0;
}

inline bool pointInTriangle(const Eigen::Vector3d& p, const Eigen::Vector3d& a, const Eigen::Vector3d& b, const Eigen::Vector3d& c)
{
    return pointsOnSameSideOfLine(p, a, b, c) && pointsOnSameSideOfLine(p, b, a, c) && pointsOnSameSideOfLine(p, c, a, b);
}


// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab
/*

Copyright (c) 2010--2018,
François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the authors at <f dot pomerleau at gmail dot com> and
<stephane at magnenat dot net>

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/
#include "SimpleSensorNoise.h"

#include "PointMatcherPrivate.h"

#include <string>
#include <vector>

#include <boost/format.hpp>

// SimpleSensorNoiseDataPointsFilter
// Constructor
template<typename T>
SimpleSensorNoiseDataPointsFilter<T>::SimpleSensorNoiseDataPointsFilter(const Parameters& params):
	PointMatcher<T>::DataPointsFilter("SimpleSensorNoiseDataPointsFilter",
	SimpleSensorNoiseDataPointsFilter::availableParameters(), params),
	sensorType(Parametrizable::get<unsigned>("sensorType")),
	gain(Parametrizable::get<T>("gain"))
{
	std::vector<std::string> sensorNames = {"Sick LMS-1xx",
											"Hokuyo URG-04LX",
											"Hokuyo UTM-30LX",
											"Kinect / Xtion","Sick Tim3xx"};
	if (sensorType >= sensorNames.size())
	{
		throw InvalidParameter(
			(boost::format("SimpleSensorNoiseDataPointsFilter: Error, sensorType id %1% does not exist.") % sensorType).str());
	}

	LOG_INFO_STREAM("SimpleSensorNoiseDataPointsFilter - using sensor noise model: " << sensorNames[sensorType]);
}

// SimpleSensorNoiseDataPointsFilter
// Compute
template<typename T>
typename PointMatcher<T>::DataPoints
SimpleSensorNoiseDataPointsFilter<T>::filter(const DataPoints& input)
{
	DataPoints output(input);
	inPlaceFilter(output);
	return output;
}

// In-place filter
template<typename T>
void SimpleSensorNoiseDataPointsFilter<T>::inPlaceFilter(DataPoints& cloud)
{
	cloud.allocateDescriptor("simpleSensorNoise", 1);
	BOOST_AUTO(noise, cloud.getDescriptorViewByName("simpleSensorNoise"));

	switch(sensorType)
	{
	case 0: // Sick LMS-1xx
	{
		noise = computeLaserNoise(0.012, 0.0068, 0.0008, cloud.features);
		break;
	}
	case 1: // Hokuyo URG-04LX
	{
		noise = computeLaserNoise(0.028, 0.0013, 0.0001, cloud.features);
		break;
	}
	case 2: // Hokuyo UTM-30LX
	{
		noise = computeLaserNoise(0.018, 0.0006, 0.0015, cloud.features);
		break;
	}
	case 3: // Kinect / Xtion
	{
		const int dim = cloud.features.rows();
		const Matrix squaredValues(cloud.features.topRows(dim-1).colwise().norm().array().square());
		noise = squaredValues*(0.5*0.00285);
		break;
	}
  case 4: // Sick Tim3xx
  {
    noise = computeLaserNoise(0.004, 0.0053, -0.0092, cloud.features);
    break;
  }
	default:
		throw InvalidParameter(
			(boost::format("SimpleSensorNoiseDataPointsFilter: Error, cannot compute noise for sensorType id %1% .") % sensorType).str());
	}

}

template<typename T>
typename PointMatcher<T>::Matrix
SimpleSensorNoiseDataPointsFilter<T>::computeLaserNoise(
	const T minRadius, const T beamAngle, const T beamConst, const Matrix& features)
{
	typedef typename Eigen::Array<T, 2, Eigen::Dynamic> Array2rows;

	const int nbPoints = features.cols();
	const int dim = features.rows();

	Array2rows evalNoise = Array2rows::Constant(2, nbPoints, minRadius);
	evalNoise.row(0) =  beamAngle * features.topRows(dim-1).colwise().norm();
	evalNoise.row(0) += beamConst;

	return evalNoise.colwise().maxCoeff();
}

template struct SimpleSensorNoiseDataPointsFilter<float>;
template struct SimpleSensorNoiseDataPointsFilter<double>;


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

#ifndef BOOST_MATH_MODF_HPP
#define BOOST_MATH_MODF_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/trunc.hpp>

namespace boost{ namespace math{

template <class T, class Policy>
inline T modf(const T& v, T* ipart, const Policy& pol)
{
   *ipart = trunc(v, pol);
   return v - *ipart;
}
template <class T>
inline T modf(const T& v, T* ipart)
{
   return modf(v, ipart, policies::policy<>());
}

template <class T, class Policy>
inline T modf(const T& v, int* ipart, const Policy& pol)
{
   *ipart = itrunc(v, pol);
   return v - *ipart;
}
template <class T>
inline T modf(const T& v, int* ipart)
{
   return modf(v, ipart, policies::policy<>());
}

template <class T, class Policy>
inline T modf(const T& v, long* ipart, const Policy& pol)
{
   *ipart = ltrunc(v, pol);
   return v - *ipart;
}
template <class T>
inline T modf(const T& v, long* ipart)
{
   return modf(v, ipart, policies::policy<>());
}

#ifdef BOOST_HAS_LONG_LONG
template <class T, class Policy>
inline T modf(const T& v, boost::long_long_type* ipart, const Policy& pol)
{
   *ipart = lltrunc(v, pol);
   return v - *ipart;
}
template <class T>
inline T modf(const T& v, boost::long_long_type* ipart)
{
   return modf(v, ipart, policies::policy<>());
}
#endif

}} // namespaces

#endif // BOOST_MATH_MODF_HPP


#include <NTL/vec_lzz_p.h>

NTL_START_IMPL

// NOTE: the signature for this is in lzz_p.h
void conv(vec_zz_p& x, const vec_ZZ& a)
{
   long i, n;

   n = a.length();
   x.SetLength(n);

   VectorConv(n, x.elts(), a.elts());
}

// NOTE: the signature for this is in lzz_p.h
void conv(vec_zz_p& x, const Vec<long>& a)
{
   long i, n;

   n = a.length();
   x.SetLength(n);

   VectorConv(n, x.elts(), a.elts());
}

void InnerProduct(zz_p& x, const vec_zz_p& a, const vec_zz_p& b)
{
   long n = min(a.length(), b.length());
   long i;

   long accum, t;
   long p = zz_p::modulus();
   mulmod_t pinv = zz_p::ModulusInverse();

   const zz_p *ap = a.elts();
   const zz_p *bp = b.elts();

   accum = 0;
   for (i = 0; i < n; i++) {
      t = MulMod(rep(ap[i]), rep(bp[i]), p, pinv);
      accum = AddMod(accum, t, p);
   }

   x.LoopHole() = accum;
}

void InnerProduct(zz_p& x, const vec_zz_p& a, const vec_zz_p& b,
                  long offset)
{
   if (offset < 0) LogicError("InnerProduct: negative offset");
   if (NTL_OVERFLOW(offset, 1, 0)) ResourceError("InnerProduct: offset too big");

   long n = min(a.length(), b.length()+offset);
   long i;

   long accum, t;
   long p = zz_p::modulus();
   mulmod_t pinv = zz_p::ModulusInverse();

   const zz_p *ap = a.elts();
   const zz_p *bp = b.elts();

   accum = 0;
   for (i = offset; i < n; i++) {
      t = MulMod(rep(ap[i]), rep(bp[i-offset]), p, pinv);
      accum = AddMod(accum, t, p);
   }

   x.LoopHole() = accum;
}

long CRT(vec_ZZ& gg, ZZ& a, const vec_zz_p& G)
{
   long n = gg.length();
   if (G.length() != n) LogicError("CRT: vector length mismatch");

   long p = zz_p::modulus();

   ZZ new_a;
   mul(new_a, a, p);

   long a_inv;
   a_inv = rem(a, p);
   a_inv = InvMod(a_inv, p);

   long p1;
   p1 = p >> 1;

   ZZ a1;
   RightShift(a1, a, 1);

   long p_odd = (p & 1);

   long modified = 0;

   long h;

   ZZ g;
   long i;
   for (i = 0; i < n; i++) {
      if (!CRTInRange(gg[i], a)) {
         modified = 1;
         rem(g, gg[i], a);
         if (g > a1) sub(g, g, a);
      }
      else
         g = gg[i];

      h = rem(g, p);
      h = SubMod(rep(G[i]), h, p);
      h = MulMod(h, a_inv, p);
      if (h > p1)
         h = h - p;

      if (h != 0) {
         modified = 1;

         if (!p_odd && g > 0 && (h == p1))
            MulSubFrom(g, a, h);
         else
            MulAddTo(g, a, h);
      }

      gg[i] = g;
   }

   a = new_a;

   return modified;
}

void mul(vec_zz_p& x, const vec_zz_p& a, zz_p b)
{
   long n = a.length();
   x.SetLength(n);

   long i;

   if (n <= 1) {

      for (i = 0; i < n; i++)
         mul(x[i], a[i], b);

   }
   else {

      long p = zz_p::modulus();
      mulmod_t pinv = zz_p::ModulusInverse();
      long bb = rep(b);
      mulmod_precon_t bpinv = PrepMulModPrecon(bb, p, pinv);

      const zz_p *ap = a.elts();
      zz_p *xp = x.elts();

      for (i = 0; i < n; i++)
         xp[i].LoopHole() = MulModPrecon(rep(ap[i]), bb, p, bpinv);

   }
}

void mul(vec_zz_p& x, const vec_zz_p& a, long b_in)
{
   zz_p b;
   b = b_in;
   mul(x, a, b);
}

void add(vec_zz_p& x, const vec_zz_p& a, const vec_zz_p& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector add: dimension mismatch");

   long p = zz_p::modulus();

   x.SetLength(n);

   const zz_p *ap = a.elts();
   const zz_p *bp = b.elts();
   zz_p *xp = x.elts();

   long i;
   for (i = 0; i < n; i++)
      xp[i].LoopHole() = AddMod(rep(ap[i]), rep(bp[i]), p);
}

void sub(vec_zz_p& x, const vec_zz_p& a, const vec_zz_p& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector sub: dimension mismatch");

   long p = zz_p::modulus();

   x.SetLength(n);

   const zz_p *ap = a.elts();
   const zz_p *bp = b.elts();
   zz_p *xp = x.elts();

   long i;
   for (i = 0; i < n; i++)
      xp[i].LoopHole() = SubMod(rep(ap[i]), rep(bp[i]), p);
}

void clear(vec_zz_p& x)
{
   long n = x.length();

   zz_p *xp = x.elts();

   long i;
   for (i = 0; i < n; i++)
      clear(xp[i]);
}

void negate(vec_zz_p& x, const vec_zz_p& a)
{
   long n = a.length();
   long p = zz_p::modulus();

   x.SetLength(n);

   const zz_p *ap = a.elts();
   zz_p *xp = x.elts();

   long i;
   for (i = 0; i < n; i++)
      xp[i].LoopHole() = NegateMod(rep(ap[i]), p);
}

long IsZero(const vec_zz_p& a)
{
   long n = a.length();

   const zz_p *ap = a.elts();

   long i;
   for (i = 0; i < n; i++)
      if (!IsZero(ap[i]))
         return 0;

   return 1;
}

vec_zz_p operator+(const vec_zz_p& a, const vec_zz_p& b)
{
   vec_zz_p res;
   add(res, a, b);
   NTL_OPT_RETURN(vec_zz_p, res);
}

vec_zz_p operator-(const vec_zz_p& a, const vec_zz_p& b)
{
   vec_zz_p res;
   sub(res, a, b);
   NTL_OPT_RETURN(vec_zz_p, res);
}

vec_zz_p operator-(const vec_zz_p& a)
{
   vec_zz_p res;
   negate(res, a);
   NTL_OPT_RETURN(vec_zz_p, res);
}

zz_p operator*(const vec_zz_p& a, const vec_zz_p& b)
{
   zz_p res;
   InnerProduct(res, a, b);
   return res;
}

void VectorCopy(vec_zz_p& x, const vec_zz_p& a, long n)
{
   if (n < 0) LogicError("VectorCopy: negative length");
   if (NTL_OVERFLOW(n, 1, 0)) ResourceError("overflow in VectorCopy");

   long m = min(n, a.length());

   x.SetLength(n);

   const zz_p *ap = a.elts();
   zz_p *xp = x.elts();

   long i;

   for (i = 0; i < m; i++)
      xp[i] = ap[i];

   for (i = m; i < n; i++)
      clear(xp[i]);
}

void random(vec_zz_p& x, long n)
{
   x.SetLength(n);
   VectorRandom(n, x.elts());
}

NTL_END_IMPL


/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright (C) 2019, Raspberry Pi (Trading) Limited
 *
 * pwl.hpp - piecewise linear functions interface
 */
#pragma once

#include <math.h>

#include <functional>
#include <vector>

#include <boost/property_tree/ptree.hpp>

class Pwl
{
public:
	struct Interval {
		Interval(double _start, double _end) : start(_start), end(_end) {}
		double start, end;
		bool Contains(double value) { return value >= start && value <= end; }
		double Clip(double value) { return value < start ? start : (value > end ? end : value); }
		double Len() const { return end - start; }
	};
	struct Point {
		Point() : x(0), y(0) {}
		Point(double _x, double _y) : x(_x), y(_y) {}
		double x, y;
		Point operator-(Point const &p) const { return Point(x - p.x, y - p.y); }
		Point operator+(Point const &p) const { return Point(x + p.x, y + p.y); }
		double operator%(Point const &p) const { return x * p.x + y * p.y; }
		Point operator*(double f) const { return Point(x * f, y * f); }
		Point operator/(double f) const { return Point(x / f, y / f); }
		double Len2() const { return x * x + y * y; }
		double Len() const { return sqrt(Len2()); }
	};
	Pwl() {}
	Pwl(std::vector<Point> const &points) : points_(points) {}
	void Read(boost::property_tree::ptree const &params);
	void Append(double x, double y, const double eps = 1e-6);
	void Prepend(double x, double y, const double eps = 1e-6);
	Interval Domain() const;
	Interval Range() const;
	bool Empty() const;
	// Evaluate Pwl, optionally supplying an initial guess for the
	// "span". The "span" may be optionally be updated.  If you want to know
	// the "span" value but don't have an initial guess you can set it to
	// -1.
	double Eval(double x, int *span_ptr = nullptr, bool update_span = true) const;
	// Find perpendicular closest to xy, starting from span+1 so you can
	// call it repeatedly to check for multiple closest points (set span to
	// -1 on the first call). Also returns "pseudo" perpendiculars; see
	// PerpType enum.
	enum class PerpType {
		NotFound, // no perpendicular found
		Start, // start of Pwl is closest point
		End, // end of Pwl is closest point
		Vertex, // vertex of Pwl is closest point
		Perpendicular // true perpendicular found
	};
	PerpType Invert(Point const &xy, Point &perp, int &span, const double eps = 1e-6) const;
	// Compose two Pwls together, doing "this" first and "other" after.
	Pwl Compose(Pwl const &other, const double eps = 1e-6) const;
	// Apply function to (x,y) values at every control point.
	void Map(std::function<void(double x, double y)> f) const;
	// Apply function to (x, y0, y1) values wherever either Pwl has a
	// control point.
	static void Map2(Pwl const &pwl0, Pwl const &pwl1,
					 std::function<void(double x, double y0, double y1)> f);
	// Combine two Pwls, meaning we create a new Pwl where the y values are
	// given by running f wherever either has a knot.
	static Pwl Combine(Pwl const &pwl0, Pwl const &pwl1,
					   std::function<double(double x, double y0, double y1)> f,
					   const double eps = 1e-6);
	// Make "this" match (at least) the given domain. Any extension my be
	// clipped or linear.
	void MatchDomain(Interval const &domain, bool clip = true, const double eps = 1e-6);
	// Generate a LUT for this funciton.
	template <typename T> std::vector<T> GenerateLut() const
	{
		int end = Domain().end + 1, span = 0;
		std::vector<T> lut(end);
		for (int x = 0; x < end; x++)
			lut[x] = Eval(x, &span);
		return lut;
	}
	Pwl &operator*=(double d);
	void Debug(FILE *fp = stderr) const;

private:
	int findSpan(double x, int span) const;
	std::vector<Point> points_;
};