#include "vo.h"

#include <fstream>
#include <string>
#include <cstdlib>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

namespace global_localization
{

Eigen::Matrix<double,3,1> toVector3d(const cv::Mat &cvVector)
{
    Eigen::Matrix<double,3,1> v;
    v << cvVector.at<double>(0), cvVector.at<double>(1), cvVector.at<double>(2);

    return v;
}

Eigen::Matrix<double,3,3> toMatrix3d(const cv::Mat &cvMat3)
{
    Eigen::Matrix<double,3,3> M;

    M << cvMat3.at<double>(0,0), cvMat3.at<double>(0,1), cvMat3.at<double>(0,2),
    cvMat3.at<double>(1,0), cvMat3.at<double>(1,1), cvMat3.at<double>(1,2),
    cvMat3.at<double>(2,0), cvMat3.at<double>(2,1), cvMat3.at<double>(2,2);

    return M;
}
void VO::setOrign(const std::vector<float>& pose){

    Eigen::Quaternionf q(pose[6], pose[3], pose[4], pose[5]);
//    cout << q.x() << endl << endl;
//    cout << q.y() << endl << endl;
//    cout << q.z() << endl << endl;
//    cout << q.w() << endl << endl;

    Eigen::Matrix3f Rx = q.toRotationMatrix();
//    cout << Rx << endl << endl;

    Two_ = (cv::Mat_<double>(4, 4) <<
                Rx(0,0), Rx(0,1), Rx(0,2), pose[0],
                Rx(1,0), Rx(1,1), Rx(1,2), pose[1],
                Rx(2,0), Rx(2,1), Rx(2,2), pose[2],
                0, 0, 0, 1);
//    cout<<Two_<<endl;
}
VO::VO() : rosMode_(true)
{
    camera_cx_ = 319.5;
    camera_cy_ = 239.5;
    camera_fx_ = 525;
    camera_fy_ = 525;

    K_ = (cv::Mat_<double>(3, 3) <<
                525, 0, 319.5,
                0, 525, 239.5,
                0, 0, 1);

    traj_ = cv::Mat::zeros(600, 600, CV_8UC3);
}
VO::~VO(){
    f_.close();
    std::cout<<"close."<<std::endl;
}
void VO::setColor(const cv::Mat& color){
    color.copyTo(color_);
}

void VO::setDepth(const cv::Mat& depth){
    depth.copyTo(depth_);
}
void VO::init(){
    cout<<"initial VO..."<<endl;

	extractKeyPoints(color_, depth_, mapPoints_, featurePoints_);

    std::string fname = "voPosesBag.txt";
    f_.open(fname.c_str(), std::fstream::out);

	color_.copyTo(refImg_);

    initialize_ = true;
}

cv::Mat VO::estimatePose(double hTime){
    index_++;

	cv::imshow("image", color_);
	cv::imshow("depth", depth_);

	std::vector<double> pose;

	std::vector<cv::Point3f> mapPointsRef;
	std::vector<cv::Point2f> featurePointsRef;

	tracking(refImg_, color_, featurePoints_, mapPoints_, mapPointsRef, featurePointsRef);

	if(mapPointsRef.size() == 0)
	    return cv::Mat();

	cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, CV_64F);
	cv::Mat rvec, tvec;

	vector<int> inliers;
	//3d frame to 2d frame
	cv::solvePnPRansac(mapPointsRef, featurePointsRef, K_, dist_coeffs, rvec, tvec, false, 100, 8.0, 0.99, inliers);

	if(inliers.size() < 5)
	    return cv::Mat();

	float inliers_ratio = inliers.size()/float(mapPointsRef.size());

	cv::Mat R_matrix;
	cv::Rodrigues(rvec, R_matrix);
	R_matrix = R_matrix.t();
	cv::Mat t_vec = -R_matrix*tvec;

	cv::Mat inv_transform = cv::Mat::eye(4,4,CV_64F);
	R_matrix.copyTo(inv_transform.rowRange(0,3).colRange(0,3));
	t_vec.copyTo(inv_transform.rowRange(0,3).col(3));

    createNewPoints(color_, depth_, inv_transform, featurePoints_, mapPoints_);

    //save pose.txt
    std::stringstream strs;
    strs << std::setprecision(6) << std::fixed << hTime << " ";

    cv::Mat Twc = Two_ * inv_transform;

    Eigen::Vector3d trans = toVector3d(Twc.rowRange(0,3).col(3));
    Eigen::Matrix3d rot = toMatrix3d(Twc.rowRange(0,3).colRange(0,3));

    f_ << strs.str() << trans(0) << " " << trans(1) << " " << trans(2) << " ";
    //cout<<index_<<" "<<trans(0) << " " << trans(1) << " " << trans(2) <<endl;

    Eigen::Quaterniond currentCameraRotation(rot);
    f_ << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n";

    for(int i=0; i<3; i++)
        pose.push_back(trans[i]);
    pose.push_back(currentCameraRotation.x());
    pose.push_back(currentCameraRotation.y());
    pose.push_back(currentCameraRotation.z());
    pose.push_back(currentCameraRotation.w());

    //note must depth copy
	color_.copyTo(refImg_);

	// plot the information
	string text  = "Red color: estimated trajectory";

	t_vec.convertTo(t_vec, CV_32F);

	cv::Point2f center = cv::Point2f(int(60*t_vec.at<float>(0)) + 300, int(60*t_vec.at<float>(2)) + 100);

	cv::circle(traj_, center ,1, cv::Scalar(0,0,255), 2);
	cv::rectangle(traj_, cv::Point2f(10, 30), cv::Point2f(550, 50),  cv::Scalar(0,0,0), cv::FILLED);
	putText(traj_, text, cv::Point2f(10,50), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0,255), 1, 5);
	cv::imshow( "Trajectory", traj_);
	cv::waitKey(1);

	return inv_transform;
}
void VO::extractKeyPoints(const cv::Mat& color, const cv::Mat& depth, std::vector<cv::Point3f>&mapPoints, std::vector<cv::Point2f>& featurePoints) {
    // Feature Detection and Extraction
	cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create(350);

	vector<cv::KeyPoint> keypoints;
	detector->detect(color, keypoints);

    for(auto kp:keypoints){
        cv::Point2f p = kp.pt;

        float d;
        if(rosMode_){
            d = depth_.ptr<float>((int)p.y)[(int)p.x];
        }
        else{
            ushort dtemp = depth.ptr<ushort>((int)p.y)[(int)p.x];
            d = double(dtemp)/5000;
        }

        //nan is zero in raw image
        if(d <= 0.00001 || isnan(d))
            continue;

        cv::Point3f p3;
        float z = d;
        float x = (p.x - camera_cx_) * z / camera_fx_;
        float y = (p.y - camera_cy_) * z / camera_fy_;
        //cout<<"3d: "<<x<<" "<<y<<" "<<z<<endl;

        featurePoints.push_back(p);
        mapPoints.push_back(cv::Point3f(x, y, z));
    }
}
void VO::tracking(const cv::Mat& ref_img, const cv::Mat& curImg, const std::vector<cv::Point2f>& featurePoints, const std::vector<cv::Point3f>& mapPoints, std::vector<cv::Point3f>& mapPointsRef, std::vector<cv::Point2f>& featurePointsRef) {

	vector<cv::Point2f> nextPts;
	vector<uchar> status;
	vector<float> err;

	cv::calcOpticalFlowPyrLK(ref_img, curImg, featurePoints, nextPts, status, err);

	for (int  j = 0; j < status.size(); j++) {
		if (status[j] == 1) {
			featurePointsRef.push_back(nextPts[j]);
			mapPointsRef.push_back(mapPoints[j]);
		}
	}
}
void VO::createNewPoints(const cv::Mat& color, const cv::Mat& depth, const cv::Mat& inv_transform, std::vector<cv::Point2f>& featurePoints, std::vector<cv::Point3f>& mapPoints){

	if(featurePoints.size() != 0){
		featurePoints.clear();
		mapPoints.clear();
	}

	vector<cv::Point3f>  mapPointsNew;
	vector<cv::Point2f>  featurePointsNew;

	extractKeyPoints(color, depth, mapPointsNew, featurePointsNew);

	// // cout << inv_transform << endl;

   for (int k = 0; k < mapPointsNew.size(); k++) {
//
	   	const cv::Point3f& pt = mapPointsNew[k];

	   	cv::Point3f p;

	   	p.x = inv_transform.at<double>(0, 0)*pt.x + inv_transform.at<double>(0, 1)*pt.y + inv_transform.at<double>(0, 2)*pt.z + inv_transform.at<double>(0, 3);
	   	p.y = inv_transform.at<double>(1, 0)*pt.x + inv_transform.at<double>(1, 1)*pt.y + inv_transform.at<double>(1, 2)*pt.z + inv_transform.at<double>(1, 3);
	   	p.z = inv_transform.at<double>(2, 0)*pt.x + inv_transform.at<double>(2, 1)*pt.y + inv_transform.at<double>(2, 2)*pt.z + inv_transform.at<double>(2, 3);

	   	// cout << p << endl;
	   	if (p.z > 0) {
			mapPoints.push_back(p);
			featurePoints.push_back(featurePointsNew[k]);
	   	}

   }
}
void VO::playSequence(const std::vector<std::string>& inputRGBPaths,
                      const std::vector<std::string>& inputDepthPaths,
                      const std::vector<std::string>& inputTimeStamps) {

    cv::Mat color = cv::imread(inputRGBPaths[0]);
    cv::Mat depth = cv::imread(inputDepthPaths[0], -1);

	cv::Mat& ref_img  = color;

	vector<cv::Point3f> mapPoints;
	vector<cv::Point2f> featurePoints;

	extractKeyPoints(color, depth, mapPoints, featurePoints);

	cv::Mat curImg;
	cv::Mat traj = cv::Mat::zeros(600, 600, CV_8UC3);

    std::string fname = "voPoses.txt";
    std::ofstream f;
    f.open(fname.c_str(), std::fstream::out);
	for(int i=1; i<inputRGBPaths.size(); i++){

		curImg = cv::imread(inputRGBPaths[i]);
		cv::imshow( "image", curImg);

		std::vector<cv::Point3f> mapPointsRef;
		std::vector<cv::Point2f> featurePointsRef;

		tracking(ref_img, curImg, featurePoints, mapPoints, mapPointsRef, featurePointsRef);

		if(mapPointsRef.size() == 0)
		    continue;

		cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, CV_64F);
		cv::Mat rvec, tvec;

		vector<int> inliers;
		//3d frame to 2d frame
 		cv::solvePnPRansac(mapPointsRef, featurePointsRef, K_, dist_coeffs,rvec, tvec, false, 100, 8.0, 0.99, inliers);

		if(inliers.size() < 5)
		    continue;

		float inliers_ratio = inliers.size()/float(mapPointsRef.size());

		//cout << "inliers ratio: " << inliers_ratio << endl;

		cv::Mat R_matrix;
		cv::Rodrigues(rvec,R_matrix);
		R_matrix = R_matrix.t();
		cv::Mat t_vec = -R_matrix*tvec;

		cv::Mat inv_transform = cv::Mat::eye(4,4,CV_64F);
		R_matrix.copyTo(inv_transform.rowRange(0,3).colRange(0,3));
		t_vec.copyTo(inv_transform.rowRange(0,3).col(3));

        cv::Mat depth = cv::imread(inputDepthPaths[i], -1);
	    createNewPoints(curImg, depth, inv_transform, featurePoints, mapPoints);

        //save pose.txt
        std::stringstream strs;
        strs << std::setprecision(6) << std::fixed << inputTimeStamps[i] << " ";

        cv::Mat Twc = Two_ * inv_transform;

        Eigen::Vector3d trans = toVector3d(Twc.rowRange(0,3).col(3));
        Eigen::Matrix3d rot = toMatrix3d(Twc.rowRange(0,3).colRange(0,3));

        f << strs.str() << trans(0) << " " << trans(1) << " " << trans(2) << " ";

        Eigen::Quaterniond currentCameraRotation(rot);
        f << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n";

		ref_img = curImg;

		// plot the information
		string text  = "Red color: estimated trajectory";

		t_vec.convertTo(t_vec, CV_32F);
		//cout << t_vec.t() << endl;
        //cout<<t_vec.at<float>(0)<<" "<<t_vec.at<float>(1)<<" "<<t_vec.at<float>(2)<<endl;

		cv::Point2f center = cv::Point2f(int(60*t_vec.at<float>(0)) + 300, int(60*t_vec.at<float>(2)) + 100);

		cv::circle(traj, center ,1, cv::Scalar(0,0,255), 2);
		cv::rectangle(traj, cv::Point2f(10, 30), cv::Point2f(550, 50),  cv::Scalar(0,0,0), cv::FILLED);
		putText(traj, text, cv::Point2f(10,50), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0,255), 1, 5);
		cv::imshow( "Trajectory", traj);
		cv::waitKey(1);

	}
	f.close();

}

}


#include <stdlib.h> /* srand, rand */
#include <iostream>
#include <fstream> // creating (.csv) files
#include <vector>
#include <cstring>
#include <string>
#include <functional>
#include <armadillo> // http://arma.sourceforge.net/docs.html
#include <thread>    /* std::this_thread::sleep_for */

#include "EvolutionaryAlgorithm.hpp"

#include "mypch.hpp"

#include "EAController.hpp"

// NOT USED??
// enum events{
//     BASE_RATE = 0,
//     MUTA_RATE,
//     PRED_RATE,
//     PART_RATE,
//     GENO_RATE,
//     EV_A_RATE,
// };

namespace EvoAlg
{
    //arma::rowvec semiFinalsAndFinals[SEMI_FINALS][TOURNMENT_RATE];
    //arma::rowvec indvFinals[TOURNMENT_RATE];
    void EvolutionaryAlgorithm::saveBestIndvParamsCSV()
    {
        std::fstream myFile;
        myFile.open("constValues.cfg", std::ios_base::app);

        myFile << attributesValues[0].name << " = " << population(bestFitIndex, 0);
        for (size_t i = 1; i < NB_PARAMETERS; i++)
            myFile << std::endl
                   << attributesValues[i].name << " = " << population(bestFitIndex, i);
        //printf("k%sk\n", parametersList[i]);
        myFile.close();
    }

    // 1st step: initialize population
    void EvolutionaryAlgorithm::initializePop(int tournmentType)
    {
        double minVal, maxVal;
        std::cout << "INITIALIZING POPULATION\n";
        // for (int i = 0; i < POPULATION_SIZE; i++) {
        //     for (int j = 0; j < NB_PARAMETERS; j++) {
        //         population[i][j] = (double) (rand() % MAX_PARAM_VALUE); // number range = [0, MAX_PARAM_VALUE[
        //     }
        // }
        population.randu(); // initialize with values between 0 and 1

        //TODO: find a better way to pass a non-variable to lambda
        // auto &_attributesValues = attributesValues;
        if (tournmentType == INITIALS) {
            int index = 0;
            population.each_col([=, &index](arma::vec& popCol) { popCol = attributesValues[index].min + popCol * (attributesValues[index].max - attributesValues[index].min); index++; } );
            // for(int j = 0; j < NB_PARAMETERS; j++)
            //     population.col(j) = attributesValues[j].min + population.col(j)*(attributesValues[j].max - attributesValues[j].min); // if a normal EA is taking place, just multiply the population by a givern value
        }
        else
        {
            int middle = TOURNMENT_RATE + (POPULATION_SIZE - TOURNMENT_RATE) / 2;
            fillInitialsWithBests(tournmentType);
            for (int j = 0; j < NB_PARAMETERS; j++)
            { // Min and max values are defined for each col, and converting the current col values to a number between this constraints.
                // this starts after the first TOURNMENT_RATE individuls, and the remaining is divided into two pars, one with biased rand values, and other without
                minVal = MIN_MULT * bestTournmentIndv[tournmentType].col(j).min();
                maxVal = MAX_MULT * bestTournmentIndv[tournmentType].col(j).max();

                for (int i = TOURNMENT_RATE; i < middle; i++)
                {
                    population(i, j) = minVal + (maxVal - minVal) * (population(i, j));
                }
            }
            for (int i = middle; i < POPULATION_SIZE; i++)
                for (int j = 0; j< NB_PARAMETERS; i++)
                    population(i,j) = attributesValues[j].min + population(i,j)*(attributesValues[j].max - attributesValues[j].min);
        }

        population.print("Population matrix initialized:");

        return;
    }

    // Function to normalize fitness (values between 0 and 1)
    void EvolutionaryAlgorithm::normalizeFitness(double *normalizedFitness)
    {
        if (normalizedFitness == NULL)
            return;

        for (int i = 0; i < POPULATION_SIZE; i++)
            normalizedFitness[i] = (fitness[i] - bestFitness) / (worstFitness - bestFitness);

        return;
    }

    // 2nd step: evaluate population (calculate fitness)
    void EvolutionaryAlgorithm::evaluatePop()
    {
        //Limit the values in each col
        // for(int j = 0; j < NB_PARAMETERS; j++){
        //     population.col(j) = arma::clamp(population.col(j),attributesValues[j].min,attributesValues[j].max);
        // }
        int index = 0;

        //TODO: find a better way to pass a non-variable to lambda
        auto &_attributesValues = attributesValues;
        population.each_col([&_attributesValues, &index](arma::vec& popCol) { popCol = arma::clamp(popCol, _attributesValues[index].min, _attributesValues[index].max); index++; } );

        std::cout << "EVALUATING POPULATION\n";

        nbNoImprovementGens++; // we begin considering there was no improvement in the generation

        std::vector<std::vector<double>> populationVec2(population.n_rows);
        for (size_t i = 0; i < population.n_rows; ++i)
            populationVec2[i] = (arma::conv_to<std::vector<double>>::from(population.row(i)));

        DE_TRACE("(EvolutionaryAlgorithm) Sending population to EAController to execute it");
        auto gameplayResults = m_EAController.RunPopulationInGame(populationVec2);
        DE_TRACE("(EvolutionaryAlgorithm) Population received from EAController");

        DE_TRACE("Calculating fitness of population after gameplay results received");
        calcFitness(gameplayResults);

        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            DE_ASSERT(fitness[i] >= 0);
            if (fitness[i] < bestFitness)
            { // searching for the  max fitness from new generation
                bestFitness = fitness[i];
                bestFitIndex = i;
                nbNoImprovementGens = 0; // this generation shows improvement -> reset counter
            }
            else if (fitness[i] > worstFitness)
            {
                worstFitness = fitness[i];
                worstFitIndex = i;
            }

            // printf("fitness[%d] %lf\n", i, fitness[i]);
        }

        printf("BEST FITNESS: %lf - INDEX: %d\n", bestFitness, bestFitIndex);
        printf("WORST FITNESS: %lf - INDEX: %d\n", worstFitness, worstFitIndex);

        return;
    }

    void EvolutionaryAlgorithm::crossover(int indv, arma::rowvec parent1, arma::rowvec parent2)
    {
        // for(int j = 0; j < NB_PARAMETERS; j++){
        //     population[i][j] = (parent1[j] + parent2[j])/2.0;
        // }
        population.row(indv) = (parent1 + parent2) / 2.0;

        return;
    }

    void EvolutionaryAlgorithm::elitism()
    {
        std::cout << "ELITISM\n";
        arma::rowvec bestIndv = population.row(bestFitIndex);

        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            // crossover
            crossover(i, population.row(i), bestIndv);
        }

        return;
    }

    void EvolutionaryAlgorithm::tournament()
    {
        std::cout << "TOURNAMENT\n";
        arma::mat::fixed<POPULATION_SIZE, NB_PARAMETERS> oldPopulation;
        int parentIndex[2];

        // copying last population (new one will be different)
        // for (int i = 0; i < POPULATION_SIZE; i++) {
        //     for (int j = 0; j < NB_PARAMETERS; j++) {
        //         oldPopulation[i][j] = population[i][j];
        //     }
        // }
        oldPopulation = population;

        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            if (i == bestFitIndex)
                continue;

            // chossing parents for new individual
            for (int j = 0; j < 2; j++)
            {
                int indexIndA = rand() % POPULATION_SIZE; // indv 1 that will "fight" to be parent
                int indexIndB = rand() % POPULATION_SIZE; // indv 2 that will "fight" to be parent

                parentIndex[j] = (fitness[indexIndA] < fitness[indexIndB] ? indexIndA : indexIndB);
            }

            // crossover
            crossover(i, oldPopulation.row(parentIndex[0]), oldPopulation.row(parentIndex[1]));
        }

        return;
    }

    //TODO: swap priorities. Smaller fitness must be morre relevant
    void EvolutionaryAlgorithm::roulette()
    {
        std::cout << "ROULETTE\n";
        arma::mat::fixed<POPULATION_SIZE, NB_PARAMETERS> oldPopulation;
        double standardizedFitness[POPULATION_SIZE];
        int parentIndex[2], rNb;
        double probSum = 0.0, partialSum = 0.0;

        (void) standardizedFitness[0];

        // copying last population (new one will be different)
        // for (int i = 0; i < POPULATION_SIZE; i++) {
        //     for (int j = 0; j < NB_PARAMETERS; j++) {
        //         oldPopulation[i][j] = population[i][j];
        //     }
        // }
        oldPopulation = population;

        // Standardize fitness (set probabilites that add up to 100%)
        for (int i = 0; i < POPULATION_SIZE; i++)
            probSum += fitness[i];
        for (int i = 0; i < POPULATION_SIZE; i++)
            standardizedFitness[i] = fitness[i] / probSum;

        // Chosing new parents for each individual
        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            if (i == bestFitIndex) // preserves best individual
                continue;

            for (int k = 0; k < 2; k++)
            {                                      // chosing 2 parents
                rNb = ((double)rand() / RAND_MAX); // rand between 0 and 1
                partialSum = 0.0;

                for (int j = 0; j < POPULATION_SIZE; j++)
                { // randomly chosing and individual according to its fitness (+fitness = +probabity)
                    partialSum += fitness[j];
                    if (partialSum >= rNb)
                    {
                        parentIndex[k] = j; // new parent at index j
                        break;
                    }
                }
            }

            // crossover
            crossover(i, oldPopulation.row(parentIndex[0]), oldPopulation.row(parentIndex[1]));
        }
    }

    void EvolutionaryAlgorithm::mutate(int indIndex)
    {
        int plusMinusSign = (rand() % 2 ? MUTATE_POSITIVE_MULT : MUTATE_NEGATIVE_MULT); // mutation will increase the param value or decrease it
        int mutateParamIndex = rand() % NB_PARAMETERS;                                  // index of parameter that will be mutated

        population(indIndex, mutateParamIndex) += population(indIndex, mutateParamIndex) * mutationRate * plusMinusSign;

        return;
    }

    // 3rd step: selection + mutation and crossover
    void EvolutionaryAlgorithm::selectionAndMutation()
    { // tournament, elitism, roulette...
        std::cout << "SELECTION\n";

        // Selection method (+ crossover)
        (this->*(selectionType[selectionMethod]))(); // void (*selectionType[])() => tournament(), elitism() or roulette()

        // Mutation

        // Mutating all params from individuals
        // arma::rowvec bestIndv = population.row(maxFitIndex);

        // arma::mat mutateMatrix(POPULATION_SIZE, NB_PARAMETERS, arma::fill::randu);
        // mutateMatrix = ((mutateMatrix * MAX_PARAM_VALUE) - MAX_PARAM_VALUE/2.0) * mutationRate;

        // population = population + mutateMatrix;

        // population.row(maxFitIndex) = bestIndv;
        // population.transform( [](double x) { return ((x < 0 || x > MAX_PARAM_VALUE) ? abs(MAX_PARAM_VALUE-abs(x)) : x); } );

        // Mutating only one parameter from each individual
        for (int i = 0; i < bestFitIndex; i++)
            mutate(i);
        // (don't mutate best one)
        for (int i = bestFitIndex + 1; i < POPULATION_SIZE; i++)
            mutate(i);

        return;
    }

    // Initialize .csv file to sabe data from the EA
    // Creates the column's headers
    // generation, paramsIndv1, fitnessIndv1 ..., paramsIndvN, fitnessIndvN, bestParams, bestFitness
    void EvolutionaryAlgorithm::createCSV(std::string pathPos)
    {
        std::ofstream csvFileWriter;

        csvFileWriter.open("historyEA-" + pathPos + ".csv");
        if (!csvFileWriter.good())
        {
            std::cout << "[!] Error occurred while trying to create historyEA" << pathPos << ".csv!\n";
            return;
        }

        csvFileWriter << "generation,";
        for (int i = 0; i < POPULATION_SIZE; i++)
            csvFileWriter << "paramsIndv" << i << ",fitnessIndv" << i << ",";
        csvFileWriter << "paramsBestIndv,fitnessBestIndv\n";

        return;
    }

    std::string EvolutionaryAlgorithm::formatParamsString(int indvIndex)
    {
        std::string paramsFormated = "[ ";
        for (int i = 0; i < NB_PARAMETERS; i++)
        {
            paramsFormated += std::to_string(population(indvIndex, i));
            paramsFormated += " ";
        }
        paramsFormated += "]";

        return paramsFormated;
    }

    // Saves information about a generation in a .csv file
    // Information: generation, paramsIndv1, fitnessIndv1 ..., paramsIndvN, fitnessIndvN, bestParams, bestFitness
    void EvolutionaryAlgorithm::saveGenerationData(int generation, const std::string& pathPos)
    {
        std::ofstream csvFileWriter;

        csvFileWriter.open("historyEA-" + pathPos + ".csv", std::ios_base::app); // append instead of overwrite
        if (!csvFileWriter.good())
        {
            std::cout << "[!] Error occurred while trying to open historyEA.csv!\n";
            return;
        }

        csvFileWriter << generation << ",";
        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            csvFileWriter << formatParamsString(i) << "," << fitness[i] << ",";
        }
        csvFileWriter << formatParamsString(bestFitIndex) << "," << bestFitness << "\n";
        csvFileWriter.close();
    }

    void EvolutionaryAlgorithm::increaseMutation()
    {
        if (mutationRate < 1e-3 && mutationRate > -1e-3)
        {
            mutationRate = INITIAL_MUTATION;
        }
        else
        {
            mutationRate *= MUTATION_INCREASE_RATIO;

            if (mutationRate > MAX_MUTATION_RATE) // mutation reached its max value
                mutationRate = MAX_MUTATION_RATE;
        }

        return;
    }

    // Function that will kill the worst individual every "APPLY_PREDATION_INTERVAL" number of generations
    void EvolutionaryAlgorithm::predationOfOne()
    {
        arma::rowvec newInd(NB_PARAMETERS, arma::fill::randu); // creating totally new individual
        // for(int j = 0; j < NB_PARAMETERS; j++){
        //     newInd(j) = attributesValues[j].min + newInd(j)*(attributesValues[j].max - attributesValues[j].min);
        // }
        int index = 0;
        //TODO: find a better way to pass a non-variable to lambda
        auto &_attributesValues = attributesValues;
        newInd.for_each([&_attributesValues, &index](double &paramNewInd) { paramNewInd = _attributesValues[index].min + paramNewInd * (_attributesValues[index].max - _attributesValues[index].min); index++; } );

        population.row(worstFitIndex) = newInd;

        return;
    }

    // A sequence of selections will run, this method will indicate the selection method that will be used by the next batch of generations
    void EvolutionaryAlgorithm::partIncrease()
    {
        const static int selectionMethodCount = sizeof(partsSelectionMethods)/sizeof(selectionMethods);
        partPos++;
        if (partPos > selectionMethodCount)
            partPos = selectionMethodCount;
        selectionMethod = partsSelectionMethods[partPos];

        return;
    }

    // Function that will kill all individuals but the best to reset the population and generate new individuals (without biases)
    void EvolutionaryAlgorithm::oneRemainingPopReset()
    {
        std::cout << "APPLYING POPULATION RESET\n";

        arma::rowvec best = population.row(bestFitIndex);

        initializePop(INITIALS);
        population.row(0) = best;
        partIncrease();

        return;
    }

    // Ends the batch of generations from current EA
    void EvolutionaryAlgorithm::endEABatch()
    {
        std::cout << "END EA BATCH";

        continueBatch = false;

        return;
    }

    // Generic function that checks if certain event should happen (predation, population reset, mutation increase...)
    bool EvolutionaryAlgorithm::eventHappens(int eventType)
    {
        if (nbNoImprovementGens % eventTriggerModule[eventType] == 0)
            return true; // the event being verified should happen!
        return false;
    }

    void EvolutionaryAlgorithm::checkEvents()
    {
        if (nbNoImprovementGens == 0)
            return;

        for (int i = 4; i >= 0; i--)
        {
            if (eventHappens(i + 1))
            {
                (this->*(eventTypes[i]))(); // calls one of these functions: increaseMutation, predationOfOne, partIncrease, oneRemainingPopReset, fullPopReset
                return;
            }
        }
    }

    void EvolutionaryAlgorithm::startEventTriggerList()
    {
        for (int i = 1; i < 6; i++)
            eventTriggerModule[i] *= eventTriggerModule[i - 1];
    }

    void EvolutionaryAlgorithm::evoAlg(int createPopType, const std::string& csvStr)
    {
        initializePop(createPopType);
        nbNoImprovementGens = 0;
        createCSV(csvStr);
        continueBatch = true;
        partPos = 0; // defines selection method of current batch of generations

        generationIndex = 0;
        while (continueBatch)
        {
            std::cout << "\n==== Generation " << generationIndex << " ====\n";
            // population.print("Current population:");
            evaluatePop();
            selectionAndMutation();

            saveGenerationData(generationIndex, csvStr);

            checkEvents(); // checks if mutation should increase, predation or population reset should occur etc.
            // if fullPopReset() is called, continueEA = false

            generationIndex++;
        }

        return;
    }

    void EvolutionaryAlgorithm::fillInitialsWithBests(int tournmentType)
    {
        for (int i = 0; i < TOURNMENT_RATE; i++)
            population.row(i) = bestTournmentIndv[tournmentType].row(i);
    }

    void EvolutionaryAlgorithm::semiFinalsTournment(int semiFinalPos)
    {
        for (int i = 0; i < TOURNMENT_RATE; i++)
        {
            evoAlg(INITIALS,"SF-" + std::to_string(semiFinalPos) + "_EA-" + std::to_string(i));
            bestTournmentIndv[SEMI_FINALS].row(i) = population.row(bestFitIndex); // saves the best individual from current EA
        }
        evoAlg(SEMI_FINALS,"SF-" + std::to_string(semiFinalPos));                               // this EA will use the best individuals from each previous EA
        bestTournmentIndv[FINALS].row(semiFinalPos) = population.row(bestFitIndex); // saves the best individual from current EA
        return;
    }

    void EvolutionaryAlgorithm::finalTournment()
    {
        for (int i = 0; i < TOURNMENT_RATE; i++)
            semiFinalsTournment(i);

        evoAlg(FINALS,"FINAL"); // this EA will use the best individuals from each semifinal

        // bestIndividual = population.row(bestFitIndex); not used, but I'm too afraid to delete
        std::cout << "EVOLUTIONARY ALGORITHM FINISHED!" << std::endl;
        //eaFinished = true;
        return;
    }

    EvolutionaryAlgorithm::EvolutionaryAlgorithm(EAController &eaController) : m_EAController(eaController), eaFinished(false)
    {
        srand(time(NULL));
        startEventTriggerList();
    }

    static void *runScript(void *scriptFuncObject)
    {
        EvolutionaryAlgorithm *script = (EvolutionaryAlgorithm *)scriptFuncObject;
        script->finalTournment();
        return NULL;
    }

    void EvolutionaryAlgorithm::startAlgorithm()
    {
        DE_ASSERT(scriptThread == nullptr);
        scriptThread = new std::thread(runScript, this);
    }

    EvolutionaryAlgorithm::~EvolutionaryAlgorithm()
    {
        if (scriptThread != nullptr && scriptThread->joinable())
            scriptThread->join();
        delete scriptThread;
    }

    void EvolutionaryAlgorithm::calcFitness(const std::vector<IndividualRunResult> &gameplayResultsPop)
    {
        int pos = 0;
        for (auto &indivResults : gameplayResultsPop)
        {
            fitness[pos] = 0;
            for (auto &timeResult : indivResults)
            {
                fitness[pos] += std::abs(timeResult.TargetTime - timeResult.MeasuredTime) / timeResult.TargetTime;
            }
            pos++;
        }
        return;
    }

} // namespace EvoAlg


///////////////////////////////////////////////////////////////////////////////
// lost_df.hpp                                                               //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_LOST_DF_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_LOST_DF_HPP_ER_2010
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/range.hpp>
#include <boost/accumulators/framework/depends_on.hpp> //contains_feature_of
#include <boost/accumulators/framework/accumulator_set.hpp> //visit_if
#include <boost/statistics/detail/non_parametric/contingency_table/factor/vec_levels.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/tag.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chisq_statistic{
namespace independence_between_aux{

    template<typename A>
    struct lost_df_accumulator{

        lost_df_accumulator(const A& acc,std::size_t& df)
            :acc_(acc),df_(df){}

        template<typename Key>
        void operator()(const boost::mpl::detail::wrapper<Key>& wrapper)const{
            namespace ns = contingency_table::extract;
            this->df_ += (ns::levels<Key>( this->acc_ ).size() - 1);
        }

        private:
        const A& acc_;
        mutable std::size_t& df_;
    };

    template<typename A>
    independence_between_aux::lost_df_accumulator<A>
    make_lost_df_accumulator(const A& acc,std::size_t& df){
        return independence_between_aux::lost_df_accumulator<A>(acc,df);
    }

}// independence_between_aux

  	template<typename Keys,typename AccSet>
    std::size_t lost_degrees_of_freedom(
        const boost::mpl::detail::wrapper<
            pearson_chisq_statistic::tag::independence_between<Keys>
        >& statistic,
        const AccSet& acc
    )
    {
        typedef boost::mpl::detail::wrapper<boost::mpl::_> op_;
        std::size_t result = 0;
        boost::mpl::for_each<Keys,op_>(
            independence_between_aux::make_lost_df_accumulator(acc, result)
        );
        return result;
  	}

}// pearson_chisq_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


/**
 * @file concat.hpp
 * @author Marcus Edel
 *
 * Definition of the Concat class, which acts as a concatenation contain.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_LAYER_CONCAT_HPP
#define MLPACK_METHODS_ANN_LAYER_CONCAT_HPP

#include <mlpack/prereqs.hpp>

#include "../visitor/delete_visitor.hpp"
#include "../visitor/delta_visitor.hpp"
#include "../visitor/output_parameter_visitor.hpp"

#include <boost/ptr_container/ptr_vector.hpp>

#include "layer_types.hpp"

namespace mlpack {
namespace ann /** Artificial Neural Network. */ {

/**
 * Implementation of the Concat class. The Concat class works as a
 * feed-forward fully connected network container which plugs various layers
 * together.
 *
 * @tparam InputDataType Type of the input data (arma::colvec, arma::mat,
 *         arma::sp_mat or arma::cube).
 * @tparam OutputDataType Type of the output data (arma::colvec, arma::mat,
 *         arma::sp_mat or arma::cube).
 */
template <
    typename InputDataType = arma::mat,
    typename OutputDataType = arma::mat
>
class Concat
{
 public:
  /**
   * Create the Concat object using the specified parameters.
   *
   * @param model Expose all network modules.
   * @param same Merge the error in the backward pass.
   */
  Concat(const bool model = true, const bool same = true);

  /**
   * Ordinary feed forward pass of a neural network, evaluating the function
   * f(x) by propagating the activity forward through f.
   *
   * @param input Input data used for evaluating the specified function.
   * @param output Resulting output activation.
   */
  template<typename eT>
  void Forward(arma::Mat<eT>&& input, arma::Mat<eT>&& output);

  /**
   * Ordinary feed backward pass of a neural network, using 3rd-order tensors as
   * input, calculating the function f(x) by propagating x backwards through f.
   * Using the results from the feed forward pass.
   *
   * @param input The propagated input activation.
   * @param gy The backpropagated error.
   * @param g The calculated gradient.
   */
  template<typename eT>
  void Backward(const arma::Mat<eT>&& /* input */,
                arma::Mat<eT>&& gy,
                arma::Mat<eT>&& g);

  /*
   * Calculate the gradient using the output delta and the input activation.
   *
   * @param input The input parameter used for calculating the gradient.
   * @param error The calculated error.
   * @param gradient The calculated gradient.
   */
  template<typename eT>
  void Gradient(arma::Mat<eT>&& /* input */,
                arma::Mat<eT>&& error,
                arma::Mat<eT>&& /* gradient */);

  /*
   * Add a new module to the model.
   *
   * @param layer The Layer to be added to the model.
   */
  template<typename LayerType>
  void Add(const LayerType& layer) { network.push_back(new LayerType(layer)); }

  /*
   * Add a new module to the model.
   *
   * @param args The layer parameter.
   */
  template <class LayerType, class... Args>
  void Add(Args... args) { network.push_back(new LayerType(args...)); }

  /*
   * Add a new module to the model.
   *
   * @param layer The Layer to be added to the model.
   */
  void Add(LayerTypes layer) { network.push_back(layer); }

  //! Return the model modules.
  std::vector<LayerTypes>& Model()
  {
    if (model)
    {
      return network;
    }

    return empty;
  }

  //! Return the initial point for the optimization.
  const arma::mat& Parameters() const { return parameters; }
  //! Modify the initial point for the optimization.
  arma::mat& Parameters() { return parameters; }

  arma::mat const& InputParameter() const { return inputParameter; }
  //! Modify the input parameter.
  arma::mat& InputParameter() { return inputParameter; }

  //! Get the output parameter.
  arma::mat const& OutputParameter() const { return outputParameter; }
  //! Modify the output parameter.
  arma::mat& OutputParameter() { return outputParameter; }

  //! Get the delta.e
  arma::mat const& Delta() const { return delta; }
  //! Modify the delta.
  arma::mat& Delta() { return delta; }

  //! Get the gradient.
  arma::mat const& Gradient() const { return gradient; }
  //! Modify the gradient.
  arma::mat& Gradient() { return gradient; }

  /**
   * Serialize the layer
   */
  template<typename Archive>
  void Serialize(Archive& /* ar */, const unsigned int /* version */);

 private:
  //! Parameter which indicates if the modules should be exposed.
  bool model;

  //! If true merge the error in the backward pass.
  bool same;

  //! Locally-stored network modules.
  std::vector<LayerTypes> network;

  //! Locally-stored model parameters.
  arma::mat parameters;

  //! Locally-stored delta visitor.
  DeltaVisitor deltaVisitor;

  //! Locally-stored output parameter visitor.
  OutputParameterVisitor outputParameterVisitor;

  //! Locally-stored delete visitor.
  DeleteVisitor deleteVisitor;

  //! Locally-stored empty list of modules.
  std::vector<LayerTypes> empty;

  //! Locally-stored delta object.
  arma::mat delta;

  //! Locally-stored input parameter object.
  arma::mat inputParameter;

  //! Locally-stored output parameter object.
  arma::mat outputParameter;

  //! Locally-stored gradient object.
  arma::mat gradient;
}; // class Concat

} // namespace ann
} // namespace mlpack

// Include implementation.
#include "concat_impl.hpp"

#endif


/**  \file randgen.hpp
    \brief Boost types for random number generation */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

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

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

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

#ifndef  _RANDGEN_HPP_
#define  _RANDGEN_HPP_

//#include <boost/version.hpp>
#include <boost/random.hpp>

// Types for pseudorandom number generators.

typedef boost::mt19937                                              randEngine;

typedef boost::uniform_real<>				       realUniformDist;
typedef boost::uniform_int<> 					intUniformDist;
typedef boost::normal_distribution<>                                normalDist;
typedef boost::gamma_distribution<>                                  gammaDist;

typedef boost::variate_generator<randEngine&, intUniformDist>          randInt;
typedef boost::variate_generator<randEngine&, normalDist>           randNFloat;
typedef boost::variate_generator<randEngine&, gammaDist>            randGFloat;
typedef boost::variate_generator<randEngine&, realUniformDist>       randFloat;

#endif


// -*- C++ -*-
//
// Jiao Lin <jiao.lin@gmail.com>
//

#include <sstream>
#include <boost/python.hpp>
#include "mccomponents/kernels/sample/phonon/AbstractDispersion_3D.h"
#include "histogram/NdArray.h"

namespace wrap_mccomponents {

  // helper functions
  typedef DANSE::Histogram::NdArray<double *, double, unsigned int, size_t, 2> array_2d_t;
  typedef DANSE::Histogram::NdArray<double *, double, unsigned int, size_t, 4> array_4d_t;
  // Qarr: NQX3 array
  // Earr: NQXnbranches array
  void disp_energies(DANSE::phonon::AbstractDispersion_3D &disp, const array_2d_t &Qarr, array_2d_t &Earr)
  {
    typedef mcni::Vector3<double> V3;
    size_t nbr = disp.nBranches();
    size_t nQ = Qarr.shape()[0];
    assert (Qarr.shape()[1]==3);
    array_2d_t::index_t xind[2], yind[2], zind[2];
    xind[1]=0; yind[1]=1; zind[1]=2;
    array_2d_t::index_t Eind[2];
    for (int iQ=0; iQ<nQ; iQ++) {
      xind[0]=yind[0]=zind[0]=Eind[0] = iQ;
      V3 Q(Qarr[xind], Qarr[yind], Qarr[zind]);
      for (int ibr=0; ibr<nbr; ibr++) {
	Eind[1] = ibr;
	Earr[Eind] = disp.energy(ibr, Q);
      }
    }
  }
  // Qarr: NQX3 array
  // realpolarr, imagpolarr: NQXnbranchesXnatomsX3 array
  void disp_pols
  (DANSE::phonon::AbstractDispersion_3D &disp, const array_2d_t &Qarr, array_4d_t &realpolarr, array_4d_t &imagpolarr)
  {
    typedef mcni::Vector3<double> V3;
    size_t nbr = disp.nBranches();
    size_t natom = disp.nAtoms();
    size_t nQ = Qarr.shape()[0];
    assert (Qarr.shape()[1]==3);
    array_2d_t::index_t xind[2], yind[2], zind[2];
    xind[1]=0; yind[1]=1; zind[1]=2;
    array_4d_t::index_t xpolind[4], ypolind[4], zpolind[4];
    xpolind[3]=0; ypolind[3]=1; zpolind[3]=2;
    DANSE::phonon::AbstractDispersion_3D::epsilon_t pol;
    for (int iQ=0; iQ<nQ; iQ++) {
      xind[0]=yind[0]=zind[0]=xpolind[0] = ypolind[0] = zpolind[0] = iQ;
      V3 Q(Qarr[xind], Qarr[yind], Qarr[zind]);
      for (int ibr=0; ibr<nbr; ibr++) {
	xpolind[1] = ypolind[1] = zpolind[1] = ibr;
	for (int iatom=0; iatom<natom; iatom++) {
	  xpolind[2] = ypolind[2] = zpolind[2] = iatom;
	  pol = disp.polarization(ibr, iatom, Q);
	  realpolarr[xpolind] = pol[0].real(); imagpolarr[xpolind] = pol[0].imag();
	  realpolarr[ypolind] = pol[1].real(); imagpolarr[ypolind] = pol[1].imag();
	  realpolarr[zpolind] = pol[2].real(); imagpolarr[zpolind] = pol[2].imag();
	}
      }
    }
  }

  void wrap_AbstractDispersion_3D()
  {
    using namespace boost::python;
    typedef DANSE::phonon::AbstractDispersion_3D w_t;

    class_<w_t, boost::noncopyable>
      ("AbstractDispersion_3D", no_init)
      .def("nBranches", &w_t::nBranches )
      .def("nAtoms", &w_t::nAtoms )
      .def("energy", &w_t::energy )
      .def("polarization", &w_t::polarization )
      .def("energy_arr", disp_energies)
      .def("polarization_arr", disp_pols)
      .def("max_energy", &w_t::max_energy)
      .def("min_energy", &w_t::min_energy)
      ;
  }

}

// End of file


#include <fstream>
#include <set>
#include <map>
#include <iostream>
#include <vector>
#include <range/v3/all.hpp>
#include <array>
#include <optional>
#include <fmt/format.h>
#include <fmt/ranges.h>
#include <string_view>
#include <array>
#include <complex>
#include <boost/range.hpp>
#include <boost/algorithm/string.hpp>
#include "../aoc2020/matrix.h"
#include "../aoc2020/mypair.h"
namespace views = ranges::views;
namespace algo = boost::algorithm;
using Regs = std::array<int, 4>;
using namespace std::complex_literals;
auto turn_right(Mypair p)
{
  std::complex<int> c(p.y, -p.x);
  std::complex<int> m(0, -1);
  auto a = c * m;
  return Mypair(-a.imag(), a.real());
}
auto turn_left(Mypair p)
{
  std::complex<int> c(p.y, -p.x);
  std::complex<int> m(0, 1);
  auto a = c * m;
  return Mypair(-a.imag(), a.real());
}
class MineCart;
class Cart
{
  Mypair s_;
  Mypair d_;
  std::map<Mypair, char> *pturnright_;
  std::map<Mypair, char> *pturnleft_;

  int cnt_ = 0;
  FixedMatrix<char, 150> *pm_;
  FixedMatrix<char, 150> *pcartpos_;
  std::optional<Mypair> *pcollision_;
  char &cur()
  {
    return (*pm_)(s_.x, s_.y);
  }
  bool iscart() const
  {
    return (*pcartpos_)(s_.x, s_.y) == '#';
  }
  void setcart() const
  {
    (*pcartpos_)(s_.x, s_.y) = '#';
  }
  void clearcart() const
  {
    (*pcartpos_)(s_.x, s_.y) = '.';
  }
  void turn()
  {
    auto &turnright_ = *pturnright_;
    auto &turnleft_ = *pturnleft_;
    if (iscart()) {
      *pcollision_ = s_;
      return;
    }
    //std::cout << s_ << "," << d_ << std::endl;
    auto rightc = turnright_[d_];
    auto leftc = turnleft_[d_];
    if (cur() == rightc) {
      d_ = turn_right(d_);
    } else if (cur() == leftc) {
      d_ = turn_left(d_);
    } else if (cur() == '+') {
      if (cnt_ % 3 == 0) {
        d_ = turn_left(d_);
      }
      if (cnt_ % 3 == 2) {
        d_ = turn_right(d_);
      }
      ++cnt_;
    }
  }

public:
  auto getStartPoint() const
  {
    return s_;
  }
  friend std::ostream &operator<<(std::ostream &os, Cart const &c)
  {
    return os << c.s_;
  }
  Cart() = default;
  Cart(Mypair s, char c, FixedMatrix<char, 150> *pm, FixedMatrix<char, 150> *pcp, std::optional<Mypair> *po, std::map<Mypair, char> *pturnright, std::map<Mypair, char> *pturnleft) : s_(s), pm_(pm), pcartpos_(pcp), pcollision_(po), pturnright_(pturnright), pturnleft_(pturnleft)
  {
    std::map<char, Mypair> dms_;
    dms_['>'] = Mypair(0, 1);
    dms_['v'] = Mypair(1, 0);
    dms_['^'] = Mypair(-1, 0);
    dms_['<'] = Mypair(0, -1);
    d_ = dms_[c];
    //cur() = '#';
  }
  auto operator<=>(const Cart &c) const
  {
    return s_ <=> c.s_;
  }
  bool operator==(const Cart &c) const
  {
    return s_ == c.s_;
  }
  Cart &operator++()
  {
    //cur() = '.';
    clearcart();
    s_ += d_;
    turn();
    setcart();

    return *this;
  }
};
class MineCart
{
  FixedMatrix<char, 150> m_;
  FixedMatrix<char, 150> cartpos_;
  std::vector<Cart> vc_;
  std::optional<Mypair> collision_;

  std::map<Mypair, char> turnright_;
  std::map<Mypair, char> turnleft_;

public:
  MineCart()
  {
    turnright_[Mypair{ 0, 1 }] = '\\';
    turnleft_[Mypair{ 0, 1 }] = '/';

    turnright_[Mypair{ 1, 0 }] = '/';
    turnleft_[Mypair{ 1, 0 }] = '\\';

    turnright_[Mypair{ 0, -1 }] = '\\';
    turnleft_[Mypair{ 0, -1 }] = '/';
    turnright_[Mypair{ -1, 0 }] = '/';
    turnleft_[Mypair{ -1, 0 }] = '\\';
  }
  friend std::istream &operator>>(std::istream &is, MineCart &m)
  {
    ranges::getlines_view iv(is);
    int i = 0;
    ranges::for_each(iv, [&m, &i](std::string_view s) {
      ranges::copy(s, m.m_.row(i).begin());
      typedef std::vector<boost::iterator_range<std::string_view::iterator>> find_vector_type;
      for (auto c : { "<", ">", "^", "v" }) {
        find_vector_type FindVec;
        algo::find_all(FindVec, s, c);
        for (auto r : FindVec) {
          m.vc_.emplace_back(Mypair(i, r.begin() - s.begin()), c[0], &m.m_, &m.cartpos_, &m.collision_, &m.turnright_, &m.turnleft_);
        }
      }
      ++i;
    });
    ranges::sort(m.vc_);
    std::ostream_iterator<Cart> oi(std::cout, ",");
    ranges::copy(m.vc_, oi);
    std::cout << "size:" << m.vc_.size() << std::endl;
    ranges::fill(m.cartpos_.array(), '.');
    for (auto cart : m.vc_) {
      auto pos = cart.getStartPoint();
      m.cartpos_(pos.x, pos.y) = '#';
    }
    int sz = m.vc_.size();
    for (int i = 0; !m.collision_; ++i) {
      std::cout << "i:" << i << std::endl;
      auto &c = m.vc_[i % sz];
      ++c;
    }
    return is;
  }
  friend std::ostream &operator<<(std::ostream &os, const MineCart &m)
  {
    for (int i = 120; i != 150; ++i) {
      for (int j = 120; j != 150; ++j) {
        os << m.m_(i, j);
      }
      os << std::endl;
    }
    return os;
  }
  Mypair collisionPoint()
  {
    return *collision_;
  }
};
int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    MineCart m;
    ifs >> m;
    std::cout << m.collisionPoint() << std::endl;
  }
}


#pragma once
#include "branch_tree_node.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include "leaf_tree_node.hpp"
#include "marching_cubes.hpp"
#include "object.hpp"
#include "tree_config.hpp"
#include "tree_node.hpp"
#include "vector.hpp"
#include <algorithm>
#include <array>
#include <memory>
#include <vector>

namespace dmc
{
template <class Scalar>
class tree
{
public:
	typedef Scalar scalar_type;
	typedef vector<scalar_type, 3> vector_type;
	typedef object<scalar_type> object_type;
	typedef tree_config<scalar_type> config_type;
	typedef tree_node<scalar_type> node_type;
	typedef branch_tree_node<scalar_type> branch_node_type;
	typedef leaf_tree_node<scalar_type> leaf_node_type;
	typedef vertex<scalar_type> vertex_type;

	explicit tree(const vector_type &minimum, const vector_type &maximum, const config_type &config = config_type())
			: minimum_(minimum), config_(config)
	{
		auto v = maximum - minimum;

		size_ = v.map([&](auto x) {
							 return std::max(static_cast<scalar_type>(1.0), std::ceil(x / config_.grid_width));
						 })
								.template cast<std::size_t>();

		children_.resize(size_.product());
	}

	void generate(const object_type &obj)
	{
		for (std::size_t iz = 0; iz < size_.z(); ++iz)
		{
			for (std::size_t iy = 0; iy < size_.y(); ++iy)
			{
				for (std::size_t ix = 0; ix < size_.x(); ++ix)
				{
					auto minimum = minimum_ + vector<std::size_t, 3>(ix, iy, iz).cast<scalar_type>() * config_.grid_width;
					auto maximum = minimum_ + vector<std::size_t, 3>(ix + 1, iy + 1, iz + 1).cast<scalar_type>() * config_.grid_width;
					children_[index(ix, iy, iz)] = generate_impl(obj, minimum, maximum, 0);
				}
			}
		}
	}

	template <class Receiver>
	void enumerate(Receiver receiver)
	{
		for (std::size_t iz = 0; iz < size_.z(); ++iz)
		{
			for (std::size_t iy = 0; iy < size_.y(); ++iy)
			{
				for (std::size_t ix = 0; ix < size_.x(); ++ix)
				{
					enumerate_impl_c(*children_[index(ix, iy, iz)], receiver);

					if (ix != size_.x() - 1)
						enumerate_impl_f_x(*children_[index(ix, iy, iz)], *children_[index(ix + 1, iy, iz)], receiver);

					if (iy != size_.y() - 1)
						enumerate_impl_f_y(*children_[index(ix, iy, iz)], *children_[index(ix, iy + 1, iz)], receiver);

					if (iz != size_.z() - 1)
						enumerate_impl_f_z(*children_[index(ix, iy, iz)], *children_[index(ix, iy, iz + 1)], receiver);

					if (ix != size_.x() - 1 && iy != size_.y() - 1)
					{
						enumerate_impl_e_xy(
								*children_[index(ix, iy, iz)],
								*children_[index(ix + 1, iy, iz)],
								*children_[index(ix, iy + 1, iz)],
								*children_[index(ix + 1, iy + 1, iz)],
								receiver);
					}

					if (iy != size_.y() - 1 && iz != size_.z() - 1)
					{
						enumerate_impl_e_yz(
								*children_[index(ix, iy, iz)],
								*children_[index(ix, iy + 1, iz)],
								*children_[index(ix, iy, iz + 1)],
								*children_[index(ix, iy + 1, iz + 1)],
								receiver);
					}

					if (ix != size_.x() - 1 && iz != size_.z() - 1)
					{
						enumerate_impl_e_xz(
								*children_[index(ix, iy, iz)],
								*children_[index(ix + 1, iy, iz)],
								*children_[index(ix, iy, iz + 1)],
								*children_[index(ix + 1, iy, iz + 1)],
								receiver);
					}

					if (ix != size_.x() - 1 && iy != size_.y() - 1 && iz != size_.z() - 1)
					{
						enumerate_impl_v(
								*children_[index(ix, iy, iz)],
								*children_[index(ix + 1, iy, iz)],
								*children_[index(ix, iy + 1, iz)],
								*children_[index(ix + 1, iy + 1, iz)],
								*children_[index(ix, iy, iz + 1)],
								*children_[index(ix + 1, iy, iz + 1)],
								*children_[index(ix, iy + 1, iz + 1)],
								*children_[index(ix + 1, iy + 1, iz + 1)],
								receiver);
					}
				}
			}
		}
	}

private:
	std::size_t index(std::size_t ix, std::size_t iy, std::size_t iz) const
	{
		return iz * size_.y() * size_.x() + iy * size_.x() + ix;
	}

	std::unique_ptr<node_type> generate_impl(const object_type &obj, const vector_type &minimum, const vector_type &maximum, std::size_t depth) const
	{
		std::array<vector_type, 8> points =
				{{
						{minimum.x(), minimum.y(), minimum.z()},
						{maximum.x(), minimum.y(), minimum.z()},
						{minimum.x(), maximum.y(), minimum.z()},
						{maximum.x(), maximum.y(), minimum.z()},
						{minimum.x(), minimum.y(), maximum.z()},
						{maximum.x(), minimum.y(), maximum.z()},
						{minimum.x(), maximum.y(), maximum.z()},
						{maximum.x(), maximum.y(), maximum.z()},
				}};

		std::array<scalar_type, 8> values;

		std::transform(points.begin(), points.end(), values.begin(),
									 [&](const auto &p) {
										 return obj.value(p);
									 });

		std::array<vector_type, 8> grads;

		std::transform(points.begin(), points.end(), grads.begin(),
									 [&](const auto &p) {
										 return obj.grad(p);
									 });

		Eigen::Matrix<scalar_type, 11, 4> a;

		for (int i = 0; i < 8; ++i)
		{
			a(i, 0) = grads[i].x();
			a(i, 1) = grads[i].y();
			a(i, 2) = grads[i].z();
			a(i, 3) = static_cast<scalar_type>(-1.0);
		}

		a(8, 0) = config_.nominal_weight;
		a(8, 1) = 0.0;
		a(8, 2) = 0.0;
		a(8, 3) = 0.0;
		a(9, 0) = 0.0;
		a(9, 1) = config_.nominal_weight;
		a(9, 2) = 0.0;
		a(9, 3) = 0.0;
		a(10, 0) = 0.0;
		a(10, 1) = 0.0;
		a(10, 2) = config_.nominal_weight;
		a(10, 3) = 0.0;

		Eigen::Matrix<scalar_type, 11, 1> b;

		auto medium = (minimum + maximum) * static_cast<scalar_type>(0.5);

		for (int i = 0; i < 8; ++i)
			b(i) = dot_product(grads[i], points[i] - medium) - values[i];

		b(8) = 0.0;
		b(9) = 0.0;
		b(10) = 0.0;

		Eigen::Matrix<scalar_type, 4, 1> x = a.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);

		auto center = vector_type(x(0), x(1), x(2)) + medium;
		auto offset = obj.value(center);

		auto error = scalar_type();

		for (int i = 0; i < 8; ++i)
			error += squared(offset - values[i] - dot_product(grads[i], center - points[i]));

		if (depth >= config_.maximum_depth || error < squared(config_.tolerance))
		{
			return std::make_unique<leaf_node_type>(vertex_type(center, offset));
		}
		else
		{
			std::array<std::unique_ptr<node_type>, 8> nodes =
					{{
							generate_impl(obj, {minimum.x(), minimum.y(), minimum.z()}, {medium.x(), medium.y(), medium.z()}, depth + 1),
							generate_impl(obj, {medium.x(), minimum.y(), minimum.z()}, {maximum.x(), medium.y(), medium.z()}, depth + 1),
							generate_impl(obj, {minimum.x(), medium.y(), minimum.z()}, {medium.x(), maximum.y(), medium.z()}, depth + 1),
							generate_impl(obj, {medium.x(), medium.y(), minimum.z()}, {maximum.x(), maximum.y(), medium.z()}, depth + 1),
							generate_impl(obj, {minimum.x(), minimum.y(), medium.z()}, {medium.x(), medium.y(), maximum.z()}, depth + 1),
							generate_impl(obj, {medium.x(), minimum.y(), medium.z()}, {maximum.x(), medium.y(), maximum.z()}, depth + 1),
							generate_impl(obj, {minimum.x(), medium.y(), medium.z()}, {medium.x(), maximum.y(), maximum.z()}, depth + 1),
							generate_impl(obj, {medium.x(), medium.y(), medium.z()}, {maximum.x(), maximum.y(), maximum.z()}, depth + 1),
					}};

			return std::make_unique<branch_node_type>(std::move(nodes));
		}
	}

	template <class Receiver>
	void enumerate_impl_c(const node_type &n, Receiver receiver)
	{
		if (auto b = dynamic_cast<const branch_node_type *>(&n))
		{
			enumerate_impl_c(*b->children()[0], receiver);
			enumerate_impl_c(*b->children()[1], receiver);
			enumerate_impl_c(*b->children()[2], receiver);
			enumerate_impl_c(*b->children()[3], receiver);
			enumerate_impl_c(*b->children()[4], receiver);
			enumerate_impl_c(*b->children()[5], receiver);
			enumerate_impl_c(*b->children()[6], receiver);
			enumerate_impl_c(*b->children()[7], receiver);

			enumerate_impl_f_x(*b->children()[0], *b->children()[1], receiver);
			enumerate_impl_f_x(*b->children()[2], *b->children()[3], receiver);
			enumerate_impl_f_x(*b->children()[4], *b->children()[5], receiver);
			enumerate_impl_f_x(*b->children()[6], *b->children()[7], receiver);

			enumerate_impl_f_y(*b->children()[0], *b->children()[2], receiver);
			enumerate_impl_f_y(*b->children()[1], *b->children()[3], receiver);
			enumerate_impl_f_y(*b->children()[4], *b->children()[6], receiver);
			enumerate_impl_f_y(*b->children()[5], *b->children()[7], receiver);

			enumerate_impl_f_z(*b->children()[0], *b->children()[4], receiver);
			enumerate_impl_f_z(*b->children()[1], *b->children()[5], receiver);
			enumerate_impl_f_z(*b->children()[2], *b->children()[6], receiver);
			enumerate_impl_f_z(*b->children()[3], *b->children()[7], receiver);

			enumerate_impl_e_xy(*b->children()[0], *b->children()[1], *b->children()[2], *b->children()[3], receiver);
			enumerate_impl_e_xy(*b->children()[4], *b->children()[5], *b->children()[6], *b->children()[7], receiver);

			enumerate_impl_e_yz(*b->children()[0], *b->children()[2], *b->children()[4], *b->children()[6], receiver);
			enumerate_impl_e_yz(*b->children()[1], *b->children()[3], *b->children()[5], *b->children()[7], receiver);

			enumerate_impl_e_xz(*b->children()[0], *b->children()[1], *b->children()[4], *b->children()[5], receiver);
			enumerate_impl_e_xz(*b->children()[2], *b->children()[3], *b->children()[6], *b->children()[7], receiver);

			enumerate_impl_v(
					*b->children()[0],
					*b->children()[1],
					*b->children()[2],
					*b->children()[3],
					*b->children()[4],
					*b->children()[5],
					*b->children()[6],
					*b->children()[7],
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_f_x(const node_type &n1, const node_type &n2, Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);

		if (b1 || b2)
		{
			enumerate_impl_f_x(b1 ? *b1->children()[1] : n1, b2 ? *b2->children()[0] : n2, receiver);
			enumerate_impl_f_x(b1 ? *b1->children()[3] : n1, b2 ? *b2->children()[2] : n2, receiver);
			enumerate_impl_f_x(b1 ? *b1->children()[5] : n1, b2 ? *b2->children()[4] : n2, receiver);
			enumerate_impl_f_x(b1 ? *b1->children()[7] : n1, b2 ? *b2->children()[6] : n2, receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[1] : n1,
					b2 ? *b2->children()[0] : n2,
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[1] : n1,
					b2 ? *b2->children()[0] : n2,
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[1] : n1,
					b2 ? *b2->children()[0] : n2,
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_f_y(const node_type &n1, const node_type &n2, Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);

		if (b1 || b2)
		{
			enumerate_impl_f_y(b1 ? *b1->children()[2] : n1, b2 ? *b2->children()[0] : n2, receiver);
			enumerate_impl_f_y(b1 ? *b1->children()[3] : n1, b2 ? *b2->children()[1] : n2, receiver);
			enumerate_impl_f_y(b1 ? *b1->children()[6] : n1, b2 ? *b2->children()[4] : n2, receiver);
			enumerate_impl_f_y(b1 ? *b1->children()[7] : n1, b2 ? *b2->children()[5] : n2, receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[2] : n1,
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[1] : n2,
					receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[4] : n2,
					b2 ? *b2->children()[5] : n2,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[1] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[5] : n2,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[2] : n1,
					b2 ? *b2->children()[0] : n2,
					b1 ? *b1->children()[6] : n1,
					b2 ? *b2->children()[4] : n2,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[2] : n1,
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[1] : n2,
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[4] : n2,
					b2 ? *b2->children()[5] : n2,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_f_z(const node_type &n1, const node_type &n2, Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);

		if (b1 || b2)
		{
			enumerate_impl_f_z(b1 ? *b1->children()[4] : n1, b2 ? *b2->children()[0] : n2, receiver);
			enumerate_impl_f_z(b1 ? *b1->children()[5] : n1, b2 ? *b2->children()[1] : n2, receiver);
			enumerate_impl_f_z(b1 ? *b1->children()[6] : n1, b2 ? *b2->children()[2] : n2, receiver);
			enumerate_impl_f_z(b1 ? *b1->children()[7] : n1, b2 ? *b2->children()[3] : n2, receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[4] : n1,
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[1] : n2,
					receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[2] : n2,
					b2 ? *b2->children()[3] : n2,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[4] : n1,
					b1 ? *b1->children()[6] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[2] : n2,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[5] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[1] : n2,
					b2 ? *b2->children()[3] : n2,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[4] : n1,
					b1 ? *b1->children()[5] : n1,
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[1] : n2,
					b2 ? *b2->children()[2] : n2,
					b2 ? *b2->children()[3] : n2,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_e_xy(
			const node_type &n1,
			const node_type &n2,
			const node_type &n3,
			const node_type &n4,
			Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);
		auto b3 = dynamic_cast<const branch_node_type *>(&n3);
		auto b4 = dynamic_cast<const branch_node_type *>(&n4);

		if (b1 || b2 || b3 || b4)
		{
			enumerate_impl_e_xy(
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					b3 ? *b3->children()[1] : n3,
					b4 ? *b4->children()[0] : n4,
					receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[5] : n3,
					b4 ? *b4->children()[4] : n4,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					b3 ? *b3->children()[1] : n3,
					b4 ? *b4->children()[0] : n4,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[5] : n3,
					b4 ? *b4->children()[4] : n4,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_e_yz(
			const node_type &n1,
			const node_type &n2,
			const node_type &n3,
			const node_type &n4,
			Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);
		auto b3 = dynamic_cast<const branch_node_type *>(&n3);
		auto b4 = dynamic_cast<const branch_node_type *>(&n4);

		if (b1 || b2 || b3 || b4)
		{
			enumerate_impl_e_yz(
					b1 ? *b1->children()[6] : n1,
					b2 ? *b2->children()[4] : n2,
					b3 ? *b3->children()[2] : n3,
					b4 ? *b4->children()[0] : n4,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[5] : n2,
					b3 ? *b3->children()[3] : n3,
					b4 ? *b4->children()[1] : n4,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[4] : n2,
					b2 ? *b2->children()[5] : n2,
					b3 ? *b3->children()[2] : n3,
					b3 ? *b3->children()[3] : n3,
					b4 ? *b4->children()[0] : n4,
					b4 ? *b4->children()[1] : n4,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_e_xz(
			const node_type &n1,
			const node_type &n2,
			const node_type &n3,
			const node_type &n4,
			Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);
		auto b3 = dynamic_cast<const branch_node_type *>(&n3);
		auto b4 = dynamic_cast<const branch_node_type *>(&n4);

		if (b1 || b2 || b3 || b4)
		{
			enumerate_impl_e_xz(
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					b3 ? *b3->children()[1] : n3,
					b4 ? *b4->children()[0] : n4,
					receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[3] : n3,
					b4 ? *b4->children()[2] : n4,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[1] : n3,
					b4 ? *b4->children()[0] : n4,
					b3 ? *b3->children()[3] : n3,
					b4 ? *b4->children()[2] : n4,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_v(
			const node_type &n1,
			const node_type &n2,
			const node_type &n3,
			const node_type &n4,
			const node_type &n5,
			const node_type &n6,
			const node_type &n7,
			const node_type &n8,
			Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);
		auto b3 = dynamic_cast<const branch_node_type *>(&n3);
		auto b4 = dynamic_cast<const branch_node_type *>(&n4);
		auto b5 = dynamic_cast<const branch_node_type *>(&n5);
		auto b6 = dynamic_cast<const branch_node_type *>(&n6);
		auto b7 = dynamic_cast<const branch_node_type *>(&n7);
		auto b8 = dynamic_cast<const branch_node_type *>(&n8);

		if (b1 || b2 || b3 || b4 || b5 || b6 || b7 || b8)
		{
			enumerate_impl_v(
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[5] : n3,
					b4 ? *b4->children()[4] : n4,
					b5 ? *b5->children()[3] : n5,
					b6 ? *b6->children()[2] : n6,
					b7 ? *b7->children()[1] : n7,
					b8 ? *b8->children()[0] : n8,
					receiver);
		}
		else
		{
			auto l1 = static_cast<const leaf_node_type *>(&n1);
			auto l2 = static_cast<const leaf_node_type *>(&n2);
			auto l3 = static_cast<const leaf_node_type *>(&n3);
			auto l4 = static_cast<const leaf_node_type *>(&n4);
			auto l5 = static_cast<const leaf_node_type *>(&n5);
			auto l6 = static_cast<const leaf_node_type *>(&n6);
			auto l7 = static_cast<const leaf_node_type *>(&n7);
			auto l8 = static_cast<const leaf_node_type *>(&n8);

			std::array<const vertex_type *, 8> vertices =
					{{
							&l1->vertex(),
							&l2->vertex(),
							&l3->vertex(),
							&l4->vertex(),
							&l5->vertex(),
							&l6->vertex(),
							&l7->vertex(),
							&l8->vertex(),
					}};

			marching_cubes<scalar_type>(vertices, receiver);
		}
	}

	vector_type minimum_;
	config_type config_;

	std::size_t grid_size_;
	scalar_type grid_width_;

	vector<std::size_t, 3> size_;
	std::vector<std::unique_ptr<node_type>> children_;
};
} // namespace dmc


#ifndef UTILS_HPP
#define UTILS_HPP

#include <src/algorithms/neal2_algorithm.h>
#include <src/algorithms/semihdp_sampler.h>
#include <src/collectors/file_collector.h>
#include <src/collectors/memory_collector.h>
#include <src/includes.h>
#include <src/utils/rng.h>

#include <Eigen/Dense>
#include <stan/math/prim.hpp>
#include <vector>

using namespace Eigen;

MemoryCollector run_semihdp(const std::vector<Eigen::MatrixXd> data,
                 std::string chainfile,
                 std::string params_file,
                 int niter=10000, int nburn=10000, int thin=10,
                 std::string update_c = "full");

std::vector<Eigen::MatrixXd> eval_uni_dens(
    MemoryCollector& coll, const Eigen::VectorXd &xgrid, int ngroups);

Eigen::MatrixXd get_latent_vars(MemoryCollector& coll, int ngroups);

Eigen::MatrixXi get_rest_allocs(MemoryCollector& coll, int ngroups);

#endif


//
// Copyright (c) 2015 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#include <iostream>
# include <eigen3/Eigen/Core>
#include <pinocchio/spatial/skew.hpp>

#include <boost/test/unit_test.hpp>

template<int N>
void udut( Eigen::Matrix<double,N,N> & M )
{
    typedef Eigen::Matrix<double,N,N> MatrixNd;
    typedef Eigen::Matrix<double,1,N> VectorNd;
    typedef typename MatrixNd::DiagonalReturnType D_t;
    typedef typename MatrixNd::template TriangularViewReturnType<Eigen::StrictlyUpper>::Type U_t;

    VectorNd tmp;
    D_t D = M.diagonal();
    U_t U = M.template triangularView<Eigen::StrictlyUpper>();

    for(int j=N-1;j>=0;--j )
      {
	typename VectorNd::SegmentReturnType DUt = tmp.tail(N-j-1);
	if( j<N-1 ) DUt = M.row(j).tail(N-j-1).transpose().cwiseProduct( D.tail(N-j-1) );

	D[j] -= M.row(j).tail(N-j-1).dot(DUt);

	for(int i=j-1;i>=0;--i)
	  { U(i,j) -= M.row(i).tail(N-j-1).dot(DUt); U(i,j) /= D[j]; }
      }
  }

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( udut )
{
  using namespace Eigen;

  Matrix<double,6,6> A = Matrix<double,6,6>::Random(); A = A*A.transpose();
  double m = A(1,1);
  Matrix3d I = A.bottomRightCorner<3,3>();
  Vector3d c = A.block<3,1>(0,3);

  A.topLeftCorner<3,3>() = Matrix3d::Identity()*m;
  A.topRightCorner<3,3>() = se3::skew(c).transpose();
  A.bottomLeftCorner<3,3>() = se3::skew(c);
  A.bottomRightCorner<3,3>() -= A.bottomLeftCorner<3,3>()*A.bottomLeftCorner<3,3>();

  std::cout << "A = [\n" << A << "];" << std::endl;
  udut(A);
  std::cout << "U = [\n" << A << "];" << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


#include <stdint.h>
#include <iostream>
#include <map>
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/array.hpp>

#include "cxx/foreach.hpp"
#include "externals/cxx/array.hpp"
#include "externals/cxx/utility.hpp"
#include "cxx/iterator/product.hpp"
#include "cxx/sugar.hpp"

#include <rysq.hpp>

namespace rysq {

    template<class C>
    struct Index {
	typedef std::vector<C> vector_type;
	typedef std::map<uint32_t,size_t> map_type;

	static uint32_t key(const Quartet<rysq::type> &quartet) {
	    return cxx::utility::pack(QUARTET(quartet));
	}

	static vector_type generate(const Quartet<rysq::type> &quartet) {
	    rysq::shell shells[] = { QUARTET(quartet) };
	    vector_type index;
	    typedef boost::array<int,4> product;
	    boost::fortran_storage_order order;
	    // std::cout << quartet << std::endl;
	    foreach (const product p, (cxx::iterator::product(shells, order))) {
		// std::cout << p << "\n";
	        int x = 0, y = 0, z = 0;
		int m = 1;
	        for (int i = 0; i < 4; ++i) {
		    x += rysq::LX[p[i]]*m;
		    y += rysq::LY[p[i]]*m;
		    z += rysq::LZ[p[i]]*m;
		    m *= (shells[i].L+1);
	        }
		typename vector_type::value_type value = { x,y,z };
		// std::cout <<  p << value << std::endl;
	        index.push_back(value);
	    }
	    return index;
	}

	Index() {
	    std::vector<rysq::type> types;
	    foreach (rysq::type type, rysq::types) types.push_back(type);
	    initialize(types);
	}

	Index(const std::vector<rysq::type> &types) {
	    initialize(types);
	}

	void initialize(const std::vector<rysq::type> &types) {
	    typename vector_type::iterator it = index_.begin();

	    // std::ostream_iterator<ushort3, char> ushort3_output(std::cout, "\n");
	    // std::ostream_iterator< int > key_output(std::cout, " " );

	    typedef boost:: array< rysq:: type,4> product;
	    foreach (const product q,  (cxx::iterator::product<4>(types ))) {
		size_t size =  rysq::Quartet<rysq::Shell>::size(q.elems);
		if (size > 2048) continue;
		// std::cout << q << " " << size << "\n";
		assert(map_.size() < map_.max_size());

		if (!this->find(q)) {
		    vector_type index = generate(q);
		    map_[key(q)] = index_.size();
		    std::copy(index.begin(), index.end(), std::back_inserter(index_));
		}
	    }
	}
	const C* find(const boost::array<rysq::type,4> &quartet) const {
	    typename map_type::const_iterator it = map_.find(key(quartet));
	    if (it == map_.end()) return NULL;
	    return &index_[(*it).second];
	}
	const vector_type& data() const { return index_; }
	const map_type& map() const { return map_; }

    private:
	map_type map_;
	vector_type index_;
    };
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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 NT2_OPERATOR_FUNCTIONS_COMPLEX_GENERIC_IF_ELSE_HPP_INCLUDED
#define NT2_OPERATOR_FUNCTIONS_COMPLEX_GENERIC_IF_ELSE_HPP_INCLUDED

#include <nt2/operator/functions/if_else.hpp>
#include <nt2/include/functions/if_else.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  //complex/complex 1 -1
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)
                             , (generic_< complex_< arithmetic_<A0> > >)
                               (generic_< complex_< arithmetic_<A0> > >)
                               (generic_< complex_< arithmetic_<A0> > >)
                             )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A0& a1, const A0&a2) const
    {
      return result_type(if_else(is_nez(a0), nt2::real(a1), nt2::real(a2)),
                         if_else(is_nez(a0), nt2::imag(a1), nt2::imag(a2)));
    }
  };

  //complex/complex 1 -1
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)
                             , (generic_< logical_<A0> >)
                               (generic_< complex_< arithmetic_<A1> > >)
                               (generic_< complex_< arithmetic_<A1> > >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A1&a2) const
    {
      return result_type(if_else(a0, nt2::real(a1), nt2::real(a2)),
                         if_else(a0, nt2::imag(a1), nt2::imag(a2)));
    }
  };

  //complex/complex 2
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)
                             , (generic_< fundamental_<A0> >)
                               (generic_< complex_< arithmetic_<A1> > >)
                               (generic_< complex_< arithmetic_<A1> > >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A1&a2) const
    {
      return if_else( is_nez(a0), a1, a2 );
    }
  };

  //dry/dry 1 -5
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)
                             , (generic_< logical_<A0> >)
                               (generic_< dry_< arithmetic_<A1> > >)
                               (generic_< dry_< arithmetic_<A1> > >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A1&a2) const
    {
      return result_type(if_else(a0, nt2::real(a1), nt2::real(a2)));
    }
  };

  //dry/dry 2
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)
                             , (generic_< fundamental_<A0> >)
                               (generic_< dry_< arithmetic_<A1> > >)
                               (generic_< dry_< arithmetic_<A1> > >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A1&a2) const
    {
      return if_else(is_nez(a0), a1, a2);
    }
  };

  //dry/complex 1 -6
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< logical_<A0> >)
                               (generic_< dry_< arithmetic_<A1> > >)
                               (generic_< complex_< arithmetic_<A2> > >)
                             )
  {
    typedef A2 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return if_else(a0, result_type(nt2::real(a1)), a2);
    }
  };

  //dry/complex 2
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< fundamental_<A0> >)
                               (generic_< dry_< arithmetic_<A1> > >)
                               (generic_< complex_< arithmetic_<A2> > >)
                             )
  {
    typedef A2 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return if_else(is_nez(a0), a1, a2);
    }
  };

  //complex/dry/ 1 -7
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< logical_<A0> >)
                               (generic_< complex_< arithmetic_<A1> > >)
                               (generic_< dry_< arithmetic_<A2> > >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return if_else(a0, a1, result_type(nt2::real(a2)));
    }
  };

  //complex/dry/ 2
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< fundamental_<A0> >)
                               (generic_< complex_< arithmetic_<A1> > >)
                               (generic_< dry_< arithmetic_<A2> > >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return if_else(is_nez(a0), a1, a2);
    }
  };

  //arithmetic/dry 1
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< logical_<A0> >)
                               (generic_< arithmetic_<A1> >)
                               (generic_< dry_< arithmetic_<A2> > >)
                             )
  {
    typedef A2 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return result_type(if_else(a0, a1, nt2::real(a2)));
    }
  };

  //arithmetic/dry 2
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< fundamental_<A0> >)
                               (generic_< arithmetic_<A1> >)
                               (generic_< dry_< arithmetic_<A2> > >)
                             )
  {
    typedef A2 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return if_else(is_nez(a0), a1, a2);
    }
  };

  //arithmetic/complex 1
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< logical_<A0> >)
                               (generic_< arithmetic_<A1> >)
                               (generic_< complex_< arithmetic_<A2> > >)
                             )
  {
    typedef A2 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return if_else(a0, result_type(a1), a2);
    }
  };

  //arithmetic/complex 2
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< fundamental_<A0> >)
                               (generic_< arithmetic_<A1> >)
                               (generic_< complex_< arithmetic_<A2> > >)
                             )
  {
    typedef A2 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return if_else(is_nez(a0), a1, a2);
    }
  };

  //dry/arithmetic/   1
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< logical_<A0> >)
                               (generic_< dry_< arithmetic_<A1> > >)
                               (generic_< arithmetic_<A2> >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return result_type(if_else(a0, nt2::real(a1), a2));
    }
  };

  //dry/arithmetic_ 2
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< fundamental_<A0> >)
                               (generic_< dry_< arithmetic_<A1> > >)
                               (generic_< arithmetic_<A2> >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return if_else(is_nez(a0), a1, a2);
    }
  };

  //complex/arithmetic/   1
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< logical_<A0> >)
                               (generic_< complex_< arithmetic_<A1> > >)
                               (generic_< arithmetic_<A2> >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return result_type(if_else(a0, a1, result_type(a2)));
    }
  };

  //complex/arithmetic_ 2
  BOOST_DISPATCH_IMPLEMENT   ( if_else_, tag::cpu_, (A0)(A1)(A2)
                             , (generic_< fundamental_<A0> >)
                               (generic_< complex_< arithmetic_<A1> > >)
                               (generic_< arithmetic_<A2> >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1, const A2&a2) const
    {
      return if_else(is_nez(a0), a1, a2);
    }
  };

} } }

#endif


#include <stdexcept>
#include <stdint.h>

#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>

#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <Eigen/Dense>

#include <pcl_conversions/pcl_conversions.h>
#include <kuri_mbzirc_challenge_2_exploration/gps_conversion.h>
#include <kuri_mbzirc_challenge_2_exploration/pointcloud_gps_filter.h>
#include <kuri_mbzirc_challenge_2_tools/pose_conversion.h>

PointcloudGpsFilter::PointcloudGpsFilter():
  ready_mask_(0)
{

}

void PointcloudGpsFilter::filterBounds (PcCloud::Ptr cloud_out)
{
  if (!isReady())
  {
    throw std::runtime_error("PointcloudGpsFilter not ready");
  }

  bool is_visualized = false;

  // Get transformation (Yaw -> rotation matrix -> transformation matrix)
  Eigen::Matrix3d R = pose_conversion::getRotationMatrix(ref_gps_quaternion_);
  Eigen::Matrix4d Ti = pose_conversion::getTransformationMatrix(R);

  /*
  double yaw = pose_conversion::getYawFromQuaternion(ref_gps_quaternion_);
  geometry_msgs::Quaternion q = pose_conversion::getQuaternionFromYaw(yaw);
  Eigen::Matrix3d R = pose_conversion::getRotationMatrix(q);
  Eigen::Matrix4d Ti = pose_conversion::getTransformationMatrix(R);

  printf("Yaw: %lf\n", yaw);
  */

  // Orient pointcloud towards north
  PcCloud::Ptr pc_rotated (new PcCloud);
  pcl::transformPointCloud (*pc_, *pc_rotated, Ti);

  // Transform GPS bounds to cartesian
  std::vector<PcPoint> bounds_cartesian;

  for (int i=0; i < arena_bounds_.size(); i++)
  {
    GeoPoint g = arena_bounds_[i];
    PcPoint p;

    ref_gps_.projectGPSToCartesian(g.lat, g.lon, &p.x, &p.y);

    bounds_cartesian.push_back(p);
  }

  // Append bounds for visualization
  if (is_visualized)
  {
    for (int b=0; b < bounds_cartesian.size(); b++)
    {
      PcPoint bp = bounds_cartesian[b];
      for (int i=-2; i<=2; i++)
      {
        for (int j=-2; j<=2; j++)
        {
          PcPoint p;
          p.x = bp.x + i*1.0;
          p.y = bp.y + j*1.0;
          cloud_out->points.push_back(p);
        }
      }
    }
  }

  // Check which points are within bounds
  for (int i=0; i < pc_->points.size(); i++)
  {
    PcPoint p = pc_rotated->points[i];

    // Check if gps coordinates within predefined bounds
    bool isWithinBounds = pointWithinBounds(bounds_cartesian, p);
    if (isWithinBounds)
    {
      //cloud_out->points.push_back(p);
      cloud_out->points.push_back(pc_->points[i]);
    }
  }

}

GeoPoint PointcloudGpsFilter::getRefGPS()
{
  GeoPoint g;
  g.lat = ref_gps_.getLat();
  g.lon = ref_gps_.getLon();
  return g;
}

geometry_msgs::Quaternion PointcloudGpsFilter::getRefQuaternion()
{
  geometry_msgs::Quaternion q;
  q.x = ref_gps_quaternion_.x;
  q.y = ref_gps_quaternion_.y;
  q.z = ref_gps_quaternion_.z;
  q.w = ref_gps_quaternion_.w;

  return q;
}

bool PointcloudGpsFilter::isReady()
{
  return (ready_mask_ == 15);
}

bool PointcloudGpsFilter::pointWithinBounds (GeoPoint p)
{
  // Adapted from http://wiki.unity3d.com/index.php?title=PolyContainsPoint
  bool inside = false;

  int j = arena_bounds_.size()-1;

  for (int i = 0; i < arena_bounds_.size(); j = i++)
  {
    GeoPoint c1 = arena_bounds_[i];
    GeoPoint c2 = arena_bounds_[j];

    if ( ((c1.lat <= p.lat && p.lat < c2.lat) || (c2.lat <= p.lat && p.lat < c1.lat)) &&
       (p.lon < (c2.lon - c1.lon) * (p.lat - c1.lat) / (c2.lat - c1.lat) + c1.lon))
       inside = !inside;
  }

  return inside;
}

bool PointcloudGpsFilter::pointWithinBounds (std::vector<PcPoint> bounds, PcPoint p)
{
  // Adapted from http://wiki.unity3d.com/index.php?title=PolyContainsPoint
  bool inside = false;

  int j = bounds.size()-1;

  for (int i = 0; i < bounds.size(); j = i++)
  {
    PcPoint c1 = bounds[i];
    PcPoint c2 = bounds[j];

    bool intersect = ((c1.y > p.y) != c2.y > p.y) &&
                      (p.x < (c2.x - c1.x)*(p.y-c1.y)/(c2.y-c1.y) + c1.x);

    if (intersect)
      inside = !inside;
  }

  return inside;
}

std::vector<std::string> PointcloudGpsFilter::readyStrings()
{
  std::vector<std::string> vec;
  std::string str;

  str = "GPS Bounds -- ";
  if (ready_mask_ & 1)
    str += "OK";
  else
    str += "NOT READY";
  vec.push_back(str);

  str = "GPS Cloud -- ";
  if (ready_mask_ & 2)
    str += "OK";
  else
    str += "NOT READY";
  vec.push_back(str);

  str = "GPS Reference -- ";
  if (ready_mask_ & 4)
    str += "OK";
  else
    str += "NOT READY";
  vec.push_back(str);

  str = "GPS Orientation -- ";
  if (ready_mask_ & 8)
    str += "OK";
  else
    str += "NOT READY";
  vec.push_back(str);

  return vec;
}

void PointcloudGpsFilter::setBounds(std::vector<GeoPoint> bounds)
{
  arena_bounds_ = bounds;
  ready_mask_ |= 1;
}

void PointcloudGpsFilter::setCloud(PcCloud::Ptr cloud_in)
{
  pc_ = cloud_in;
  ready_mask_ |= 2;
}

void PointcloudGpsFilter::setRefGPS(double lat, double lon, uint64_t timestamp)
{
  ref_gps_.update(lat, lon, timestamp);
  ready_mask_ |= 4;
}

void PointcloudGpsFilter::setRefOrientation(geometry_msgs::Quaternion q)
{
  ref_gps_quaternion_ = q;
  ready_mask_ |= 8;
}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::models::meta::is_survival_model.hpp               //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_COMMON_META_IS_SURVIVAL_MODEL_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_COMMON_META_IS_SURVIVAL_MODEL_HPP_ER_2009
#include <boost/mpl/bool.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace distribution{
namespace survival {
namespace meta{

    template<typename Model>
    struct is_survival_model : boost::mpl::bool_<false>{};

}// meta
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


#include <boost/test/unit_test.hpp>
#include "rectangle.hpp"
#include <cmath>

const double accuracy = 0.00001;

// =====================test initialization rectangle=====================
BOOST_AUTO_TEST_SUITE(InitializeRectangle)

  BOOST_AUTO_TEST_CASE(InitializeRectangle_OnCorrectValue_NoError)
  {
    jianing::Rectangle rectangle {{1.1, 2.2, 3.3, 4.4}};

    BOOST_REQUIRE_EQUAL(1.1, rectangle.getWidth());
    BOOST_REQUIRE_EQUAL(2.2, rectangle.getHeight());
    BOOST_REQUIRE_EQUAL(3.3, rectangle.getCenter().x);
    BOOST_REQUIRE_EQUAL(4.4, rectangle.getCenter().y);
    BOOST_REQUIRE_CLOSE(1.1 * 2.2, rectangle.getArea(), accuracy);
  }

  BOOST_AUTO_TEST_CASE(InitializeRectangle_WithAngleOnCorrectValue_NoError)
  {
    jianing::Rectangle rectangle {{2.0, 4.0, 2.0, 2.0}, 90};

    BOOST_REQUIRE_EQUAL(2.0, rectangle.getWidth());
    BOOST_REQUIRE_EQUAL(4.0, rectangle.getHeight());
    BOOST_REQUIRE_EQUAL(2.0, rectangle.getCenter().x);
    BOOST_REQUIRE_EQUAL(2.0, rectangle.getCenter().y);
    BOOST_REQUIRE_CLOSE(2.0 * 4.0, rectangle.getArea(), accuracy);
  }

  BOOST_AUTO_TEST_CASE(InitializeRectangle_OnWrongValue_ThrowError)
  {
    BOOST_REQUIRE_THROW(jianing::Rectangle({-1.1, 2.1, 3.1, 4.1}), std::domain_error);
    BOOST_REQUIRE_THROW(jianing::Rectangle({1.2, -2.2, 3.2, 4.2}), std::domain_error);
    BOOST_REQUIRE_THROW(jianing::Rectangle({-1.3, -2.3, 3.3, 4.3}), std::domain_error);
    BOOST_REQUIRE_THROW(jianing::Rectangle({0.0, 2.4, 3.4, 4.4}), std::domain_error);
    BOOST_REQUIRE_THROW(jianing::Rectangle({1.5, 0.0, 3.5, 4.5}), std::domain_error);
    BOOST_REQUIRE_THROW(jianing::Rectangle({0.0, 0.0, 3.6, 4.6}), std::domain_error);
    BOOST_REQUIRE_THROW(jianing::Rectangle({-1.7, 0.0, 3.7, 4.7}), std::domain_error);
    BOOST_REQUIRE_THROW(jianing::Rectangle({0.0, -2.8, 3.8, 4.8}), std::domain_error);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test initialization rectangle(FrameRect)=====================
BOOST_AUTO_TEST_SUITE(TestFrameRectWhenInitializeRectangle)

  BOOST_AUTO_TEST_CASE(TestFrameRectWhenInitializeRectangle_OnCorrectValue_NoError)
  {
  jianing::Rectangle rectangle {{1.1, 2.2, 3.3, 4.4}};

  BOOST_REQUIRE_EQUAL(1.1, rectangle.getFrameRect().width);
  BOOST_REQUIRE_EQUAL(2.2, rectangle.getFrameRect().height);
  BOOST_REQUIRE_EQUAL(3.3, rectangle.getFrameRect().pos.x);
  BOOST_REQUIRE_EQUAL(4.4, rectangle.getFrameRect().pos.y);
  }

  BOOST_AUTO_TEST_CASE(TestFrameRectWhenInitializeRectangle_WithAngleOnCorrectValue_NoError)
  {
    jianing::Rectangle rectangle {{2.0, 4.0, 2.0, 2.0}, 90};

    BOOST_REQUIRE_CLOSE(4.0, rectangle.getFrameRect().width, accuracy);
    BOOST_REQUIRE_CLOSE(2.0, rectangle.getFrameRect().height, accuracy);
    BOOST_REQUIRE_EQUAL(2.0, rectangle.getFrameRect().pos.x);
    BOOST_REQUIRE_EQUAL(2.0, rectangle.getFrameRect().pos.y);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test move rectangle=====================
BOOST_AUTO_TEST_SUITE(MoveRectangle)
  BOOST_AUTO_TEST_CASE(MoveRectangle_ToPointXY_CorrectWigthHeightXYAfterMove)
  {
    jianing::Rectangle rectangle {{5.1, 10.1, 3.1, 4.1}};

    rectangle.move({10.9, 5.9});

    BOOST_CHECK_EQUAL(5.1, rectangle.getWidth());
    BOOST_CHECK_EQUAL(10.1, rectangle.getHeight());
    BOOST_CHECK_EQUAL(10.9, rectangle.getCenter().x);
    BOOST_CHECK_EQUAL(5.9, rectangle.getCenter().y);
    BOOST_CHECK_CLOSE(5.1 * 10.1, rectangle.getArea(), accuracy);
  }

  BOOST_AUTO_TEST_CASE(MoveRectangle_Bydxdy_CorrectWigthHeightXYAfterMove)
  {
    jianing::Rectangle rectangle {{1.1, 2.2, 3.3, 4.4}};

    rectangle.move(-0.8, 0.9);

    BOOST_CHECK_EQUAL(1.1, rectangle.getWidth());
    BOOST_CHECK_EQUAL(2.2, rectangle.getHeight());
    BOOST_CHECK_CLOSE(3.3 - 0.8, rectangle.getCenter().x, accuracy);
    BOOST_CHECK_CLOSE(4.4 + 0.9, rectangle.getCenter().y, accuracy);
    BOOST_CHECK_CLOSE(1.1 * 2.2, rectangle.getArea(), accuracy);
  }
BOOST_AUTO_TEST_SUITE_END()

// =====================test move rectangle(FrameRect)=====================
BOOST_AUTO_TEST_SUITE(TestFrameRectAfterMoveRectangle)
  BOOST_AUTO_TEST_CASE(TestFrameRectAfterMoveRectangle_ToPointXY_CorrectFrameAfterMove)
  {
    jianing::Rectangle rectangle {{5.1, 10.1, 3.1, 4.1}};

    rectangle.move({10.9, 5.9});

    BOOST_CHECK_EQUAL(5.1, rectangle.getFrameRect().width);
    BOOST_CHECK_EQUAL(10.1, rectangle.getFrameRect().height);
    BOOST_CHECK_EQUAL(10.9, rectangle.getFrameRect().pos.x);
    BOOST_CHECK_EQUAL(5.9, rectangle.getFrameRect().pos.y);
  }

  BOOST_AUTO_TEST_CASE(TestFrameRectAfterMoveRectangle_Bydxdy_CorrectFrameAfterMove)
  {
    jianing::Rectangle rectangle {{1.1, 2.2, 3.3, 4.4}};

    rectangle.move(-0.8, 0.9);

    BOOST_CHECK_EQUAL(1.1, rectangle.getFrameRect().width);
    BOOST_CHECK_EQUAL(2.2, rectangle.getFrameRect().height);
    BOOST_CHECK_CLOSE(3.3 - 0.8, rectangle.getFrameRect().pos.x, accuracy);
    BOOST_CHECK_CLOSE(4.4 + 0.9, rectangle.getFrameRect().pos.y, accuracy);
  }
BOOST_AUTO_TEST_SUITE_END()

// =====================test scale rectangle=====================
BOOST_AUTO_TEST_SUITE(ScaleRectangle)
  BOOST_AUTO_TEST_CASE(ScaleRectangle_OnWrongCoefficientNegativeOrZero_ThrowError)
  {
    jianing::Rectangle rectangle = {{14.15, 9.26, 0.9, 4.8}};

    BOOST_CHECK_THROW(rectangle.scale(0.0), std::domain_error);
    BOOST_CHECK_THROW(rectangle.scale(-5.7), std::domain_error);
  }

  BOOST_AUTO_TEST_CASE(ScaleRectangle_OnCorrectCoef_CorrectWigthHeightXYAfterScale)
  {
    jianing::Rectangle rectangle {{5.5, 6.6, 7.7, 8.8}};

    rectangle.scale(2.1);

    BOOST_CHECK_EQUAL(7.7, rectangle.getCenter().x);
    BOOST_CHECK_EQUAL(8.8, rectangle.getCenter().y);
    BOOST_CHECK_CLOSE(5.5 * 2.1, rectangle.getWidth(), accuracy);
    BOOST_CHECK_CLOSE(6.6 * 2.1, rectangle.getHeight(), accuracy);
    BOOST_CHECK_CLOSE(2.1 * 5.5 * 2.1 * 6.6, rectangle.getArea(), accuracy);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test scale rectangle(FrameRect)=====================
BOOST_AUTO_TEST_SUITE(TestFrameRectAfterScaleRectangle)

  BOOST_AUTO_TEST_CASE(TestFrameRectAfterScaleRectangle_OnCorrectCoef_CorrectFrameAfterScale)
  {
    jianing::Rectangle rectangle {{5.5, 6.6, 7.7, 8.8}};

    rectangle.scale(2.1);
    BOOST_CHECK_CLOSE(5.5 * 2.1, rectangle.getFrameRect().width, accuracy);
    BOOST_CHECK_CLOSE(6.6 * 2.1, rectangle.getFrameRect().height, accuracy);
    BOOST_CHECK_EQUAL(7.7, rectangle.getFrameRect().pos.x);
    BOOST_CHECK_EQUAL(8.8, rectangle.getFrameRect().pos.y);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test rotate rectangle=====================
BOOST_AUTO_TEST_SUITE(RotateRectangle)

  BOOST_AUTO_TEST_CASE(RotateRectangle_InPositiveDirection_CorrectWigthHeightXYAfterRotate)
  {
    jianing::Rectangle rectangle {{2.3, 4.6, 5.7, 7.5}};

    rectangle.rotate(27);

    BOOST_CHECK_EQUAL(5.7, rectangle.getCenter().x);
    BOOST_CHECK_EQUAL(7.5, rectangle.getCenter().y);
    BOOST_CHECK_EQUAL(2.3, rectangle.getWidth());
    BOOST_CHECK_EQUAL(4.6, rectangle.getHeight());
    BOOST_CHECK_CLOSE(2.3 * 4.6, rectangle.getArea(), accuracy);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test rotate rectangle(FrameRect)=====================
BOOST_AUTO_TEST_SUITE(TestFrameRectAfterRotateRectangle)

  BOOST_AUTO_TEST_CASE(TestFrameRectAfterRotateRectangle_InPositiveDirection_CorrectFrameAfterRotate)
  {
    jianing::Rectangle rectangle {{4.0, 5.0, 2.0, 3.0}};

    rectangle.rotate(60);

    BOOST_CHECK_EQUAL(2.0, rectangle.getFrameRect().pos.x);
    BOOST_CHECK_EQUAL(3.0, rectangle.getFrameRect().pos.y);
    BOOST_CHECK_CLOSE(4.0 * std::fabs(cos(M_PI * 60 / 180)) + 5.0 * std::fabs(sin(M_PI * 60 / 180)), rectangle.getFrameRect().width, accuracy);
    BOOST_CHECK_CLOSE(5.0 * std::fabs(cos(M_PI * 60 / 180)) + 4.0 * std::fabs(sin(M_PI * 60 / 180)), rectangle.getFrameRect().height, accuracy);
  }

BOOST_AUTO_TEST_SUITE_END()