/**

Copyright (c) 2016, Aumann Florian, Heller Florian, Meißner Pascal
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

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

*/

#include <iostream>

#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */
#include <vector>
#include <MathHelpers/Resectionsolver/resectionsolver.h>
#include <iostream>     //inp
#include <unistd.h>
#include <QObject>
#include <math.h>
#include <Transformation/transformationfile_manager_XML.h>
#include <Transformation/transformationfile_manager_data.h>
#include <Transformation/transformation_data.h>
#ifndef Q_MOC_RUN
#include <Eigen/Dense>
#include <ros/ros.h>
#include <ros/init.h>
#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>
#endif
#include <CameraUtils/marker_manager.h>

/*class MarkerDetectionHelper : public QObject
{

   public slots:
     void markerFound(int markerNumber, Eigen::Matrix4d transformation)
     {

     }
};*/

using namespace std;
string workspacePath;

bool test_averageframe(string filename)
{
    std::ifstream input(filename);
    std::string line;
    unsigned int lineNumber = 1;
    std::vector<Eigen::Quaterniond> rotationVector;
    std::vector<Eigen::Vector3d> translationVector;
    while(std::getline( input, line ))
    {
        std::vector<std::string> strs;
        boost::split(strs, line, boost::is_any_of(";"));
        if (strs.size() == 7)
        {
            std::vector<double> numericValues;
            bool success = true;
            for (unsigned int i = 0; i < strs.size(); i++)
            {
                try
                {
                    std::string temp = strs.at(i);
                    boost::algorithm::trim(temp);
                    numericValues.push_back(boost::lexical_cast<double>(temp));
                }
                catch (boost::bad_lexical_cast const&)
                {
                    ROS_ERROR_STREAM("Line " << lineNumber << ": " <<strs.at(i) << " is not a number.");
                    success = false;
                    break;
                }
            }
            if (success)
            {
                Eigen::Quaterniond rotation(numericValues.at(3),numericValues.at(0),numericValues.at(1),numericValues.at(2));
                Eigen::Vector3d translation(numericValues.at(4),numericValues.at(5),numericValues.at(6));
                rotationVector.push_back(rotation);
                translationVector.push_back(translation);
            }
        }
        else
        {
            ROS_ERROR_STREAM("Line " << lineNumber << ": Expected 7 items, got " << strs.size());
        }
        lineNumber ++;
    }
    if (rotationVector.size() > 0)
    {
        Eigen::Vector3d averageTranslation = translationVector.at(0);
        Eigen::Quaterniond averageRotation = rotationVector.at(0);

        for (unsigned int i = 1; i < rotationVector.size(); i++)
        {
            Eigen::Quaterniond tempRotation  = rotationVector.at(i);
            Eigen::Vector3d tempTranslation = translationVector.at(i);
            averageTranslation = averageTranslation + tempTranslation;
            averageRotation = averageRotation.slerp((double)i/(i+1.0), tempRotation);
        }
        averageTranslation = averageTranslation * 1.0/(double)rotationVector.size();
        ROS_INFO_STREAM("Position X=" << averageTranslation(0) << " Y=" << averageTranslation(1) << " Z=" << averageTranslation(2));
        ROS_INFO_STREAM("Rotation w=" << averageRotation.w() << " x=" << averageRotation.x() << " y=" << averageRotation.y() << " z=" << averageRotation.z());
    }
    return true;
}

bool test_markerDetection()
{
    bool success = false;
    Marker_Manager markerManager(4.0, 3.0);
    //MarkerDetectionHelper helper;
    //connect(markerManager, SIGNAL(markerFound(int,Eigen::Matrix4d )),helper, SLOT(markerFound(int, Eigen::Matrix4d )));

    return success;
}

bool test_serialisation(Abstract_TransformationFile_Manager * manager)
{
    double pan, tilt;
    bool success = false;
    unsigned int testCount = 3;
    Eigen::Matrix4d m1_array[testCount];
    double pan_array[testCount];
    double tilt_array[testCount];
    for (unsigned int k = 0; k < testCount; k++)
    {
        Eigen::Matrix4d m1, m2;
        pan = rand() % 1000;
        tilt = rand() & 1000;
        pan_array[k] = pan;
        tilt_array[k] = tilt;
        double roll = fmod((double)rand(),2.0*M_PI);
        double pitch = fmod((double)rand(),2.0*M_PI);
        double yaw = fmod((double)rand(),2.0*M_PI);
        ROS_INFO_STREAM("Roll: " << roll << " pitch: " << pitch << " yaw: " << yaw);
        Eigen::Affine3d tr1 = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::Translation3d(rand() % 1000, rand()% 1000, rand()% 1000);
        roll = fmod((double)rand(),2.0*M_PI);
        pitch = fmod((double)rand(),2.0*M_PI);
        yaw = fmod((double)rand(),2.0*M_PI);
        ROS_INFO_STREAM("Roll: " << roll << " pitch: " << pitch << " yaw: " << yaw);
        Eigen::Affine3d tr2 = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::Translation3d(rand()% 1000, rand()% 1000, rand()% 1000);
        m1 = tr1.matrix();
        m2 = tr2.matrix();
        m1_array[k] = m1;
        manager->writeToFile(m1, m2, pan, tilt);
    }

    std::vector<Transformation_Data> dataCollection = manager->readFromFile();
    if (dataCollection.size() >= testCount)
    {
        success = true;
        for (unsigned int k = 0; k < testCount; k++)
        {
            Eigen::Matrix4d m1;
            m1 = m1_array[k];
            pan = pan_array[k];
            tilt = tilt_array[k];
            Transformation_Data data = dataCollection[k];
            success &= (fabs(pan - data.pan) < 0.00001);
            if (success) {cout << "Pan correct" <<  endl;}
            else {ROS_ERROR_STREAM("Pan incorrect. Expected " << pan << ", got " << data.pan << " difference " << fabs(pan - data.pan));return success;}
            success &= (fabs(tilt - data.tilt) < 0.00001);
            if (success) {cout << "Tilt correct" <<  endl;}
            else {ROS_ERROR_STREAM("Tilt incorrect. Expected " << tilt << ", got " << data.tilt << " difference " << fabs(tilt - data.tilt));return success;}
            for (unsigned int i = 0; i < 4; i++)
            {
                for (unsigned int j = 0; j < 4; j++)
                {
                    success &= (fabs(m1(i,j) - data.PTU_Frame(i,j)) < 0.001);
                    if (!success) {ROS_ERROR_STREAM("PTU frame incorrect. Expected " << m1(i,j) << ", got " << data.PTU_Frame(i,j) << " difference " << fabs(m1(i,j) - data.PTU_Frame(i,j)));return success;}
                }
            }
            if (success) {cout << "PTU frame correct" <<  endl;}
        }
    }
    else
    {
        ROS_ERROR_STREAM("Not enough dataNodes found. Expected " << testCount << ", got " << dataCollection.size());
    }
    return success;
}

bool test_resectionsolver()
{
     Eigen::Vector3d top, temp;
     Eigen::Vector2d base1, base2, base3;
     double angle12, angle23, angle31;
     double side1, side2, side3, sideBase;

     bool success;
     /* initialize random seed: */

         top[0] = (rand() % 1000 + 1) /100.0;
         top[1] = (rand() % 1000 + 1) /100.0;
         top[2] = (rand() % 1000 + 1) /500.0;

         base1[0]  = (rand() % 1000 + 1) /100.0;
         base1[1]  = (rand() % 1000 + 1) /100.0;
         base2[0]  = (rand() % 1000 + 1) /100.0;
         base2[1]  = (rand() % 1000 + 1) /100.0;
         base3[0]  = (rand() % 1000 + 1) /100.0;
         // Nur rechten Winkel zulassen
         base3[1]  = (base1[0]*base3[0]-base2[0]*base3[0]-base1[0]*base2[0]+base2[0]*base2[0]-base1[1]*base2[1]+base2[1]*base2[1])/(base2[1]-base1[1]);
         // Alle Winkel zulassen
         //base3[1]  = (rand() % 1000 + 1) /100.0;

         temp = Eigen::Vector3d(base1.x(), base1.y(), 0.0f);
         side1 = (top - temp).norm();
         temp = Eigen::Vector3d(base2.x(), base2.y(), 0.0f);
         side2 = (top - temp).norm();
         temp = Eigen::Vector3d(base3.x(), base3.y(), 0.0f);
         side3 = (top - temp).norm();
         sideBase = (base1 - base2).norm();
         angle12 = acos((-sideBase*sideBase + side1*side1 + side2*side2)/(2*side1*side2));

         sideBase = (base2 - base3).norm();
         angle23 = acos((-sideBase*sideBase + side3*side3 + side2*side2)/(2*side3*side2));

         sideBase = (base3 - base1).norm();
         angle31 = acos((-sideBase*sideBase + side1*side1 + side3*side3)/(2*side1*side3));

         cout << "Testing vectors [" << base1.x() << ", " << base1.y() << "] ";
         cout << "[" << base2.x() << ", " << base2.y() << "] ";
         cout << "[" << base3.x() << ", " << base3.y() << "]" << endl;
         cout << "with top-point [" << top.x() << ", " << top.y() << ", " << top.z() << "]" << endl;
         cout << "Distances " << side1 << ", " << side2 << ", " << side3 << "" << endl;
         cout << "Angles " << angle12 << ", " << angle23 << ", " << angle31 << "" << endl;

         //Call Testfunction
         boost::shared_ptr<Calibration_Object> calibrationObject(new Calibration_Object());
         calibrationObject->top_angle_ab = angle12;
         calibrationObject->top_angle_bc = angle23;
         calibrationObject->top_angle_ca = angle31;
         calibrationObject->side_a = (base1 - base2).norm();
         calibrationObject->side_b = (base2 - base3).norm();
         calibrationObject->side_c = (base1 - base3).norm();

         boost::shared_ptr<FeasibilityChecker> feasibilityChecker(new FeasibilityChecker(calibrationObject, Eigen::Vector3d(0.0,0.0,1.0), 0.15));
         ResectionSolver solver(calibrationObject, feasibilityChecker);
         solver.solve(base1, base2, base3);
         cout << "Results:" << endl;
         success = false;
         if (solver.solutions_a.size() == 0) {cout << "No solutions found." << endl;}
         for (unsigned int i = 0; i < solver.solutions_a.size(); i++)
         {
             if (abs(solver.solutions_b[i] - side1) <= 0.0001f && abs(solver.solutions_c[i] - side2) <= 0.0001f && abs(solver.solutions_a[i] - side3) <= 0.0001f)
             {
                 //system("Color 02");
                 cout << "[" << solver.solutions_b[i] << ", " << solver.solutions_c[i] << ", " << solver.solutions_a[i]<< "]" << endl;
                 success = true;
             }
             else
             {
                 cout << "[" << solver.solutions_b[i] << ", " << solver.solutions_c[i] << ", " << solver.solutions_a[i] << "]" << endl;
                 //system("Color 04");
             }
         }
         if (success) {
             cout << "Distance test successful" << endl;
             success = false;
             for (unsigned int i = 0; i < solver.solutions.size(); i++)
             {
                 if (abs(solver.solutions[i].x() - top.x()) <= 0.0001f && abs(solver.solutions[i].y() - top.y()) <= 0.0001f && abs(solver.solutions[i].z() - top.z()) <= 0.0001f)
                 {
                     //system("Color 02");
                     cout << "[" << solver.solutions[i][0] << ", " <<solver.solutions[i][1] << ", " << solver.solutions[i][2]<< "]" << endl;
                     success = true;
                 }
                 else
                 {
                     cout << "[" << solver.solutions[i][0] << ", " <<solver.solutions[i][1] << ", " << solver.solutions[i][2]<< "]" << endl;
                     //system("Color 04");
                 }
             }
             if (success) {cout << "Top-point test successful!" << endl;}
        }

    return success;
}

bool is_numeric(string string_)
{
    int sizeOfString = string_.size();
    int iteration = 0;
    bool isNumeric = true;

    while(iteration < sizeOfString)
    {
        if(!isdigit(string_[iteration]))
        {
            isNumeric = false;
            break;
        }

        iteration++;
    }
    return isNumeric;
}

bool start_test(string args)
{
    bool testFound = false;
    if (args.substr(0,9) == "resection")
    {
        string numerArg = args.substr(9);
        numerArg.erase(std::remove(numerArg.begin(), numerArg.end(), ' '), numerArg.end());
        srand (time(NULL));
        if (is_numeric(numerArg) && numerArg.size() > 0) {
            unsigned int numbers = atoi(numerArg.c_str());
            unsigned int success = 0;
            for (unsigned int i = 0;  i < numbers; i++)
            {
                if (test_resectionsolver()) {success++;}
                clock_t time_end;
                time_end = clock() + 20 * CLOCKS_PER_SEC/1000;
                while (clock() < time_end)
                {
                }
                cout <<  endl;
            }
            cout << success << "/" << numbers << " were successful." <<  endl;
        }
        else
        {
            int inp;
            do
            {
                test_resectionsolver();
                inp = cin.get();
            }
            while (inp != 32);
        }
        testFound = true;
    }
    else if (args.substr(0,13) == "serialization")
    {
        string numerArg = args.substr(13);
        numerArg.erase(std::remove(numerArg.begin(), numerArg.end(), ' '), numerArg.end());
        srand (time(NULL));
        if (numerArg.size() > 0) {
            std::string writer = numerArg.c_str();
            Abstract_TransformationFile_Manager * manager;
            if (writer == "XML")
            {
                manager = new TransformationFile_Manager_XML(workspacePath + "/test.xml");
            }
            else
            {
                manager = new TransformationFile_Manager_Data(workspacePath + "/test.data");
            }
            if (test_serialisation(manager))
            {
                ROS_INFO("Test was successful.");
            }
            else
            {
                ROS_INFO("Test failed.");
            }
        }
        testFound = true;
    }
    else if (args.substr(0,15) == "markerdetection")
    {
        test_markerDetection();
        testFound = true;
    }
    else if (args.substr(0,12) == "averageframe")
    {
        string filename = args.substr(13);
        filename.erase(std::remove(filename.begin(), filename.end(), ' '), filename.end());
        if (filename.size() > 0) test_averageframe(filename);
        testFound = true;
    }
    else
    {
        cout << "Testcase '" << args << "'not declared!" <<  endl;

    }
    return testFound;
}

int main(int argc, char *argv[])
{
    string inp = "";
    char *arg[0];
    int x = 0;
    ros::init(x, arg, "calibrationTool_tests");
    ros::NodeHandle nh(ros::this_node::getName());
    string myWorkspace;
    nh.getParam("test_workspace", myWorkspace);
    ROS_INFO_STREAM("Using workspace " << myWorkspace);
    workspacePath = myWorkspace;
    std::cout << "Please choose a test to run: " << std::endl;

    std::getline(std::cin, inp);
    string args_ = inp;
    while (args_ != "exit")
    {
        if (!start_test(args_))
        {
            std::cout << "Available tests are: " << std::endl;
            std::cout << " resection <count>" << std::endl;
            std::cout << " serialization XML|DATA" << std::endl;
            std::cout << " markerdetection" << std::endl;
            std::cout << " averageframe <filename>" << std::endl;
        }
        std::cout << "Please choose a test to run: " << std::endl;
        std::getline(std::cin, inp);
        args_ = inp;
    }

    return 0;
}


/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file SvdDecomposition.cpp
 * @date 06/06/2018
 * @author Alessandro Bianco
 */

/*!
 * @addtogroup DFNsTest
 *
 * Unit Test for the DFN implementation SvdDecomposition.
 *
 *
 * @{
 */

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */
#include <catch.hpp>
#include <BundleAdjustment/SvdDecomposition.hpp>
#include <Converters/MatToFrameConverter.hpp>
#include <Eigen/Dense>

using namespace CDFF::DFN::BundleAdjustment;
using namespace Converters;
using namespace CorrespondenceMap2DWrapper;
using namespace PoseWrapper;
using namespace BaseTypesWrapper;

/* --------------------------------------------------------------------------
 *
 * Test Cases
 *
 * --------------------------------------------------------------------------
 */
TEST_CASE( "Call to process (SVD Decomposition) and fail", "[failprocess]" )
{
	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int index = 0; index < 6; index++)
		{
		CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
		AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
		}

	// Instantiate DFN
	SvdDecomposition* svd = new SvdDecomposition;

	// Send input data to DFN
	svd->correspondenceMapsSequenceInput(*correspondenceMapsSequence);

	// Run DFN
	svd->process();

	// Query output data from DFN
	const Poses3DSequence& output = svd->posesSequenceOutput();
	bool success = svd->successOutput();

	REQUIRE(!success);

	// Cleanup
	delete(svd);
}

TEST_CASE( "Call to configure (SVD Decomposition)", "[configure]" )
{
	// Instantiate DFN
	SvdDecomposition* svd = new SvdDecomposition;

	// Setup DFN
	svd->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/SvdDecomposition_Conf1.yaml");
	svd->configure();

	// Cleanup
	delete(svd);
}

#define EPSILON 0.001
#define GROUND_TRUTH_EPSILON 0.01
#define CLOSE_GROUND_TRUTH(a, b) (b > a - GROUND_TRUTH_EPSILON && b < a + GROUND_TRUTH_EPSILON)
#define CLOSE(a, b) (b > a - EPSILON && b < a + EPSILON)
#define CLOSE_POSITION(pose, x, y, z) ( CLOSE(GetXPosition(pose), x) && CLOSE(GetYPosition(pose), y) && CLOSE(GetZPosition(pose), z))
#define CLOSE_HALF_ORIENTATION(pose, x, y, z, w) ( CLOSE(GetXOrientation(pose), x) && CLOSE(GetYOrientation(pose), y) && CLOSE(GetZOrientation(pose), z) && CLOSE(GetWOrientation(pose), w))
#define CLOSE_ORIENTATION(pose, x, y, z, w) ( CLOSE_HALF_ORIENTATION(pose, x, y, z, w) || CLOSE_HALF_ORIENTATION(pose, -x, -y, -z, -w) )

TEST_CASE( "Call to process (SVD Decomposition) Translation", "[processOnTranslation]" )
{
	return; // The test does not pass
	//Initialize Data
	int numberOfImages = 4;
	int numberOfPoints = 5;
	float baseline = 5;
	float smallTranslation = 5;
	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 1, 0, 0,   0, 1, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(0, baseline, 0);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[5] = {Eigen::Vector3f(0, 0, 5), Eigen::Vector3f(50, 0, 5), Eigen::Vector3f(0, 50, 5), Eigen::Vector3f(50, 50, 5), Eigen::Vector3f(10, 10, 2.5) };
	Eigen::Vector3f transformedPointVector[4][5];
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < numberOfPoints; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);
				AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);

				double mandatoryOutput;
				ASSERT(
					CLOSE(std::modf(std::abs(sourcePoint.x), &mandatoryOutput), 0) && CLOSE(std::modf(std::abs(sourcePoint.y), &mandatoryOutput), 0) &&
					CLOSE(std::modf(std::abs(sinkPoint.x), &mandatoryOutput), 0) && CLOSE(std::modf(std::abs(sinkPoint.y), &mandatoryOutput), 0)
					, "The test points should be interger pixel coordinates"
					);
				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}

	// Instantiate DFN
	SvdDecomposition* svd = new SvdDecomposition;

	// Setup DFN
	svd->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/SvdDecomposition_Conf1.yaml");
	svd->configure();

	// Send input data to DFN
	svd->correspondenceMapsSequenceInput(*correspondenceMapsSequence);

	// Run DFN
	svd->process();

	// Query output data from DFN
	const Poses3DSequence& output = svd->posesSequenceOutput();
	bool success = svd->successOutput();

	REQUIRE(success);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), 0, 5, 0);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), 5, 5, 0);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), 0, 0, 0, 1);
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), 0, 0, 0, 1);
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	// Cleanup
	delete(svd);
}

TEST_CASE( "Call to process (SVD Decomposition) RotoTranslation", "[processOnRotoTranslation]" )
{
	return; // This test does not pass, Ceres fails to converge
	//Initialize Data
	int numberOfImages = 4;
	int numberOfPoints = 5;
	float baseline = 5;
	float smallTranslation = 5;
	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 10, 0, 0,   0, 10, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(0, -baseline, 3*baseline);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[5] = {Eigen::Vector3f(5, 0, 10), Eigen::Vector3f(45, 450, 10), Eigen::Vector3f(20, 200, 10), Eigen::Vector3f(20, 150, 10), Eigen::Vector3f(20, 20, 5) };
	Eigen::Vector3f transformedPointVector[4][5];
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < numberOfPoints; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);

				// This is jus trying to extract the integer part as pixel coordinates. I cannot use the normal cast because I got something like static_cast<int>(-10) = -9.
				double decimalPart[4] = { sourcePoint.x, sourcePoint.y, sinkPoint.x, sinkPoint.y };
				double integerPart[4] = {0, 0, 0, 0};
				for(int index = 0; index < 4; index++)
					{
					while (decimalPart[index] > 0.5 || decimalPart[index] <= -0.5)
						{
						integerPart[index] += (decimalPart[index] > 0.5 ? 1 : -1);
						decimalPart[index] += (decimalPart[index] > 0.5 ? -1 : 1);
						}
					ASSERT( CLOSE_GROUND_TRUTH(decimalPart[index], 0), "The test points should be interger pixel coordinates");
					}
				sourcePoint.x = integerPart[0];
				sourcePoint.y = integerPart[1];
				sinkPoint.x = integerPart[2];
				sinkPoint.y = integerPart[3];
				AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);
				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}

	// Instantiate DFN
	SvdDecomposition* svd = new SvdDecomposition;

	// Setup DFN
	svd->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/SvdDecomposition_Conf2.yaml");
	svd->configure();

	// Send input data to DFN
	svd->correspondenceMapsSequenceInput(*correspondenceMapsSequence);

	// Run DFN
	svd->process();

	// Query output data from DFN
	const Poses3DSequence& output = svd->posesSequenceOutput();
	bool success = svd->successOutput();

	REQUIRE(success);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), 0, -baseline, 3*baseline);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), baseline, -baseline, 3*baseline);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	// Cleanup
	delete(svd);
}

TEST_CASE( "Call to process (SVD Decomposition) Rotation", "[processOnRotation]" )
{
	return; // This test does not pass, Ceres fails to converge
	//Initialize Data
	int numberOfImages = 4;
	int numberOfPoints = 5;
	float baseline = 5;
	float smallTranslation = 5;
	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 1, 0, 0,   0, 1, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(0, 0, 0);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[5] = {Eigen::Vector3f(0, 0, 5), Eigen::Vector3f(50, 0, 5), Eigen::Vector3f(0, 50, 5), Eigen::Vector3f(50, 50, 5), Eigen::Vector3f(20, 20, 2.5) };
	Eigen::Vector3f transformedPointVector[4][5];
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < numberOfPoints; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);

				// This is jus trying to extract the integer part as pixel coordinates. I cannot use the normal cast because I got something like static_cast<int>(-10) = -9.
				double decimalPart[4] = { sourcePoint.x, sourcePoint.y, sinkPoint.x, sinkPoint.y };
				double integerPart[4] = {0, 0, 0, 0};
				for(int index = 0; index < 4; index++)
					{
					while (decimalPart[index] > 0.5 || decimalPart[index] <= -0.5)
						{
						integerPart[index] += (decimalPart[index] > 0.5 ? 1 : -1);
						decimalPart[index] += (decimalPart[index] > 0.5 ? -1 : 1);
						}
					ASSERT( CLOSE(decimalPart[index], 0), "The test points should be interger pixel coordinates");
					}
				sourcePoint.x = integerPart[0];
				sourcePoint.y = integerPart[1];
				sinkPoint.x = integerPart[2];
				sinkPoint.y = integerPart[3];
				AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);

				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}

	// Instantiate DFN
	SvdDecomposition* svd = new SvdDecomposition;

	// Setup DFN
	svd->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/SvdDecomposition_Conf1.yaml");
	svd->configure();

	// Send input data to DFN
	svd->correspondenceMapsSequenceInput(*correspondenceMapsSequence);

	// Run DFN
	svd->process();

	// Query output data from DFN
	const Poses3DSequence& output = svd->posesSequenceOutput();
	bool success = svd->successOutput();

	REQUIRE(success);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), 0, 0, 0);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), -baseline, 0, 0);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	// Cleanup
	delete(svd);
}

TEST_CASE( "Call to process (SVD Decomposition) 90 Degree Rotation", "[processOn90DegreeRotation]" )
{
	return; // The test does not pass
	//Initialize Data
	int numberOfImages = 4;
	int numberOfPoints = 5;
	float baseline = 5;
	float smallTranslation = 5;
	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 10, 0, 0,   0, 10, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(-baseline, 0, baseline);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(-M_PI/2, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[5] = {Eigen::Vector3f(5, 0, 10), Eigen::Vector3f(45, 450, 10), Eigen::Vector3f(20, 200, 10), Eigen::Vector3f(20, 150, 10), Eigen::Vector3f(20, 20, 12.5) };
	Eigen::Vector3f transformedPointVector[4][5];
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < numberOfPoints; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);
				AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);

				double mandatoryOutput;
				ASSERT(
					CLOSE(std::modf(std::abs(sourcePoint.x), &mandatoryOutput), 0) && CLOSE(std::modf(std::abs(sourcePoint.y), &mandatoryOutput), 0) &&
					CLOSE(std::modf(std::abs(sinkPoint.x), &mandatoryOutput), 0) && CLOSE(std::modf(std::abs(sinkPoint.y), &mandatoryOutput), 0)
					, "The test points should be interger pixel coordinates"
					);
				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}

	// Instantiate DFN
	SvdDecomposition* svd = new SvdDecomposition;

	// Setup DFN
	svd->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/SvdDecomposition_Conf2.yaml");
	svd->configure();

	// Send input data to DFN
	svd->correspondenceMapsSequenceInput(*correspondenceMapsSequence);

	// Run DFN
	svd->process();

	// Query output data from DFN
	const Poses3DSequence& output = svd->posesSequenceOutput();
	bool success = svd->successOutput();

	REQUIRE(success);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), -baseline, 0, baseline);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), -baseline, 0, 0);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	//TRY TO recompute the transform with the value of the computation
	// Cleanup
	delete(svd);
}

/** @} */


#include <algorithm>
#include <bitset>
// #include <boost/algorithm/string/replace.hpp>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <map>
#include <set>
#include <string>
#include <vector>

using namespace std;

#define FOR(i, j, n) for (int i = j; i < n; i++)
#define OUT(s) cout << s << endl;
#define IN(a) cin >> a;
#define PACK(a, n)                                                             \
  for (int i = 0; i < n; i++)                                                  \
    cin >> a[i];
#define LIST(a, n)                                                             \
  for (int i = 0; i < n; i++)                                                  \
    cout << i << ": " << a[i] << endl;
#define PP(label, obj) cout << "[" << label << "] " << obj << endl;

inline int pow2_(int _x, int _r, int _n) {
  if (_n == 0)
    return _r;

  return pow2_(_x, _r * _x, _n - 1); //末尾再帰最適化
}
inline int pow2(int _x, int _n) {
  return pow2_(_x, 1, _n);
}

int main() {
  long long N;
  IN(N);

  string S;
  IN(S);

  OUT(S);

  vector<string> st;

  int ran = 5;
  while (ran > 2) {
    for (int k = 0; k < ran; k++) {
      // レンジを狭めていく ran
      int testNum = N - ran + 1;
      for (int i = 0; i < testNum; i += ran) {
        string sub = S.substr(k * i, ran);
        st.push_back(sub);
        OUT(sub);
      }
      sort(st.begin(), st.end());
      int uniqueCount = unique(st.begin(), st.end()) - st.begin();
      OUT(testNum - uniqueCount);
      if (testNum - uniqueCount > 0) {
        OUT(ran);
        return 0;
      }
      st.clear();
    }
    ran -= 1;
  }
  OUT(0);
}


#ifndef AIKIDO_CONSTRAINT_DIFFERENTIABLE_HPP_
#define AIKIDO_CONSTRAINT_DIFFERENTIABLE_HPP_

#include <memory>

#include <Eigen/Dense>

#include "aikido/common/pointers.hpp"
#include "aikido/statespace/StateSpace.hpp"

namespace aikido {
namespace constraint {

AIKIDO_DECLARE_POINTERS(Differentiable)

/// Enum for classifying constraints used in Differentiable.
/// Equality constraint is satisfied by f(x) = 0.
/// Inequality constraint is satisfied by f(x) <= 0.
enum class ConstraintType
{
  EQUALITY,
  INEQUALITY
};

/// A differentiable constraint.
/// Contains n constraints that can be evaluated in real-value.
class Differentiable
{
public:
  /// Destructor
  virtual ~Differentiable() = default;

  /// Gets the StateSpace that this constraint operates on.
  virtual statespace::ConstStateSpacePtr getStateSpace() const = 0;

  /// Returns a vector of constraints' types, i-th element correspoinding to
  /// the type of i-th constraint.
  virtual std::vector<ConstraintType> getConstraintTypes() const = 0;

  /// Size of constraints
  virtual std::size_t getConstraintDimension() const = 0;

  /// Get the value of constraints at _s.
  /// Should be 0 to satisfy equality constraints.
  /// Should be <=0 to satisfy inequality constraints.
  /// \param _s State to be evaluated at.
  /// \param[out] _out Vector to store the value. Length should match the number
  ///        of constraints.
  virtual void getValue(
      const statespace::StateSpace::State* _s, Eigen::VectorXd& _out) const = 0;

  /// Get the jacobian of constraints evaluated at _s,
  /// expressed in the frame each state space is expressed in).
  /// \param _s State to be evaluated at.
  /// \param[out] _out Jacobian matrix. The dimension should be the following:
  ///     SO3 StateSpace  : m x 3
  ///     SO2             : m x 1
  ///     SE2             : m x 3
  ///     SE3             : m x 6
  ///     Rn              : m x n
  ///     CartesianProduct: m x k (k = sum of all state jacobian cols)
  /// If Matrix of incorrect dimension is given, false will be returned.
  virtual void getJacobian(
      const statespace::StateSpace::State* _s, Eigen::MatrixXd& _out) const = 0;

  /// Get both Value and Jacobian.
  /// \param _s State to be evaluated.
  /// \param[out] _val Value of constraints.
  /// \param[out] _jac Jacoiban of constraints.
  virtual void getValueAndJacobian(
      const statespace::StateSpace::State* _s,
      Eigen::VectorXd& _val,
      Eigen::MatrixXd& _jac) const;
};

} // namespace constraint
} // namespace aikido

#endif


// Copyright 2017 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.
//
// Endian-dependent test

#include <boost/hash2/hash_append.hpp>
#include <boost/hash2/fnv1a.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/array.hpp>
#if !defined(BOOST_NO_CXX11_HDR_ARRAY)
# include <array>
#endif

template<class H, class R> void test( R r )
{
    {
        int v[4] = { 1, 2, 3, 4 };

        H h;

        hash_append( h, v[0] );
        hash_append( h, v[1] );
        hash_append( h, v[2] );
        hash_append( h, v[3] );

        BOOST_TEST_EQ( h.result(), r );
    }

    {
        int v[4] = { 1, 2, 3, 4 };

        H h;

        hash_append( h, v );

        BOOST_TEST_EQ( h.result(), r );
    }

    {
        int v[2][2] = { { 1, 2 }, { 3, 4 } };

        H h;

        hash_append( h, v );

        BOOST_TEST_EQ( h.result(), r );
    }

    {
        boost::array<int, 4> v = {{ 1, 2, 3, 4 }};

        H h;

        hash_append( h, v );

        BOOST_TEST_EQ( h.result(), r );
    }

#if !defined(BOOST_NO_CXX11_HDR_ARRAY)

    {
        std::array<int, 4> v = {{ 1, 2, 3, 4 }};

        H h;

        hash_append( h, v );

        BOOST_TEST_EQ( h.result(), r );
    }

#endif
}

int main()
{
    test<boost::hash2::fnv1a_32>( 1041505217ul );
    test<boost::hash2::fnv1a_64>( 9566659391000707361ull );

    return boost::report_errors();
}


// practice6.0

#include <iostream>

#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/type_traits/is_function.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/multiplies.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/size.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/mpl/max_element.hpp>
#include <boost/mpl/min_element.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/copy.hpp>

using namespace boost::mpl::placeholders;
namespace mpl = boost::mpl;
using namespace boost::lambda;

#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>
#include <algorithm>

struct none{};
template<class T=int>
struct wrapAssert : T
{
	typedef typename T type;
	BOOST_STATIC_ASSERT(0);
};

struct sizeandType_tag{};
template<class Size, class Type>
struct sizeAndType
{
//	typedef typename Size::value tag;
	typedef typename Size type;
	typedef typename Type value;
};

struct size2Int
{
	template<class T1>
	struct apply
	{
		typedef typename
			sizeAndType< mpl::integral_c<int, mpl::sizeof_<T1>::value >
			, T1> type;
	};
};

struct compare1
{
	template<class T1, class T2>
	struct apply
	{
		typedef typename T1 type;
		typedef typename T1 value;
	};
};

template<class S>
struct smallest
{
	typedef typename
		mpl::deref<
			typename mpl::min_element<
				typename mpl::copy<S>::type
				//typename mpl::transform<S,size2Int>::type
				,mpl::less<mpl::sizeof_<mpl::_1>, mpl::sizeof_<mpl::_1> >
				//, compare1

			>::type
		>::type type;
};

struct v :
	mpl::vector<double&, int[2], char[100], char, short, int[4], long[2], float[20]>
	//mpl::vector_c<int, 4,5,2,3,1,4,7,3,2>
{
};

struct test1 :
	mpl::eval_if<
		boost::is_same<
			short
			,typename smallest<v>::type
		>
		,none
		//,wrapAssert<>
		,none
	>
{};

//typedef mpl::int_< mpl::sizeof_<_>::value > toSize;

typedef mpl::sizeof_<_> toSize;
typedef mpl::plus<_, mpl::int_<5> > add5;
typedef mpl::vector_c<int,1,2,3> v123;
typedef mpl::transform<v123, toSize>::type v678;

struct test2 :
	mpl::eval_if<
		mpl::equal<v678
		, mpl::vector_c<int,4,4,4> >
		,none
		//,wrapAssert<>
		,none
	>
{
};

//BOOST_STATIC_ASSERT(  );

void main()
{

	typedef std::list<int> IntList;
	IntList iList;
	iList.push_back(0);
	iList.push_back(1);
	iList.push_back(2);
	iList.push_back(3);
	int x;
	//std::for_each(iList.begin(), iList.end(), bind(&x, mpl::_1) );

 	smallest<v>::type a;
	mpl::transform<v,size2Int>::type b;

// 	mpl::min_element<
//
// 		//typename mpl::copy<v>::type
// 		typename mpl::transform<v, add5>::type
//
// 		,//mpl::less<mpl::sizeof_<_>, mpl::sizeof_<_> >
// 		mpl::less<_,_>
// 	>::type b;
//
	v123 aa;
	v678 cc;
	int n0 = mpl::sizeof_<int>::value;
	int n1 = mpl::sizeof_<int[2]>::value;
	int n2 = mpl::sizeof_<char>::value;
	int n3 = mpl::sizeof_<double&>::value;

}


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

#ifndef MTL_SCALE_INCLUDE
#define MTL_SCALE_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <boost/numeric/mtl/matrix/map_view.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>
#include <boost/numeric/mtl/utility/enable_if.hpp>
#include <boost/numeric/mtl/utility/true_copy.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

    namespace vector {

	template <typename Value1, typename Vector>
	typename traits::enable_if_vector<Vector, scaled_view<typename mtl::traits::true_copy<Value1>::type, Vector> >::type
	inline scale(const Value1& value1, const Vector& vector)
	{
	    vampir_trace<2028> tracer;
	    return scaled_view<typename mtl::traits::true_copy<Value1>::type, Vector>(value1, vector);
	}
    }

    namespace matrix {

	template <typename Value1, typename Matrix>
	typename traits::enable_if_matrix<Matrix, scaled_view<Value1, Matrix> >::type
	inline scale(const Value1& value1, const Matrix& matrix)
	{
		vampir_trace<3029> tracer;
	    return scaled_view<Value1, Matrix> (value1, matrix);
	}
    }

    using vector::scale;
    using matrix::scale;

    namespace tfunctor {

	// AlgebraicCategory is by default tag::scalar
	template <typename Value1, typename Value2, typename AlgebraicCategory>
	struct scale
	{
	    typedef typename Multiplicable<Value1, Value2>::result_type result_type;
	    explicit scale(const Value1& v1) : v1(v1) {}

	    result_type operator() (const Value2& v2) const
	    {
		return v1 * v2;
	    }

	    Value1 value() const { return v1; }
	private:
	    Value1 v1;
	};

	template <typename Value1, typename Matrix>
	struct scale<Value1, Matrix, tag::matrix>
	{
	    typedef matrix::scaled_view<Value1, Matrix> result_type;
	    explicit scale(const Value1& v1) : v1(v1) {}

	    result_type operator() (const Matrix& matrix) const
	    {
		return result_type(v1, matrix);
	    }
	private:
	    typename mtl::traits::true_copy<Value1>::type v1;
	};

	template <typename Value1, typename Vector>
	struct scale<Value1, Vector, tag::vector>
	{
	    typedef typename mtl::traits::true_copy<Value1>::type type1;

	    typedef vector::scaled_view<type1, Vector> result_type;
	    explicit scale(const Value1& v1) : v1(type1(v1)) {}

	    result_type operator() (const Vector& vector) const
	    {
		return result_type(v1, vector);
	    }
	private:
	    type1 v1;
	};

    } // namespace tfunctor

} // namespace mtl

#endif // MTL_SCALE_INCLUDE


#include <vector>
#include <functional>
#include <random>
#include <chrono>

#include <NTL/ZZ.h>

#include "../src/elements/element.hpp"
#include "../src/algorithms/rem_forest.hpp"
#include "../src/algorithms/rem_chunk.hpp"
#include "../src/algorithms/utils.hpp"

#include "../src/algorithms/tree_io.hpp"

#include "wolstenholme.hpp"
#include "kurepa.hpp"
#include "trinomial.hpp"

using std::vector;
using NTL::ZZ;

vector<Elt<ZZ>> A_main(long lower, long upper){
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; i++){
        if(i == 0){
            output[0] = Elt<ZZ>(1);
        }
        else{
            output[i-lower] = Elt<ZZ>(i);
        }
    }
    return output;
}

vector<Elt<ZZ>> m_main(long lower, long upper){
    vector<Elt<ZZ>> output(upper-lower);
    for(long i = lower; i < upper; i++){
        ZZ num(i+1);
        if(ProbPrime(num)){
	  power(num, num, 2);
	  output[i-lower] = Elt<ZZ>(num);
        }
        else{
            output[i-lower] = Elt<ZZ>(1);
        }
    }
    return output;
}

int main()
{

    // std::string line = "x^5+43*x^2-x-9";
    // parse_matrix_entry(line);

    std::function<vector<Elt<ZZ>> (long, long)> A_gen = A_main;
    std::function<vector<Elt<ZZ>> (long, long)> m_gen = m_main;

    PolyMatrix mat = parse_matrix_formula("[x]");

    //vector<std::function<vector<Elt<ZZ>> ()>> chunks = chunkify(A_gen, m_gen, 0, 1<<26, 1<<21, 0, 0, mat);

    vector<std::function<vector<Elt<ZZ> > ()> > numerators = chunkify(gen_wolstenholme_numerator, gen_fifth_prime_power, 0, 1<<24, 1<<19, 0, 0, mat);

    vector<std::function<vector<Elt<ZZ> > ()> > denominators = chunkify(gen_wolstenholme_denominator, gen_fourth_prime_power, 0, 1<<24, 1<<19, 0, 0, mat);

    std::cout << "Finished chunkifying!" << std::endl;

    auto start = std::chrono::high_resolution_clock::now();

    Elt<ZZ>p(1);
    for(long i = 0, chunks_size = chunks.size(); i < chunks_size; i++){
  		std::cout << " i = " << i << std::endl;
        vector<Elt<ZZ> > chunknums = numerators[i]();
	vector<Elt<ZZ> > chunkdenoms = denominators[i]();

	for (auto&& i : chunknums) {
	  i.t /= p.t;

	  ZZ mod;
	  power(mod, p.1, 4);

	}
	for (auto&& i : chunkreturn) {
	  if (i.t+1 == (p*p).t) {
	    std::cout << "Found one! It was: " << p << std::endl;
	  }

	  p.t += 1;
	}
    }

    auto end = std::chrono::high_resolution_clock::now();
	auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds> (end-start).count();

	std::cout << "TIME TAKEN: " << elapsed << " MILLISECONDS" << std::endl;
}


#ifndef INLIERDETECTOR_HPP
#define INLIERDETECTOR_HPP

#include <Eigen/Dense>
#include <stdio.h>
#include <iostream>
#include <map>
#include "../global.hpp"
#include "NeighbourFinder.hpp"

typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::Matrix< float, 1, registration::NUM_FEATURES> FeatureVec; //matrix Mx6 of type float
typedef Eigen::Vector3f Vec3Float;
typedef Eigen::Matrix< float, Eigen::Dynamic, 3> Vec3Mat; //matrix Mx3 of type float
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic> MatDynFloat;

namespace registration {

class InlierDetector
{
    /*
    # GOAL
    Determine which elements are inliers ('normal') and which are outliers
    ('abnormal'). There are different ways to to that, but they should all
    result in a scalar assigmnent that represents the probability of an element
    being an inlier.

    # INPUTS
    -_inFeatures
    -_inCorrespondingFeatures
    -_inCorrespondingFlags

    # PARAMETERS
    -_kappa(=3): Mahalanobis distance that determines cut-off in- vs outliers

    # OUTPUTS
    -_ioProbability

    # RETURNS
    */

    private:
        //# Inputs
        const FeatureMat * _inFeatures = NULL;
        const FeatureMat * _inCorrespondingFeatures = NULL;
        const VecDynFloat * _inCorrespondingFlags = NULL;

        //# Outputs
        VecDynFloat *_ioProbability = NULL;

        //# User Parameters
        float _kappa = 3.0;
        bool _useOrientation = true;

        //# Internal variables
        size_t _numElements;
        float _minimalSigma = 0.1f;
        float _maximalSigma = 10.0f;
        const size_t _numNeighbours = 10;
        const float _minWeight = 0.00001f;
        const size_t _numSmoothingPasses = 2;

        //# Internal Data structures
        NeighbourFinder<Vec3Mat> _neighbourFinder;
        MatDynFloat _smoothingWeights;

        //# Internal functions
        //## Find nearest neighbours (required for smoothing inlier weights)
        void _determine_neighbours();
        //## Update the weights used for smoothing the inlier weights (distance-based)
        void _update_smoothing_weights();
        //## Smooth the inlier weights
        void _smooth_inlier_weights();

    protected:

    public:
        void set_input(const FeatureMat * const inFeatures,
                        const FeatureMat * const inCorrespondingFeatures,
                        const VecDynFloat * const inCorrespondingFlags);
        void set_output(VecDynFloat * const _ioProbability);
        void set_parameters(const float kappa, const bool useOrientation);
        void update();
};

} //namespace registration

#endif


#include "Mesh.h"
#include "VisualizerImpl.h"

#include <Eigen/Geometry>

#include <iostream>

namespace VolViz {
namespace Private_ {

using Lock = std::lock_guard<std::mutex>;

Mesh::Mesh(MeshDescriptor const &descriptor, VisualizerImpl &visualizer)
    : Geometry(descriptor, visualizer) {
  scale = descriptor.scale;
  updateQueue_.enqueue(descriptor);
}

void Mesh::doInit() { uploadMesh(); }

void Mesh::doRender(std::uint32_t index, bool selected) {
  Length const rScale = visualizer_.cachedScale;
  auto cameraClient = visualizer_.cameraClient();
  auto &shaders = visualizer_.shaders();

  auto const viewMat = cameraClient.viewMatrix(rScale);
  auto const destScale = static_cast<float>(scale / rScale);
  // auto const volSize = visualizer_.volumeSize();

  Matrix4 const modelMat =
      (Eigen::Translation3f(position) * orientation * Eigen::Scaling(destScale))
          .matrix();
  // Matrix4 const modelMat = (Eigen::Translation3f(position * destScale) *
  //                           orientation * volSize.asDiagonal())
  //                              .matrix();
  Matrix4 const modelViewMat = (viewMat * modelMat).eval();
  Matrix3 const inverseModelViewMatrix =
      modelViewMat.block<3, 3>(0, 0).inverse();

  // Setup shader uniforms
  assertGL("Pevious OpenGL error");
  shaders["geometryStage"].use();
  visualizer_.attachVolumeToShader(shaders["geometryStage"]);
  shaders["geometryStage"]["index"] = index;
  shaders["geometryStage"]["color"] = selected ? (color * 1.2f).eval() : color;
  shaders["geometryStage"]["shininess"] = selected ? 10.0f : 1000.f;
  shaders["geometryStage"]["modelViewProjectionMatrix"] =
      (cameraClient.projectionMatrix() * modelViewMat).eval();
  shaders["geometryStage"]["inverseModelViewMatrix"] = inverseModelViewMatrix;
  shaders["geometryStage"]["textureTransformMatrix"] =
      (visualizer_.textureTransformationMatrix() * modelMat).eval();

  assertGL("Setting uniforms failed");

  auto const vaoBinding = GL::binding(vertexArrayObject_);
  glDrawElements(GL_TRIANGLES, 3 * static_cast<GLsizei>(numTriangles_),
                 GL_UNSIGNED_INT, nullptr);
  assertGL("glDrawElements failed");
}

void Mesh::doUpdate() { uploadMesh(); }

void Mesh::uploadMesh() {
  using GL::Buffer;
  using GL::VertexArray;

  MeshDescriptor descriptor;

  if (!updateQueue_.try_dequeue(descriptor)) return;

  auto const N = descriptor.vertices.rows();
  auto const M = descriptor.indices.rows();
  auto const vertBuffSize = N * 8 * narrow_cast<int>(sizeof(float));
  auto const indexBufferSize = M * 3 * narrow_cast<int>(sizeof(std::uint32_t));

  // Create and map vertex buffer
  Buffer vertBuffer;
  auto const vertBinding =
      binding(vertBuffer, static_cast<GLenum>(GL_ARRAY_BUFFER));
  glBufferData(GL_ARRAY_BUFFER, vertBuffSize, nullptr, GL_STATIC_DRAW);
  assertGL("glBufferData failed");
  float *mappedVertBuffer =
      reinterpret_cast<float *>(glMapBuffer(GL_ARRAY_BUFFER, GL_READ_WRITE));

  Ensures(mappedVertBuffer != nullptr);

  Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 8, Eigen::RowMajor>> vertices(
      mappedVertBuffer, N, 8);

  // Create and map index buffer
  Buffer indexBuffer;
  auto const indexBinding =
      binding(indexBuffer, static_cast<GLenum>(GL_ELEMENT_ARRAY_BUFFER));
  glBufferData(GL_ELEMENT_ARRAY_BUFFER, indexBufferSize, nullptr,
               GL_STATIC_DRAW);
  std::uint32_t *mappedIndexBuffer = reinterpret_cast<std::uint32_t *>(
      glMapBuffer(GL_ELEMENT_ARRAY_BUFFER, GL_WRITE_ONLY));
  Ensures(mappedIndexBuffer != nullptr);
  Eigen::Map<Eigen::Matrix<std::uint32_t, Eigen::Dynamic, 3, Eigen::RowMajor>>
      indices(mappedIndexBuffer, M, 3);

  assertGL("Failed to create or map buffers");

  // copy vertices
  for (int i = 0; i < N; ++i) {
    vertices.row(i) << descriptor.vertices.row(i), 0.f, 0.f, 0.f, 0.f, 0.f;
  }

  // copy indices
  indices = descriptor.indices;

  // compute normals
  for (int i = 0; i < M; ++i) {
    auto const &I = descriptor.indices;
    auto &V = vertices;
    Vector3f const normal =
        (V.row(I(i, 1)) - V.row(I(i, 0)))
            .head<3>()
            .cross((V.row(I(i, 2)) - V.row(I(i, 0))).head<3>())
            .normalized();
    V.block<1, 3>(I(i, 0), 4) += normal;
    V.block<1, 3>(I(i, 1), 4) += normal;
    V.block<1, 3>(I(i, 2), 4) += normal;
  }

  // normalize normals
  for (int i = 0; i < N; ++i) vertices.block<1, 3>(i, 4).normalize();

  // unmap buffers
  glUnmapBuffer(GL_ELEMENT_ARRAY_BUFFER);
  glUnmapBuffer(GL_ARRAY_BUFFER);

  // setup VAO
  VertexArray vao;
  auto const vaoBinding = binding(vao);
  indexBuffer.bind(GL_ELEMENT_ARRAY_BUFFER);
  vao.enableVertexAttribArray(0);
  vao.enableVertexAttribArray(1);
  glVertexAttribPointer(0, 3, GL_FLOAT, false, 8 * sizeof(float), nullptr);
  glVertexAttribPointer(1, 3, GL_FLOAT, false, 8 * sizeof(float),
                        reinterpret_cast<void const *>(4 * sizeof(float)));

  vertexBuffer_ = std::move(vertBuffer);
  indexBuffer_ = std::move(indexBuffer);
  vertexArrayObject_ = std::move(vao);
  numTriangles_ = static_cast<std::size_t>(M);
}

void Mesh::doEnqueueUpdate(GeometryDescriptor const &descriptor) {
  updateQueue_.enqueue(dynamic_cast<MeshDescriptor const &>(descriptor));
}

void Mesh::doEnqueueUpdate(GeometryDescriptor &&descriptor) {
  updateQueue_.enqueue(std::move(dynamic_cast<MeshDescriptor &&>(descriptor)));
}

} // namespace Private_
} // namespace VolViz


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "cuNDArray_fileio.h"
#include "cuNDArray_math.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "cuPartialDifferenceOperator.h"
#include "cuTvOperator.h"
#include "cuTv1dOperator.h"
#include "cuTvPicsOperator.h"
#include "CBSubsetOperator.h"
#include "osSPSSolver.h"
#include "osMOMSolver.h"
#include "cuOSMOMSolverD.h"
#include "osMOMSolverD2.h"
#include "hoCuNCGSolver.h"
#include "osMOMSolverD3.h"
#include "osMOMSolverL1.h"
#include "osMOMSolverF.h"
#include "osAHZCSolver.h"
#include "ADMMSolver.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <boost/make_shared.hpp>
#include <GPUTimer.h>
#include <operators/cuGaussianFilterOperator.h>
#include <multiplicationOperatorContainer.h>
#include <cuDownsampleOperator.h>
#include <operators/cuSmallConvOperator.h>
#include <denoise/nonlocalMeans.h>
#include <solvers/osMOMSolverW.h>
#include <operators/cuTFFT.h>
#include "cuSolverUtils.h"
#include "osPDsolver.h"
#include "osLALMSolver.h"
#include "osLALMSolver2.h"
#include "cuATrousOperator.h"
#include "hdf5_utils.h"
#include "cuEdgeATrousOperator.h"
#include "cuDCTOperator.h"
#include "cuDCTDerivativeOperator.h"
#include "dicomWriter.h"
#include "conebeam_projection.h"
#include "weightingOperator.h"
#include "hoNDArray_math.h"
#include "cuNCGSolver.h"
#include "CT_acquisition.h"
#include "cuScaleOperator.h"
#include "cuTVPrimalDualOperator.h"
#include "cuWTVPrimalDualOperator.h"
#include "cuATVPrimalDualOperator.h"
#include "cuSSTVPrimalDualOperator.h"
#include "cuBilateralPriorPrimalDualOperator.h"
#include "cuTV4DPrimalDualOperator.h"
#include "CBSubsetWeightOperator.h"
#include "BilateralPriorOperator.h"
#include "cuPICSDualOperator.h"
#include "cuTVTFFT.h"

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr<hoCuNDArray<float>> downsample_projections(hoCuNDArray<float>* projections, unsigned int num_downsamples )
{

    if (num_downsamples == 0) return boost::make_shared<hoCuNDArray<float>>(*projections);

    auto tmp = Gadgetron::downsample<float,2>(projections);

    for (int k = 1; k < num_downsamples; k++)
        tmp = Gadgetron::downsample<float,2>(tmp.get());

    return boost::make_shared<hoCuNDArray<float>>(*tmp);
}

boost::shared_ptr<cuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
    std::cout << "Calculating FDK prior" << std::endl;
    boost::shared_ptr<CBCT_binning> binning_pics=binning->get_3d_binning();
    std::vector<size_t> is_dims3d = is_dims;
    is_dims3d.pop_back();
    boost::shared_ptr< hoCuConebeamProjectionOperator >
            Ep( new hoCuConebeamProjectionOperator() );
    Ep->setup(ps,binning_pics,imageDimensions);
    Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
    Ep->set_domain_dimensions(&is_dims3d);
    Ep->set_use_filtered_backprojection(true);
    boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
    Ep->mult_MH(&projections,prior3d.get());

    hoCuNDArray<float> tmp_proj(*ps->get_projections());
    Ep->mult_M(prior3d.get(),&tmp_proj);
    float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
    *prior3d *= s;
    auto prior = boost::make_shared<cuNDArray<float>>(*prior3d);
    return prior;
}

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

    string acquisition_filename;
    string outputFile;

    uintd3 imageSize;
    floatd3 voxelSize;
    int device;
    floatd2 scale_factor;
    unsigned int iterations;
    unsigned int subsets;
    float rho,tau,nlm_noise,bil_weight;
    float tv_weight,pics_weight, wavelet_weight,huber,sigma,dct_weight,sfr_weight,framelet_weight,atv_weight, sstv_weight;
    float framelet_weight4d;
    float tv_4d,atv_4d;
    bool use_non_negativity;
    int reg_iter;

    po::options_description desc("Allowed options");

    desc.add_options()
            ("help", "produce help message")
            ("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
            ("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    ("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
            ("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
            ("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
            ("SAG","Use exact SAG correction if present")
            ("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
            ("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
            ("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    ("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
            ("downsample,D",po::value<floatd2>(&scale_factor)->default_value(floatd2(1,1)),"Downsample projections this factor")
            ("subsets,u",po::value<unsigned int>(&subsets)->default_value(10),"Number of subsets to use")
    ("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight in spatial dimensions")
            ("TV4D",po::value<float>(&tv_4d)->default_value(0),"Total variation weight in temporal dimensions")
            ("ATV4D",po::value<float>(&atv_4d)->default_value(0),"Advanced Total variation weight in temporal dimensions")
            ("ATV",po::value<float>(&atv_weight)->default_value(0),"Advanced Total variation weight ")
            ("SSTV",po::value<float>(&sstv_weight)->default_value(0),"Scale space total variation weight ")
            ("PICS",po::value<float>(&pics_weight)->default_value(0),"PICS weight")
            ("Wavelet,W",po::value<float>(&wavelet_weight)->default_value(0),"Weight of the wavelet operator")
            ("Framelet",po::value<float>(&framelet_weight)->default_value(0),"Weight of the framelet operator")
            ("Framelet4D",po::value<float>(&framelet_weight4d)->default_value(0),"Weight of the framelet operator")
            ("Huber",po::value<float>(&huber)->default_value(0),"Huber weight")
            ("use_prior","Use an FDK prior")
            ("use_non_negativity",po::value<bool>(&use_non_negativity)->default_value(true),"Prevent image from having negative attenuation")
            ("sigma",po::value<float>(&sigma)->default_value(0.1),"Sigma for billateral filter")
            ("DCT",po::value<float>(&dct_weight)->default_value(0),"DCT regularization")
            ("SFR",po::value<float>(&sfr_weight)->default_value(0),"SFR regularization")
            ("3D","Only use binning for selecting valid projections")
            ("tau",po::value<float>(&tau)->default_value(1e-5),"Tau value for solver")
            ("reg_iter",po::value<int>(&reg_iter)->default_value(2))
            ("bilateral-weight",po::value<float>(&bil_weight)->default_value(0),"Bilateral weight")
            ("prior",po::value<string>(),"prior image")
            ("projection_weights",po::value<string>(),"Array containing weights to be applied to the projections.")

            ("NLM",po::value<float>(&nlm_noise)->default_value(0),"Use non-local means based on the 3d image")
            ;

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

    if (vm.count("help")) {
        cout << desc << "\n";
        return 1;
    }

    std::stringstream command_line_string;
    std::cout << "Command line options:" << std::endl;
    for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
        boost::any a = it->second.value();
        command_line_string << it->first << ": ";
        if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
        else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
        else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
        else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
        else if (a.type() == typeid(vector_td<float,3>)) command_line_string << it->second.as<vector_td<float,3> >();
        else if (a.type() == typeid(vector_td<float,2>)) command_line_string << it->second.as<vector_td<float,2> >();
        else if (a.type() == typeid(vector_td<int,3>)) command_line_string << it->second.as<vector_td<int,3> >();
        else if (a.type() == typeid(vector_td<unsigned int,3>)) command_line_string << it->second.as<vector_td<unsigned int,3> >();
        else if (a.type() == typeid(bool)) command_line_string << it->second.as<bool>();
        else command_line_string << "Unknown type" << std::endl;
        command_line_string << std::endl;
    }
    std::cout << command_line_string.str();

    cudaSetDevice(device);
    cudaDeviceReset();

    //Really weird stuff. Needed to initialize the device?? Should find real bug.
    cudaDeviceManager::Instance()->lockHandle();
    cudaDeviceManager::Instance()->unlockHandle();

    boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
    ps->load(acquisition_filename);
    ps->get_geometry()->print(std::cout);

    float SDD = ps->get_geometry()->get_SDD();
    float SAD = ps->get_geometry()->get_SAD();

    boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
    if (vm.count("binning")){
        std::cout << "Loading binning data" << std::endl;
        binning->load(vm["binning"].as<string>());
        if (vm.count("3D"))
            binning = binning->get_3d_binning();
    } else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
    binning->print(std::cout);

    floatd3 imageDimensions;
    if (vm.count("dimensions")){
        imageDimensions = vm["dimensions"].as<floatd3>();
        voxelSize = imageDimensions/imageSize;
    }
    else imageDimensions = voxelSize*imageSize;

    float lengthOfRay_in_mm = norm(imageDimensions);
    unsigned int numSamplesPerPixel = 3;
    float minSpacing = min(voxelSize)/numSamplesPerPixel;

    unsigned int numSamplesPerRay;
    if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
    else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

    float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
    size_t numProjs = ps->get_projections()->get_size(2);
    size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
    std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

    std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
    std::cout << "Image size " << imageDimensions << std::endl;
    std::cout << "FRAMELET WEIGHT " << framelet_weight << std::endl;
    is_dims.push_back(binning->get_number_of_bins());

    //scatter_correct(binning,ps,ps->get_projections().get(),is_dims,imageDimensions);

    if (scale_factor[0] != 1 || scale_factor[1] != 1)
        ps->downsample(scale_factor[0],scale_factor[1]);

    //osLALMSolver<cuNDArray<float>> solver;
    osMOMSolverD<cuNDArray<float>> solver;
    //osMOMSolverW<cuNDArray<float>> solver;
    //osMOMSolverL1<cuNDArray<float>> solver;
    //osAHZCSolver<cuNDArray<float>> solver;
    //osMOMSolverF<cuNDArray<float>> solver;
    //ADMMSolver<cuNDArray<float>> solver;
    solver.set_dump(false);

    boost::shared_ptr<cuNDArray<float>> prior;
    if (vm.count("use_prior") ) {
        auto projections = *ps->get_projections();
        prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
        solver.set_x0(expand(prior.get(),binning->get_number_of_bins()));
        //prior = calculate_weightImage(binning,ps,projections,is_dims,imageDimensions);
//        solver.set_x0(prior);
    }
    if (vm.count("prior")){
        prior = boost::make_shared<cuNDArray<float>>(*read_nd_array<float>(vm["prior"].as<string>().c_str()));
    }

    solver.set_max_iterations(iterations);
    solver.set_output_mode(osSPSSolver<cuNDArray<float>>::OUTPUT_VERBOSE);
    solver.set_tau(tau);
    solver.set_non_negativity_constraint(use_non_negativity);
    solver.set_huber(huber);
    solver.set_reg_steps(reg_iter);
    //solver.set_rho(rho);

    if (atv_weight > 0) {

        auto ATV = boost::make_shared<cuATVPrimalDualOperator<float>>(0.0);
        ATV->set_weight(atv_weight);
        solver.add_regularization_operator(ATV);

    }

    if (sstv_weight > 0){
        auto SSTV = boost::make_shared<cuSSTVPrimalDualOperator<float>>();
        SSTV->set_weight(sstv_weight);
        solver.add_regularization_operator(SSTV);
    }

    if (tv_weight > 0) {

        //auto TV = boost::make_shared<cuWTVPrimalDualOperator<float>>(0.0,1e-3);
//

        auto TV = boost::make_shared<cuTVPrimalDualOperator<float>>(0.0);
        TV->set_weight(tv_weight);

        solver.add_regularization_operator(TV);

/*
      auto Dx = boost::make_shared<cuPartialDifferenceOperator<float, 3>>(0);
      Dx->set_weight(tv_weight);
      Dx->set_domain_dimensions(&is_dims);
      Dx->set_codomain_dimensions(&is_dims);

      auto Dy = boost::make_shared<cuPartialDifferenceOperator<float, 3>>(1);
      Dy->set_weight(tv_weight);
      Dy->set_domain_dimensions(&is_dims);
      Dy->set_codomain_dimensions(&is_dims);

      auto Dz = boost::make_shared<cuPartialDifferenceOperator<float, 3>>(2);
      Dz->set_weight(tv_weight);
      Dz->set_domain_dimensions(&is_dims);
      Dz->set_codomain_dimensions(&is_dims);
      solver.add_regularization_group({Dx, Dy, Dz});
*/

    }
    if (tv_4d > 0) {

        auto Dt = boost::make_shared<cuPartialDifferenceOperator<float, 4>>(3);
        Dt->set_weight(tv_4d);
        Dt->set_domain_dimensions(&is_dims);
        Dt->set_codomain_dimensions(&is_dims);
        solver.add_regularization_operator(Dt);
/**/

    }
    if (atv_4d > 0) {

        auto TVF =  boost::make_shared<cuTVTFFT>();
        TVF->set_weight(atv_4d);
        TVF->set_domain_dimensions(&is_dims);
        solver.add_regularization_operator(TVF);

/*
		  auto Dt = boost::make_shared<cuTV4DPrimalDualOperator<float>>(0);
		  Dt->set_weight(atv_4d);
		  solver.add_regularization_operator(Dt);
*/

    }

    if (framelet_weight > 0){
        auto stencils = std::vector<vector_td<float,3>>({
                                                                vector_td<float,3>(-std::sqrt(2.0f),0,std::sqrt(2.0f)),vector_td<float,3>(-1,2,-1),vector_td<float,3>(1,2,1) });
        std::cout << "Framelet weight " << framelet_weight << std::endl;
        for (int step_size = 0; step_size < 3; step_size++) {
            for (auto stencil : stencils) {
                std::cout << "Adding operators " << std::endl;
                auto Rx = boost::make_shared<cuSmallConvOperator<float, 4, 3>>(stencil, 0,std::pow(2,step_size));
                Rx->set_weight(framelet_weight);
                Rx->set_domain_dimensions(&is_dims);
                Rx->set_codomain_dimensions(&is_dims);

                auto Ry = boost::make_shared<cuSmallConvOperator<float, 4, 3>>(stencil, 1,std::pow(2,step_size));
                Ry->set_weight(framelet_weight);
                Ry->set_domain_dimensions(&is_dims);
                Ry->set_codomain_dimensions(&is_dims);

                auto Rz = boost::make_shared<cuSmallConvOperator<float, 4, 3>>(stencil, 2,std::pow(2,step_size));
                Rz->set_weight(framelet_weight);
                Rz->set_domain_dimensions(&is_dims);
                Rz->set_codomain_dimensions(&is_dims);
                solver.add_regularization_group({Rx, Ry, Rz});
//                solver.add_regularization_operator(Rx);
//                solver.add_regularization_operator(Ry);
//                solver.add_regularization_operator(Rz);

            }
        }
    }
    if (framelet_weight4d > 0){
        auto stencils = std::vector<vector_td<float,3>>({
                                                                vector_td<float,3>(-std::sqrt(2.0f),0,std::sqrt(2.0f)),vector_td<float,3>(-1,2,-1),vector_td<float,3>(1,2,1) });
        for (int step_size = 0; step_size < 3; step_size++) {
            for (auto stencil : stencils) {
                auto Rt = boost::make_shared<cuSmallConvOperator<float, 4, 3>>(stencil, 3,std::pow(2,step_size));
                Rt->set_weight(framelet_weight4d);
                Rt->set_domain_dimensions(&is_dims);
                Rt->set_codomain_dimensions(&is_dims);
                solver.add_regularization_operator(Rt);
            }
        }
    }

    if (dct_weight > 0){
        auto dctOp = boost::make_shared<cuDCTOperator<float>>();
//        auto dctOp = boost::make_shared<cuTFFT>();
        dctOp->set_domain_dimensions(&is_dims);
        dctOp->set_weight(dct_weight);
        solver.add_regularization_operator(dctOp);
    }

    if (sfr_weight > 0){
        auto sfrOp = boost::make_shared<cuTFFT>();
        sfrOp->set_domain_dimensions(&is_dims);
        sfrOp->set_weight(sfr_weight);
        solver.add_regularization_operator(sfrOp);
    }

    if (bil_weight > 0){
        auto bilOp = boost::make_shared<cuBilateralPriorPrimalDualOperator>(0.002,3.0,prior);
        bilOp->set_weight(bil_weight);
        solver.add_regularization_operator(bilOp);

        /*
        auto bilOp = boost::make_shared<BilateralPriorOperator>();
        bilOp->set_domain_dimensions(&is_dims);
        bilOp->set_codomain_dimensions(&is_dims);
        bilOp->set_weight(bil_weight);
        bilOp->set_sigma_spatial(3.0);
        bilOp->set_sigma_int(0.01);

        bilOp->set_prior(cuBilPrior);
        solver.add_regularization_operator(bilOp,0.01);
         */
    }

    if (pics_weight > 0){
        auto pics = boost::make_shared<cuPICSPrimalDualOperator<float>>();
        pics->set_weight(pics_weight);
        pics->set_prior(prior);
        solver.add_regularization_operator(pics);
    }

    boost::shared_ptr<CBSubsetOperator<cuNDArray>> E;
    if ( vm.count("projection_weights")) {
        auto weights = read_nd_array<float>(vm["projection_weights"].as<string>().c_str());
        if (scale_factor[0] != 1 || scale_factor[1] != 1) {
            cuNDArray<float> tmp_weights(*weights);
            auto dims = *tmp_weights.get_dimensions();

            tmp_weights = downsample_projections(&tmp_weights,scale_factor[0],scale_factor[1]);

            weights = tmp_weights.to_host();
        }

        if (!ps->get_projections()->dimensions_equal(weights.get()))
            throw std::runtime_error("Weight dimensions must match that of the projection data");
        auto EW  = boost::make_shared<CBSubsetWeightOperator<cuNDArray>>(subsets);
        EW->setup(ps,binning,imageDimensions,weights);
        E = EW;
    } else {
        std::cout <<"Normal projections" << std::endl;
        E = boost::make_shared<CBSubsetOperator<cuNDArray> >(subsets);

        E->setup(ps, binning, imageDimensions);
    }
    E->set_domain_dimensions(&is_dims);
    E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

    solver.set_encoding_operator(E);

    auto projections = boost::make_shared<cuNDArray<float>>(*ps->get_projections());
    std::cout << "Projection norm:" << nrm2(projections.get()) << std::endl;

    boost::shared_ptr<cuNDArray<float>> result;
    {
        GPUTimer tim("Solver");
        result = solver.solve(projections.get());
    }
//	global_timer.reset();
    std::cout << "Penguin" << nrm2(result.get()) << std::endl;

    std::cout << "Result sum " << asum(result.get()) << std::endl;

    //apply_mask(result.get(),mask.get());

    std::cout << "Result sum " << asum(result.get()) << std::endl;
    //saveNDArray2HDF5(result.get(),outputFile,imageDimensions,vector_td<float,3>(0),command_line_string.str(),iterations);

    write_nd_array(result.get(),"reconstruction.real");

    if (wavelet_weight > 0){
        osMOMSolverF<cuNDArray<float>> solverF;
        solverF.set_max_iterations(10);
        solverF.set_x0(result);
        solverF.set_encoding_operator(E);
        solverF.set_non_negativity_constraint(true);

        auto wave = boost::make_shared<cuEdgeATrousOperator<float>>();

        wave->set_sigma(sigma);
        wave->set_domain_dimensions(&is_dims);
        if (binning->get_number_of_bins() == 1)
            wave->set_levels({2,2,2});
        else
            wave->set_levels({2,2,2,2});
        wave->set_weight(wavelet_weight);
        solverF.add_regularization_operator(wave);

        result = solverF.solve(projections.get());

    }
/*
	if (nlm_noise > 0){

		float* result_data = result->get_data_ptr();

		for (int i = 0; i < result->get_size(3); i++){
			cuNDArray<float> result_view(prior->get_dimensions(),result_data);

			nonlocal_means_ref(&result_view,prior.get(),nlm_noise);
			result_data += result_view.get_number_of_elements();
		}
	}
*/
    saveNDArray2HDF5(result.get(),outputFile,imageDimensions,floatd3(0,0,0),command_line_string.str(),iterations);
//	write_nd_array(result.get(),"reconstruction.real");
    //write_dicom(result.get(),command_line_string.str(),imageDimensions);

}


/****************************************************************
 *
 * Copyright (c) 2011
 * All rights reserved.
 *
 * Hochschule Bonn-Rhein-Sieg
 * University of Applied Sciences
 * Computer Science Department
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Author:
 * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
 * Supervised by:
 * Gerhard K. Kraetzschmar
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * This sofware is published under a dual-license: GNU Lesser General Public
 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
 * code may choose which terms they prefer.
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 2.1 of the
 * License, or (at your option) any later version or the BSD license.
 *
 * This program 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 Lesser General Public License LGPL and the BSD license for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL and BSD license along with this program.
 *
 ****************************************************************/

#ifndef YOUBOT_UNITS_HPP
#define	YOUBOT_UNITS_HPP
#include <boost/units/io.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/temperature/celsius.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/make_scaled_unit.hpp>
#include <boost/units/systems/si/prefixes.hpp>

using namespace boost::units;
using namespace boost::units::si;
using namespace boost::units::angle;

//typedef boost::units::si::length meter;
using boost::units::si::meters;
namespace youbot {

typedef boost::units::make_scaled_unit<si::length, boost::units::scale<10, boost::units::static_rational<-3> > >::type millimeter;
typedef boost::units::make_scaled_unit<si::length, boost::units::scale<10, boost::units::static_rational<-2> > >::type centimeter;
BOOST_UNITS_STATIC_CONSTANT(centimeters, centimeter);

typedef boost::units::make_scaled_unit<si::time, boost::units::scale<10, boost::units::static_rational<-3> > >::type millisecond;
//BOOST_UNITS_STATIC_CONSTANT(millimeters, millimeter);

} // namespace youbot

#endif	/* YOUBOT_UNITS_HPP */