#include <igl/per_vertex_normals.h>
#include <igl/principal_curvature.h>
#include <igl/avg_edge_length.h>
#include <igl/massmatrix.h>
#include <igl/adjacency_list.h>
#include <igl/per_face_normals.h>
#include <igl/barycenter.h>
#include <igl/pinv.h>
#include <igl/edges.h>
#include <Eigen/SparseCore>
#include <igl/adjacency_list.h>
#include <igl/adjacency_matrix.h>
#include <igl/per_face_normals.h>
#include <igl/per_vertex_normals.h>
#include <igl/avg_edge_length.h>
#include <igl/edge_flaps.h>
#include <igl/unique_edge_map.h>
#include <igl/vertex_triangle_adjacency.h>
#include <igl/principal_curvature.h>
#include <igl/collapse_edge.h>
#include <igl/C_STR.h>
#include <igl/flip_edge.h>
using namespace std;

void equalize_valences(Eigen::MatrixXd & V,Eigen::MatrixXi & F, Eigen::VectorXi & feature){
    using namespace igl;
    using namespace Eigen;
    VectorXd p;
    std::vector<bool> is_feature_vertex;
    Eigen::MatrixXi E,uE,EI,EF;
    Eigen::VectorXi EMAP;
    std::vector<std::vector<int>> uE2E,A;
    igl::unique_edge_map(F,E,uE,EMAP,uE2E);

    int m = F.rows();
    int n = V.rows();
    int a,b,c,d;
    igl::adjacency_list(E,A);
    VectorXi vertex_valences;
//
    vertex_valences.setZero(n);

   // std::cout << "A" << std::endl;
    for(int j = 0; j < m; j++){
        vertex_valences(F(j,0)) = vertex_valences(F(j,0))+1;
        vertex_valences(F(j,1)) = vertex_valences(F(j,1))+1;
        vertex_valences(F(j,2)) = vertex_valences(F(j,2))+1;
    }
 //   std::cout << vertex_valences << std::endl;

//
    int k = uE.rows();
    int num_feat = feature.size();
    is_feature_vertex.resize(n);

   // std::cout << "B" << std::endl;

    for (int s = 0; s < num_feat; s++) {
        is_feature_vertex[feature(s)] = true;
    }

//
  //  std::cout << "C" << std::endl;

    std::function<void(
            Eigen::MatrixXi &, //F
            Eigen::MatrixXi &, //E
            Eigen::MatrixXi &, //uE
            Eigen::VectorXi &, //EMAP
            std::vector<std::vector<int>>  &, //uE2E
            int &)> flip_edge_adjacency = [&vertex_valences,&V,&A](
            Eigen::MatrixXi & F, //F
            Eigen::MatrixXi & E, //E
            Eigen::MatrixXi & uE, //uE
            Eigen::VectorXi & EMAP, //EMAP
            std::vector<std::vector<int>>  & uE2E, //uE2E
            int & uei)->void{
      //  std::cout << "Lambda call" << std::endl;
        int num_faces = F.rows();
        auto& half_edges = uE2E[uei];
        int f1 = half_edges[0] % num_faces;
        int f2 = half_edges[1] % num_faces;
        int c1 = half_edges[0] / num_faces;
        int c2 = half_edges[1] / num_faces;
        assert(c1 < 3);
        assert(c2 < 3);

        assert(f1 != f2);
        int v1 = F(f1, (c1+1)%3);
        int v2 = F(f1, (c1+2)%3);
        int v4 = F(f1, c1);
        int v3 = F(f2, c2);
        assert(F(f2, (c2+2)%3) == v1);
        assert(F(f2, (c2+1)%3) == v2);
        // Assert new triangle's area is nonzero
          // f1_new
          double a1 = (V.row(v1)-V.row(v3)).norm();
          double b1 = (V.row(v1)-V.row(v4)).norm();
          double c11 = (V.row(v4)-V.row(v3)).norm();
          double s1 = (a1+b1+c11)/2;
          double area_1_squared = s1*(s1-a1)*(s1-b1)*(s1-c11);
          // f2_new
          double a2 = (V.row(v2)-V.row(v3)).norm();
          double b2 = (V.row(v2)-V.row(v4)).norm();
          double c22 = (V.row(v4)-V.row(v3)).norm();
          double s2 = (a2+b2+c22)/2;
          double area_2_squared = s2*(s2-a2)*(s2-b2)*(s2-c22);
          bool bad = false;

          Eigen::RowVector3d v21,v31,v41,v24,v34,v43,v23,normf10,normf11,
                  normf12,normf20,normf21,normf22;
          v21 = V.row(v2)-V.row(v1);
          v31 = V.row(v3)-V.row(v1);
          v41 = V.row(v4)-V.row(v1);
          v24 = V.row(v2)-V.row(v4);
          v34 = V.row(v3)-V.row(v4);
          v43 = V.row(v4)-V.row(v3);
          v23 = V.row(v2)-V.row(v3);
          normf10 = v21.cross(v31);
          normf11 = v41.cross(v31);
          normf12 = v24.cross(v34);
          normf20 = v41.cross(v21);
          normf21 = v43.cross(v23);
          normf22 = v41.cross(v31);
          normf10.normalize();
          normf11.normalize();
          normf12.normalize();
          normf20.normalize();
          normf21.normalize();
          normf22.normalize();

          if (normf10.dot(normf11) < normf11.norm()/2 || normf10.dot(normf12) < normf12.norm()/2) {
              bad = true;
          }
          if (normf20.dot(normf21) < normf21.norm()/2 || normf20.dot(normf22) < normf22.norm()/2) {
              bad = true;
          }

        if (area_1_squared == 0) {
            bad = true;
        }
        if (area_2_squared == 0) {
            bad = true;
        }

                if(std::count(A[v3].begin(),A[v3].end(),v4)){
                    bad = true; // is it gonna generate non-manifold??
                }

        if(uE2E[uei].size() != 2){
            bad = true;
            }

         if (!bad){
             //std::cout << "D1" << std::endl;
        igl::flip_edge(F,E,uE,EMAP,uE2E,uei);
             //std::cout << "D2" << std::endl;
        //std::cout << "Lambda call test" << std::endl;
        assert(uE(uei,0)==v3);
        assert(uE(uei,1)==v4);

      //  std::cout << "updating_valences" << std::endl;
        vertex_valences(v1) = vertex_valences(v1)-1;
        vertex_valences(v2) = vertex_valences(v2)-1;
        vertex_valences(v3) = vertex_valences(v3)+1;
        vertex_valences(v4) = vertex_valences(v4)+1;

             std::remove_if(A[v1].begin(),A[v1].begin(),[&v2](const int & v){return v==v2;});
             std::remove_if(A[v2].begin(),A[v2].begin(),[&v1](const int & v){return v==v1;});
             A[v3].push_back(v4);
             A[v4].push_back(v3);

         }
        //std::cout << "Lambda call end" << std::endl;
    };

    //std::cout << "D" << std::endl;

    for(int i = 0; i < k; i++){
        //std::cout << uE2E[i].size() << std::endl;
        if(uE2E[i].size()!=2){
            continue;
        }
        bool is_feature_edge = false;
        int f1 = uE2E[i][0] % m;
        int f2 = uE2E[i][1] % m;
        int c1 = uE2E[i][0] / m;
        int c2 = uE2E[i][1] / m;
//        std::cout << f1 << std::endl;
//        std::cout << f2 << std::endl;
//        std::cout << c1 << std::endl;
//        std::cout << c2 << std::endl;
        int a = F(f1, (c1+1)%3);
        int b = F(f1, (c1+2)%3);
        int c = F(f1, c1);
        int d = F(f2, c2);
        if (is_feature_vertex[a] || is_feature_vertex[b] || is_feature_vertex[c] || is_feature_vertex[d]) {
            is_feature_edge = true;
        }
        //std::cout << "E" << std::endl;

        if(!is_feature_edge){
            // FIND VALENCES
            int deviation_pre = abs(vertex_valences(a)-6)+
                    abs(vertex_valences(b)-6)+
                    abs(vertex_valences(c)-6)+
                    abs(vertex_valences(d)-6);
//std::cout << i << std::endl;
            // igl::flip_edge(V,F,E,uE,EMAP,uE2E,i);
            int deviation_post = abs(vertex_valences(a)-1-6)+
                    abs(vertex_valences(b)-6-1)+
                    abs(vertex_valences(c)-6+1)+
                    abs(vertex_valences(d)-6+1);
            // std::cout << i << std::endl;
       //     std::cout << deviation_pre << std::endl;
       //     std::cout << deviation_post << std::endl;

            if(deviation_pre > deviation_post){
                flip_edge_adjacency(F,E,uE,EMAP,uE2E,i);
            }

        }

        //std::cout << uE.row(i+1) << std::endl;
    }
//    std::cout << flipped << std::endl;
//    std::cout << k << std::endl;
//

    // PLACEHOLDER
}

// g++ -I/usr/local/libigl/external/eigen -I/usr/local/libigl/include -std=c++11 -framework Accelerate main.cpp remesh_botsch.cpp -o main


#include "Modeler3D.h"

#include <cmath>
#include <iostream>

#include <boost/filesystem.hpp>
#include <GL/glew.h>

#include "GUI/AllWidgets.h"
#include "GUI/IAction.h"
#include "Math/ModelerMath.h"

#include "FileIO.h"
#include "GuiRenderer.h"
#include "ModelerActions.h"

using namespace std;
using namespace Core;
using namespace Core::Math;
using namespace Video;

std::string VertSource = ""
        "#version 120 \n"
        ""
        "attribute vec3 aPosition; \n"
        "attribute vec3 aNormal; \n"
        ""
        "varying vec3 vViewPosition; \n"
        "varying vec3 vNormal; \n"
        ""
        "uniform mat4 Projection; \n"
        "uniform mat4 View; \n"
        "uniform mat4 Model; \n"
        "uniform mat3 NormalMat; \n"
        ""
        "void main() \n"
        "{ \n"
        "   vNormal = normalize(NormalMat * aNormal); \n"
        "   gl_Position = View * Model * vec4(aPosition, 1.0); \n"
        "   vViewPosition = gl_Position.xyz; \n "
        "   gl_Position = Projection * gl_Position; \n"
        "} \n";

std::string FragSource = ""
        "#version 120 \n"
        ""
        "varying vec3 vViewPosition; \n"
        "varying vec3 vNormal; \n"
        ""
        "uniform vec3 LightDirection = vec3(-1, -0.5, -1); \n"
        "uniform mat4 View; \n"
        ""
        "uniform vec3 Color; \n"
        ""
        "float Diffuse(vec3 normal, vec3 lightDir) \n"
        "{ \n"
        "   return clamp((dot(normal, -lightDir)), 0.0, 1.0); \n"
        "} \n"
        ""
        "float Specular(vec3 normal, vec3 lightDir, vec3 cameraDir, float power) \n"
        "{ \n"
        "   vec3 halfVec = normalize(lightDir + cameraDir); \n"
        "   return pow(clamp(dot(normal, -halfVec), 0.0, 1.0), power); "
        "} \n"
        ""
        "void main() \n"
        "{ \n"
        "   vec3 normal = normalize((View * vec4(vNormal, 0.0)).xyz); \n"
        "   vec3 lightDir = normalize((View * vec4(LightDirection, 1.0)).xyz); \n"
        "   vec3 cameraDir = normalize(vViewPosition); \n"
        ""
        "   float diffuse = Diffuse(normal, lightDir); \n"
        "   float specular = Specular(normal, lightDir, cameraDir, 100); \n"
        ""
        "   gl_FragColor = vec4(Color * (diffuse * 0.4 + 0.4 + specular * 0.4), 1.0); \n"
        "} \n";

Video::IShader* Shader = nullptr;

float Angle = 0.0f;

namespace Core
{

Video::VertexFormat vboFormat = Video::VertexFormat()
        .AddElement(Video::Attribute::Position, 3)
        .AddElement(Video::Attribute::Normal, 3);

struct VertexPosition3Normal3
{
    Vector3f Position;
    Vector3f Normal;
};

Modeler3D::Modeler3D(IBackend* backend)
    : Application(backend),
      mEnv(nullptr),
      mGuiRenderer(nullptr),
      mShader(nullptr),
      mGeometry(nullptr),
      mVbo(nullptr),
      mAngle(0),
	  mMouse(backend->GetWindow()->GetMouse()),
	  mCamera(new Camera(backend->GetWindow()->GetWidth(),backend->GetWindow()->GetHeight(), Math::Vector3f(0,0,1), Math::Quaternionf())),
	  mZoom(2),
	  mColor(Vector3f(0.8, 0.6, 0.4)),
	  mScale(Vector3f(1)) {}

Modeler3D::~Modeler3D() {}

void Modeler3D::LoadObj(const string& file)
{
    boost::filesystem::path obj(file);

	FileIO objFile;

	vector<VertexPosition3Normal3> vertices;

    std::vector<std::vector<double>> positions;
    std::vector<std::vector<double>> textures;
    std::vector<std::vector<double>> normals;
    std::vector<std::vector<std::vector<int>>> faces;

    objFile.LoadObj2(obj , positions, textures, normals, faces);

    for (uint i = 0; i < faces.size(); i++)
    {
        VertexPosition3Normal3 verts[3];
        for (uint j = 0; j < 3; j++)
        {
            vector<double> pos = positions[faces[i][j][0] - 1];

            for (uint k = 0; k < 3; k++)
            {
                verts[j].Position[k] = pos[k] * 1.5;
            }
        }

        Vector3f normal = Cross(Normalize( verts[1].Position -  verts[0].Position), Normalize( verts[2].Position -  verts[0].Position));
        verts[0].Normal = normal;
        verts[1].Normal = normal;
        verts[2].Normal = normal;

        vertices.push_back(verts[0]);
        vertices.push_back(verts[1]);
        vertices.push_back(verts[2]);
    }

    mVbo = Graphics->CreateVertexBuffer(vboFormat, vertices.size(), Video::BufferHint::Static);
    mVbo->SetData((float32*)(&vertices[0]), 0, vertices.size());
    mGeometry->SetVertexBuffer(mVbo);
}

void Modeler3D::OnInit()
{
    cout << "Initializing Modeler3D" << endl;

    mGeometry = Graphics->CreateGeometry();

    mEnv = Backend->GetWindow()->GetEnvironment();
    mGuiRenderer = new GuiRenderer(Graphics);
    mShader = Graphics->CreateShader(VertSource, FragSource);

    Video::ITexture2D* tex = Graphics->CreateTexture2D("Assets/button.png");
    mGuiRenderer->SetTexture(tex);

    //Create load buttons
    Gui::Widget* LoadButton1 = new Gui::Button(10, 10 + 50 * 0, 80, 40, new LoadAction(this, "Assets/bunny.obj"), "bunny");
    Gui::Widget* LoadButton2 = new Gui::Button(10, 10 + 50 * 1, 80, 40, new LoadAction(this, "Assets/cube.obj"), "cube");
    Gui::Widget* LoadButton3 = new Gui::Button(10, 10 + 50 * 2, 80, 40, new LoadAction(this, "Assets/dragon-big.obj"), "dragon");
    Gui::Widget* LoadButton4 = new Gui::Button(10, 10 + 50 * 3, 80, 40, new LoadAction(this, "Assets/ferrari.obj"), "ferrari");

    //Create zoom buttons
    Gui::Widget* ZoomButton1 = new Gui::Button(10, 10 + 50 * 0,96,40, new ZoomAction(this, mCamera, 1), "Zoom 1x");
    Gui::Widget* ZoomButton2 = new Gui::Button(10, 10 + 50 * 1,96,40, new ZoomAction(this, mCamera, 50), "Zoom 50x");
    Gui::Widget* ZoomButton3 = new Gui::Button(10, 10 + 50 * 2,96,40, new ZoomAction(this, mCamera, 300), "Zoom 300x");
    Gui::Widget* ZoomButton4 = new Gui::Button(10, 10 + 50 * 3,96,40, new ZoomAction(this, mCamera, 1000), "Zoom 1000x");

    //Create rotation buttons
//    Gui::Widget* RotatePitchNegButton = new Gui::Button(10 + 106 * 0, 10 + 58 * 0,96,48, new RotateAction(this, mCamera, 1, -1), "Up");
//    Gui::Widget* RotatePitchPosButton = new Gui::Button(10 + 106 * 1, 10 + 58 * 0,96,48, new RotateAction(this, mCamera,1, 1), "Down");
//    Gui::Widget* RotateYawNegButton = new Gui::Button(10 + 106 * 0, 10 + 58 * 1,96,48, new RotateAction(this, mCamera, 2, -1), "Left");
//    Gui::Widget* RotateYawPosButton = new Gui::Button(10 + 106 * 1, 10 + 58 * 1,96,48, new RotateAction(this, mCamera,2, 1), "Right");

    //Create color changing buttons
    Gui::Widget* PlusRButton = new Gui::Button(10 + 40 * 0, 10 + 50 * 1,30,40, new ChangeColorAction(this, 0, .05), "+R");
    Gui::Widget* MinusRButton = new Gui::Button(10 + 40 * 0, 10 + 50 * 0,30,40, new ChangeColorAction(this, 0, -.05), "-R");
    Gui::Widget* PlusGButton = new Gui::Button(10 + 40 * 1, 10 + 50 * 1,30,40, new ChangeColorAction(this, 1, .05), "+G");
    Gui::Widget* MinusGButton = new Gui::Button(10 + 40 * 1, 10 + 50 * 0,30,40, new ChangeColorAction(this, 1, -.05), "-G");
    Gui::Widget* PlusBButton = new Gui::Button(10 + 40 * 2, 10 + 50 * 1,30,40, new ChangeColorAction(this, 2, .05), "+B");
    Gui::Widget* MinusBButton = new Gui::Button(10 + 40 * 2, 10 + 50 * 0,30,40, new ChangeColorAction(this, 2, -.05), "-B");

    //Create scaling buttons
    Gui::Widget* DisplayScale = new Gui::Button(10, 20 + 50 * 3, 160, 40, new NoOpAction(), "Cur. Scale: 1x");
    Gui::Widget* PlusScaleButton = new Gui::Button(10 + 60, 20 + 50 * 2, 50, 40, new ScaleAction(this, .1, DisplayScale), "+.1x");
    Gui::Widget* MinusScaleButton = new Gui::Button(10, 20 + 50 * 2,50, 40, new ScaleAction(this, -.1, DisplayScale), "-.1x");

    //Create view buttons
    Gui::Widget* ResetCameraButton = new Gui::Button(10, 10 + 50 * 0, 144,40, new ResetAction(this, mCamera), "Reset Camera");
    Gui::Widget* ToggleProjectionTypeButton = new Gui::Button(10, 10 + 50 * 1, 144,40, new ChangeViewAction(this, mCamera), "To Orthographic");

    //Create screen
    Gui::Screen* Screen = new Gui::Screen(new ScreenMoveAction(this));

    //Set alignments
    LoadButton1->SetAlignment(0, 1);
    LoadButton2->SetAlignment(0, 1);
    LoadButton3->SetAlignment(0, 1);
    LoadButton4->SetAlignment(0, 1);

    ZoomButton1->SetAlignment(1, 1);
    ZoomButton2->SetAlignment(1, 1);
    ZoomButton3->SetAlignment(1, 1);
    ZoomButton4->SetAlignment(1, 1);

//    RotatePitchNegButton->SetAlignment(0, 0);
//    RotatePitchPosButton->SetAlignment(0, 0);
//    RotateYawNegButton->SetAlignment(0, 0);
//    RotateYawPosButton->SetAlignment(0, 0);

    PlusRButton->SetAlignment(0, 0);
    MinusRButton->SetAlignment(0, 0);
    PlusGButton->SetAlignment(0, 0);
    MinusGButton->SetAlignment(0, 0);
    PlusBButton->SetAlignment(0, 0);
    MinusBButton->SetAlignment(0, 0);

    DisplayScale->SetAlignment(1, 0);
    PlusScaleButton->SetAlignment(1, 0);
    MinusScaleButton->SetAlignment(1, 0);

    ResetCameraButton->SetAlignment(1, 0);
    ToggleProjectionTypeButton->SetAlignment(1, 0);

    //Add buttons to the environment
    mEnv->AddWidget(LoadButton1);
    mEnv->AddWidget(LoadButton2);
    mEnv->AddWidget(LoadButton3);
    mEnv->AddWidget(LoadButton4);

    mEnv->AddWidget(ZoomButton1);
    mEnv->AddWidget(ZoomButton2);
    mEnv->AddWidget(ZoomButton3);
    mEnv->AddWidget(ZoomButton4);

//    mEnv->AddWidget(RotatePitchNegButton);
//    mEnv->AddWidget(RotatePitchPosButton);
//    mEnv->AddWidget(RotateYawNegButton);
//    mEnv->AddWidget(RotateYawPosButton);

    mEnv->AddWidget(PlusRButton);
    mEnv->AddWidget(MinusRButton);
    mEnv->AddWidget(PlusGButton);
    mEnv->AddWidget(MinusGButton);
    mEnv->AddWidget(PlusBButton);
    mEnv->AddWidget(MinusBButton);

    mEnv->AddWidget(DisplayScale);
    mEnv->AddWidget(PlusScaleButton);
    mEnv->AddWidget(MinusScaleButton);

    mEnv->AddWidget(ResetCameraButton);
    mEnv->AddWidget(ToggleProjectionTypeButton);

    //Set up the screen
    mEnv->AddWidget(Screen);
}

void Modeler3D::OnUpdate(float64 dt)
{
    mAngle += 1.0 * dt;

    //Scroll wheel to zoom
	int32 amt = mMouse->GetWheelScroll();
	float32 zoom = 1.1;
	if (amt < 0)
	{
	    mZoom *= zoom;
	}
	else if (amt > 0)
	{
	    mZoom /= zoom;
	}

	mCamera->SetPosition(Normalize(mCamera->GetPosition()) * mZoom);

    mEnv->SetSize(Window->GetWidth(), Window->GetHeight());
    mEnv->Update(dt);
}

void Modeler3D::OnRender()
{
    Graphics->SetClearColor(0.3, 0.3, 0.3);
    Graphics->Clear();

    if (mVbo) //if vertices are set, render
    {
    	Camera::Projection proj = mCamera->GetProjectionType();
    	Matrix4f projection;
    	if(proj == Camera::Projection::PERSPECTIVE) projection = mCamera->GetProjection(Math::ToRadians(70.0f), Graphics->GetAspectRatio(), 0.05f, 5000.0f);
    	else projection = mCamera->GetProjection(-6000.0f * mZoom, 6000.0f * mZoom, 10 * Window->GetAspectRatio() * mZoom, -10 * Window->GetAspectRatio() * mZoom, 10 * mZoom, -10 * mZoom);

        Matrix4f view = mCamera->GetView();

        Matrix4f model = Matrix4f::Identity * Matrix4f::ToScale(mScale);
//        Matrix4f model = Matrix4f::ToYaw(mAngle) * Matrix4f::ToPitch(mAngle * 1.3) * Matrix4f::ToRoll(mAngle * 1.7);// * Matrix4f::ToTranslation(Vector3f(0.2, -0.8, 0));

        Matrix3f normalMat(Inverse(Transpose(model)));

        mShader->SetMatrix4f("Projection", projection);
        mShader->SetMatrix4f("View", view);
        mShader->SetMatrix4f("Model", model);
        mShader->SetMatrix3f("NormalMat", normalMat);
        mShader->SetVector3f("Color", mColor);

        Graphics->SetShader(mShader);
        Graphics->SetGeometry(mGeometry);

        Graphics->Draw(Video::Primitive::TriangleList, 0, mVbo->GetLength() / 3);
    }

    mGuiRenderer->Reset();
    mEnv->Draw(mGuiRenderer);
}

void Modeler3D::SetZoom(float32 zoom) { mZoom = zoom; }

void Modeler3D::SetColor(Math::Vector3f color) { mColor = color; }

void Modeler3D::SetScale(Math::Vector3f scale) { mScale = scale; }

void Modeler3D::OnDestroy()
{
    cout << "Destroying Modeler3D" << endl;
    mGuiRenderer->Release();
    mShader->Release();
    mGeometry->SetVertexBuffer(nullptr);
    mGeometry->Release();
    if(mVbo)
    	mVbo->Release();
}

}


/**
 * This program uses NTL:
 * http://shoup.net/ntl/
 */
#include <string>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>
#include <errno.h>
#include <openssl/sha.h>
extern "C" {
#include "mtgp32-fast.h"
#include "mtgp64-fast.h"
}

using namespace NTL;
using namespace std;

static void u32_poly_check(int mexp, int no, int seed);
static void u64_poly_check(int mexp, int no, int seed);
static void poly_sha1(string& str, unsigned char sha1[], const GF2X& poly);
static void poly_check(vec_GF2& vec, int mexp, unsigned char param_sha1[]);

int main(int argc, char *argv[]) {
    int bit_size;
    int mexp;
    int no;
    uint32_t seed = 1;

    if (argc <= 3) {
	cout << argv[0] << ": bit_size mexp no." << endl;
	return 1;
    }
    bit_size = strtol(argv[1], NULL, 10);
    if (errno) {
	cout << argv[0] << ": bit_size error." << endl;
	return 1;
    }
    if (bit_size != 32 && bit_size != 64) {
	cout << argv[0] << ": bit_size error. bit size is 32 or 64" << endl;
	return 1;
    }
    mexp = strtol(argv[2], NULL, 10);
    if (errno) {
	cout << argv[0] << ": mexp no." << endl;
	return 1;
    }
    no = strtol(argv[3], NULL, 10);
    if (errno) {
	cout << argv[0] << ": mexp no.\n" << endl;
	return 3;
    }
    if (bit_size == 32) {
	u32_poly_check(mexp, no, seed);
    } else {
	u64_poly_check(mexp, no, seed);
    }
    return 0;
}

static void poly_sha1(string& str, unsigned char sha1[], const GF2X& poly) {
    SHA_CTX ctx;
    SHA1_Init(&ctx);
    if (deg(poly) < 0) {
	SHA1_Update(&ctx, "-1", 2);
    }
    for(int i = 0; i <= deg(poly); i++) {
	if(rep(coeff(poly, i)) == 1) {
	    SHA1_Update(&ctx, "1", 1);
	} else {
	    SHA1_Update(&ctx, "0", 1);
	}
    }
    unsigned char md[SHA_DIGEST_LENGTH];
    SHA1_Final(md, &ctx);
    stringstream ss;
    for (int i = 0; i < SHA_DIGEST_LENGTH; i++) {
	ss << setfill('0') << setw(2) << hex
	   << static_cast<int>(md[i]);
	sha1[i] = md[i];
    }
    sha1[SHA_DIGEST_LENGTH] = 0;
    ss >> str;
}

static void poly_check(vec_GF2& vec, int mexp, unsigned char param_sha1[]) {
    GF2X poly;
    string s;
    unsigned char sha1[SHA_DIGEST_LENGTH + 1];

    MinPolySeq(poly, vec, mexp);
    poly_sha1(s, sha1, poly);
    cout << s << endl;
    if (strcmp((char *)sha1, (char *)param_sha1) == 0) {
	cout << "poly sha1 OK" << endl;
    } else {
	cout << "poly sha1 NG" << endl;
    }
    if (deg(poly) == mexp) {
	cout << "poly deg = " << deg(poly) << " OK" << endl;
    } else {
	cout << "poly deg = " << deg(poly) << " NG" << endl;
    }
    if (IterIrredTest(poly)) {
	cout << "poly irreducible OK" << endl;
    } else {
	cout << "poly irreducible OK" << endl;
    }
}

static void u32_poly_check(int mexp, int no, int seed) {
    mtgp32_params_fast_t *params;
    mtgp32_fast_t mtgp32;
    vec_GF2 vec;
    int rc;

    switch (mexp) {
    case 11213:
	params = mtgp32_params_fast_11213;
	break;
    case 23209:
	params = mtgp32_params_fast_23209;
	break;
    case 44497:
	params = mtgp32_params_fast_44497;
	break;
    default:
	cout << "mexp shuould be 11213, 23209 or 44497" << endl;
	exit(1);
    }
    if (no >= 128 || no < 0) {
	cout << "no must be between 0 and 127" << endl;
	exit(1);
    }
    params += no;
    rc = mtgp32_init(&mtgp32, params, seed);
    if (rc) {
	cout << "failure in mtgp32_init" << endl;
	exit(1);
    }
    mtgp32_print_idstring(&mtgp32, stdout);
    vec.SetLength(2 * mexp);
    for (int i = 0; i < 2 * mexp; i++) {
	vec[i] = mtgp32_genrand_uint32(&mtgp32) & 1;
    }
    poly_check(vec, mexp, mtgp32.params.poly_sha1);
    mtgp32_free(&mtgp32);
}

static void u64_poly_check(int mexp, int no, int seed) {
    mtgp64_params_fast_t *params;
    mtgp64_fast_t mtgp64;
    vec_GF2 vec;
    int rc;

    switch (mexp) {
    case 23209:
	params = mtgp64_params_fast_23209;
	break;
    case 44497:
	params = mtgp64_params_fast_44497;
	break;
    case 110503:
	params = mtgp64_params_fast_110503;
	break;
    default:
	cout << "mexp shuould be 11213, 23209 or 44497" << endl;
	exit(1);
    }
    if (no >= 128 || no < 0) {
	cout << "no must be between 0 and 127" << endl;
	exit(1);
    }
    params += no;
    rc = mtgp64_init(&mtgp64, params, seed);
    if (rc) {
	cout << "failure in mtgp64_init." << endl;
	exit(1);
    }
    mtgp64_print_idstring(&mtgp64, stdout);
    vec.SetLength(2 * mexp);
    for (int i = 0; i < 2 * mexp; i++) {
	vec[i] = mtgp64_genrand_uint64(&mtgp64) & 1;
    }
    poly_check(vec, mexp, mtgp64.params.poly_sha1);
    mtgp64_free(&mtgp64);
}


// Boost.Geometry

// Copyright (c) 2017-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DENSIFY_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DENSIFY_HPP

#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/signed_size_type.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/cross_product.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/arithmetic/normalize.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/formulas/spherical.hpp>
#include <boost/geometry/formulas/interpolate_point_spherical.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/srs/sphere.hpp>
#include <boost/geometry/strategies/densify.hpp>
#include <boost/geometry/strategies/spherical/get_radius.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace densify
{

/*!
\brief Densification of spherical segment.
\ingroup strategies
\tparam RadiusTypeOrSphere \tparam_radius_or_sphere
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
[link geometry.reference.algorithms.densify.densify_4_with_strategy densify (with strategy)]
}
 */
template
<
    typename RadiusTypeOrSphere = double,
    typename CalculationType = void
>
class spherical
{
public:
    typedef typename strategy_detail::get_radius
        <
            RadiusTypeOrSphere
        >::type radius_type;

    // For consistency with area strategy the radius is set to 1
    inline spherical()
        : m_radius(1.0)
    {}

    template <typename RadiusOrSphere>
    explicit inline spherical(RadiusOrSphere const& radius_or_sphere)
        : m_radius(strategy_detail::get_radius
                    <
                        RadiusOrSphere
                    >::apply(radius_or_sphere))
    {}

    template <typename Point, typename AssignPolicy, typename T>
    inline void apply(Point const& p0, Point const& p1, AssignPolicy & policy, T const& length_threshold) const
    {
        typedef typename AssignPolicy::point_type out_point_t;
        typedef typename select_most_precise
            <
                typename coordinate_type<Point>::type,
                typename coordinate_type<out_point_t>::type,
                CalculationType
            >::type calc_t;

        calc_t angle01;

        formula::interpolate_point_spherical<calc_t> formula;
        formula.compute_angle(p0, p1, angle01);

        BOOST_GEOMETRY_ASSERT(length_threshold > T(0));

        signed_size_type n = signed_size_type(angle01 * m_radius / length_threshold);
        if (n <= 0)
            return;

        formula.compute_axis(p0, angle01);

        calc_t step = angle01 / (n + 1);

        calc_t a = step;
        for (signed_size_type i = 0 ; i < n ; ++i, a += step)
        {
            out_point_t p;
            formula.compute_point(a, p);

            geometry::detail::conversion::point_to_point
                <
                    Point, out_point_t,
                    2, dimension<out_point_t>::value
                >::apply(p0, p);

            policy.apply(p);
        }
    }

    inline radius_type radius() const
    {
        return m_radius;
    }

private:
    radius_type m_radius;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <>
struct default_strategy<spherical_equatorial_tag>
{
    typedef strategy::densify::spherical<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::densify

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DENSIFY_HPP


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-exponential
    This function object returns the cubic root of its argument: \f$\sqrt[3]{x}\f$

    @par Header <boost/simd/function/cbrt.hpp>

    @par Decorators

      - std_ for floating entries calls @c std::cbrt

    @see pow, sqrt

    @par Example:

      @snippet cbrt.cpp cbrt

    @par Possible output:

      @snippet cbrt.txt cbrt

  **/
  IEEEValue cbrt(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/cbrt.hpp>
#include <boost/simd/function/simd/cbrt.hpp>

#endif


//============================================================================
// Name        : TCN 5080 Secure Telecom Transaction. Project 1
// Author      :  Abhaykumar Kumbhar 5115320
// Version     :
// File name:  : tcn5080project1.cpp
// Description : Main function for simplifies DES encrption/decrption.
//============================================================================

//system defined header inclusion
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <boost/algorithm/string.hpp> //used in string compare operation

//user defined header inclusion
#include "utility.h"
#include "defs.h"
#include "des.h"

//Namespace
using namespace std;

//Macros

//global variables
utility* myUtility;
des* myDes;

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

	if(argc != NUM_OF_INPUT_PARAMS )
	{
		cerr << "ERROR: Main Func: Not enough parameters." << endl;
	}
	else
	{
		// Read all the user inputs and save it in variables
//		clog << "DEBUG: Main Func: valid number of parameters." << endl;
		string desTask        = argv[1];
		string initKey        = argv[2];
		string initVector     = argv[3];
		string plaintextFile  = argv[4];
		string ciphertextFile = argv[5];

		// convert the file names from string to char
		char *cipherBinaryFile = &ciphertextFile[0u];
		char *plainBinaryFile  = &plaintextFile[0u];

		cout << "INFO: Main func: user inputs are: \r\n" <<"Encrypt\\Decrypt: " << desTask << "\r\ninitial key:\t" << initKey
		<<" \r\ninitial Vector\t" << initVector << "\r\nplain text file name\t" << plainBinaryFile << "\r\ncipher text file name \t" << cipherBinaryFile << endl;

		// create the utilily and DES class
		myUtility =  new utility();
		myDes = new des();

		if ((myUtility != NULL) and (myDes != NULL))
		{

			//Based on the user input, we either encrypt or decrypt or do nothing.
			if (boost::iequals(desTask, "encrypt"))
			{
				clog << "DEBUG: Main func: user input task is to encrpyt the binary text"<<endl;
				//Main func: Read the  Binary input from plain text file.
			        myUtility->readPlainTextFile(plainBinaryFile);
				string binaryStr8Bit1 = myUtility->getBinaryPlainTextLeft();
				clog << "DEBUG: Main func: first block of 8 bits of plain text: " << binaryStr8Bit1 << endl;
				string binaryStr8Bit2 = myUtility->getBinaryPlainTextRight();
				clog << "DEBUG: Main func: second block of 8 bits of plain text: " << binaryStr8Bit2 << endl;

				if (myUtility->CheckifStringIsBinary(binaryStr8Bit1) and
				    myUtility->CheckifStringIsBinary(binaryStr8Bit2) and
                                    myUtility->CheckifStringIsBinary(initKey))
				{
//					clog<< "DEBUG: main: Input strings are valid and binary" << endl;
					myDes->generateRoundKeys(initKey);
					cout << "================== SIMPLE S-DES Encrypt ==================" <<endl;
					string simplifedDes1 = myDes->encrypt(binaryStr8Bit1);
				        string simplifedDes2 = myDes->encrypt(binaryStr8Bit2);

					cout<< "Encryption Simplified S-DES for " <<binaryStr8Bit1 <<" " <<binaryStr8Bit2 <<" is "
                                                                                <<simplifedDes1  <<" " <<simplifedDes2 << endl;

					cout << "==================== CBC S-DES Encrypt ===================" <<endl;
					//from the text Fig.4.5 C1 = IV XOR m1
					string c1 = myDes->encrypt(myUtility->XOR(binaryStr8Bit1, initVector));
					// c2 = c1 XOR m2
					string c2 = myDes->encrypt(myUtility->XOR(binaryStr8Bit2, c1));
					cout<< "CBC S-DES C1 is " << c1 <<endl;
					cout<< "CBC S-DES C2 is " << c2 <<endl;
					cout<< "Encryption Simplified S-DES for " <<binaryStr8Bit1 <<" " <<binaryStr8Bit2 <<" is "
									         <<c1             <<" " <<c2 << endl;
					cout << "=========================================================" <<endl;
					//Write the output to file
					myUtility->writeText2File(cipherBinaryFile, (c1+" "+c2));
				}
			}
			else if (boost::iequals(desTask, "decrypt"))
			{
				clog << "DEBUG: Main func: user input task is to decrpyt the binary text"<<endl;
			        //Main func: Read the  Binary input from cipher text file.
				myUtility->readCiphertTextFile(cipherBinaryFile);
				string binaryStr8Bit1 = myUtility->getBinaryCiphertTextLeft();
				clog << "DEBUG: Main func: first block of 8 bits of cipher text: " << binaryStr8Bit1 << endl;
				string binaryStr8Bit2 = myUtility->getBinaryCiphertTextRight();
				clog << "DEBUG: Main func: second block of 8 bits of cipher text: " << binaryStr8Bit2 << endl;

				if (myUtility->CheckifStringIsBinary(binaryStr8Bit1) and
				    myUtility->CheckifStringIsBinary(binaryStr8Bit2) and
                                    myUtility->CheckifStringIsBinary(initKey))
				{
//					clog<< "DEBUG: main: Input strings are valid and binary" << endl;
					myDes->generateRoundKeys(initKey);
					cout << "================== SIMPLE S-DES Decrypt ==================" <<endl;
					string simplifedDes1 = myDes->decrypt(binaryStr8Bit1);
				        string simplifedDes2 = myDes->decrypt(binaryStr8Bit2);

					cout<< "Decryption Simplified S-DES for " <<binaryStr8Bit1 <<" " <<binaryStr8Bit2 <<" is "
                                                                                <<simplifedDes1   <<" " <<simplifedDes2 << endl;
					cout << "==================== CBC S-DES Decrypt ===================" <<endl;
					//from the text Fig.4.5 m1 = IV XOR D(c1)
					string decrypt1 = myDes->decrypt(binaryStr8Bit1);
					string m1 = myUtility->XOR(decrypt1, initVector);

					string decrypt2 = myDes->decrypt(binaryStr8Bit2);
					string m2 = myUtility->XOR(binaryStr8Bit1, decrypt2);

					cout<< "CBC S-DES C1 is " << m1 <<endl;
					cout<< "CBC S-DES C2 is " << m2 <<endl;
					cout<< "Decryption Simplified S-DES for " <<binaryStr8Bit1 <<" " <<binaryStr8Bit2 <<" is "
									          <<m1             <<" " <<m2 << endl;
					cout << "==========================================================" <<endl;
					//Write the output to file
					myUtility->writeText2File(cipherBinaryFile, (binaryStr8Bit1+" "+binaryStr8Bit2));
					myUtility->writeText2File(plainBinaryFile, (m1+" "+m2));
				}
			}
			else
			{
				cerr << "ERROR: main func: Wrong user input task" <<endl;
			}
		}
		else
		{
			cerr <<"ERROR: Main func: utility object is null." << endl;
		}

		//House keeping: free all the memory alloaction
		delete myUtility;
		myUtility = 0;
		delete myDes;
		myDes = 0;

	}

	return 0;
}


#ifndef SM_NUMERICAL_DIFF_HPP
#define SM_NUMERICAL_DIFF_HPP

#include <Eigen/Core>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <sm/assert_macros.hpp>

namespace sm {
namespace eigen {

template <typename RESULT_VEC_T, typename INPUT_VEC_T, typename JACOBIAN_T = Eigen::MatrixXd>
struct NumericalDiffFunctor {
    typedef RESULT_VEC_T value_t;
    typedef typename value_t::Scalar scalar_t;
    typedef INPUT_VEC_T input_t;
    typedef JACOBIAN_T jacobian_t;

    NumericalDiffFunctor(boost::function<value_t(input_t)> f) : _f(f) {}

    value_t operator()(const input_t& x) { return _f(x); }

    input_t update(const input_t& x, int c, scalar_t delta) {
        input_t xnew = x;
        xnew[c] += delta;
        return xnew;
    }
    boost::function<value_t(input_t)> _f;
};

// A simple implementation of central differences to estimate a Jacobian matrix
template <typename FUNCTOR_T>
struct NumericalDiff {
    typedef FUNCTOR_T functor_t;
    typedef typename functor_t::input_t input_t;
    typedef typename functor_t::value_t value_t;
    typedef typename functor_t::scalar_t scalar_t;
    typedef typename functor_t::jacobian_t jacobian_t;

    NumericalDiff(functor_t f, scalar_t eps = sqrt(std::numeric_limits<scalar_t>::epsilon())) : functor(f), eps(eps) {}

    jacobian_t estimateJacobian(input_t const& x0) {
        // evaluate the function at the operating point:
        value_t fx0 = functor(x0);
        size_t N = x0.size();
        size_t M = fx0.size();

        // std::cout << "Size: " << M << ", " << N << std::endl;
        jacobian_t J;
        J.resize(M, N);

        SM_ASSERT_EQ(std::runtime_error, x0.size(), J.cols(), "Unexpected number of columns for input size");
        SM_ASSERT_EQ(std::runtime_error, fx0.size(), J.rows(), "Unexpected number of columns for output size");

        for (unsigned c = 0; c < N; c++) {
            // Calculate a central difference.
            // This step size was stolen from cminpack: temp = eps * fabs(x[j]);
            scalar_t rcEps = std::max(static_cast<scalar_t>(fabs(x0(c))) * eps, eps);

            value_t fxp = functor(functor.update(x0, c, rcEps));
            value_t fxm = functor(functor.update(x0, c, -rcEps));
            J.block(0, c, M, 1) = (fxp - fxm).template cast<typename jacobian_t::Scalar>() /
                                  (typename jacobian_t::Scalar)(rcEps * (scalar_t)2.0);
        }
        return J;
    }

    functor_t functor;
    scalar_t eps;
};

template <typename ValueType_, typename InputType_>
Eigen::MatrixXd numericalDiff(
    std::function<ValueType_(const InputType_&)> function, InputType_ const& input,
    double eps =
        sqrt(std::numeric_limits<typename NumericalDiffFunctor<ValueType_, InputType_>::scalar_t>::epsilon())) {
    typedef NumericalDiffFunctor<ValueType_, InputType_> Functor;

    NumericalDiff<Functor> numDiff(Functor(function), eps);
    return numDiff.estimateJacobian(input);
}

}  // namespace eigen
}  // namespace sm

#endif /* SM_NUMERICAL_DIFF_HPP */


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <gtest/gtest.h>

#include <Eigen/Core>

#include "smooth/feedback/collocation.hpp"

template<typename T>
using Vec = Eigen::VectorX<T>;

using Vecd = Vec<double>;

TEST(Collocation, Mesh)
{
  smooth::feedback::Mesh<5, 10> m;
  m.refine_ph(0, 5 * 10);
  ASSERT_EQ(m.N_ivals(), 10);

  for (auto i = 0u; i < 10; ++i) {
    auto [n, w] = m.interval_nodes_and_weights(i);
    ASSERT_DOUBLE_EQ(n(0), i * 0.1);
  }

  // will only increase degree
  m.refine_ph(1, 10);
  ASSERT_EQ(m.N_ivals(), 10);
  {
    auto [n, w] = m.interval_nodes_and_weights(1);
    ASSERT_DOUBLE_EQ(n(0), 0.1);
  }

  // actually split it
  m.refine_ph(1, 13);
  ASSERT_EQ(m.N_ivals(), 12);
  {
    auto [n1, w1] = m.interval_nodes_and_weights(1);
    auto [n2, w2] = m.interval_nodes_and_weights(2);
    auto [n3, w3] = m.interval_nodes_and_weights(3);
    ASSERT_DOUBLE_EQ(n1(0), 0.1);
    ASSERT_DOUBLE_EQ(n2(0), 0.1 + 0.1 / 3);
    ASSERT_DOUBLE_EQ(n3(0), 0.1 + 2 * 0.1 / 3);
  }

  m.refine_ph(2, 27);

  m.refine_ph(7, 33);
  m.refine_ph(9, 22);

  const auto [alln, allw] = m.all_nodes_and_weights();

  for (auto i = 0u; i + 1 < alln.size(); ++i) { ASSERT_LE(alln[i], alln[i + 1]); }
}

TEST(Collocation, DifferentiationIntegration)
{
  smooth::feedback::Mesh<8, 8> m;
  m.refine_ph(0, 40);

  // define a function and its derivative
  const auto x  = [](double t) -> double { return 1 + 2 * t + 3 * t * t + 4 * t * t * t; };
  const auto dx = [](double t) -> double { return 2 + 3 * 2 * t + 4 * 3 * t * t; };

  for (auto ival = 0u; ival < m.N_ivals(); ++ival) {
    const auto N               = m.N_colloc_ival(ival);
    const auto [taus, weights] = m.interval_nodes_and_weights(ival);

    // specify function values on mesh
    Eigen::RowVectorXd xvals(N + 1);
    for (auto i = 0u; i < N + 1; ++i) { xvals(i) = x(taus(i)); }
    Eigen::RowVectorXd dxvals(N);
    for (auto i = 0u; i < N; ++i) { dxvals(i) = dx(taus(i)); }

    // derivative and integral matrices
    const auto D = m.interval_diffmat(ival);
    const auto I = m.interval_intmat(ival);

    // expect [dx0 ... dxN-1] = [x0 ... xN] * D
    ASSERT_TRUE(dxvals.isApprox(xvals * D));

    // expect [x1 ... xN] = [x0 ... x0] + [dx0 ... dxN-1] * I
    ASSERT_TRUE(xvals.rightCols(N).isApprox(xvals.leftCols(1).replicate(1, N) + dxvals * I));
  }
}

TEST(Collocation, TimeTrajectory)
{
  // given trajectory
  std::size_t nx = 1;
  const auto x = [](double t) -> Vec<double> { return Vec<double>{{0.1 * t * t - 0.4 * t + 0.2}}; };

  // running constraints
  std::size_t ncr = 2;
  const auto cr   = []<typename T>(const T &, const Vec<T> & x, const Vec<T> &) -> Vec<T> {
    return Vec<T>{{x.x(), 0}};
  };

  // system dynamics
  std::size_t nu = 0;
  const auto f   = []<typename T>(const T & t, const Vec<T> &, const Vec<T> &) -> Vec<T> {
    return Vec<T>{{0.2 * t - 0.4}};
  };

  // integrand
  std::size_t nq = 1;
  const auto g   = []<typename T>(const T &, const Vec<T> & x, const Vec<T> &) -> Vec<T> {
    return Vec<T>{{0.1 + x.squaredNorm()}};
  };

  double t0 = 3;
  double tf = 5;

  smooth::feedback::Mesh<5, 5> m;
  m.refine_ph(0, 40);
  ASSERT_EQ(m.N_ivals(), 8);

  Eigen::MatrixXd X(nx, m.N_colloc() + 1);
  Eigen::MatrixXd U(nu, m.N_colloc());
  Eigen::MatrixXd C(ncr, m.N_colloc());

  // fill X with curve values at the two intervals
  std::size_t M = 0;
  for (auto p = 0u; p < m.N_ivals(); ++p) {
    const auto [tau_s, w_s] = m.interval_nodes_and_weights(p);
    for (auto i = 0u; i + 1 < tau_s.size(); ++i) {
      X.col(M + i) = x(t0 + (tf - t0) * tau_s[i]);
      C.col(M + i) = cr.operator()<double>(0, X.col(M + i), U.col(M + i));
    }
    M += m.N_colloc_ival(p);
  }
  X.col(m.N_colloc()) = x(tf);

  Eigen::VectorXd Q{{0}};

  const Eigen::VectorXd dyn_vals = smooth::feedback::colloc_dyn<false>(nx, f, m, t0, tf, X, U);
  ASSERT_EQ(dyn_vals.rows(), m.N_colloc());
  ASSERT_EQ(dyn_vals.cols(), 1);
  ASSERT_LE(dyn_vals.cwiseAbs().maxCoeff(), 1e-8);

  const Eigen::VectorXd cr_vals =
    smooth::feedback::colloc_eval<false>(ncr, cr, m, t0, tf, X, U).reshaped();
  ASSERT_EQ(cr_vals.rows(), 2 * m.N_colloc());
  ASSERT_EQ(cr_vals.cols(), 1);
  ASSERT_TRUE(C.reshaped().isApprox(cr_vals));

  const Eigen::VectorXd q_vals = smooth::feedback::colloc_int<false>(nq, g, m, t0, tf, Q, X, U);
  ASSERT_NEAR(q_vals.x(), 0.217333 + 0.1 * (tf - t0), 1e-4);
}

TEST(Collocation, DynError)
{
  // given trajectory
  std::size_t nx = 1;
  const auto x = [](double t) -> Vec<double> { return Vec<double>{{0.1 * t * t - 0.4 * t + 0.2}}; };

  // system dynamics
  std::size_t nu = 0;
  const auto f   = []<typename T>(const T & t, const Vec<T> &, const Vec<T> &) -> Vec<T> {
    return Vec<T>{{0.2 * t - 0.4}};
  };

  double t0 = 3;
  double tf = 5;

  smooth::feedback::Mesh<5, 5> m;

  // trajectory is not a polynomial, so we need a couple of intervals for a good approximation
  m.refine_ph(0, 16 * 5);
  ASSERT_EQ(m.N_ivals(), 16);

  // fill X with curve values at the two intervals
  std::size_t M = 0;
  Eigen::MatrixXd X(nx, m.N_colloc() + 1);
  for (auto p = 0u; p < m.N_ivals(); ++p) {
    const auto [tau_s, w_s] = m.interval_nodes_and_weights(p);
    for (auto i = 0u; i + 1 < tau_s.size(); ++i) { X.col(M + i) = x(t0 + (tf - t0) * tau_s[i]); }
    M += m.N_colloc_ival(p);
  }
  X.col(m.N_colloc()) = x(tf);

  auto xfun = [&](const double t) -> Eigen::VectorXd {
    return m.eval<Eigen::VectorXd>((t - t0) / (tf - t0), X.colwise(), 0, true);
  };
  auto ufun = [&](const double) -> Eigen::VectorXd { return Eigen::VectorXd::Zero(nu); };

  auto rel_errs = smooth::feedback::mesh_dyn_error(nx, f, m, t0, tf, xfun, ufun);

  ASSERT_LE(rel_errs.cwiseAbs().maxCoeff(), 1e-8);

  const auto Npre = m.N_ivals();
  smooth::feedback::mesh_refine(m, rel_errs, 1e-8);

  ASSERT_EQ(m.N_ivals(), Npre);
}

TEST(Collocation, StateTrajectory)
{
  // given trajectory and system dynamics
  std::size_t nx = 1;
  std::size_t nu = 0;
  const auto x   = [](double t) { return Vec<double>{{1.5 * exp(-t)}}; };
  const auto f   = []<typename T>(const T &, const Vec<T> & x, const Vec<T> &) -> Vec<T> {
    return Vec<T>{{-x.x()}};
  };

  // integrals
  std::size_t nq = 1;
  const auto g   = []<typename T>(const T &, const Vec<T> & x, const Vec<T> &) -> Vec<T> {
    return Vec<T>{{x.squaredNorm()}};
  };

  double t0 = 3;
  double tf = 5;

  smooth::feedback::Mesh<5, 5> m;

  // trajectory is not a polynomial, so we need a couple of intervals for a good approximation
  m.refine_ph(0, 16 * 5);
  ASSERT_EQ(m.N_ivals(), 16);

  Eigen::MatrixXd X(1, m.N_colloc() + 1);

  // fill X with curve values at the two intervals
  std::size_t M = 0;
  for (auto p = 0u; p < m.N_ivals(); ++p) {
    const auto [tau_s, w_s] = m.interval_nodes_and_weights(p);
    for (auto i = 0u; i + 1 < tau_s.size(); ++i) { X.col(M + i) = x(t0 + (tf - t0) * tau_s[i]); }
    M += m.N_colloc_ival(p);
  }
  X.col(m.N_colloc()) = x(tf);

  Eigen::MatrixXd U(nu, m.N_colloc());
  Eigen::VectorXd Q{{0}};

  const auto dyn_vals = smooth::feedback::colloc_dyn<false>(nx, f, m, t0, tf, X, U);
  ASSERT_LE(dyn_vals.cwiseAbs().maxCoeff(), 1e-8);

  const auto q_vals = smooth::feedback::colloc_int<false>(nq, g, m, t0, tf, Q, X, U);
  ASSERT_NEAR(q_vals.x(), 0.00273752, 1e-4);
}

TEST(Collocation, FunctionEval)
{
  smooth::feedback::Mesh<5, 5> m;

  {
    Eigen::MatrixXd vals = Eigen::MatrixXd::Ones(3, m.N_colloc() + 1);

    const auto x1 = m.eval<Eigen::VectorXd>(0, vals.colwise());
    ASSERT_TRUE(x1.isApprox(Eigen::VectorXd::Ones(3)));

    const auto x2 = m.eval<Eigen::VectorXd>(0.5, vals.colwise());
    ASSERT_TRUE(x2.isApprox(Eigen::VectorXd::Ones(3)));

    const auto x3 = m.eval<Eigen::VectorXd>(1, vals.colwise());
    ASSERT_TRUE(x3.isApprox(Eigen::VectorXd::Ones(3)));
  }

  {
    Eigen::MatrixXd vals = Eigen::MatrixXd::Ones(3, m.N_colloc());

    const auto x1 = m.eval<Eigen::VectorXd>(0, vals.colwise(), 0, false);
    ASSERT_TRUE(x1.isApprox(Eigen::VectorXd::Ones(3)));

    const auto x2 = m.eval<Eigen::VectorXd>(0.5, vals.colwise(), 0, false);
    ASSERT_TRUE(x2.isApprox(Eigen::VectorXd::Ones(3)));

    const auto x3 = m.eval<Eigen::VectorXd>(1, vals.colwise(), 0, false);
    ASSERT_TRUE(x3.isApprox(Eigen::VectorXd::Ones(3)));
  }

  m.refine_ph(0, 40);

  {
    Eigen::MatrixXd vals_refined = Eigen::MatrixXd::Ones(3, m.N_colloc() + 1);

    const auto x1 = m.eval<Eigen::VectorXd>(0, vals_refined.colwise());
    ASSERT_TRUE(x1.isApprox(Eigen::VectorXd::Ones(3)));

    const auto x2 = m.eval<Eigen::VectorXd>(0.5, vals_refined.colwise());
    ASSERT_TRUE(x2.isApprox(Eigen::VectorXd::Ones(3)));

    const auto x3 = m.eval<Eigen::VectorXd>(1, vals_refined.colwise());
    ASSERT_TRUE(x3.isApprox(Eigen::VectorXd::Ones(3)));
  }
}


#include "particle.hpp"
#include "array_helpers.hpp"

#include <Eigen/Core>

using namespace Optima;
using namespace Eigen;

Particle::Particle(const ParameterSpace &paramSpace)
    : parameterSpace(paramSpace) {

  parameterNames.reserve(paramSpace.size());
  for (const auto &param : paramSpace) {
    parameterNames.push_back(param.first);
  }
  std::sort(parameterNames.begin(), parameterNames.end());

  const auto bounds = makeBoundsFromParameters(parameterSpace);
  const ArrayXd pos = ArrayHelpers::uniformFromBounds(bounds);
  initPosition(pos);
  initVelocity(bounds);
}

const Parameters &Particle::getParameters() {
  return getParametersFromArray(parameters, position);
}

const Parameters &Particle::getBestParameters() {
  return getParametersFromArray(bestParameters, bestPosition);
}

void Particle::initPosition(const ArrayXd &pos) {
  position = pos;
  bestPosition = pos;

  int index = 0;
  for (const auto &name : parameterNames) {
    parameters.emplace(name, pos(index));
    bestParameters.emplace(name, pos(index));
    ++index;
  }
}

void Particle::initVelocity(const Bounds &bounds) {
  const ArrayXd lower = bounds.first;
  const ArrayXd upper = bounds.second;
  const ArrayXd range = upper - lower;

  double v = range.matrix().norm();
  const ArrayXd vrange = ArrayXd::Constant(position.size(), -v);
  auto vbound = std::make_pair(-vrange, vrange);
  velocity = ArrayHelpers::uniformFromBounds(vbound);
}

Bounds
Particle::makeBoundsFromParameters(const ParameterSpace &parameters) const {
  auto nDims = parameters.size();
  ArrayXd lower(nDims);
  ArrayXd upper(nDims);

  int index = 0;
  for (const auto &name : parameterNames) {
    auto values = parameters.at(name);
    lower(index) = values.first;
    upper(index) = values.second;
    ++index;
  }

  return std::make_pair(lower, upper);
}

void Particle::setParametersFromArray(const ArrayXd &array,
                                      Parameters &params) {
  int index = 0;
  for (const auto &param : params) {
    auto name = param.first;
    auto value = array(index);
    params[name] = value;
    ++index;
  }
}

const Parameters &Particle::getParametersFromArray(Parameters &params,
                                                   const ArrayXd &array) {
  int index = 0;
  for (auto &name : parameterNames) {
    params[name] = array(index);
    ++index;
  }
  return params;
}


#pragma once
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <cstdlib>

namespace edp
{
template<typename T, class ColFunc>
auto constructMat(size_t dim, ColFunc&& colFunc) -> Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
{
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> res(dim, dim);
    res.setZero();

    for(size_t i = 0; i < dim; i++)
    {
        auto m = colFunc(i);
        for(auto& elt : m)
        {
            res(elt.first, i) = elt.second;
        }
    }
    return res;
}

template<typename T, class ColFunc>
auto constructSparseMat(size_t dim, ColFunc&& colFunc) -> Eigen::SparseMatrix<T>
{
    using TripletT = Eigen::Triplet<T>;
    std::vector<TripletT> tripletList;
    tripletList.reserve(3 * dim);
    for(size_t col = 0; col < dim; ++col)
    {
        auto m = colFunc(col);
        for(const auto& v : m)
        {
            tripletList.emplace_back(v.first, col, v.second);
        }
    }

    Eigen::SparseMatrix<T> res(dim, dim);
    res.setFromTriplets(tripletList.begin(), tripletList.end());
    return res;
}

// basis must be sorted
template<typename T, typename ColFunc>
auto constructSubspaceMat(ColFunc&& t, const std::vector<uint32_t>& basis) -> Eigen::SparseMatrix<T>
{
    const size_t n = basis.size();

    using TripletT = Eigen::Triplet<T>;
    std::vector<TripletT> tripletList;
    for(size_t i = 0; i < n; i++)
    {
        std::map<uint32_t, T> m = t(basis[i]);
        auto iter = basis.begin();
        for(auto& kv : m)
        {
            iter = std::lower_bound(iter, basis.end(), kv.first);
            if(iter == basis.end())
            {
                break;
            }
            auto j = std::distance(basis.begin(), iter);
            {
                tripletList.emplace_back(i, j, kv.second);
            }
        }
    }

    Eigen::SparseMatrix<T> res(n, n);
    res.setFromTriplets(tripletList.begin(), tripletList.end());
    return res;
}
}


////////////////////////////////////////////////////////////////////////////////////
// The MIT License (MIT)                                                          //
//                                                                                //
// Copyright (c) 2015 Whit Armstrong                                              //
//                                                                                //
// Permission is hereby granted, free of charge, to any person obtaining a copy   //
// of this software and associated documentation files (the "Software"), to deal  //
// in the Software without restriction, including without limitation the rights   //
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell      //
// copies of the Software, and to permit persons to whom the Software is          //
// furnished to do so, subject to the following conditions:                       //
//                                                                                //
// The above copyright notice and this permission notice shall be included in all //
// copies or substantial portions of the Software.                                //
//                                                                                //
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR     //
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,       //
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE    //
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER         //
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,  //
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE  //
// SOFTWARE.                                                                      //
////////////////////////////////////////////////////////////////////////////////////

#pragma once

#include <armadillo>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/factorials.hpp>

namespace arma {

  // lgamma
  class eop_lgamma : public eop_core<eop_lgamma> {};

  template<> template<typename eT> arma_hot arma_pure arma_inline eT
  eop_core<eop_lgamma>::process(const eT val, const eT  ) {
    return boost::math::lgamma(val);
  }

  // Base
  template<typename T1>
  arma_inline
  const eOp<T1, eop_lgamma> lgamma(const Base<typename T1::elem_type,T1>& A) {
    arma_extra_debug_sigprint();
    return eOp<T1, eop_lgamma>(A.get_ref());
  }

  // BaseCube
  template<typename T1>
  arma_inline
  const eOpCube<T1, eop_lgamma> lgamma(const BaseCube<typename T1::elem_type,T1>& A) {
    arma_extra_debug_sigprint();
    return eOpCube<T1, eop_lgamma>(A.get_ref());
  }

  // factln
  double factln(const int i) {
    static std::vector<double> factln_table;

    if(i < 0) {
      return -std::numeric_limits<double>::infinity();
    }

    if(i > 100) {
      return boost::math::lgamma(static_cast<double>(i) + 1);
    }

    if(factln_table.size() < static_cast<size_t>(i+1)) {
      for(int j = factln_table.size(); j < (i+1); j++) {
        factln_table.push_back(std::log(boost::math::factorial<double>(static_cast<double>(j))));
      }
    }
    //for(auto v : factln_table) { std::cout << v << "|"; }  std::cout << std::endl;
    return factln_table[i];
  }

  class eop_factln : public eop_core<eop_factln> {};

  template<> template<typename eT> arma_hot arma_pure arma_inline eT
  eop_core<eop_factln>::process(const eT val, const eT  ) {
    return factln(val);
  }

  // Base
  template<typename T1>
  arma_inline
  const eOp<T1, eop_factln> factln(const Base<typename T1::elem_type,T1>& A) {
    arma_extra_debug_sigprint();
    return eOp<T1, eop_factln>(A.get_ref());
  }

  // BaseCube
  template<typename T1>
  arma_inline
  const eOpCube<T1, eop_factln> factln(const BaseCube<typename T1::elem_type,T1>& A) {
    arma_extra_debug_sigprint();
    return eOpCube<T1, eop_factln>(A.get_ref());
  }

  // cube
  //! element-wise multiplication of BaseCube objects with same element type
  template<typename T1, typename T2>
  arma_inline
  const eGlueCube<T1, T2, eglue_schur>
  schur_prod
  (
   const BaseCube<typename T1::elem_type,T1>& X,
   const BaseCube<typename T1::elem_type,T2>& Y
   )
  {
    arma_extra_debug_sigprint();
    return eGlueCube<T1, T2, eglue_schur>(X.get_ref(), Y.get_ref());
  }

  //! element-wise multiplication of BaseCube objects with different element types
  template<typename T1, typename T2>
  inline
  const mtGlueCube<typename promote_type<typename T1::elem_type, typename T2::elem_type>::result, T1, T2, glue_mixed_schur>
  schur_prod
  (
   const BaseCube< typename force_different_type<typename T1::elem_type, typename T2::elem_type>::T1_result, T1>& X,
   const BaseCube< typename force_different_type<typename T1::elem_type, typename T2::elem_type>::T2_result, T2>& Y
   )
  {
    arma_extra_debug_sigprint();
    typedef typename T1::elem_type eT1;
    typedef typename T2::elem_type eT2;
    typedef typename promote_type<eT1,eT2>::result out_eT;
    promote_type<eT1,eT2>::check();
    return mtGlueCube<out_eT, T1, T2, glue_mixed_schur>( X.get_ref(), Y.get_ref() );
  }

  // matrix
  template<typename T1, typename T2>
  arma_inline
  const eGlue<T1, T2, eglue_schur>
  schur_prod(const Base<typename T1::elem_type,T1>& X, const Base<typename T1::elem_type,T2>& Y) {
    arma_extra_debug_sigprint();
    return eGlue<T1, T2, eglue_schur>(X.get_ref(), Y.get_ref());
  }

  //! element-wise multiplication of Base objects with different element types
  template<typename T1, typename T2>
  inline
  const mtGlue<typename promote_type<typename T1::elem_type, typename T2::elem_type>::result, T1, T2, glue_mixed_schur>
  schur_prod
  (
   const Base< typename force_different_type<typename T1::elem_type, typename T2::elem_type>::T1_result, T1>& X,
   const Base< typename force_different_type<typename T1::elem_type, typename T2::elem_type>::T2_result, T2>& Y
   )
  {
    arma_extra_debug_sigprint();
    typedef typename T1::elem_type eT1;
    typedef typename T2::elem_type eT2;
    typedef typename promote_type<eT1,eT2>::result out_eT;
    promote_type<eT1,eT2>::check();
    return mtGlue<out_eT, T1, T2, glue_mixed_schur>( X.get_ref(), Y.get_ref() );
  }

  //! Base * scalar
  template<typename T1>
  arma_inline
  const eOp<T1, eop_scalar_times>
  schur_prod
  (const Base<typename T1::elem_type,T1>& X, const typename T1::elem_type k)
  {
    arma_extra_debug_sigprint();
    return eOp<T1, eop_scalar_times>(X.get_ref(),k);
  }

  //! scalar * Base
  template<typename T1>
  arma_inline
  const eOp<T1, eop_scalar_times>
  schur_prod
  (const typename T1::elem_type k, const Base<typename T1::elem_type,T1>& X)
  {
    arma_extra_debug_sigprint();
    return eOp<T1, eop_scalar_times>(X.get_ref(),k);  // NOTE: order is swapped
  }

  double schur_prod(const int x, const double y) { return x * y; }
  double schur_prod(const double x, const int y) { return x * y; }
  double schur_prod(const double& x, const double& y) { return x * y; }
  double schur_prod(const int& x, const int& y) { return x * y; }

  // insert an 'any' function for bools into the arma namespace
  bool any(const bool x) {
    return x;
  }

  bool vectorise(bool x) {
    return x;
  }
} // namespace arma


#include <stdexcept>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <Eigen/Core>
#include <spectra/GenEigsSolver.h>
#include "diffusion_map.h"
#include "distance_matrix.h"

#ifdef _OPENMP
#include <omp.h>
#endif

using namespace Spectra;

namespace dmaps
{

    diffusion_map::diffusion_map(const matrix_t& d, const vector_t& w, int num_threads) :
    dint_(d), d_(dint_), w_(w)
    {
        check_params();

        #ifdef _OPENMP
        if(num_threads) omp_set_num_threads(num_threads);
        #endif
    }

    diffusion_map::diffusion_map(const distance_matrix& dm, const vector_t& w, int num_threads) :
    d_(dm.get_distances()), w_(w)
    {
        check_params();

        #ifdef _OPENMP
        if(num_threads) omp_set_num_threads(num_threads);
        #endif
    }

    void diffusion_map::check_params()
    {
        if(d_.cols() != d_.rows())
            throw std::invalid_argument("Distance matrix must be square.");

        if(w_.size() == 0)
            w_ = vector_t::Ones(d_.cols());
        else if(w_.size() != d_.cols())
            throw std::invalid_argument("Weights vector length must match distance matrix size.");
    }

    void diffusion_map::set_kernel_bandwidth(f_type eps)
    {
        if(eps <= 0)
            throw std::invalid_argument("Kernel bandwidth must be positive.");
        eps_ = eps;
    }

    f_type diffusion_map::get_kernel_bandwidth() const
    {
        return eps_;
    }

    f_type diffusion_map::sum_similarity_matrix(f_type eps, f_type alpha) const
    {
        matrix_t wwt = w_*w_.transpose();
        return ((-0.5/eps*d_.array().square().pow(alpha)).exp()*wwt.array()).sum();
    }

    /*
    void diffusion_map::estimate_local_scale(int k)
    {
        // Default choice of k.
        if(k == 0) k = static_cast<int>(std::sqrt(d_.rows()));

        // Set local epsilon scale for each entry.
        #ifdef _OPENMP
        #pragma omp parallel
        #endif
        {
            // Create sort indexer.
            std::vector<size_t> idx(d_.rows());
            std::iota(std::begin(idx), std::end(idx), static_cast<size_t>(0));

            #ifdef _OPENMP
            #pragma for schedule(static)
            #endif
            for(size_t i = 0; i < eps_.size(); ++i)
            {
                const vector_t& dist = d_.row(i);

                // Get indices of sorted distances (ascending).
                std::sort(std::begin(idx), std::end(idx),
                    [&](size_t a, size_t b) { return dist[a] < dist[b]; }
                );

                // Sum and determine weight.
                // We skip the first element which is itself.
                f_type sum = 0.;
                for(size_t j = 1; j < idx.size(); ++j)
                {
                    sum += w_[idx[j]];
                    // Break at k value.
                    if(sum >= k)
                    {
                        eps_[i] = dist[idx[j]];
                        break;
                    }
                }
            }
        }
    }
    */

    void diffusion_map::compute(int n, f_type alpha, f_type beta)
    {
        if(eps_ == 0)
            throw std::runtime_error("Kernel bandwidth must be defined before computing diffusion coordinates.");

        if(alpha <= 0 || alpha > 1)
            throw std::invalid_argument("Distance scaling must be in the interval (0,1].");

        // Compute similarity matrix and row normalize
        // to get right stochastic matrix.
        k_ = -0.5/eps_*d_.array().square().pow(alpha);
        k_.array() = k_.array().exp();
        k_.array() *= (w_*w_.transpose()).array();

        // Density normalization.
        vector_t rsum =  k_.rowwise().sum().array().pow(-beta);
        k_ = rsum.asDiagonal()*k_*rsum.asDiagonal();

        // Right stochastic matrix.
        rsum =  k_.rowwise().sum().array().cwiseInverse();
        k_ = rsum.asDiagonal()*k_;

        // Define eigensolver.
        DenseGenMatProd<f_type> op(k_);
        GenEigsSolver <f_type, LARGEST_MAGN, DenseGenMatProd<f_type>> eigs(&op, n, 2*n);

        // Solve.
        eigs.init();
        eigs.compute();

        if(eigs.info() != SUCCESSFUL)
            throw std::runtime_error("Eigensolver did not converge.");

        dvals_ = eigs.eigenvalues().real();
        dvecs_ = eigs.eigenvectors().real();
    }

    vector_t diffusion_map::nystrom(const vector_t& distances, f_type alpha, f_type beta)
    {
        if(dvals_.size() == 0)
            throw std::runtime_error("Eigenvectors must be computed first.");

        vector_t k = -0.5/eps_*distances.array().square().pow(alpha);
        k.array() = k.array().exp();
        k /= k.sum();

        vector_t point(dvals_.size());

        for(int i = 0; i < dvals_.size(); ++i)
            point[i] = 1./dvals_[i]*(k.array()*dvecs_.col(i).array()).sum();

        return point;
    }

    const matrix_t& diffusion_map::get_eigenvectors() const
    {
        return dvecs_;
    }

    const vector_t& diffusion_map::get_eigenvalues() const
    {
        return dvals_;
    }

    const matrixc_t& diffusion_map::get_kernel_matrix() const
    {
        return k_;
    }
}


#include <iostream>
#include <Eigen/Dense>
#include <manifold/S.h>

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

  S3d q;
  std::cout << q << std::endl;

  std::cout << q.Exp(Eigen::Vector3d(0.,M_PI/2.,0.)).norm() << std::endl;
  std::cout << q.Exp(q.ToAmbient(Eigen::Vector2d(0.,M_PI/2.))).norm() << std::endl;

  S3d mu;
  mu.vector() << 1./sqrt(2),1./sqrt(2), 0.;
  std::cout << mu << std::endl;

  double delta = 0.1;
  double f_prev = 1e99;
  double f = mu.dot(q);
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100; ++it) {
    Eigen::Vector3d J = -2.*(mu.vector() - q.vector()*q.dot(mu));

    q = q.Exp(-delta*J);
//    q = q.RetractOrtho(-delta*J);

    f_prev = f;
    f = mu.dot(q);
    std::cout << "@" << it << ": f=" << f
      << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) > -1e-10) break;
  }
  std::cout << std::endl << mu << std::endl;
  std::cout << std::endl << q << std::endl;
}


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

#include "required_defines.hpp"

#include "performance_measure.hpp"

#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/array.hpp>

#define T double
#  include "../test/log1p_expm1_data.ipp"

template <std::size_t N>
double log1p_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::log1p(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(log1p_test, "log1p")
{
   double result = log1p_evaluate2(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

template <std::size_t N>
double expm1_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::expm1(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(expm1_test, "expm1")
{
   double result = expm1_evaluate2(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

#ifdef TEST_DCDFLIB
#include <dcdflib.h>

template <std::size_t N>
double log1p_evaluate2_dcd(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
   {
      double t = data[i][0];
      result += ::alnrel(&t);
   }
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(log1p_test_dcd, "log1p-dcd")
{
   double result = log1p_evaluate2_dcd(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

template <std::size_t N>
double expm1_evaluate2_dcd(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
   {
      double t = data[i][0];
      result += ::dexpm1(&t);
   }
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(expm1_test_dcd, "expm1-dcd")
{
   double result = expm1_evaluate2_dcd(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

#endif


/*
 * @file SE2Config.hpp
 * @date Nov 24, 2015
 * @author Renaud Dubé
 */

#ifndef SE2CONFIG_H_
#define SE2CONFIG_H_

#include <Eigen/Core>
#include "gtsam/geometry/Pose2.h"

namespace curves {

typedef Eigen::Matrix<double, 3, 1> Vector3d;

struct SE2Config {
  typedef gtsam::Pose2 ValueType;
  typedef Vector3d DerivativeType;
};

}  // namespace curves

#endif // SE2CONFIG_H_


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_vertex_base_with_info_3.h>
#include <boost/iterator/transform_iterator.hpp>
#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel         K;
typedef CGAL::Triangulation_vertex_base_with_info_3<unsigned, K>    Vb;
typedef CGAL::Triangulation_data_structure_3<Vb>                    Tds;
typedef CGAL::Delaunay_triangulation_3<K, Tds>                      Delaunay;
typedef Delaunay::Point                                             Point;

//a functor that returns a std::pair<Point,unsigned>.
//the unsigned integer is incremented at each call to
//operator()
struct Auto_count : public std::unary_function<const Point&,std::pair<Point,unsigned> >{
  mutable unsigned i;
  Auto_count() : i(0){}
  std::pair<Point,unsigned> operator()(const Point& p) const {
    return std::make_pair(p,i++);
  }
};

int main()
{
  std::vector<Point> points;
  points.push_back(Point(0,0,0));
  points.push_back(Point(1,0,0));
  points.push_back(Point(0,1,0));
  points.push_back(Point(0,0,1));
  points.push_back(Point(2,2,2));
  points.push_back(Point(-1,0,1));

  Delaunay T( boost::make_transform_iterator(points.begin(),Auto_count()),
              boost::make_transform_iterator(points.end(),  Auto_count() )  );

  CGAL_assertion( T.number_of_vertices() == 6 );

  // check that the info was correctly set.
  Delaunay::Finite_vertices_iterator vit;
  for (vit = T.finite_vertices_begin(); vit != T.finite_vertices_end(); ++vit)
    if( points[ vit->info() ] != vit->point() ){
      std::cerr << "Error different info" << std::endl;
      exit(EXIT_FAILURE);
    }
  std::cout << "OK" << std::endl;

  return 0;
}


#ifndef SPARSEMATRIX_HPP
#define SPARSEMATRIX_HPP

#include <vector>
#include <fstream>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/binary_object.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/complex.hpp>

#include <Eigen/Core>
#include <Eigen/Sparse>

template<class scalar>
class sparseMatrix
{
public:

  sparseMatrix();

  sparseMatrix(int rows, int cols);

  sparseMatrix(int rows, int cols, int nnz);

  sparseMatrix(std::vector<scalar> &data, std::vector<int> &outer_, std::vector<int> &inner_, int rows, int cols);

  void resize(int rows, int cols);

  void reserve(int rows, int cols, int nnz);

  void clear();

  void save(std::string fname);
  void load(std::string fname);

  int nnz() { return nnz_; }
  int nnz() const { return nnz_; }

  int rows() { return rows_; }
  int rows() const { return rows_; }

  int cols() { return cols_; }
  int cols() const { return cols_; }

  std::vector<scalar>& data_container() { return data_; }
  std::vector<scalar>  data_container() const { return data_; }

  std::vector<int>& inner_container() { return inner_; }
  std::vector<int>  inner_container() const { return inner_; }

  std::vector<int>& outer_container() { return outer_; }
  std::vector<int>  outer_container() const { return outer_; }

  scalar* data() { return data_.data(); }
  scalar* data() const { return data_.data(); }

  int* innerPtr() { return inner_.data(); }
  int* innerPtr() const { return inner_.data(); }

  int* outerPtr() { return outer_.data(); }
  int* outerPtr() const { return outer_.data(); }

  scalar &operator[](int i) { return data_[i]; }
  scalar &operator[](int i) const { return data_[i]; }
  //scalar &operator()(int row, int col);

  sparseMatrix& operator=(const  sparseMatrix& other);
  sparseMatrix& operator+=(const sparseMatrix& other);
  sparseMatrix& operator*=(const double a);
  sparseMatrix  operator*(const double a);
  sparseMatrix  operator+(const sparseMatrix& other);
  sparseMatrix  operator*(const sparseMatrix& other);

  /// extra member functions to fascilitation python wrapping
  void assign(const sparseMatrix& other);
  void setElem(scalar elem, int row, int col);

  size_t size();

  void print();

private:

  friend class boost::serialization::access;

  template <typename Archive>
  void serialize(Archive &ar, const unsigned int version)
  {
    ar & nnz_;
    ar & cols_;
    ar & rows_;
    ar & data_;
    ar & inner_;
    ar & outer_;
  }

  /// size of matrix
  int cols_;
  int rows_;
  int nnz_;

  /// container class to hold data for eigen map
  std::vector<scalar> data_;

  /// container classes to hold indices
  std::vector<int> inner_;
  std::vector<int> outer_;

};

#include "sparseMatrix_impl.hpp"

#endif /*SPARSEMATRIX_HPP*/


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <utility>
#include <vector>

#include <revdoor.hpp>

using namespace std;

BOOST_AUTO_TEST_CASE(test_combinations) {
  int n = 5;
  int t = 3;
  revdoor::combinations combs(n, t);
  vector<vector<int>> states;
  vector<int> init_state = combs.state();
  states.push_back(init_state);
  vector<int> expected_init_state = {0, 1, 2};
  BOOST_CHECK_EQUAL_COLLECTIONS(
      init_state.begin(),
      init_state.end(),
      expected_init_state.begin(),
      expected_init_state.end());
  vector<pair<int,int>> swaps;
  vector<pair<int,int>> expected_swaps = {
    {1, 3}, {0, 1}, {2, 0}, {1, 4}, {0, 1},
    {1, 2}, {3, 0}, {0, 1}, {2, 0}
  };
  int out, in;
  while (combs.step(&out, &in)) {
    swaps.push_back({out, in});
    states.push_back(combs.state());
  }
  BOOST_REQUIRE_EQUAL(swaps.size(), expected_swaps.size());
  for (int i = 0; i < (int)swaps.size(); ++i) {
    BOOST_CHECK_EQUAL(swaps[i].first, expected_swaps[i].first);
    BOOST_CHECK_EQUAL(swaps[i].second, expected_swaps[i].second);
  }
  vector<int> end_state = states[states.size() - 1];
  vector<int> expected_end_state = {0, 1, 4};
  BOOST_CHECK_EQUAL_COLLECTIONS(
      end_state.begin(),
      end_state.end(),
      expected_end_state.begin(),
      expected_end_state.end());
  for (int i = 0; i < (int)states.size(); ++i) {
    combs.set_state(states[i]);
    swaps.clear();
    while (combs.step(&out, &in)) {
      swaps.push_back({out, in});
    }
    BOOST_REQUIRE_EQUAL(swaps.size(), expected_swaps.size() - i);
    for (int j = 0; j < (int)swaps.size(); ++j) {
      BOOST_CHECK_EQUAL(swaps[j].first, expected_swaps[i + j].first);
      BOOST_CHECK_EQUAL(swaps[j].second, expected_swaps[i + j].second);
    }
    end_state = states[states.size() - 1];
    BOOST_CHECK_EQUAL_COLLECTIONS(
        end_state.begin(),
        end_state.end(),
        expected_end_state.begin(),
        expected_end_state.end());
  }
}

BOOST_AUTO_TEST_CASE(test_combinations_with_replacement) {
  int n = 5;
  int t = 3;
  revdoor::combinations_with_replacement combs(n, t);
  vector<vector<int>> states;
  vector<int> init_state = combs.state();
  states.push_back(init_state);
  vector<int> expected_init_state = {0, 0, 0};
  BOOST_CHECK_EQUAL_COLLECTIONS(
      init_state.begin(),
      init_state.end(),
      expected_init_state.begin(),
      expected_init_state.end());
  vector<vector<int>> swaps;
  vector<vector<int>> expected_swaps = {
    {0, 1, 0, 1}, {0, 1}, {1, 0, 1, 0}, {0, 2, 1, 2}, {0, 1},
    {1, 2}, {2, 1, 2, 0}, {0, 1}, {1, 0, 1, 0}, {0, 3, 2, 3},
    {0, 1}, {1, 2}, {2, 3}, {3, 2, 3, 0}, {0, 1}, {1, 2},
    {2, 1, 2, 0}, {0, 1}, {1, 0, 1, 0}, {0, 4, 3, 4}, {0, 1},
    {1, 2}, {2, 3}, {3, 4}, {4, 3, 4, 0}, {0, 1}, {1, 2},
    {2, 3}, {3, 2, 3, 0}, {0, 1}, {1, 2}, {2, 1, 2, 0}, {0, 1},
    {1, 0, 1, 0}
  };
  int out1, in1, out2, in2;
  while (combs.step(&out1, &in1, &out2, &in2)) {
    vector<int> v;
    if (out1 != in1) {
      v.push_back(out1);
      v.push_back(in1);
    }
    v.push_back(out2);
    v.push_back(in2);
    swaps.push_back(v);
    states.push_back(combs.state());
  }
  BOOST_REQUIRE_EQUAL(swaps.size(), expected_swaps.size());
  for (int i = 0; i < (int)swaps.size(); ++i) {
    BOOST_CHECK_EQUAL_COLLECTIONS(
        swaps[i].begin(),
        swaps[i].end(),
        expected_swaps[i].begin(),
        expected_swaps[i].end());
  }
  vector<int> end_state = combs.state();
  vector<int> expected_end_state = {0, 0, 4};
  BOOST_CHECK_EQUAL_COLLECTIONS(
      end_state.begin(),
      end_state.end(),
      expected_end_state.begin(),
      expected_end_state.end());
  for (int i = 0; i < (int)states.size(); ++i) {
    combs.set_state(states[i]);
    swaps.clear();
    while (combs.step(&out1, &in1, &out2, &in2)) {
      vector<int> v;
      if (out1 != in1) {
        v.push_back(out1);
        v.push_back(in1);
      }
      v.push_back(out2);
      v.push_back(in2);
      swaps.push_back(v);
    }
    BOOST_REQUIRE_EQUAL(swaps.size(), expected_swaps.size() - i);
    for (int j = 0; j < (int)swaps.size(); ++j) {
      BOOST_CHECK_EQUAL_COLLECTIONS(
          swaps[j].begin(),
          swaps[j].end(),
          expected_swaps[i + j].begin(),
          expected_swaps[i + j].end());
    }
    end_state = states[states.size() - 1];
    BOOST_CHECK_EQUAL_COLLECTIONS(
        end_state.begin(),
        end_state.end(),
        expected_end_state.begin(),
        expected_end_state.end());
  }
}


//=======================================================================
// Copyright (c) 2018 Yi Ji
//
// 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)
//
//=======================================================================

#include <iostream>
#include <vector>
#include <string>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/maximum_weighted_matching.hpp>

using namespace boost;

typedef property<edge_weight_t, float, property<edge_index_t, int> > EdgeProperty;
typedef adjacency_list<vecS, vecS, undirectedS, no_property, EdgeProperty> my_graph;

int main(int argc, const char * argv[])
{
    graph_traits<my_graph>::vertex_iterator vi, vi_end;
    const int n_vertices = 18;
    my_graph g(n_vertices);

    // vertices can be refered by integers because my_graph use vector to store them

    add_edge(1, 2, EdgeProperty(5), g);
    add_edge(0, 4, EdgeProperty(1), g);
    add_edge(1, 5, EdgeProperty(4), g);
    add_edge(2, 6, EdgeProperty(1), g);
    add_edge(3, 7, EdgeProperty(4), g);
    add_edge(4, 5, EdgeProperty(7), g);
    add_edge(6, 7, EdgeProperty(5), g);
    add_edge(4, 8, EdgeProperty(2), g);
    add_edge(5, 9, EdgeProperty(5), g);
    add_edge(6, 10, EdgeProperty(6), g);
    add_edge(7, 11, EdgeProperty(5), g);
    add_edge(10, 11, EdgeProperty(4), g);
    add_edge(8, 13, EdgeProperty(4), g);
    add_edge(9, 14, EdgeProperty(4), g);
    add_edge(10, 15, EdgeProperty(7), g);
    add_edge(11, 16, EdgeProperty(6), g);
    add_edge(14, 15, EdgeProperty(6), g);
    add_edge(12, 13, EdgeProperty(2), g);
    add_edge(16, 17, EdgeProperty(5), g);

    // print the ascii graph into terminal (better to use fixed-width font)
    // this graph has a maximum cardinality matching of size 8
    // but maximum weighted matching is of size 7

    std::vector<std::string> ascii_graph_weighted;

    ascii_graph_weighted.push_back("                     5                 ");
    ascii_graph_weighted.push_back("           A       B---C       D       ");
    ascii_graph_weighted.push_back("           1\\  7  /4   1\\  5  /4     ");
    ascii_graph_weighted.push_back("             E---F       G---H         ");
    ascii_graph_weighted.push_back("            2|   |5     6|   |5        ");
    ascii_graph_weighted.push_back("             I   J       K---L         ");
    ascii_graph_weighted.push_back("           4/    3\\    7/  4  \\6     ");
    ascii_graph_weighted.push_back("       M---N       O---P       Q---R   ");
    ascii_graph_weighted.push_back("         2           6           5     ");

    // our maximum weighted matching and result

    std::cout << "In the following graph:" << std::endl << std::endl;

    for(std::vector<std::string>::iterator itr = ascii_graph_weighted.begin(); itr != ascii_graph_weighted.end(); ++itr)
        std::cout << *itr << std::endl;

    std::cout << std::endl;

    std::vector<graph_traits<my_graph>::vertex_descriptor> mate1(n_vertices), mate2(n_vertices);
    maximum_weighted_matching(g, &mate1[0]);

    std::cout << "Found a weighted matching:" << std::endl;
    std::cout << "Matching size is " << matching_size(g, &mate1[0]) << ", total weight is " << matching_weight_sum(g, &mate1[0]) << std::endl;
    std::cout << std::endl;

    std::cout << "The matching is:" << std::endl;
    for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
        if (mate1[*vi] != graph_traits<my_graph>::null_vertex() && *vi < mate1[*vi])
            std::cout << "{" << *vi << ", " << mate1[*vi] << "}" << std::endl;
    std::cout << std::endl;

    // now we check the correctness by compare the weight sum to a brute-force matching result
    // note that two matchings may be different because of multiple optimal solutions

    brute_force_maximum_weighted_matching(g, &mate2[0]);

    std::cout << "Found a weighted matching by brute-force searching:" << std::endl;
    std::cout << "Matching size is " << matching_size(g, &mate2[0]) << ", total weight is " << matching_weight_sum(g, &mate2[0]) << std::endl;
    std::cout << std::endl;

    std::cout << "The brute-force matching is:" << std::endl;
    for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
        if (mate2[*vi] != graph_traits<my_graph>::null_vertex() && *vi < mate2[*vi])
            std::cout << "{" << *vi << ", " << mate2[*vi] << "}" << std::endl;
    std::cout << std::endl;

    assert(matching_weight_sum(g, &mate1[0]) == matching_weight_sum(g, &mate2[0]));

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

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

#include <iostream>

#include <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/concepts/polygon_concept.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#include <initializer_list>
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST

template <typename P>
bg::model::polygon<P> create_polygon()
{
    bg::model::polygon<P> pl1;
    P p1;
    P p2;
    P p3;
    bg::assign_values(p1, 1, 2);
    bg::assign_values(p2, 2, 0);
    bg::assign_values(p3, 0, 0);

    bg::append(pl1, p1);
    bg::append(pl1, p2);
    bg::append(pl1, p3);
    bg::append(pl1, p1);
    return pl1;
}

template <typename PL, typename P>
void check_polygon(PL& to_check, P p1, P p2, P p3)
{
    PL cur;
    bg::append(cur, p1);
    bg::append(cur, p2);
    bg::append(cur, p3);
    bg::append(cur, p1);

    std::ostringstream out1, out2;
    out1 << bg::dsv(to_check);
    out2 << bg::dsv(cur);
    BOOST_CHECK_EQUAL(out1.str(), out2.str());
}

template <typename P>
void test_default_constructor()
{
    bg::model::polygon<P> pl1(create_polygon<P>());
    check_polygon(pl1, P(1, 2), P(2, 0), P(0, 0));
}

template <typename P>
void test_copy_constructor()
{
    bg::model::polygon<P> pl1 = create_polygon<P>();
    check_polygon(pl1, P(1, 2), P(2, 0), P(0, 0));
}

template <typename P>
void test_copy_assignment()
{
    bg::model::polygon<P> pl1(create_polygon<P>()), pl2;
    pl2 = pl1;
    check_polygon(pl2, P(1, 2), P(2, 0), P(0, 0));
}

template <typename P>
void test_concept()
{
    typedef bg::model::polygon<P> PL;

    BOOST_CONCEPT_ASSERT( (bg::concepts::ConstPolygon<PL>) );
    BOOST_CONCEPT_ASSERT( (bg::concepts::Polygon<PL>) );

    typedef typename bg::coordinate_type<PL>::type T;
    typedef typename bg::point_type<PL>::type PPL;
    boost::ignore_unused<T, PPL>();
}

template <typename P>
void test_all()
{
    test_default_constructor<P>();
    test_copy_constructor<P>();
    test_copy_assignment<P>();
    test_concept<P>();
}

template <typename P>
void test_custom_polygon(bg::model::ring<P> IL)
{
#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    std::initializer_list<bg::model::ring<P> > RIL = {IL};
    bg::model::polygon<P> pl1(RIL);
    std::ostringstream out;
    out << bg::dsv(pl1);
    BOOST_CHECK_EQUAL(out.str(), "(((3, 3), (3, 0), (0, 0), (0, 3), (3, 3)))");
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST
}

template <typename P>
void test_custom()
{
    std::initializer_list<P> IL = {P(3, 3), P(3, 0), P(0, 0), P(0, 3), P(3, 3)};
    bg::model::ring<P> r1(IL);
    test_custom_polygon<P>(r1);
}

template <typename CS>
void test_cs()
{
    test_all<bg::model::point<int, 2, CS> >();
    test_all<bg::model::point<float, 2, CS> >();
    test_all<bg::model::point<double, 2, CS> >();

    test_custom<bg::model::point<double, 2, CS> >();
}

int test_main(int, char* [])
{
    test_cs<bg::cs::cartesian>();
    test_cs<bg::cs::spherical<bg::degree> >();
    test_cs<bg::cs::spherical_equatorial<bg::degree> >();
    test_cs<bg::cs::geographic<bg::degree> >();

    test_custom<bg::model::d2::point_xy<double> >();

    return 0;
}


#include <iostream>
#include <boost/date_time/gregorian/gregorian.hpp>

using namespace std;
using namespace boost::gregorian;

int main(){
    weeks w(3);     // 3 weeks
    cout << "Days of 3 weeks = " << w.days() << endl;

    months m(5);
    years  y(2);

    months m2 = y + m;      //  2 Years and 5 months
    cout << "Months of 2 years and 5 months is " << m2.number_of_months() << endl;
    cout << "Years of 2 years x 2 is " << (y * 2).number_of_years() << endl;
    return 0;
}