#include "rememberBoostHistogram.h"
#include <iostream>
#include <fstream>
#include <string>
#include <boost/lexical_cast.hpp>
#include "sgcu/sgcu.hpp"
#include "utils.hpp"

using std::cout;
using std::cerr;
using std::endl;
using std::string;

namespace SG{

//using namespace boost::accumulators;

// build a histogram for a data vector
template <typename T>
void getHistogram(std::vector<T> const & data, histogram_type & hist)
{
	// create a boost accumulator
	acc myAccumulator( boost::accumulators::tag::density::num_bins = data.size()/10.0, boost::accumulators::tag::density::cache_size = data.size()-2);
	// fill accumulator
	for (unsigned int j = 0; j < data.size(); ++j){
		myAccumulator(data[j]);
	}
	// do the boost histogram
	hist = boost::accumulators::density(myAccumulator);
}

unsigned int rememberBoostHistogram::size() const
{
	unsigned int thisSize = values.size();
	if (indices.size() < thisSize)
		thisSize = indices.size();
	return thisSize;
}

void rememberBoostHistogram::clear()
{
	values.clear();
	indices.clear();
}

void rememberBoostHistogram::rememberThisHistogram(histogram_type& hist)
{
	clear();
	for (unsigned int i = 0; i < (unsigned int) hist.size(); ++i)
	{
		indices.push_back(hist[i].first);
		values.push_back(hist[i].second);
	}
}

void rememberBoostHistogram::operator()(histogram_type& hist)
{
	rememberThisHistogram(hist);
}

rememberBoostHistogram::rememberBoostHistogram()
{
}

rememberBoostHistogram::rememberBoostHistogram(histogram_type& hist)
{
	rememberThisHistogram(hist);
}

double computeArea(struct rememberBoostHistogram const & data)
{
	double area = 0.0;
	for (unsigned int i = 0; i < data.size(); ++i)
	{
		area += data.values[i];
	}
	return area;
}

void getFitHistogramOwn(struct rememberBoostHistogram const & dataOrg, struct rememberBoostHistogram& fit)
{
	// NOTE: this implicitly assumes all histogram values to be positive. true for a histograms, not necessarily true for all inputs!
	if ( computeArea(dataOrg) == 0.0 )
	{
		fit = dataOrg;
		return;
	}

	const bool doDebugOutput = false;

	struct rememberBoostHistogram data = dataOrg;
	if (doDebugOutput) saveHistogramToFile(data, "comparisonValsNormalization3.txt");

	// make sure the histogram is normalized to 1.0
	normalizeHistogramAreaTo(data, 1.0);

	if (doDebugOutput) saveHistogramToFile(data, "comparisonValsNormalization3Renorm.txt");

	unsigned int lastIndex = computeLastIndex(data);

	//	cerr << "new: setting last entry of histogram to zero" << endl;
	data.values[data.size()-1] = 0.0;

	struct rememberBoostHistogram dataBkp = data;
	struct rememberBoostHistogram dataRenormalizedBkp = data;

	smoothHistogram(data);

	if (doDebugOutput) saveHistogramToFile(data, "comparisonValsNormalization3Smoothed.txt");

	unsigned int firstIndex = getFirstIndex(data, lastIndex);

	// do not consider values from before the tail to fit the tail!
	eraseEverythingButTail(data, firstIndex);
	eraseEverythingButTail(dataBkp, firstIndex);

	double areaBefore = computeArea(dataBkp);
	normalizeHistogramAreaTo(data, areaBefore);

	if (doDebugOutput) saveHistogramToFile(data, "comparisonValsNormalization3SmoothedRenorm.txt");

	double scalingFactor = 0.0;
	double exponent = 0.0;
	obtainFitParameters(data, firstIndex, lastIndex, dataRenormalizedBkp, scalingFactor, exponent);

	generateActualFit(data, scalingFactor, exponent, fit);

	mergeOriginalAndFit(dataRenormalizedBkp, firstIndex, lastIndex, fit);

	if (doDebugOutput) saveHistogramToFile(fit, "comparisonValsNormalization3Fitted.txt");
}

void getFitHistogramOwn(const std::vector<double>& data, struct rememberBoostHistogram& fit)
{
	// do a histogram
	acc myAccumulator( boost::accumulators::tag::density::num_bins = 100, boost::accumulators::tag::density::cache_size = data.size());
	for (unsigned int i = 0; i < data.size(); ++i)
	{
		myAccumulator(data[i]);
	}
	histogram_type hist = boost::accumulators::density(myAccumulator);
	struct rememberBoostHistogram histNew(hist);

	getFitHistogramOwn(histNew, fit);
}

void outputHistogram(histogram_type const & hist, std::string fileName)
{
	std::ofstream raus;
	raus.open(fileName.c_str());
	if ( (! raus.is_open()) || (! raus.good()) )
	{
		std::cerr << "Failed to open file " << fileName << " for histogram output." << endl;
		throw -1;
	}

	for (unsigned int i = 0; i < (unsigned int) hist.size(); ++i)
	{
		raus << hist[i].first << " " << hist[i].second << endl;
	}

	if (! raus.good() )
	{
		std::cerr << "Failed to write histogram to file " << fileName << endl;
		throw -1;
	}
	raus.close();

}

void outputRememberHistogram(rememberBoostHistogram const & fit, std::string fileName)
{
	std::ofstream raus;
	raus.open(fileName.c_str());
	if ( (! raus.is_open()) || (! raus.good()) )
	{
		std::cerr << "Failed to open file " << fileName << " for rememberBoostHistogram output." << endl;
		throw -1;
	}

	for (unsigned int i = 0; i < fit.size(); ++i)
	{
		raus << fit.indices[i] << " " << fit.values[i] << endl;
	}
	raus.close();

	if (! raus.good() )
	{
		std::cerr << "Failed to write rememberBoostHistogram to file " << fileName << endl;
		throw -1;
	}
	raus.close();
}

// TODO: check if both outputs are actually used!
// for a fixed segmentation threshold: compute histograms for each metric from the clusters found in all pseudo experiments
void computeAndOutputHistogramsFromClusters(std::string folderName, unsigned int n, std::vector<std::vector<double> > & relevanceComparisonVals,
		std::string searchStringSegmentationThreshold,
		std::vector<std::vector<struct rememberBoostHistogram > >& histsAllSegmentationsAllRelevances)
{
		histsAllSegmentationsAllRelevances[n].resize(relevanceComparisonVals.size());
		for (unsigned int j = 0; j < relevanceComparisonVals.size(); ++j)	// for all metrics
		{
			// create a histogram from all clusters that match this segmentation and metric
			acc myAcc(boost::accumulators::tag::density::num_bins = 100, boost::accumulators::tag::density::cache_size = relevanceComparisonVals[j].size());
			for (unsigned int i = 0; i < relevanceComparisonVals[j].size(); ++i)
			{
				myAcc(relevanceComparisonVals[j][i]);
			}
			histogram_type hist = boost::accumulators::density(myAcc);
			outputHistogram(hist, folderName + "/hist_Size" + searchStringSegmentationThreshold + "_Metric" + boost::lexical_cast<std::string>(j) + ".txt");

			struct rememberBoostHistogram data(hist);
			struct rememberBoostHistogram fit;
			// compute a fit that approximates the tail of the observed data
			getFitHistogramOwn(data, fit);
			outputRememberHistogram(fit, folderName + "/hist_Size" + searchStringSegmentationThreshold + "_Metric" + boost::lexical_cast<std::string>(j) + "fit.txt");

			histsAllSegmentationsAllRelevances[n][j] = fit;
		}
}

void saveHistogramToFile(struct rememberBoostHistogram const & data, std::string const & fileName)
{
	std::ofstream raus(fileName.c_str());
	if (! raus.good())
	{
		std::cerr << "Failed to open file " << fileName << " for output of histogram data" << std::endl;
		throw (-1);
	}
	for (unsigned int i = 0; i < data.size(); ++i)
	{
		raus << data.indices[i] << " " << data.values[i] << endl;
		if (! raus.good())
		{
			std::cerr << "Failed to write histogram data entry " << i << " of " << data.size() << " to file " << fileName << std::endl;
			throw (-2);
		}
	}
	if (! raus.good())
	{
		std::cerr << "Failed to write last of " << data.size() << " histogram data entries to file " << fileName << std::endl;
		throw (-2);
	}
	raus.close();
}

void normalizeHistogramAreaTo(struct rememberBoostHistogram & data, double targetValue)
{
	double area = computeArea(data);
	double rescaleFactor = 0.0;
	if (area != 0.0)
	{
		rescaleFactor = targetValue/area;
		for (unsigned int i = 0; i < data.size(); ++i)
		{
			data.values[i] *= rescaleFactor;
		}
	}
}

unsigned int computeLastIndex(struct rememberBoostHistogram const & data)
{
	unsigned int lastIndex = 0;
	unsigned int notZeroIndex = 0;
	unsigned int notZeroIndexBefore = 0;
	for (unsigned int i = 0; i < data.size(); ++i)
	{
		if (data.values[i] > 0.0)
		{
			lastIndex = notZeroIndexBefore;
			notZeroIndexBefore = notZeroIndex;
			notZeroIndex = i;
		}
	}
	if (lastIndex == 0)
	{
		lastIndex = notZeroIndexBefore;
	}
	if (lastIndex == 0)
	{
		lastIndex = notZeroIndex;
	}
	if (lastIndex == 0)
	{
		lastIndex = data.size();
	}
	return lastIndex;
}

void smoothHistogram(struct rememberBoostHistogram & data)
{
	// NOTE: I just noted that the number of smoothings correlates with the size of the vector.
	// This has been intentional, but it hasn't been checked if this scales to less or more entries.
	for (unsigned int i = 0; i < 0.75*data.size(); ++i)
		smoothNoEnds(data.values);
	for (unsigned int i = 0; i < 0.25*data.size(); ++i)
		sgcu::smooth(data.values);
}

double evaluateFitL2(const std::vector<double>& data, const std::vector<double>& fit){
	if (data.size() != fit.size()){
		cerr << "evaluateFitL2 cannot compare data and fit of different size, data: " << data.size() << " and fit: " << fit.size() << endl;
		return std::numeric_limits<double>::max();
	}

	double diffSum = 0.0;
	for (unsigned int i = 0; i < data.size(); ++i){
		double diff = data[i] - fit[i];
		diffSum += (diff*diff);
	}
	return diffSum;
}

void obtainFitParameters(struct rememberBoostHistogram & data, unsigned int firstIndex, unsigned int lastIndex,
		struct rememberBoostHistogram const & dataRenormalizedBkp,
		double & scalingFactorFitFinal, double & exponent)
{
	// evaluate when each value of the smoothed right flanc is below half of its value - store the distance in x
	//cout << "maxIndex = " << maxIndex << endl;
	//cout << "firstIndex = " << firstIndex << endl;
	//cout << "lastIndex = " << lastIndex << endl;
	unsigned int num = 0;
	double howLongUntilBelowHalf = 0.0;
	for (unsigned int i = firstIndex; i < lastIndex; ++i){
		double currentY100 = data.values[i];
		if (currentY100 <= 0.0)
			break;
		double currentX = data.indices[i];
		for (unsigned int j = i+1; j < data.size(); ++j){
	//		for (unsigned int j = i+1; j < lastIndex; ++j){	// this was the default
			double currentY = data.values[j];
			if (currentY <= 0.5*currentY100){
				if (currentY > 0.0){
					double distance = data.indices[j] - currentX;
					//cout << i << " " << j << " distance: " << distance << "    histjfirst = " << data.indices[j] << " currentX = " << currentX << " currentY100 = " << currentY100 << " currentY = " << currentY << endl;
					howLongUntilBelowHalf += distance;
					++num;
				}
				break;
			}
		}
	}
	//cout << "nowLongUntil = " << howLongUntilBelowHalf << endl;
	//cout << "num = " << num << endl;
	if (num != 0){
		howLongUntilBelowHalf /= static_cast<double>(num);
	}

	double stepSize = data.indices[1] - data.indices[0];

	//cout << "Mean x distance until y value is below half of its starting value: " << howLongUntilBelowHalf << endl;
	howLongUntilBelowHalf  -= 0.5*stepSize;
	//cout << "x distance corrected for stepsize: " << howLongUntilBelowHalf << endl;
	//double
	exponent = -1.0/howLongUntilBelowHalf;
	//cout << "exponent of fit: " << exponent << endl;

	double minDistance = std::numeric_limits<double>::max();
	//double
	scalingFactorFitFinal = 0.0;

	std::vector<double> histValsFitComparison = dataRenormalizedBkp.values;
	for (unsigned int j = 0; j < firstIndex; ++j){
		histValsFitComparison[j] = 0.0;
	}
	for (unsigned int j = lastIndex; j < histValsFitComparison.size(); ++j){
		histValsFitComparison[j] = 0.0;
	}

	std::vector<double> fitVals = histValsFitComparison;
	for (unsigned int i = firstIndex; i < lastIndex; ++i){
		double startingXValue = data.indices[i];
		double expectedStartingValue = data.values[i];
		double plainStartingValue = pow(2.0, exponent*startingXValue);
		double scalingFactorFit = expectedStartingValue/plainStartingValue;

		for (unsigned int j = firstIndex; j < lastIndex; ++j){
			double currentX = data.indices[j];
			fitVals[j] = scalingFactorFit*pow(2.0, exponent*currentX);
		}

		double distance = evaluateFitL2(histValsFitComparison, fitVals);
		//cout << "index " << i << " distance " << distance << endl;
		if (distance < minDistance){
			minDistance = distance;
			scalingFactorFitFinal = scalingFactorFit;
		}
	}

	//cout << "scalingfactor: " << scalingFactorFitFinal << endl;
}

void generateActualFit(struct rememberBoostHistogram const & data, double scalingFactor, double exponent,
		struct rememberBoostHistogram& fit, double longerFactor, double finerFactor)
{
	double stepSize = data.indices[1] - data.indices[0];
	unsigned int targetSize = data.size()*longerFactor*finerFactor;
	fit.clear();
	fit.indices.resize(targetSize);
	fit.values.resize(targetSize);
	double startIndex = data.indices[0];
	for (unsigned int i = 0; i < targetSize; ++i){
		fit.indices[i] = startIndex+stepSize*(static_cast<double>(i)/finerFactor);
	}
	for (unsigned int i = 0; i < targetSize; ++i){
		fit.values[i] = scalingFactor*pow(2.0, exponent*fit.indices[i]);
	}
}

void mergeOriginalAndFit(struct rememberBoostHistogram const & dataRenormalizedBkp, unsigned int firstIndex,
		unsigned int lastIndex, struct rememberBoostHistogram& fit, double finerFactor)
{
	unsigned int firstIndexFiner = static_cast<unsigned int>(static_cast<double>(firstIndex)*finerFactor+0.5);
	unsigned int lastIndexFiner = static_cast<unsigned int>(static_cast<double>(lastIndex)*finerFactor+0.5);

	// only start using the fit when you don't trust the actual distribution anymore:
//	double shiftBack = 0.8;	// default
	double shiftBack = 0.6;

	unsigned int startReallyUsingIt = ((1.0-shiftBack)*firstIndex + shiftBack*lastIndex);
	unsigned int startReallyUsingItFiner = ((1.0-shiftBack)*firstIndexFiner + shiftBack*lastIndexFiner)-0.55*finerFactor;

	double area2 = 0.0;
	for (unsigned int i = startReallyUsingIt; i < dataRenormalizedBkp.size(); ++i){
		area2 += dataRenormalizedBkp.values[i];
	}

	double areaFit = 0.0;
	for (unsigned int i = startReallyUsingItFiner; i < fit.values.size(); ++i){
		areaFit += fit.values[i];
	}
	double rescalingFactor2 = 1.0/finerFactor;
	if (areaFit != 0.0)
		rescalingFactor2 = area2/areaFit;

	for (unsigned int i = 0; i < startReallyUsingItFiner; ++i){
		fit.values[i] = 0.0;
	}
	for (unsigned int i = startReallyUsingItFiner; i < fit.size(); ++i){
		fit.values[i] *= rescalingFactor2;
	}
	for (unsigned int i = 0; i < startReallyUsingIt; ++i){
		fit.values[static_cast<unsigned int>(static_cast<double>(i)*finerFactor+0.5)] = dataRenormalizedBkp.values[i];
	}

	// make sure the histogram is normalized to 1.0
	normalizeHistogramAreaTo(fit, 1.0);
}

unsigned int getFirstIndex(struct rememberBoostHistogram const & data, unsigned int lastIndex)
{
	unsigned int maxIndex = (std::max_element(data.values.begin(), data.values.end())-data.values.begin());
	return static_cast<unsigned int>((2.0*maxIndex+1.0*lastIndex)/3.0);
}

void eraseEverythingButTail(struct rememberBoostHistogram & data, unsigned int firstIndex)
{
	for (unsigned int i = 0; i < firstIndex; ++i)
	{
		data.values[i] = 0.0;
	}
}

} // end of namespace SG


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2015 The ziftrCOIN developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "core.h"
#include "main.h"
#include "chainparams.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    // Takes a little longer, but is more accurate

    int64_t nMinSum = 0;
    int64_t nMaxSum = 0;

    int nTotalDays = 2 * Params().GetMidwayPoint();
    for (int nDay = 0; nDay < nTotalDays; nDay++)
    {
        int nHeight = nDay * 24 * 60;

        nMinSum += GetBlockValue(nHeight, 0, false);
        nMaxSum += GetBlockValue(nHeight, 0, true);

        int64_t nMinSubsidy = GetBlockValue(nHeight + 1, 0, false);
        int64_t nMaxSubsidy = GetBlockValue(nHeight + 1, 0, true);

        BOOST_CHECK(nMinSubsidy >= MIN_SUBSIDY);
        BOOST_CHECK(nMaxSubsidy >= MIN_SUBSIDY);

        // Should have the same value through out the day (1440 block period)
        BOOST_CHECK(nMinSubsidy == GetBlockValue(nHeight + 24 * 60 - 2, 0, false));
        BOOST_CHECK(nMaxSubsidy == GetBlockValue(nHeight + 24 * 60 - 2, 0, true));

        nMinSum += nMinSubsidy * (24 * 60 - 1);
        nMaxSum += nMaxSubsidy * (24 * 60 - 1);

        BOOST_CHECK(MoneyRange(nMinSum));
        BOOST_CHECK(MoneyRange(nMaxSum));
    }

    // printf("min sum : %llu\n", nMinSum);
    // printf("max sum : %llu\n", nMaxSum);

    BOOST_CHECK(nMinSum == 993742021533401ULL);
    BOOST_CHECK(nMaxSum == 1041179115130496ULL);

}

BOOST_AUTO_TEST_SUITE_END()


#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include "../example.hpp" // Include short list of convenience functions for rendering

#include <iostream>

#include <pcl/filters/passthrough.h>

#include <boost/make_shared.hpp>               //boost指针相关头文件
#include <pcl/point_types.h>                   //点类型定义头文件
#include <pcl/point_cloud.h>                   //点云类定义头文件
#include <pcl/point_representation.h>          //点表示相关的头文件
#include <pcl/io/pcd_io.h>                     //PCD文件打开存储类头文件
#include <pcl/filters/voxel_grid.h>             //用于体素网格化的滤波类头文件
#include <pcl/filters/filter.h>                //滤波相关头文件
#include <pcl/features/normal_3d.h>            //法线特征头文件
#include <pcl/registration/icp.h>              //ICP类相关头文件
#include <pcl/registration/icp_nl.h>           //非线性ICP 相关头文件
#include <pcl/registration/transforms.h>       //变换矩阵类头文件
#include <pcl/visualization/pcl_visualizer.h>  //可视化类头文件

#include <unistd.h>

using pcl::visualization::PointCloudColorHandlerCustom;
using pcl::visualization::PointCloudColorHandlerGenericField;

//定义
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud; //申明pcl::PointXYZ数据
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;

class MyPointRepresentation : public pcl::PointRepresentation<PointNormalT>
{
    using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;

public:
    MyPointRepresentation()
    {
        nr_dimensions_ = 4; //定义点的维度
    }

    // 重载copyToFloatArray方法将点转化为四维数组
    virtual void copyToFloatArray(const PointNormalT &p, float *out) const
    {
        // < x, y, z, curvature >
        out[0] = p.x;
        out[1] = p.y;
        out[2] = p.z;
        out[3] = p.curvature; // 曲率
    }
};

// Struct for managing rotation of pointcloud view
struct state {
    state() : yaw(0.0), pitch(0.0), last_x(0.0), last_y(0.0),
              ml(false), offset_x(0.0f), offset_y(0.0f) {}
    double yaw, pitch, last_x, last_y; bool ml; float offset_x, offset_y;
};

using pcl_ptr = pcl::PointCloud<pcl::PointXYZ>::Ptr;

// Helper functions
void register_glfw_callbacks(window& app, state& app_state);
void draw_pointcloud(window& app, state& app_state, const std::vector<pcl_ptr>& points);

pcl_ptr points_to_pcl(const rs2::points& points)
{
    pcl_ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    auto sp = points.get_profile().as<rs2::video_stream_profile>();
    cloud->width = sp.width();
    cloud->height = sp.height();
    cloud->is_dense = false;
    cloud->points.resize(points.size());
    auto ptr = points.get_vertices();
    for (auto& p : cloud->points)
    {
        p.x = ptr->x;
        p.y = ptr->y;
        p.z = ptr->z;
        ptr++;
    }

    return cloud;
}

float3 colors[] { { 0.8f, 0.1f, 0.3f },
                  { 0.1f, 0.9f, 0.5f },
};

int main(int argc, char * argv[]) try
{
//    // Create a simple OpenGL window for rendering:
    window app(1280, 720, "RealSense PCL Pointcloud Example");
//    // Construct an object to manage view state
    state app_state;
//    // register callbacks to allow manipulation of the pointcloud
    register_glfw_callbacks(app, app_state);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
    //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PCDReader reader;
    reader.read (argv[1], *cloud_in);

    std::vector<pcl_ptr> layers;
    layers.push_back(cloud_in);
    draw_pointcloud(app, app_state, layers);

    while (app) // Application still alive?
//    {
//        draw_pointcloud(app, app_state, layers);
//    }
    {

        rs2::pointcloud pc;
        rs2::points points;

        // Declare RealSense pipeline, encapsulating the actual device and sensors
        rs2::pipeline pipe;
        // Start streaming with default recommended configuration
        pipe.start();

        // Wait for the next set of frames from the camera
        auto frames = pipe.wait_for_frames();

        auto depth = frames.get_depth_frame();

        points = pc.calculate(depth);
        // Generate the pointcloud and texture mappings

        auto clouds = points_to_pcl(points);

        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud(clouds);

        //设置点云距离相机的位置
        pass.setFilterFieldName("z");
        pass.setFilterLimits(0.1, 0.25);

        pass.filter(*clouds);

        pcl::io::savePCDFileASCII("out.pcd", *clouds);
        std::cerr << "Saved " << clouds->points.size() << " data points to out.pcd." << std::endl;
        //cloud_out = clouds;
        std::vector<pcl_ptr> layers;
        //        layers.push_back(clouds);
        layers.push_back(clouds);
        draw_pointcloud(app, app_state, layers);

        //        pcl::PCDWriter writer;
        //        writer.write("./test_pcd.pcd", cloud_filtered, false);

        // 创建IterativeClosestPoint的实例
        // setInputSource将cloud_in作为输入点云
        // setInputTarget将平移后的cloud_out作为目标点云
        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        icp.setInputSource(cloud_in);
        icp.setInputTarget(clouds);

        // 创建一个 pcl::PointCloud<pcl::PointXYZ>实例 Final 对象,存储配准变换后的源点云,
        // 应用 ICP 算法后, IterativeClosestPoint 能够保存结果点云集,如果这两个点云匹配正确的话
        // （即仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云）,那么 icp. hasConverged()= 1 (true),
        // 然后会输出最终变换矩阵的匹配分数和变换矩阵等信息。
        pcl::PointCloud<pcl::PointXYZ> Final;
        icp.align(Final);
        std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
        const pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 &matrix = icp.getFinalTransformation();
        std::cout << matrix << std::endl;

    }

    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception & e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

// Registers the state variable and callbacks to allow mouse control of the pointcloud
void register_glfw_callbacks(window& app, state& app_state)
{
    app.on_left_mouse = [&](bool pressed)
    {
        app_state.ml = pressed;
    };

    app.on_mouse_scroll = [&](double xoffset, double yoffset)
    {
        app_state.offset_x += static_cast<float>(xoffset);
        app_state.offset_y += static_cast<float>(yoffset);
    };

    app.on_mouse_move = [&](double x, double y)
    {
        if (app_state.ml)
        {
            app_state.yaw -= (x - app_state.last_x);
            app_state.yaw = std::max(app_state.yaw, -120.0);
            app_state.yaw = std::min(app_state.yaw, +120.0);
            app_state.pitch += (y - app_state.last_y);
            app_state.pitch = std::max(app_state.pitch, -80.0);
            app_state.pitch = std::min(app_state.pitch, +80.0);
        }
        app_state.last_x = x;
        app_state.last_y = y;
    };

    app.on_key_release = [&](int key)
    {
        if (key == 32) // Escape
        {
            app_state.yaw = app_state.pitch = 0; app_state.offset_x = app_state.offset_y = 0.0;
        }
    };
}

// Handles all the OpenGL calls needed to display the point cloud
void draw_pointcloud(window& app, state& app_state, const std::vector<pcl_ptr>& points)
{
    // OpenGL commands that prep screen for the pointcloud
    glPopMatrix();
    glPushAttrib(GL_ALL_ATTRIB_BITS);

    float width = app.width(), height = app.height();

    glClearColor(153.f / 255, 153.f / 255, 153.f / 255, 1);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    gluPerspective(60, width / height, 0.01f, 10.0f);

    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0);

    glTranslatef(0, 0, +0.5f + app_state.offset_y*0.05f);
    glRotated(app_state.pitch, 1, 0, 0);
    glRotated(app_state.yaw, 0, 1, 0);
    glTranslatef(0, 0, -0.5f);

    glPointSize(width / 640);
    glEnable(GL_TEXTURE_2D);

    int color = 0;

    for (auto&& pc : points)
    {
        auto c = colors[(color++) % (sizeof(colors) / sizeof(float3))];

        glBegin(GL_POINTS);
        glColor3f(c.x, c.y, c.z);

        /* this segment actually prints the pointcloud */
        for (int i = 0; i < pc->points.size(); i++)
        {
            auto&& p = pc->points[i];
            if (p.z)

            {
                // upload the point and texture coordinates only for points we have depth data for
                glVertex3f(p.x, p.y, p.z);
            }
        }

        glEnd();
    }

    // OpenGL cleanup
    glPopMatrix();
    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glPopAttrib();
    glPushMatrix();
}


#pragma once
#include <Eigen/Core>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

namespace gtsam
{

    class GPSAutoAlignFactor2D : public NoiseModelFactor2<Pose2, Pose3>
    {
        using Base = NoiseModelFactor2<X1, X2>;
        using This = GPSAutoAlignFactor2D;
        using GPSMeasure = Point2;

    public:
        using AlignT = X1;
        GPSAutoAlignFactor2D(Key key1, Key key2, const GPSMeasure &gps_measure, const Eigen::Isometry3d&  T_X_gps,const SharedNoiseModel &model)
            : Base(model, key1, key2), _gps_measure(gps_measure), _T_X_gps(T_X_gps.matrix()){}

        virtual ~GPSAutoAlignFactor2D() {}

        // (T_earth_map * T_map_X * T_X_gps).translation - gps_measure
        Vector evaluateError(const Pose2 &T_earth_map, const Pose3 &pose,
                             boost::optional<Matrix &> H1 = boost::none,
                             boost::optional<Matrix &> H2 = boost::none) const
        {
            Matrix36 H3;
            Vector2 error = (X2(T_earth_map).transformPoseFrom(pose.transformPoseFrom(_T_X_gps, H2), H1)).translation(H1 || H2 ? &H3 : 0).topRows(2) - _gps_measure;
            if (H1)
            {
                *H1 = H3.topRows(2) * *H1 * (Matrix63() << 0.0, 0.0, 0.0,
                                                           0.0, 0.0, 0.0,
                                                           0.0, 0.0, 1.0,
                                                           1.0, 0.0, 0.0,
                                                           0.0, 1.0, 0.0,
                                                           0.0, 0.0, 0.0).finished();
            }
            if (H2)
            {
                *H2 = H3.topRows(2) * *H2;
            }

            return error;
        }

        virtual NonlinearFactor::shared_ptr clone() const
        {
            return boost::static_pointer_cast<NonlinearFactor>(
                NonlinearFactor::shared_ptr(new This(*this)));
        }

        virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
        {
            const This *e = dynamic_cast<const This *>(&expected);
            return e != nullptr && Base::equals(*e, tol) && traits<GPSMeasure>::Equals(_gps_measure, e->_gps_measure, tol)
                                && traits<Pose3>::Equals(_T_X_gps, e->_T_X_gps);
        }

        virtual void print(const std::string &s = "",
                           const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
        {
            cout << s << ":\nGPSAutoAlignFactor2D on (" << keyFormatter(key1())
                 << ", " << keyFormatter(key2()) << ")\n"
                 << "  GPS Measurement: " << _gps_measure.transpose() << "\n"
                 << "  T_X_gps: " << _T_X_gps.matrix() << "\n";
            noiseModel_->print("  noise model: ");
        }

    private:
        GPSMeasure _gps_measure;
        Pose3 _T_X_gps;
    }; // class GPSAutoAlignFactor2D

    class GPSAutoAlignFactor3D : public NoiseModelFactor2<Pose3, Pose3>
    {
        using Base = NoiseModelFactor2<X1, X2>;
        using This = GPSAutoAlignFactor3D;
        using GPSMeasure = Point3;

    public:
        using AlignT = X1;
        GPSAutoAlignFactor3D(Key key1, Key key2, const GPSMeasure &gps_measure, const Eigen::Isometry3d& T_X_gps, const SharedNoiseModel &model)
            : Base(model, key1, key2), _gps_measure(gps_measure), _T_X_gps(T_X_gps.matrix()){}

        virtual ~GPSAutoAlignFactor3D() {}

        // (T_earth_map * T_map_X * T_X_gps).translation - gps_measure
        Vector evaluateError(const X1 &T_earth_map, const X2 &pose,
                             boost::optional<Matrix &> H1 = boost::none,
                             boost::optional<Matrix &> H2 = boost::none) const
        {
            Matrix36 H3;
            Vector3 error = T_earth_map.transformPoseFrom(pose.transformPoseFrom(_T_X_gps, H2), H1).translation(H1 || H2 ? &H3 : 0) - _gps_measure;
            // Vector3 error = T_earth_map.transformPoseFrom(pose.transformPoseFrom(_T_X_gps, H2), H1).translation(H1 || H2 ? &H3 : 0) - gtsam::Vector3(_gps_measure[0], _gps_measure[1], 0);
            if (H1)
            {
                *H1 = H3 * *H1;
                // *H1 = H3 * *H1;

            }
            if (H2)
            {
                *H2 = H3 * *H2;
                // *H2 = H3 * *H2;
            }

            return error;
        }

        virtual NonlinearFactor::shared_ptr clone() const
        {
            return boost::static_pointer_cast<NonlinearFactor>(
                NonlinearFactor::shared_ptr(new This(*this)));
        }

        virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
        {
            const This *e = dynamic_cast<const This *>(&expected);
            return e != nullptr && Base::equals(*e, tol) && traits<GPSMeasure>::Equals(_gps_measure, e->_gps_measure, tol)
                                && traits<Pose3>::Equals(_T_X_gps, e->_T_X_gps);
        }

        virtual void print(const std::string &s = "",
                           const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
        {
            cout << s << ":\nGPSAutoAlignFactor3D on (" << keyFormatter(key1())
                 << ", " << keyFormatter(key2()) << ")\n"
                 << "  GPS Measurement: " << _gps_measure.transpose() << "\n"
                 << "  T_X_gps: " << _T_X_gps.matrix() << "\n";
            noiseModel_->print("  noise model: ");
        }

    private:
        GPSMeasure _gps_measure;
        Pose3 _T_X_gps;
    }; // class GPSAutoAlignFactor3D

} // namespace gtsam


/*
 * Copyright (c) 2021 The Foundation for Research on Information Technologies in Society (IT'IS).
 *
 * This file is part of iSEG
 * (see https://github.com/ITISFoundation/osparc-iseg).
 *
 * This software is released under the MIT License.
 *  https://opensource.org/licenses/MIT
 */
#include <boost/test/unit_test.hpp>

#include "../Transform.h"
#include "../Vec3.h"

namespace iseg {

namespace {
void makeTransform(Transform& tr)
{
	tr.SetIdentity();

	Vec3 d0(0.1f, 0.9f, 0.1f);
	d0.Normalize();
	Vec3 d1(0.9f, -0.1f, 0.1f);
	d1.Normalize();
	Vec3 d2(d0 ^ d1);
	d2.Normalize();
	tr.SetRotation(d0, d1, d2);

	float offset[3] = {3.4f, -10.f, 2.5f};
	tr.SetOffset(offset);
}
} // namespace

BOOST_AUTO_TEST_SUITE(iSeg_suite);
BOOST_AUTO_TEST_SUITE(Transform_suite);

BOOST_AUTO_TEST_CASE(vec3_api)
{
	Vec3 x(1, 0, 0);
	Vec3 y(0, 1, 0);
	Vec3 z(x ^ y);
	BOOST_CHECK_CLOSE(z[2], 1.f, 1e-2f);
}

BOOST_AUTO_TEST_CASE(Transform_api)
{
	// from dc to transform
	Transform tr;
	float offset[3], dc[6];
	std::fill_n(offset, 3, 3.f);
	std::fill_n(dc, 6, 0.f);
	dc[0] = dc[4] = 1.f;

	tr.SetTransform(offset, dc);
	for (int r = 0; r < 4; r++)
	{
		for (int c = 0; c < 4; c++)
		{
			if (r == c)
			{
				BOOST_CHECK_CLOSE(tr[r][c], 1.f, 1e-2f);
			}
			else
			{
				BOOST_CHECK_CLOSE(tr[r][c], c == 3 ? 3.f : 0.f, 1e-2f);
			}
		}
	}

	float disp[3];
	tr.GetOffset(disp);
	for (int r = 0; r < 3; r++)
	{
		BOOST_CHECK_EQUAL(disp[r], offset[r]);
	}

	// padding
	{
		Transform tr;
		makeTransform(tr);
		Transform tr2(tr);

		// do padding
		Vec3 spacing(1.f, 2.f, 3.f);
		{
			// negative padding
			int plo[3] = {-1, -2, -3};
			tr2.PaddingUpdateTransform(plo, spacing);

			// revert padding
			for (int k = 0; k < 3; k++)
			{
				plo[k] = -plo[k];
			}
			tr2.PaddingUpdateTransform(plo, spacing);
		}

		// compare
		for (int r = 0; r < 4; r++)
		{
			for (int c = 0; c < 4; c++)
			{
				BOOST_CHECK_CLOSE(tr[r][c], tr2[r][c], 1e-2f);
			}
		}
	}

	// iSeg <-> S4L, iSeg <-> ITK ?

	// other operations...?
}

BOOST_AUTO_TEST_SUITE_END();
BOOST_AUTO_TEST_SUITE_END();

} // namespace iseg


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing remfix capabilities

    Computes the remainder of division.
    The return value is x-n*y, where n is the value x/y,
    rounded toward zero.

    This is a convenient alias of @ref rem
  **/
  const boost::dispatch::functor<tag::remfix_> remfix = {};
} }
#endif

#include <boost/simd/function/scalar/rem.hpp>
#include <boost/simd/function/simd/remfix.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_GREATESTNONINTEGER_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_GREATESTNONINTEGER_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Greatestnoninteger generic tag

     Represents the Greatestnoninteger constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Greatestnoninteger , double, 0
                                , 0x4affffffLL, 0x432fffffffffffffULL
                                );
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Greatestnoninteger, Site> dispatching_Greatestnoninteger(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Greatestnoninteger, Site>();
   }
   template<class... Args>
   struct impl_Greatestnoninteger;
  }
  /*!
    Generates the least integer value which is exactly
    representable in floating point numbers and
    equal to its integral successor.

    All floating numbers greater than Greatestnoninteger are integral.

    @par Semantic:

    @code
    T r = Greatestnoninteger<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 4503599627370495.5
    else if T is float
      r = 8388607.5f
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Greatestnoninteger, Greatestnoninteger)
} }

#include <boost/simd/constant/common.hpp>

#endif


//  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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>

//[policy_eg_7

#include <boost/math/distributions.hpp>

namespace {

using namespace boost::math::policies;

typedef policy<
   // return infinity and set errno rather than throw:
   overflow_error<errno_on_error>,
   // Don't promote double -> long double internally:
   promote_double<false>,
   // Return the closest integer result for discrete quantiles:
   discrete_quantile<integer_round_nearest>
> my_policy;

BOOST_MATH_DECLARE_DISTRIBUTIONS(double, my_policy)

} // close namespace my_namespace

int main()
{
   //
   // Start with something we know will overflow:
   //
   normal norm(10, 2);
   errno = 0;
   std::cout << "Result of quantile(norm, 0) is: "
      << quantile(norm, 0) << std::endl;
   std::cout << "errno = " << errno << std::endl;
   errno = 0;
   std::cout << "Result of quantile(norm, 1) is: "
      << quantile(norm, 1) << std::endl;
   std::cout << "errno = " << errno << std::endl;
   //
   // Now try a discrete distribution:
   //
   binomial binom(20, 0.25);
   std::cout << "Result of quantile(binom, 0.05) is: "
      << quantile(binom, 0.05) << std::endl;
   std::cout << "Result of quantile(complement(binom, 0.05)) is: "
      << quantile(complement(binom, 0.05)) << std::endl;
}

//] ends quickbook imported section


/** @brief Demonstrates the construction of a structured mesh in LehrFEM++ */

#include <boost/program_options.hpp>
#include <iostream>

#include "lf/base/base.h"
#include "lf/io/io.h"
#include "lf/mesh/hybrid2d/hybrid2d.h"
#include "lf/mesh/mesh.h"
#include "lf/mesh/test_utils/check_entity_indexing.h"
#include "lf/mesh/test_utils/check_mesh_completeness.h"
#include "lf/mesh/test_utils/test_meshes.h"
#include "lf/mesh/utils/utils.h"

int main(int argc, char** argv) {
  using size_type = lf::base::size_type;

  namespace po = boost::program_options;

  // clang-format off
  po::options_description desc("LehrFEM++ demo: construction of tensor producy triangular mesh\nUse the option `-h` to display the control variables.");
  desc.add_options()
    ("help,h", "Display help")
    ("Nx_cells", po::value<int>()->required(), "No of cells in X direction, must be > 0")
    ("Ny_cells", po::value<int>()->required(), "No of cells in Y direction, must be > 0")
  ;
  //clang-format on

  // Set control variables from command line or file "setup vars"
  // check for file with options
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  try {
    po::store(po::parse_config_file<char>("setup.vars", desc), vm);
  } catch(const po::reading_file& error) {
    std::cout << "No file `setup.vars` specifying control variables\n";
  }

  if(vm.count("help") > 0) {
    std::cout << desc << std::endl;
    return 1;
  }

  po::notify(vm);

  // number of cells cannot be 0, that would result in segfault!
  int Nx = vm["Nx_cells"].as<int>();
  int Ny = vm["Ny_cells"].as<int>();
  if (Nx == 0 || Ny == 0) {
    std::cout << "Nx and Ny must not be zero, set using options --Nx_cells and "
                 "--Ny_cells\n";
    return 0;
  }

  // Construct a triangular tensor product mesh with 2*Nx*Ny cells
  lf::mesh::utils::TPTriagMeshBuilder builder(
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2));
  // Set mesh parameters following the Builder pattern
  // Domain is the unit square
  builder.setBottomLeftCorner(Eigen::Vector2d{0, 0})
      .setTopRightCorner(Eigen::Vector2d{1, 1})
      .setNumXCells(Nx)
      .setNumYCells(Ny);
  auto mesh_p = builder.Build();

  // Some consistency checks.
  // These should not be included in a regular code
  std::cout << "Checking entity indexing" << std::endl;
  lf::mesh::test_utils::checkEntityIndexing(*mesh_p);
  std::cout << "Checking mesh completeness" << std::endl;
  lf::mesh::test_utils::checkMeshCompleteness(*mesh_p);

  // Printing mesh information
  lf::mesh::utils::PrintInfo(std::cout, *mesh_p);

  // Matlab output of mesh
  lf::io::writeMatlab(*mesh_p, "lf_tptriagmesh.m");
  return 0;
}


#ifndef STATE_H
#define STATE_H

#include <opencv2/core.hpp>
#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

struct Pose
{
	Eigen::Vector3d position;
	Eigen::Quaterniond orientation;

	Pose() {
		position = Eigen::Vector3d::Zero();
		orientation = Eigen::Quaterniond::Identity();
	}
};

struct State {
	// mean
	ros::Time time;
	Pose pose;
	Eigen::Vector3d velocity;

	State();
	ros::NodeHandle nh;
	ros::Publisher pose_pub;
	void reset();
	void showPose(bool stereo_match_flag);
};

#endif


/*
 * Copyright 2020 Tier IV, Inc. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <iostream>

#include <geometry_msgs/TwistStamped.h>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <tf2/utils.h>
#include <boost/shared_ptr.hpp>

#include <autoware_planning_msgs/Trajectory.h>

namespace vpu
{
double square(const double & a);
double calcSquaredDist2d(const geometry_msgs::Point & a, const geometry_msgs::Point & b);
double calcSquaredDist2d(const geometry_msgs::Pose & a, const geometry_msgs::Pose & b);
double calcSquaredDist2d(
  const geometry_msgs::PoseStamped & a, const geometry_msgs::PoseStamped & b);
double calcSquaredDist2d(
  const autoware_planning_msgs::TrajectoryPoint & a,
  const autoware_planning_msgs::TrajectoryPoint & b);
double calcDist2d(const geometry_msgs::Point & a, const geometry_msgs::Point & b);
double calcDist2d(const geometry_msgs::Pose & a, const geometry_msgs::Pose & b);
double calcDist2d(const geometry_msgs::PoseStamped & a, const geometry_msgs::PoseStamped & b);
double calcDist2d(
  const autoware_planning_msgs::TrajectoryPoint & a,
  const autoware_planning_msgs::TrajectoryPoint & b);
int calcClosestWaypoint(
  const autoware_planning_msgs::Trajectory & trajectory, const geometry_msgs::Point & point);
int calcClosestWaypoint(
  const autoware_planning_msgs::Trajectory & trajectory, const geometry_msgs::Pose & pose,
  const double delta_yaw_threshold);
bool extractPathAroundIndex(
  const autoware_planning_msgs::Trajectory & trajectory, const int index,
  const double & ahead_length, const double & behind_length,
  autoware_planning_msgs::Trajectory & extracted_base_waypoints);
double calcLengthOnWaypoints(
  const autoware_planning_msgs::Trajectory & trajectory, const int idx1, const int idx2);
void calcTrajectoryArclength(
  const autoware_planning_msgs::Trajectory & trajectory, std::vector<double> & arclength);
void calcTrajectoryIntervalDistance(
  const autoware_planning_msgs::Trajectory & trajectory, std::vector<double> & intervals);
void setZeroVelocity(autoware_planning_msgs::Trajectory & trajectory);
double getMaxVelocity(const autoware_planning_msgs::Trajectory & trajectory);
double getMaxAbsVelocity(const autoware_planning_msgs::Trajectory & trajectory);
void minimumVelocityFilter(const double & min_vel, autoware_planning_msgs::Trajectory & trajectory);
void maximumVelocityFilter(const double & max_vel, autoware_planning_msgs::Trajectory & trajectory);
void multiplyConstantToTrajectoryVelocity(
  const double & scalar, autoware_planning_msgs::Trajectory & trajectory);
void insertZeroVelocityAfterIdx(
  const int & stop_idx, autoware_planning_msgs::Trajectory & trajectory);
double getVx(const autoware_planning_msgs::Trajectory & trajectory, const int & i);
bool searchZeroVelocityIdx(const autoware_planning_msgs::Trajectory & trajectory, int & idx);
bool calcTrajectoryCurvatureFrom3Points(
  const autoware_planning_msgs::Trajectory & trajectory, const unsigned int & idx_dist,
  std::vector<double> & k_arr);
double normalizeRadian(const double _angle);
void convertEulerAngleToMonotonic(std::vector<double> & a);
geometry_msgs::Quaternion getQuaternionFromYaw(double yaw);
bool linearInterpTrajectory(
  const std::vector<double> & base_index,
  const autoware_planning_msgs::Trajectory & base_trajectory, const std::vector<double> & out_index,
  autoware_planning_msgs::Trajectory & out_trajectory);
}  // namespace vpu


#include <iostream>
#include <fstream>
#include <list>
#include <stdio.h>
#include <math.h>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/timer.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/extension/io/jpeg_dynamic_io.hpp>
#include "../boost_1_53_0/boost/gil/extension/numeric/sampler.hpp" //Not included in boost libraries, downloaded from Adobe's svn
#include "../boost_1_53_0/boost/gil/extension/numeric/resample.hpp"

using namespace std;
using namespace boost::gil;
using namespace boost::tuples;
using namespace boost::numeric::ublas;

//Configuration
const string imageNames[9] = {"oxford.jpg", "panda.jpg","red_dress.jpg","girl_shirt.jpg","lotus.jpg","flower_dress.jpg","flower.jpg","earth.jpg","amber_shirt.jpg"};
#define RESIZE_TO 450
#define THRESHOLD_MIN 2
#define THRESHOLD_MAX 256
#define NUM_VARIED_WEIGHTS 9
#define PERCENT_TO_TEST 0.60  //Center % of image to check
#define ENTROPY_THRESHOLD 70

//Just defining some types
typedef boost::tuple<int, int, int> rgb;
typedef struct{
	rgb value;
	string hex;
	string name;
} color_def;
typedef struct{
	rgb value;
	unsigned int occurences;
	unsigned int weight;
	int varied_weight;
	unsigned int entropy;
	long int varied_weights[NUM_VARIED_WEIGHTS];
	int avgX;
	int avgY;
} image_rgb;
typedef struct{
	list<int> entropies;
	int max;
	int min;
} region_entropies;
typedef struct{
	rgb value;
	unsigned int occurences;
	unsigned int weight;
	long int varied_weights[NUM_VARIED_WEIGHTS];
	unsigned int entropy;
	string hex;
	string name;
	int avgX;
	int avgY;
} image_color;

//Image manipulation functions
void load_image(string filename);
void kernel_1d_convolution(matrix<int> kernel);
void kernel_2d_convolution(matrix<int> kernelX, matrix<int> kernelY, float divisor = 1);
void calculate_entropies();
void generate_entropy_squares();
void extract_rgb_values();
void sort_rgb_values();

//List insertion and sorting
void load_color_definitions();
void insert_image_rgb(image_rgb rgb2);
void insert_image_color(image_color color);
bool image_color_compare (image_color first, image_color second); // comparison for image_colors to sort colorsInImage

//GLOBAL VARIABLES
rgb8_image_t sourceImage;
gray8_image_t convolutedImage;
rgb8_image_t entropySquares;
region_entropies entropies;
list<image_rgb> rgbList;
list<image_color> colorsInImage;
list<color_def> colorDefinitions;

int main() {
	//Timer from boost to test performance
	boost::timer t;

	//Print to file
	ofstream myfile;
	myfile.open ("example.html");

	myfile << "\n\n\n\n\n";

	load_color_definitions();

	cout << "Colors loaded in " << t.elapsed() << "s\n";

	for( int i = 0; i < 9; ++i ){
		rgbList.clear();
		colorsInImage.clear();
		entropies.entropies.clear();
		entropies.max = 0;
		entropies.min = 999999;

		//Load image
		load_image("test_images/"+imageNames[i]);

		/*
		//1D convolution
		matrix<int> kernel (3, 3);
		//Edge detect
		kernel(0,0) = 0; kernel(1,0) = -1; kernel(2,0) = 0;
		kernel(0,1) = -1; kernel(1,1) = 4; kernel(2,1) = -1;
		kernel(0,2) = 0; kernel(1,2) = -1; kernel(2,2) = 0;
		//Edge detect, alternative
		kernel(0,0) = 0; kernel(1,0) = 1; kernel(2,0) = 0;
		kernel(0,1) = 1; kernel(1,1) = -4; kernel(2,1) = 1;
		kernel(0,2) = 0; kernel(1,2) = 1; kernel(2,2) = 0;

		kernel_1d_convolution( kernel );
		*/

		matrix<int> kernelX (3, 3);
		matrix<int> kernelY (3, 3);
		/*
		//Sobel edge-detect
		kernelX(0,0) = -1; kernelX(1,0) = 0; kernelX(2,0) = 1;
		kernelX(0,1) = -2; kernelX(1,1) = 0; kernelX(2,1) = 2;
		kernelX(0,2) = -1; kernelX(1,2) = 0; kernelX(2,2) = 1;

		kernelY(0,0) = -1; kernelY(1,0) = -2; kernelY(2,0) = -1;
		kernelY(0,1) = 0; kernelY(1,1) = 0; kernelY(2,1) = 0;
		kernelY(0,2) = 1; kernelY(1,2) = 2; kernelY(2,2) = 1;
		*/

		//Scharr's edge-detect
		kernelX(0,0) = 3; kernelX(1,0) = 0; kernelX(2,0) = -3;
		kernelX(0,1) = 10; kernelX(1,1) = 0; kernelX(2,1) = -10;
		kernelX(0,2) = 3; kernelX(1,2) = 0; kernelX(2,2) = -3;

		kernelY(0,0) = 3; kernelY(1,0) = 10; kernelY(2,0) = 3;
		kernelY(0,1) = 0; kernelY(1,1) = 0; kernelY(2,1) = 0;
		kernelY(0,2) = -3; kernelY(1,2) = -10; kernelY(2,2) = -3;

		//Prewitt edge-detect
		/*kernelX(0,0) = -1; kernelX(1,0) = 0; kernelX(2,0) = 1;
		kernelX(0,1) = -1; kernelX(1,1) = 0; kernelX(2,1) = 1;
		kernelX(0,2) = -1; kernelX(1,2) = 0; kernelX(2,2) = 1;

		kernelY(0,0) = 1; kernelY(1,0) = 1; kernelY(2,0) = 1;
		kernelY(0,1) = 0; kernelY(1,1) = 0; kernelY(2,1) = 0;
		kernelY(0,2) = -1; kernelY(1,2) = -1; kernelY(2,2) = -1;*/

		kernel_2d_convolution( kernelX, kernelY, 11 ); //Creates a greyscale edge detect convolution

		calculate_entropies();
		//Test print
		for( list<int>::iterator it = entropies.entropies.begin(); it != entropies.entropies.end(); ++it ){
			//rgb2 = get<0>(*it);
			cout << "R: " << *it << endl;
		}
		cout << "Min: " << entropies.min << endl;
		cout << "Max: " << entropies.max << endl;

		//generate_entropy_squares();

		extract_rgb_values(); //Extracts and weights each pixel

		//Test print
		/*for( list<image_rgb>::iterator it = rgbList.begin(); it != rgbList.end(); ++it ){
			//rgb2 = get<0>(*it);
			cout << "R: " << get<0>(it->value) <<
				" G: " << get<1>(it->value) <<
				" B: " << get<2>(it->value) <<
				" Occurences: " << it->occurences <<
				" Weight: " << it->weight << endl;
		}*/

		sort_rgb_values(); //Groups rgb values by their closest match in color_definitions

		colorsInImage.sort( image_color_compare );
		/*
		for( list<image_color>::iterator it = colorsInImage.begin(); it != colorsInImage.end(); ++it ){
			cout << "Name: " << it->name <<
				" RGB: (" << get<0>(it->value) <<
				", " << get<1>(it->value) <<
				", " << get<2>(it->value) <<
				") Hex: " << it->hex <<
				" Occurences: " << it->occurences <<
				" Weight: " << it->weight <<
				" Varied Weights: (" << it->varied_weights[0];
			for( int i = 1; i < 9; ++i )
				cout << ", " << it->varied_weights[i];
			cout << ")\n";
		}*/
		jpeg_write_view(string("images/input")+(char)(i+49)+".jpg", view(sourceImage) );
		jpeg_write_view(string("images/input_convoluted")+(char)(i+49)+".jpg", view(convolutedImage) );
		jpeg_write_view(string("images/input_ent_sqs")+(char)(i+49)+".jpg", view(entropySquares) );

		myfile << "
\n";
		myfile << "
\n" <<
			  "<img src='images/input" << i+1 << ".jpg'>\n" <<
			  "<img src='images/input_convoluted" << i+1 << ".jpg'>\n" <<
			  "<img src='images/input_ent_sqs" << i+1 << ".jpg'>\n
\n";
		for( list<image_color>::iterator it = colorsInImage.begin(); it != colorsInImage.end(); ++it ){
			myfile << "<div class = 'color' style='border:solid 1px " << it->hex << ";'>\n" <<
				  "<div class = 'block' style='background-color:" << it->hex << ";'>
\n" <<
				  "Name: " << it->name <<
				" * RGB: (" << get<0>(it->value) <<
				", " << get<1>(it->value) <<
				", " << get<2>(it->value) <<
				") * Hex: " << it->hex <<
				" * Occurences: " << it->occurences <<
				" * Average Position: (" << it->avgX << ", " << it->avgY << ")\n" <<
				//"
 Weight: " << it->weight <<
				"
# Neighboring Pixels Over Treshold: (" << it->varied_weights[0];
			for( int i = 1; i < NUM_VARIED_WEIGHTS; ++i )
				myfile << ", " << it->varied_weights[i];
			myfile << ") Total Entropy: " << it->entropy << "\n
\n";
		}
		myfile << "
\n";
	}
	myfile << "\n\n";
	myfile.close();

	cout << "Time elapsed: " << t.elapsed() << "s\n";

	return 0;
}

void load_image(string filename){
	rgb8_image_t loadedImage;

	jpeg_read_image( filename, loadedImage );

	//Perform resize if necessary
	if( RESIZE_TO != 0 && (loadedImage.width() > RESIZE_TO || loadedImage.height() > RESIZE_TO) ){
		loadedImage.width() > loadedImage.height() ? sourceImage.recreate(RESIZE_TO,(RESIZE_TO*(float)loadedImage.height()/loadedImage.width()))
							   : sourceImage.recreate((RESIZE_TO*(float)loadedImage.width()/loadedImage.height()),RESIZE_TO);
		resize_view( const_view(loadedImage), view(sourceImage), bilinear_sampler() );
	}else{
		sourceImage.recreate( loadedImage.dimensions() );
		copy_pixels( const_view(loadedImage), view(sourceImage) );
	}
}

void kernel_1d_convolution(matrix<int> kernel){
	convolutedImage.recreate( sourceImage.dimensions() );
	//Create grayscale copy of original image
	copy_pixels(color_converted_view<gray8_pixel_t>( const_view(sourceImage)), view(convolutedImage) );
	gray8_image_t grayscaleImage( sourceImage.width()+2, sourceImage.height()+2 );
	resize_view( const_view(convolutedImage), view(grayscaleImage), bilinear_sampler() );

	int anchor = kernel.size1()/2;

	int xMax = convolutedImage.width(), yMax = convolutedImage.height();

	gray8_view_t src = view(grayscaleImage);
	gray8_view_t dst = view(convolutedImage);

	gray8_pixel_t pixel;
	gray8_view_t::xy_locator srcLoc = src.xy_at(1,1);
	gray8_view_t::xy_locator dstLoc = dst.xy_at(0,0);
	int x, y, i, j, dstValue;
	for( y = 0; y < yMax; ++y ){
		for( x = 0; x < xMax; ++x ){
			dstValue = 0;
			for( j = anchor; j >= -1*anchor; j-- )
				for( i = -1*anchor; i <= anchor; i++ )
					dstValue += srcLoc(i,j)*kernel(i+anchor,-1*j+anchor);
			if( dstValue < 0 )
				dstValue = 0;
			*dstLoc = dstValue;

			++srcLoc.x();
			++dstLoc.x();
		}
		srcLoc.x() -= xMax;
		dstLoc.x() -= xMax;
		srcLoc.y()++;
		dstLoc.y()++;
	}
}

void kernel_2d_convolution(matrix<int> kernelX, matrix<int> kernelY, float divisor){
	convolutedImage.recreate( sourceImage.dimensions() );
	//Create grayscale copy of original image
	copy_pixels(color_converted_view<gray8_pixel_t>( const_view(sourceImage)), view(convolutedImage) );
	gray8_image_t grayscaleImage( sourceImage.width()+2, sourceImage.height()+2 );
	resize_view( const_view(convolutedImage), view(grayscaleImage), bilinear_sampler() );

	int anchor = kernelX.size1()/2;

	int xMax = convolutedImage.width(), yMax = convolutedImage.height();

	gray8_view_t src = view(grayscaleImage);
	gray8_view_t dst = view(convolutedImage);

	gray8_pixel_t pixel;
	gray8_view_t::xy_locator srcLoc = src.xy_at(1,1);
	gray8_view_t::xy_locator dstLoc = dst.xy_at(0,0);
	int x, y, i, j, dstXValue, dstYValue;
	for( y = 0; y < yMax; ++y ){
		for( x = 0; x < xMax; ++x ){
			dstXValue = 0;
			dstYValue = 0;
			for( j = anchor; j >= -1*anchor; j-- )
				for( i = -1*anchor; i <= anchor; i++ ){
					dstXValue += srcLoc(i,j)*kernelX(i+anchor,-1*j+anchor);
					dstYValue += srcLoc(i,j)*kernelY(i+anchor,-1*j+anchor);
				}
			*dstLoc = sqrt( pow(dstXValue, 2) + pow( dstYValue, 2) )/divisor;

			//Get rid of horizontal and vertical edges
			/*float degrees;
			dstXValue != 0 ? degrees = atan ((float)dstYValue/dstXValue)*57.2957795 : degrees = 0;
			if( degrees < 0 )
				degrees += 180;
			if( degrees < 22.5 )
				degrees = 0;
			else if( degrees > 67.5 && degrees < 112.5 )
				degrees = 90;
			if( degrees == 0 || degrees == 90 )
				*dstLoc = 0;*/
			//cout << atan ((float)dstYValue/dstXValue)*57.2957795 << endl;

			++srcLoc.x();
			++dstLoc.x();
		}
		srcLoc.x() -= xMax;
		dstLoc.x() -= xMax;
		srcLoc.y()++;
		dstLoc.y()++;
	}
}

void calculate_entropies(){
	gray8_view_t test = view(convolutedImage);

	//Perform some commonly used operations so they aren't done too often
	int imageWidth = sourceImage.width(), imageHeight = sourceImage.height();

	int xSteps = imageWidth/10;
	int xRemainder = imageWidth%10;
	int ySteps = imageHeight/10;
	int yRemainder = imageHeight%10;

	float xMultiplier, yMultiplier;
	if( xRemainder != 0 )
		 xMultiplier = 10/xRemainder;
	if( yRemainder != 0 )
		yMultiplier = 10/yRemainder;

	int entropy;

	//Prepare to traverse pixels!
	int x,y, o, a, i, j;
	rgb8_pixel_t pixel;
	gray8_view_t::xy_locator testLoc;

	entropies.max = 0;
	entropies.min = 9999999;
	for( y = 0; y < ySteps; y++ ){
		for( x = 0; x < xSteps; x++ ){
			testLoc = test.xy_at(x*10+1,y*10+1);

			entropy = 0;
			for( o = 0; o < 10; ++o ){
				for( a = 0; a <= 10; ++a ){
					for( i = -1; i <= 1; ++i )
						for( j = -1; j <= 1; ++j )
							entropy += abs( *testLoc - (testLoc(i, j)) );
					testLoc.x()++;
				}
				testLoc.x() -= 10;
				testLoc.y()++;
			}
			entropies.entropies.push_back( entropy );
			if( entropy > entropies.max )
				entropies.max = entropy;
			else if( entropy < entropies.min )
				entropies.min = entropy;

		}
		if( xRemainder != 0 ){
			testLoc = test.xy_at(xSteps*10+1,ySteps*10+1);
			entropy = 0;
			for( o = 0; o < 10; ++o ){
				for( a = 0; a < xRemainder; ++a ){
					for( i = -1; i <= 1; ++i )
						for( j = -1; j <= 1; ++j )
							entropy += abs( *testLoc - (testLoc(i, j)) );
					testLoc.x()++;
				}
				testLoc.x() -= 10;
				testLoc.y()++;
			}
			entropy *= xMultiplier;
			entropies.entropies.push_back( entropy );
			if( entropy > entropies.max )
				entropies.max = entropy;
			else if( entropy < entropies.min )
				entropies.min = entropy;
		}
	}
	if( yRemainder != 0 )
		for( x = 0; x < xSteps; x++ ){
			testLoc = test.xy_at(x*10+1,ySteps*10+1);

			entropy = 0;
			for( o = 0; o < yRemainder; ++o ){
				for( a = 0; a <= 10; ++a ){
					for( i = -1; i <= 1; ++i )
						for( j = -1; j <= 1; ++j )
							entropy += abs( *testLoc - (testLoc(i, j)) );
					testLoc.x()++;
				}
				testLoc.x() -= 10;
				testLoc.y()++;
			}
			entropy *= yMultiplier;
			entropies.entropies.push_back( entropy );
			if( entropy > entropies.max )
				entropies.max = entropy;
			else if( entropy < entropies.min )
				entropies.min = entropy;
		}
	if( xRemainder != 0 && yRemainder != 0 ){
		testLoc = test.xy_at(x*10+1,ySteps*10+1);

		entropy = 0;
		for( o = 0; o < yRemainder; ++o ){
			for( a = 0; a <= xRemainder; ++a ){
				for( i = -1; i <= 1; ++i )
					for( j = -1; j <= 1; ++j )
						entropy += abs( *testLoc - (testLoc(i, j)) );
				testLoc.x()++;
			}
			testLoc.x() -= 10;
			testLoc.y()++;
		}
		entropy *= yMultiplier *= xMultiplier;
		entropies.entropies.push_back( entropy );
		if( entropy > entropies.max )
			entropies.max = entropy;
		else if( entropy < entropies.min )
			entropies.min = entropy;
	}
}

void generate_entropy_squares(){
	entropySquares.recreate( sourceImage.dimensions() );
	//Create grayscale copy of original image
	copy_pixels( const_view(sourceImage), view(entropySquares) );

	rgb8_view_t test = view(entropySquares);

	//Perform some commonly used operations so they aren't done too often
	int imageWidth = sourceImage.width(), imageHeight = sourceImage.height();

	int xSteps = imageWidth/10;
	int xRemainder = imageWidth%10;
	int ySteps = imageHeight/10;
	int yRemainder = imageHeight%10;

	float xMultiplier, yMultiplier;
	if( xRemainder != 0 )
		 xMultiplier = 10/xRemainder;
	else
		xMultiplier = 0;
	if( yRemainder != 0 )
		yMultiplier = 10/yRemainder;
	else
		yMultiplier = 0;

	float entropyMultiplier = 510/entropies.max;

	int offset = ((float)entropies.max*entropyMultiplier+(float)entropies.min*entropyMultiplier)/2;
	int entropy;
	list<int>::iterator it = entropies.entropies.begin();
	//it != entropies.entropies.end();
	//Prepare to traverse pixels!
	int x,y, o, a;
	rgb8_view_t::xy_locator testLoc;

	rgb8s_pixel_t pixelColor;

	entropies.max = 0;
	entropies.min = 99999;
	for( y = 0; y < ySteps; y++ ){
		for( x = 0; x < xSteps; x++ ){
			testLoc = test.xy_at(x*10+1,y*10+1);

			entropy = (*it)*entropyMultiplier-offset;
			if( entropy < 0 ){
				at_c<0>(pixelColor) = abs( entropy );
				at_c<1>(pixelColor) = 0;
				at_c<2>(pixelColor) = 0;
			} else {
				at_c<0>(pixelColor) = entropy;
				at_c<1>(pixelColor) = entropy;
				at_c<2>(pixelColor) = entropy;
			}

			for( o = 0; o < 10; ++o ){
				for( a = 0; a < 10; ++a ){
					at_c<0>(*testLoc) = at_c<0>(pixelColor);
					at_c<1>(*testLoc) = at_c<1>(pixelColor);
					//at_c<2>(*testLoc) = at_c<2>(pixelColor);
					testLoc.x()++;
				}
				testLoc.x() -= 10;
				testLoc.y()++;
			}
			it++;
		}
		if( xRemainder != 0 ){
			testLoc = test.xy_at(xSteps*10+1,ySteps*10+1);
			entropy = (*it)*entropyMultiplier-offset;
			/*if( entropy < 0 ){
				at_c<0>(pixelColor) = abs( entropy );
				at_c<1>(pixelColor) = 0;
				at_c<2>(pixelColor) = 0;
			} else {
				at_c<0>(pixelColor) = entropy;
				at_c<1>(pixelColor) = entropy;
				at_c<2>(pixelColor) = entropy;
			}
			for( o = 0; o < 10; ++o ){
				for( a = 0; a < xRemainder; ++a ){
					*testLoc = pixelColor;
					testLoc.x()++;
				}
				testLoc.x() -= 10;
				testLoc.y()++;
			}*/
			it++;
		}
	}
	if( yRemainder != 0 )
		for( x = 0; x < xSteps; x++ ){
			testLoc = test.xy_at(x*10+1,ySteps*10+1);

			entropy = (*it)*entropyMultiplier-offset;
			/*if( entropy < 0 ){
				at_c<0>(pixelColor) = abs( entropy );
				at_c<1>(pixelColor) = 0;
				at_c<2>(pixelColor) = 0;
			} else {
				at_c<0>(pixelColor) = entropy;
				at_c<1>(pixelColor) = entropy;
				at_c<2>(pixelColor) = entropy;
			}
			for( o = 0; o < yRemainder; ++o ){
				for( a = 0; a <= 10; ++a ){
					*testLoc = pixelColor;
					testLoc.x()++;
				}
				testLoc.x() -= 10;
				testLoc.y()++;
			}*/
			it++;
		}
	if( xRemainder != 0 && yRemainder != 0 ){
		testLoc = test.xy_at(x*10+1,ySteps*10+1);

		entropy = (*it)*entropyMultiplier-offset;
		/*if( entropy < 0 ){
			at_c<0>(pixelColor) = abs( entropy );
			at_c<1>(pixelColor) = 0;
			at_c<2>(pixelColor) = 0;
		} else {
			at_c<0>(pixelColor) = entropy;
			at_c<1>(pixelColor) = entropy;
			at_c<2>(pixelColor) = entropy;
		}
		for( o = 0; o < yRemainder; ++o ){
			for( a = 0; a <= xRemainder; ++a ){
				*testLoc = pixelColor;
				testLoc.x()++;
			}
			testLoc.x() -= 10;
			testLoc.y()++;
		}*/
		it++;
	}
}

void extract_rgb_values(){
	rgb8_view_t src = view(sourceImage);
	gray8_view_t test = view(convolutedImage);

	//Perform some commonly used operations so they aren't done too often
	int imageWidth = sourceImage.width()*PERCENT_TO_TEST, imageHeight = sourceImage.height()*PERCENT_TO_TEST;
	int xc = imageWidth/2, yc = imageHeight/2;
	int numPix = imageWidth*imageHeight;

	//Prepare to traverse pixels!
	int x,y, i, j;
	rgb8_pixel_t pixel;
	image_rgb currentRgb;
	rgb8_view_t::xy_locator srcLoc = src.xy_at(sourceImage.width()*((1-PERCENT_TO_TEST)/2),sourceImage.height()*((1-PERCENT_TO_TEST)/2));
	gray8_view_t::xy_locator testLoc = test.xy_at(sourceImage.width()*((1-PERCENT_TO_TEST)/2),sourceImage.height()*((1-PERCENT_TO_TEST)/2));

	for( y = imageHeight-1; y > 0; y-- ){
		for( x = 0; x < imageWidth-1; x++ ){
			//Calculate varied_weight from convolution
			for( i = 0; i < NUM_VARIED_WEIGHTS; ++i )
				currentRgb.varied_weights[i] = 0;

			currentRgb.varied_weight = 0;
			currentRgb.entropy = 0;
			for( i = -1; i <= 1; ++i )
				for( j = -1; j <= 1; ++j ){
					if( abs( *testLoc - (testLoc(i, j)) ) >= THRESHOLD_MIN && abs( *testLoc - (testLoc(i, j)) ) <= THRESHOLD_MAX )
						currentRgb.varied_weight++;
					currentRgb.entropy += abs( *testLoc - (testLoc(i, j)) );
				}
			if( currentRgb.entropy < ENTROPY_THRESHOLD )
				currentRgb.entropy = 0;
			currentRgb.varied_weights[currentRgb.varied_weight]++;

			pixel = *srcLoc;
			get<0>(currentRgb.value) = (int)at_c<0>(pixel);
			get<1>(currentRgb.value) = (int)at_c<1>(pixel);
			get<2>(currentRgb.value) = (int)at_c<2>(pixel);
			currentRgb.occurences = 1;

			//Eqn to calculate pixel weight... needs to be thought through more.
			//In addition to this, weight is divided by occurences in sort
			currentRgb.weight = (float)numPix/sqrt(pow( xc-x, 2 ) + pow( yc-y, 2 )+2);
			currentRgb.avgX = x;
			currentRgb.avgY = y;
			insert_image_rgb( currentRgb );
			++srcLoc.x();
			++testLoc.x();
		}
		srcLoc.x() -= (imageWidth - 1);
		testLoc.x() -= (imageWidth - 1);
		srcLoc.y()++;
		testLoc.y()++;
	}
}

void load_color_definitions(){
	//Load color definitions from file, yo
	FILE *fp;
	fp = fopen("colorDefinitions.dat", "r+");

	color_def currentColor;
	char *str = new char[35];
	do{
		currentColor.name = fgets( str, 36, fp );
		//currentColor.hex = fgets( str, 8, fp );
		if( fscanf(fp," %s %i %i %i ", str, &get<0>(currentColor.value), &get<1>(currentColor.value), &get<2>(currentColor.value) ) == 4 ){
			currentColor.hex = str;
			colorDefinitions.push_back( currentColor );
		}else
			break;
		//cout << currentColor.name << endl;
		while( fgetc(fp) != '\n' && !feof(fp) ); //Go to next line
	}while( !feof(fp) );

	delete str;  //Cleaning up after myself

	//Test print
	/*for( list<color_def>::iterator it = colorDefinitions.begin(); it != colorDefinitions.end(); ++it ){
		cout << "R: " << get<0>(it->value) <<
			" G: " << get<1>(it->value) <<
			" B: " << get<2>(it->value) <<
			" Name: " << it->name <<
			" Hex: #" << it->hex << endl;
	}*/
}

void sort_rgb_values(){
	//Match rgb values to color definitions with names, hex values, and all that good stuff
	//uses rgb values as 3d points, assuming the shortest distance between the points is the
	//closest color match.  I am still not sure, but it seems to work.  Can't find a better way, either.
	image_color color;
	color_def currentColor;
	float shortestDist, currentDist;
	for( list<image_rgb>::iterator it = rgbList.begin(); it != rgbList.end(); ++it ){
		shortestDist = 99999999;
		for( list<color_def>::iterator it2 = colorDefinitions.begin(); it2 != colorDefinitions.end(); ++it2 ){
			/*currentDist = sqrt( pow( get<0>(it2->value)-get<0>(it->value), 2 )
					  + pow( get<1>(it2->value)-get<1>(it->value), 2 )
					  + pow( get<2>(it2->value)-get<2>(it->value), 2 ) );*/
			currentDist = (pow( get<0>(it2->value)-get<0>(it->value), 2 ))*0.299
				      + (pow( get<1>(it2->value)-get<1>(it->value), 2 ))*0.587
				      + (pow( get<2>(it2->value)-get<2>(it->value), 2 ))*0.114;
			if( currentDist <= shortestDist ){
				shortestDist = currentDist;
				currentColor = *it2;
			}
		}
		color.value = currentColor.value;
		color.hex = currentColor.hex;
		color.name = currentColor.name;
		color.occurences = it->occurences;
		color.weight = it->weight;
		color.avgX = it->avgX;
		color.avgY = it->avgY;
		color.entropy = it->entropy;
		for( int i = 0; i < NUM_VARIED_WEIGHTS; ++i )
			color.varied_weights[i] = it->varied_weights[i];
		insert_image_color( color );
	}
}

void insert_image_rgb(image_rgb rgb){
	if( rgbList.empty() )
		rgbList.push_back( rgb );
	else {
		list<image_rgb>::iterator it = rgbList.begin();
		while( it != rgbList.end() && it->value < rgb.value ) ++it;

		if( it->value < rgb.value || it == rgbList.end()){
			rgbList.insert(it, rgb);
		}else{
			rgb.avgX = (rgb.avgX*rgb.occurences+it->avgX*it->occurences)/(rgb.occurences+it->occurences);
			rgb.avgY = (rgb.avgY*rgb.occurences+it->avgY*it->occurences)/(rgb.occurences+it->occurences);
			rgb.entropy += it->entropy;
			rgb.occurences += it->occurences;
			rgb.weight += it->weight;
			for( int i = 0; i < NUM_VARIED_WEIGHTS; ++i )
				rgb.varied_weights[i] += it->varied_weights[i];
			it = rgbList.erase(it);
			rgbList.insert(it, rgb);
		}
	}
}

void insert_image_color(image_color color){
	if( colorsInImage.empty() )
		colorsInImage.push_back( color );
	else {
		list<image_color>::iterator it = colorsInImage.begin();
		while( it != colorsInImage.end() && it->value < color.value ) ++it;

		if( it->value < color.value || it == colorsInImage.end())
			colorsInImage.insert(it, color);
		else{
			color.avgX = (color.avgX*color.occurences+it->avgX*it->occurences)/(color.occurences+it->occurences);
			color.avgY = (color.avgY*color.occurences+it->avgY*it->occurences)/(color.occurences+it->occurences);
			color.occurences += it->occurences;
			color.weight += it->weight;
			color.entropy += it->entropy;
			for( int i = 0; i < NUM_VARIED_WEIGHTS; ++i )
				color.varied_weights[i] += it->varied_weights[i];
			it = colorsInImage.erase(it);
			colorsInImage.insert(it, color);
		}
	}
}

bool image_color_compare (image_color first, image_color second){
	long long int firstWeight = 0, secondWeight = 0;
	for( int i = 0; i < NUM_VARIED_WEIGHTS; ++i ){
		firstWeight += first.varied_weights[i]*i;
		secondWeight += second.varied_weights[i]*i;
	}

	//if( firstWeight > secondWeight ) return true;
	if( (first.entropy+firstWeight)/2 > (second.entropy+secondWeight)/2 ) return true;
	else return false;
}


#ifndef AHRSTRACKLET_H_
#define AHRSTRACKLET_H_
#include <math.h>
#include <iostream>
#include <boost/math/quaternion.hpp>
#include "ahrs_rotor.hpp"
using ::boost::math::quaternion;

class AHRSTracklet{
private:
    AHRSRotor rotor;
    float invSampleFreq;
    // a pure quaternion for tracklet
    float q1;
    float q2;
    float q3;

public:
    AHRSTracklet(AHRSRotor rotor);
    AHRSTracklet(AHRSRotor rotor, float sampleFrequency);
    void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; rotor.begin(sampleFrequency); }
    void update(float gx, float gy, float gz, float ax, float ay, float az, float length);
    float getX() { return q1; }
    float getY() { return q2; }
    float getZ() { return q3; }
    float convertRawAcceleration(int aRaw);
    float convertRawGyro(int gRaw);
    void computeAngles(float q0, float q1, float q2, float q3);
};
#endif


#include <armadillo>
#include <iostream>
#include <cmath>
#include "LowPassFilter.hpp"
#include "LowPassFilter.cpp"
#include <ros/ros.h>
#include "std_msgs/String.h"
#include <sigproc_lib/sigproc_lib.h>
#include "sigproc_lib/sigproc.h"

using namespace arma;
using namespace as64_;

class GDMP
{
public:

  int BFs, NSamples, extra_train, Ns, id, window;
  double tNyq, dt, Tend;
  double y0, dy0, g, y0d_hat, gd_hat, Ry0d_hat, Rgd_hat;
  double az, bz, K, D, k, ks, kt;

  mat c, h, psi, w;
  mat y, dy, ddy, y_smoothed, Ry_smoothed;
  mat Ry, Rdy, Rddy;

  GDMP();
  ~GDMP();
  void training(mat y_desired,mat dy_desired,mat ddy_desired, vec timed);
  mat find_reference_desired(mat y_desired,mat dy_desired,mat ddy_desired, vec timed);
  void calculate_centers(vec timed);
  void generate_weights(mat fp_des);
  void run_solution(double goal, CanonicalStructure cs, double ts, double main_time, double extra_time, int extra_samples);
  void run_reverse_solution(double goal, CanonicalStructure cs, double ts, double main_time, double extra_time, int extra_samples);
  double fNyquistFunc(double *original, int size, double fs, double T);
  vec run_solution_dt(double t_now, double goal, double ts, int i, double f_prev, double df_prev, double ddf_prev );
  vec init_solution_dt(double goal, CanonicalStructure cs, double main_time, int extra_samples, double t_now, int i );
  vec init_Rsolution_dt(double goal, CanonicalStructure cs, double main_time, int extra_samples, double t_now, int i);
  vec run_Rsolution_dt(double t_now, double goal, double ts, int i, double f_prev, double df_prev, double ddf_prev );

};

GDMP::GDMP()
{
  std::cout << "/* Constructor of GDMP */" << '\n';
}

GDMP::~GDMP()
{
  std::cout << "/* Deconstructor of GDMP */" << '\n';
}

void GDMP::training(mat y_desired,mat dy_desired,mat ddy_desired, vec timed)
{
  std::cout << "/* Start of Training */" << '\n';

  NSamples = y_desired.n_cols;
  dt = timed(3)-timed(2);
  Tend = timed(NSamples-1);
  extra_train = ceil(NSamples/2);

  /* Reference Desired*/
  mat mat_refD(size(y_desired));
  mat_refD = find_reference_desired(y_desired, dy_desired, ddy_desired, timed);

  /* Calculate centers h psi */
  calculate_centers(timed);

  /* Generate weights */
  generate_weights(mat_refD.row(0));

  std::cout << "/* End of Training */" << '\n';
}

mat GDMP::find_reference_desired(mat y_desired,mat dy_desired,mat ddy_desired, vec timed)
{
  cout<<"Reference Desired Checked"<<endl;

  ks = 1;
  kt = 1;
  y0 = y_desired(0);
  dy0 = dy_desired(0);
  g =  y_desired(NSamples-1);

  /* sig y_desired to 0*/
  mat tsig = linspace(-Tend-extra_train*dt,extra_train*dt,NSamples+extra_train);
  mat sig = 1-1/(1+exp(-0.08/dt*tsig));

  double y_scaled_ext[NSamples+extra_train], dy_ext[NSamples+extra_train], ddy_ext[NSamples+extra_train], timed_ext[NSamples+extra_train];

  for (int i = 0; i < NSamples; i++)
  {
    y_scaled_ext[i] = sig(i)*(y_desired[i] - y0);
    dy_ext[i] = sig(i)*dy_desired[i];
    ddy_ext[i] = sig(i)*ddy_desired[i];
    timed_ext[i] = timed(i);
  }

  for (int i = NSamples;i<NSamples+extra_train;i++)
  {
    y_scaled_ext[i] = sig(i)*(y_desired[NSamples-1] - y0);
    dy_ext[i] = sig(i)*dy_desired(NSamples-1);
    ddy_ext[i] = sig(i)*ddy_desired(NSamples-1);
    timed_ext[i] = i*dt ;
  }

  /* original signal */
  double fd_original_scaled[NSamples+extra_train];
  double fd_filtered[NSamples];

  vec fd_original_ext(3*NSamples+extra_train);
  vec fd_filtered_ext(3*NSamples+extra_train);

  /* find original scaled */
  for (int i = 0; i < NSamples+extra_train; i++)
  {
    fd_original_scaled[i] = ks*(y_scaled_ext[i]-y0) + y0; //basically is ext too
  }

  /* extent before filter */
  for (int i = 0; i < NSamples; i++)
  {
    fd_original_ext(i) = fd_original_scaled[0];
  }
  for (int i = 0; i < NSamples+extra_train; i++)
  {
    fd_original_ext( i + NSamples ) = fd_original_scaled[i] ;
  }
  for (int i = 2*NSamples+extra_train; i < 3*NSamples+extra_train; i++)
  {
    fd_original_ext(i) = fd_original_scaled[NSamples+extra_train-1];
  }

  double  tau = Tend, a = 10, b = a/4;

  /* Nyquist - fc */
  double fNyquist;
  fNyquist = fNyquistFunc(fd_original_scaled, NSamples+extra_train, 1/dt, Tend + extra_train*dt );

  BFs = ceil(Tend*fNyquist + extra_train*dt*fNyquist);

  std::cout << fNyquist << '\n';
  cout<<BFs<<endl;
  std::cout << Tend*fNyquist << '\n';
  std::cout << extra_train*dt*fNyquist << '\n';

  tNyq = 1/fNyquist;
//  Ns = ceil(NSamples/BFs);

  /* Lowpass Filter */
  // LowPassFilter lpf((1/(2*tNyq)), dt);
  // for(int i = 0; i < 3*NSamples+extra_train; i++)
  // {
  // 		fd_filtered_ext[i] = lpf.update(fd_original_ext[i]) ; //Update with 1.0 as input value.
  // }

  std::cout << "Filtering...";

  arma::vec filter_coeff = spl_::fir1(100 /*filter order*/, fNyquist*dt /*normalized cutoff Frequency*/);
  fd_filtered_ext = arma::conv(fd_original_ext, filter_coeff.t(), "same");
  std::cout << "[DONE]!\n";

  /* keep specific fd_filtered size of NSamples */
  for (int i = 0; i < NSamples+extra_train; i++)
  {
    fd_filtered[i] = fd_filtered_ext(i + NSamples) + y0;
  }

  /* A to return in once Reference y,dy,ddy */
  mat A(3,NSamples+extra_train); //3 y,dy,ddy not because of DIM
  for (int i = 0; i < NSamples+extra_train; i++)
  {
    A(0,i) = fd_filtered[i];
    A(1,i) = ks*kt*dy_ext[i];
    A(2,i) = ks*pow(kt,2)*ddy_ext[i];
  }

  /* Printed in files for CHECK*/
  ofstream myFile;

  std::ostringstream oss,oss2;

  oss << "CHECK/fd_ori" << id <<".log";
  myFile.open((oss.str()).c_str());
  for (int i = 0; i < NSamples+extra_train; i++)
    myFile<<fd_original_scaled[i]<<endl;
  myFile.close();

  oss2 << "CHECK/fd_filtered" << id <<".log";
  myFile.open((oss2.str()).c_str());
  for (int i = 0; i < NSamples+extra_train; i++)
    myFile<<fd_filtered[i]<<endl;
  myFile.close();

  // oss << "CHECK/drefD" << id <<".log";
  // myFile.open((oss.str()).c_str());
  // myFile<<A.row(1)<<endl;
  // myFile.close();
  //
  // oss << "CHECK/ddrefD" << id <<".log";
  // myFile.open((oss.str()).c_str());
  // myFile<<A.row(2)<<endl;
  // myFile.close();

  return A;
}

/* Calculate centers of kernels */
void GDMP::calculate_centers(vec timed)
{
  c.set_size(1,BFs);

  for (int idx = 0; idx<BFs; idx++)
  {
    c(idx) = (idx)*tNyq;
  }

  /* ext time for extra train*/
  vec timed_ext_train(NSamples+extra_train);
  for (int i = 0; i <NSamples; i++ )
    timed_ext_train(i) = timed(i);
  for (int i = 0; i < extra_train; i++ )
    timed_ext_train(i+NSamples) =  (i+1)*dt + timed(NSamples-1) ;

  /* Sincs == psi */
  psi.set_size(BFs , NSamples + extra_train);
  double x;
  for (int b = 0; b < BFs ; b++)
  {
    for (int i = 0; i <NSamples+ extra_train; i++ )
    {
      x = ((timed_ext_train(i) - c(b))/tNyq);
      if (x != 0)
        psi(b,i) = sin(M_PI*x)/(M_PI*x);
      else
        psi(b,i) = 1;
    }
  }

}

/* Weights */
void GDMP::generate_weights(mat fp_des)
{
  std::cout << "/* start generate weights */" << '\n';

  mat scaled_fp_d = fp_des.t() - y0*ones(NSamples+extra_train);

  w.set_size(BFs ,1);
  /* weights for BFs*/
  // !! DERIVATE WITH 0 ?
  if ( g == y0 )
  {
    for (int idx = 0; idx<BFs; idx++)
    {
        w(idx) = 1;
    }
  }
  else
  {
    for (int idx = 0; idx<BFs; idx++)
    {
        w(idx) = scaled_fp_d(round(c(idx)/dt))/(g - y0);
    }
  }

}

vec GDMP::init_solution_dt(double goal, CanonicalStructure cs, double main_time, int extra_samples, double t_now, int i)
{
  /* Set size = NSamples + extra samples
      extra samples oq from extra time to run
  */
  y.set_size(1,NSamples+extra_samples);
  dy.set_size(1,NSamples+extra_samples);
  ddy.set_size(1,NSamples+extra_samples);

  az = 10;
  bz = az/4;

  K = az*bz/pow(cs.taf,2);
  D = (az + cs.dtaf)/cs.taf;

  k = goal - y0;

  /* Init fp*/
  double s = 0.0 ;
  for (int l = 0; l < BFs ; l++)
  {
    s = s + w(l)*psi(l,i);
  }

  /* Declare double mat vec fp d dd */
  double fp =  k*s + y0;
  double dfp = 0;
  double ddfp = 0;

  y0d_hat = fp;

  /* needed to get gd_hat */
  s = 0.0 ;
  for (int l = 0; l < BFs ; l++)
  {
    s = s + w(l)*psi(l,NSamples-1);
  }

  gd_hat = k*s + y0;

  // // !! DERIVATE WITH 0 ?
  // if (gd_hat == y0d_hat)
  // {
  // }
  // else
  // {
  //   ks = (goal - y0) / (gd_hat - y0d_hat);
  // }
  ks = 1;

  /* Ref compute*/
  double y_ref  = ks*(fp - y0d_hat) + y0;
  double dy_ref  = ks*dfp ;
  double ddy_ref  = ks*ddfp ;

  /* first time loop i = 0*/

  // y dy ddy: only them are stored
  y(i) = y0;
  dy(i) = 0; //dy0;
  double fs = ddy_ref + D*dy_ref + K*(y_ref-goal);
  ddy(i) = K*(goal-y(i)) - D*dy(i) + fs;

  vec info_back(3);
  info_back(0)  = fp;
  info_back(1)  = dfp;
  info_back(2)  = ddfp;

  return info_back;
}

 vec GDMP::run_solution_dt(double t_now, double goal, double ts, int i, double f_prev, double df_prev, double ddf_prev )
 {
     double fp  ;
     double dfp  ;
     double ddfp ;

     /* Compute fp */
     if (i > (NSamples-1))
     {
         fp = f_prev  ; //same as prev or (NSamples-1)
         dfp = df_prev ;
         ddfp = df_prev;
     }
     else
     {
         double s = 0.0 ;
         for (int l = 0; l < BFs ; l++)
         {
           s = s + w(l)*psi(l,i);
         }
         fp = k*s + y0;
         dfp =  (fp - f_prev)/ts;
         ddfp = (dfp - df_prev)/ts;
     }

     /* Compute refs */
     double y_ref  = ks*(fp - y0d_hat) + y0;
     double dy_ref  = ks*dfp ;
     double ddy_ref  = ks*ddfp ;

     /* Euler Solution dt*/
     dy(i) = ddy(i-1)*ts + dy(i-1);
     y(i) = dy(i)*ts + y(i-1);

     /* dynamical system */
     double fs = ddy_ref + D*dy_ref + K*(y_ref-goal);
     ddy(i) = K*(goal-y(i)) - D*dy(i) + fs;

     /* keep it for next loop dt*/
     vec info_back(3);
     info_back(0)  = fp;
     info_back(1)  = dfp;
     info_back(2)  = ddfp;

     return info_back;
 }

vec GDMP::init_Rsolution_dt(double goal, CanonicalStructure cs, double main_time, int extra_samples, double t_now, int i)
{
  /* Set size = NSamples + extra samples
      extra samples oq from extra time to run
  */
  Ry.set_size(1,NSamples+extra_samples);
  Rdy.set_size(1,NSamples+extra_samples);
  Rddy.set_size(1,NSamples+extra_samples);

  /* Swap gd,y0 */
  double Ry0 = goal;
  double Rgoal = y0;

  /* Swap gd,y0 hats */
  Ry0d_hat = Ry0; //gd_hat;
  Rgd_hat = Rgoal; //y0d_hat;

  /* init Rfp */
  double Rfp = gd_hat;
  double Rdfp = 0;
  double Rddfp = 0;

  /* Ref compute */
  double Ry_ref  = ks*(Rfp - Ry0d_hat) + Ry0;
  double Rdy_ref  = ks*Rdfp ;
  double Rddy_ref  = ks*Rddfp ;

  /* Init R solutions dt */
  Ry(i) = goal;
  Rdy(i) = 0; //Ry0;
  double Rfs = Rddy_ref + D*Rdy_ref + K*(Ry_ref-Rgoal);
  Rddy(i) = K*(Rgoal-Ry(i)) - D*Rdy(i) + Rfs;

/* keep it for the nect loop dt */
 vec info_back(3);
 info_back(0)  = Rfp;
 info_back(1)  = Rdfp;
 info_back(2)  = Rddfp;

 return info_back;
}

vec GDMP::run_Rsolution_dt(double t_now, double goal, double ts, int i, double f_prev, double df_prev, double ddf_prev )
{
    double Rfp  ;
    double Rdfp  ;
    double Rddfp ;
    double Ry0 = goal;
    double Rgoal = y0;

    /* Compute Rfp */
    if (i > (NSamples-1))
    {
        Rfp = f_prev  ; //same as prev or (NSamples-1)
        Rdfp = df_prev ;
        Rddfp = df_prev;
    }
    else
    {
        double s = 0.0 ;
        for (int l = 0; l < BFs ; l++)
        {
          s = s + w(l)*psi(l,NSamples - i );
        }

        Rfp = k*s + y0;
        Rdfp =  (Rfp - f_prev)/ts;
        Rddfp = (Rdfp - df_prev)/ts;
    }

    /* Compute R refs */
    double Ry_ref  = ks*(Rfp - Ry0d_hat) + Ry0;
    double Rdy_ref  = ks*Rdfp ;
    double Rddy_ref  = ks*Rddfp ;

    /* Euler Solution dt*/
    Rdy(i) = Rddy(i-1)*ts + Rdy(i-1);
    Ry(i) = Rdy(i)*ts + Ry(i-1);

    /* Dynamical system dt*/
    double Rfs = Rddy_ref + D*Rdy_ref + K*(Ry_ref-Rgoal);
    Rddy(i) = K*(Rgoal-Ry(i)) - D*Rdy(i) + Rfs;

    /* keep it for the nect loop dt */
    vec info_back(3);
    info_back(0)  = Rfp;
    info_back(1)  = Rdfp;
    info_back(2)  = Rddfp;

    return info_back;
}

/* fNyquist Function*/
double GDMP::fNyquistFunc(double *original, int size, double fs, double T)
{
  int L;
  L = (size % 2) == 0 ? size : size+1;

  vec signal_in(size);
  // cx_vec signal_xf(size);
  //
  // cx_mat XF_in(1,int(l1/2+1));
  // vec A1(size);
  //
  double f[int(L/2)];
  for ( int i = 0; i < (L/2); i++ )
      f[i] = fs*i/L;
  // complex<double> epi;
  // double EdXF = 0;
  // double  img_EdXF = 0;
  for (int i = 0; i<size ; i++)
    signal_in(i) = original[i];

  cx_vec P2(size);
  P2 = fft(signal_in) ;

  cx_vec Fd_original(int(L/2));
  for ( int i = 0; i < (L/2); i++ )
      Fd_original(i) = P2(i);

  double EdD = 0;
  for ( int i = 0; i < (L/2); i++)
  {
    if (f[i] < 40)
      EdD += abs(pow(Fd_original(i),2));
  }

  double h = 0.001;
  double EdDesired = (1-h)*EdD;
  double energy_inter = 0;

  int i = -1;
  while( energy_inter < EdDesired)
  {
    i++;
    energy_inter = energy_inter + abs(pow(Fd_original(i),2));
  }
  // cout<<i<<endl;
  double fNyq ;

  if ( i < 0)
  {
      fNyq = 2*f[1];
  }
  else
  {
      fNyq = 2*f[i];
  }
  return fNyq;
}


#define _USE_MATH_DEFINES
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <memory>
#include <robot_design/graph.h>
#include <robot_design/internal/dot_parsing.h>
#include <robot_design/internal/dot_rules.h>
#include <robot_design/robot.h>
#include <robot_design/types.h>
#include <sstream>
#include <stdexcept>
#include <tao/pegtl.hpp>
#include <vector>

namespace robot_design {

//constexpr Scalar RAD_PER_DEG = M_PI / 180;
    Scalar RAD_PER_DEG = M_PI / 180;

std::vector<Graph> loadGraphs(const std::string &path) {
  tao::pegtl::file_input<> input(path);
  std::vector<Graph> graphs;
  bool success;

  do {
    dot_parsing::State state;
    // Create a root subgraph with default attribute values
    state.subgraph_states_.emplace_back();
    success =
        tao::pegtl::parse<tao::pegtl::pad<dot_rules::graph, dot_rules::sep>,
                          dot_parsing::dot_action>(input, state);
    if (success) {
      graphs.push_back(std::move(state.result_));
    }
  } while (success);

  return graphs;
}

static bool parseDOTBool(const std::string &str) {
  // TODO: add case insensitivity
  if (str == "true" || str == "yes") {
    return true;
  } else if (str == "false" || str == "no") {
    return false;
  } else {
    std::istringstream in(str);
    bool value;
    in >> value;
    return value;
  }
}

void updateNodeAttributes(
    NodeAttributes &node_attrs,
    const std::vector<std::pair<std::string, std::string>> &attr_list) {
  for (const auto &attr : attr_list) {
    const std::string &key = attr.first;
    const std::string &value = attr.second;
    if (key == "label") {
      node_attrs.label_ = value;
    } else if (key == "shape") {
      if (value == "capsule") {
        node_attrs.shape_ = LinkShape::CAPSULE;
      } else if (value == "cylinder") {
        node_attrs.shape_ = LinkShape::CYLINDER;
      } else {
        throw std::runtime_error("Unexpected value \"" + value +
                                 "\" for link_shape");
      }
    } else if (key == "length") {
      std::istringstream in(value);
      in >> node_attrs.length_;
    } else if (key == "radius") {
      std::istringstream in(value);
      in >> node_attrs.radius_;
    } else if (key == "density") {
      std::istringstream in(value);
      in >> node_attrs.density_;
    } else if (key == "friction") {
      std::istringstream in(value);
      in >> node_attrs.friction_;
    } else if (key == "base") {
      node_attrs.base_ = parseDOTBool(value);
    } else if (key == "color") {
      std::istringstream in(value);
      Color &color = node_attrs.color_;
      in >> color(0) >> color(1) >> color(2);
    } else if (key == "require_label") {
      node_attrs.require_label_ = value;
    }
  }
}

void updateEdgeAttributes(
    EdgeAttributes &edge_attrs,
    const std::vector<std::pair<std::string, std::string>> &attr_list) {
  for (const auto &attr : attr_list) {
    const std::string &key = attr.first;
    const std::string &value = attr.second;
    if (key == "id") {
      edge_attrs.id_ = value;
    } else if (key == "label") {
      edge_attrs.label_ = value;
    } else if (key == "type") {
      if (value == "none") {
        edge_attrs.joint_type_ = JointType::NONE;
      } else if (value == "free") {
        edge_attrs.joint_type_ = JointType::FREE;
      } else if (value == "hinge") {
        edge_attrs.joint_type_ = JointType::HINGE;
      } else if (value == "fixed") {
        edge_attrs.joint_type_ = JointType::FIXED;
      } else {
        throw std::runtime_error("Unexpected value \"" + value +
                                 "\" for joint_type");
      }
    } else if (key == "offset") {
      std::istringstream in(value);
      in >> edge_attrs.joint_pos_;
    } else if (key == "axis_angle") {
      std::istringstream in(value);
      Vector3 axis;
      Scalar angle;
      in >> axis(0) >> axis(1) >> axis(2) >> angle;
      edge_attrs.joint_rot_ =
          Eigen::AngleAxis<Scalar>(angle * RAD_PER_DEG, axis);
    } else if (key == "joint_axis") {
      std::istringstream in(value);
      Vector3 &joint_axis = edge_attrs.joint_axis_;
      in >> joint_axis(0) >> joint_axis(1) >> joint_axis(2);
    } else if (key == "kp") {
      std::istringstream in(value);
      in >> edge_attrs.joint_kp_;
    } else if (key == "kd") {
      std::istringstream in(value);
      in >> edge_attrs.joint_kd_;
    } else if (key == "torque") {
      std::istringstream in(value);
      in >> edge_attrs.joint_torque_;
    } else if (key == "limits") {
      std::istringstream in(value);
      Scalar lower_limit, upper_limit;
      in >> lower_limit >> upper_limit;
      edge_attrs.joint_lower_limit_ = lower_limit * RAD_PER_DEG;
      edge_attrs.joint_upper_limit_ = upper_limit * RAD_PER_DEG;
    } else if (key == "control_mode") {
      if (value == "position") {
        edge_attrs.joint_control_mode_ = JointControlMode::POSITION;
      } else if (value == "velocity") {
        edge_attrs.joint_control_mode_ = JointControlMode::VELOCITY;
      } else {
        throw std::runtime_error("Unexpected value \"" + value +
                                 "\" for control_mode");
      }
    } else if (key == "scale") {
      std::istringstream in(value);
      in >> edge_attrs.scale_;
    } else if (key == "mirror") {
      edge_attrs.mirror_ = parseDOTBool(value);
    } else if (key == "color") {
      std::istringstream in(value);
      Color &color = edge_attrs.color_;
      in >> color(0) >> color(1) >> color(2);
    } else if (key == "require_label") {
      edge_attrs.require_label_ = value;
    }
  }
}

std::ostream &operator<<(std::ostream &out, const Node &node) {
  out << "Node{\"" << node.name_ << "\"}";
  return out;
}

std::ostream &operator<<(std::ostream &out, const Edge &edge) {
  out << "Edge{\"" << edge.head_ << "\", \"" << edge.tail_ << "\"}";
  return out;
}

std::ostream &operator<<(std::ostream &out, const Graph &graph) {
  out << "Graph{\"" << graph.name_ << "\", {" << std::endl;
  for (const auto &node : graph.nodes_) {
    out << "    " << node << "," << std::endl;
  }
  out << "}, {" << std::endl;
  for (const auto &edge : graph.edges_) {
    out << "    " << edge << "," << std::endl;
  }
  out << "}}";
  return out;
}

} // namespace robot_design