#include "render/mesh/MeshFactory.h"
#include "math/Defines.h"

#include <cmath>

#include <boost/format.hpp>

namespace epsilon
{
	ParametricData Parametric::Sphere(int slices, int stacks)
	{
		ParametricData data;
		float theta, phi;
		int v = 0;
		int next = 0;

		// Generate Vertices
		for (int x = 0; x < (slices+1); x++)
		{
			theta = x * PI / slices;
			for( int y = 0; y < stacks; y++)
			{
				phi = y * TWOPI / stacks;
				float px = sin(theta) * cos(phi);
				float py = cos(theta);
				float pz = -sin(theta) * sin(phi);
				data.vertices.push_back( Vector3( px, py, pz) );
				data.normals.push_back( Vector3( px, py, pz).Normalised() );
			}
		}

		// Generate Faces/Indices
		for (int x = 0; x < slices; x++ )
		{
			for (int y = 0; y < stacks; y++ )
			{
				next = ( y + 1 ) % stacks;
				// first triangle
				data.indices.push_back( v + y + stacks );
				data.indices.push_back( v + next );
				data.indices.push_back( v + y );

				// second triangle
				data.indices.push_back( v + y + stacks );
				data.indices.push_back( v + next + stacks);
				data.indices.push_back( v + next );
			}
			v += stacks;
		}

		return data;
	}

	ParametricData Parametric::Plane(int rows, int columns)
	{
		ParametricData data;

		float c, r;
		int tl, tr, bl, br;
		int v = 0;

		// Generate Vertices
		for ( int x = 0; x < (columns + 1); x++ )
		{
			c = x * ( 1.0f / columns );
			for ( int y = 0; y < (rows + 1); y++ )
			{
				r = y * ( 1.0f / rows );
				data.vertices.push_back( Vector3( (-0.5f + c),
												  0.0f,
												  (-0.5f + r) ) );
			}
		}

		// Generate Normals
		for ( int n = 0; n < ((rows + 1) * (columns + 1)); n++ )
		{
			//data.normals.push_back(Vector3::UP);
			data.normals.push_back(Vector3(0.0f, 1.0f, 0.0f));
			data.colours.push_back(Vector4(1.0f, 1.0f, 1.0f, 1.0f));
		}

		// Generate Faces/Indices
		for ( int x = 0; x < columns; x++ )
		{
			for ( int y = 0; y < rows; y++ )
			{
				bl = v + x + y;
				tl = bl + 1;
				tr = tl + rows + 1;
				br = tr - 1;
				data.indices.push_back(bl);
				data.indices.push_back(tl);
				data.indices.push_back(tr);

				data.indices.push_back(bl);
				data.indices.push_back(tr);
				data.indices.push_back(br);
			}
			v += rows;
		}

		// Generate Texture Coordinates
		for (int x = 0; x < (columns + 1); x++ )
		{
			c = x * ( 1.0f / columns);
			for ( int y = 0; y < (rows + 1); y++ )
			{
				r = y * ( 1.0f / rows );
				data.texCoords.push_back(Vector2(c, r));
			}
		}

		return data;
	}

	MeshFactory::MeshFactory(void)
	{
	}

	MeshFactory::~MeshFactory(void)
	{
	}
	Mesh::Ptr MeshFactory::GenerateGrid(int size, int resolution)
	{
		Mesh::Ptr newGrid = Mesh::Create(GL_LINES);

        newGrid->SetMeshType("GRID");
        newGrid->SetMeshParameters(str(format("size=%d|resolution=%d") % size % resolution));

		VerticesAttrib::List verts;
		//ColourAttrib::List
		float halfWidth = size / 2.0f;

		// if the resolution doesn't evenly divide into the size
		if ( (size % resolution) > 0.0f )
		{
			float diff = (float)(size % resolution);

			// modify the size so that the resolution evenly fits
			halfWidth -= diff / 2.0f;
		}

		// Impossible to have a grid smaller than its resolution
		if ( size > resolution )
		{
			float resStep =  (float)resolution;// / halfWidth;

			// 'Vertical' Lines first
			for ( float x = -halfWidth; x < halfWidth; x += resStep)
			{
				verts.push_back(Vector3(x,0,-halfWidth));
				verts.push_back(Vector3(x,0,halfWidth));
			}

			// 'Horizontal' lines
			for ( float z = -halfWidth; z < halfWidth; z += resStep)
			{
				verts.push_back(Vector3(-halfWidth,0,z));
				verts.push_back(Vector3(halfWidth,0,z));
			}

			// Border
			// TL -> TR
			verts.push_back(Vector3(-halfWidth,0,halfWidth));
			verts.push_back(Vector3(halfWidth,0,halfWidth));
			// TR - BR
			verts.push_back(Vector3(halfWidth,0,halfWidth));
			verts.push_back(Vector3(halfWidth,0,-halfWidth));
			// BR -> BL
			verts.push_back(Vector3(halfWidth,0,-halfWidth));
			verts.push_back(Vector3(-halfWidth,0,-halfWidth));
			// BL -> TL
			verts.push_back(Vector3(-halfWidth,0,-halfWidth));
			verts.push_back(Vector3(-halfWidth,0,halfWidth));
		}

		newGrid->VertexData()
			   ->SetVertices(verts);
			   //->BuildBuffers();

		return newGrid;
	}

	Mesh::Ptr MeshFactory::GenerateCube()
	{
		Mesh::Ptr newCube =  Mesh::Create();

        newCube->SetMeshType("CUBE");

		VerticesAttrib::List verts;
		NormalAttrib::List norms;
		//TexCoordAttrib::List texCoords;
		VertexIndicesBuffer::List faces;

		verts.push_back(Vector3(-0.5, -0.5, -0.5));
        norms.push_back(Vector3(-0.5, -0.5, -0.5));

        verts.push_back(Vector3(-0.5, -0.5, 0.5));
        norms.push_back(Vector3(-0.5, -0.5, 0.5));

        verts.push_back(Vector3(-0.5, 0.5, -0.5));
        norms.push_back(Vector3(-0.5, 0.5, -0.5));

        verts.push_back(Vector3(-0.5, 0.5, 0.5));
        norms.push_back(Vector3(-0.5, 0.5, 0.5));

        verts.push_back(Vector3(0.5, -0.5, -0.5));
        norms.push_back(Vector3(0.5, -0.5, -0.5));

        verts.push_back(Vector3(0.5, -0.5, 0.5));
        norms.push_back(Vector3(0.5, -0.5, 0.5));

        verts.push_back(Vector3(0.5, 0.5, -0.5));
        norms.push_back(Vector3(0.5, 0.5, -0.5));

        verts.push_back(Vector3(0.5, 0.5, 0.5));
        norms.push_back(Vector3(0.5, 0.5, 0.5));

		// left
		faces.push_back(0);
		faces.push_back(1);
		faces.push_back(3);
		faces.push_back(3);
		faces.push_back(2);
		faces.push_back(0);

		// right
		faces.push_back(4);
		faces.push_back(6);
		faces.push_back(7);
		faces.push_back(7);
		faces.push_back(5);
		faces.push_back(4);

		// front
		faces.push_back(7);
		faces.push_back(3);
		faces.push_back(1);
		faces.push_back(1);
		faces.push_back(5);
		faces.push_back(7);

		// back
		faces.push_back(0);
		faces.push_back(2);
		faces.push_back(6);
		faces.push_back(6);
		faces.push_back(4);
		faces.push_back(0);

		// top
		faces.push_back(3);
		faces.push_back(7);
		faces.push_back(2);
		faces.push_back(7);
		faces.push_back(6);
		faces.push_back(2);

		// bottom
		faces.push_back(5);
		faces.push_back(1);
		faces.push_back(4);
		faces.push_back(1);
		faces.push_back(0);
		faces.push_back(4);

		newCube->VertexData()
			   ->SetVertices(verts)
               ->SetNormals(norms)
			   ->SetIndices(faces);

		return newCube;
	}

	Mesh::Ptr MeshFactory::GenerateWireCube()
	{
		Mesh::Ptr newCube = Mesh::Create(GL_LINES);

		VerticesAttrib::List verts;

		verts.push_back(Vector3(-0.5, -0.5, -0.5));
		verts.push_back(Vector3(-0.5, -0.5, 0.5));

		verts.push_back(Vector3(-0.5, -0.5, 0.5));
		verts.push_back(Vector3( 0.5, -0.5, 0.5));

		verts.push_back(Vector3(-0.5, 0.5, -0.5));
		verts.push_back(Vector3(-0.5, 0.5, 0.5));

		verts.push_back(Vector3(-0.5, 0.5, 0.5));
		verts.push_back(Vector3( 0.5, 0.5, 0.5));

		verts.push_back(Vector3(0.5, -0.5, -0.5));
		verts.push_back(Vector3(0.5, -0.5, 0.5));

		verts.push_back(Vector3(-0.5, 0.5, -0.5));
		verts.push_back(Vector3( 0.5, 0.5, -0.5));

		verts.push_back(Vector3(0.5, 0.5, -0.5));
		verts.push_back(Vector3(0.5, 0.5, 0.5));

		verts.push_back(Vector3(-0.5, -0.5, -0.5));
		verts.push_back(Vector3(0.5,  -0.5, -0.5));

		// Corner Verticals
		verts.push_back(Vector3(-0.5, -0.5, -0.5));
		verts.push_back(Vector3(-0.5,  0.5, -0.5));

		verts.push_back(Vector3(0.5, -0.5, -0.5));
		verts.push_back(Vector3(0.5,  0.5, -0.5));

		verts.push_back(Vector3(0.5, -0.5, 0.5));
		verts.push_back(Vector3(0.5, 0.5,  0.5));

		verts.push_back(Vector3(-0.5, -0.5, 0.5));
		verts.push_back(Vector3(-0.5,  0.5, 0.5));

		newCube->VertexData()
			->SetVertices(verts);

		return newCube;
	}

	Mesh::Ptr MeshFactory::GenerateSphere(int slices, int stacks)
	{
		VerticesAttrib::List verts;
		NormalAttrib::List norms;
		ColourAttrib::List colours;

		TexCoordAttrib::List texCoords;
		VertexIndicesBuffer::List faces;

		// This is used purely for normal generation as it requires random access
		VecVec raverts;

		Mesh::Ptr newSphere = Mesh::Create();

        newSphere->SetMeshType("SPHERE");
        newSphere->SetMeshParameters(str(format("slices=%d|stacks=%d") % slices % stacks));

		int vi, next;
		float x, y, z, u, v, theta, phi, a;

		// Calculate Verts and Tex coords
		for ( int i = 0; i < (slices + 1); i++ )
		{
			theta = i * PI / stacks;
			for ( int j = 0; j < stacks; j++ )
			{
				phi = j * 2.0f * PI / stacks;

				// vertices
				x = std::sin(theta) * std::cos(phi);
				y = std::cos(theta);
				z = -std::sin(theta) * std::sin(phi);
				verts.push_back( Vector3(x, y, z) );

                norms.push_back( Vector3(x, y, z) );

				colours.push_back( Vector4(1.0f) );

				raverts.push_back( Vector3(x, y, z) );
				// Tex Coord
				v = std::acos(z) / PI;

                a = x / (std::sin(PI*(v)));

                if (y >= 0)
                {
					u = std::acos( a ) / TWOPI;
                }
				else
                {
					u = (PI + std::acos( a ) ) / TWOPI;
                }
				texCoords.push_back( Vector2(u, v) );
			}
		}

		// Calculate Faces
		vi = 0;
		for ( int i = 0; i < slices; i++ )
		{
			for ( int j = 0; j < stacks; j++ )
			{
				next = (j + 1) % stacks;

				faces.push_back( vi + j + stacks);
				faces.push_back( vi + next);
				faces.push_back( vi + j );

				faces.push_back( vi + j + stacks);
				faces.push_back( vi + next + stacks);
				faces.push_back( vi + next);
			}
			vi += stacks;
		}

		//norms = MeshFactory::GenerateNormals(raverts, faces);

		//newSphere->SetMeshData(verts, norms, texCoords, faces);
		newSphere->VertexData()
				 ->SetVertices(verts)
				 ->SetNormals(norms)
				 ->SetColours(colours)
				 ->SetTexCoords(texCoords)
				 ->SetIndices(faces);
				 //->BuildBuffers();

		return newSphere;
	}

	Mesh::Ptr MeshFactory::GenerateWireSphere()
	{
		Mesh::Ptr newSphere = Mesh::Create(GL_LINES);
		VerticesAttrib::List verts;

		ParametricData data;
		int slices = 8;
		int stacks = 8;
		float theta, phi;

		Vector3 last;
		Vector3 now;
		Vector3 first;
		// Generate Vertices

		for (int x = 0; x < (slices + 1); x++)
		{
			theta = x * PI / slices;
			for (int y = 0; y < stacks; y++)
			{
				phi = y * TWOPI / stacks;
				float px = sin(theta) * cos(phi);
				float py = cos(theta);
				float pz = -sin(theta) * sin(phi);

				now = Vector3(px, py, pz);

				if (y == 0)
				{
					first = now;
				}
				else if ( y % 2 == 0)
				{
					verts.push_back(last);
					verts.push_back(now);
				}
				else if (y == (stacks - 1))
				{
					verts.push_back(now);
					verts.push_back(first);
				}
				verts.push_back(now);

				last = now;
			}
		}

		newSphere->VertexData()
				 ->SetVertices(verts);

		return newSphere;
	}

	Mesh::Ptr MeshFactory::GeneratePlane(int widthSegments, int heightSegments)
	{
		Mesh::Ptr newPlane = Mesh::Create();

		newPlane->SetMeshType("PLANE");
		newPlane->SetMeshParameters(str(format("width_segs=%d|height_segs=%d") % widthSegments % heightSegments));

		ParametricData planeData = Parametric::Plane(heightSegments, widthSegments);

		newPlane->VertexData()
				->SetVertices(planeData.vertices)
				->SetNormals(planeData.normals)
				->SetColours(planeData.colours)
				->SetTexCoords(planeData.texCoords)
				->SetIndices(planeData.indices);
				//->BuildBuffers();

		return newPlane;
	}

	Mesh::Ptr MeshFactory::GenerateTriangle()
	{
		// Create a triangle
		VerticesAttrib::List verts;
		NormalAttrib::List norms;
		ColourAttrib::List colours;
		TexCoordAttrib::List tc;
		VertexIndicesBuffer::List inds;

		Mesh::Ptr newTriangle = Mesh::Create();

		verts.push_back(Vector3(-0.5,-0.5,0));
		verts.push_back(Vector3(0,0.5,0));
		verts.push_back(Vector3(0.5,-0.5,0));

		norms.push_back(Vector3(0,0,-1));
		norms.push_back(Vector3(0,0,-1));
		norms.push_back(Vector3(0,0,-1));

		colours.push_back(Vector4(1,0,0,1));
		colours.push_back(Vector4(0,1,0,1));
		colours.push_back(Vector4(0,0,1,1));

		tc.push_back(Vector2(0,0));
		tc.push_back(Vector2(0,0));
		tc.push_back(Vector2(0,0));

		inds.push_back(0);
		inds.push_back(1);
		inds.push_back(2);

		//newTriangle->SetMeshData(verts, norms, colours, tc, inds);
		newTriangle->VertexData()
				   ->SetVertices(verts)
				   ->SetNormals(norms)
				   ->SetTexCoords(tc)
				   ->SetIndices(inds);
				   //->BuildBuffers();

		return newTriangle;
	}

	Mesh::Ptr MeshFactory::GenerateIcoHedron()
	{
		return Mesh::Create();
	}

	Mesh::Ptr MeshFactory::GenerateOctohedron()
	{
		return Mesh::Create();
	}

	NormalAttrib::List MeshFactory::GenerateNormals(VerticesAttrib::List verts, VertexIndicesBuffer::List indices)
	{
		NormalAttrib::List norms;// = VecVec(verts.size());
		std::vector<VecVec> vertNorms = std::vector<VecVec>(verts.size());
		VerticesAttrib::List faceNormals;
		Vector3 faceNormal, v0, v1, v2, a, b;
		int vec0, vec1, vec2;
		bool found;

		// Generate face normals
		for (VertexIndicesBuffer::List::iterator ind = indices.begin(); ind != indices.end(); ind++ )
		{
			vec0 = *ind++;
			vec1 = *ind++;
			vec2 = *ind++;

			v0 = verts[vec0];
			v1 = verts[vec1];
			v2 = verts[vec2];

			a = v0 - v1;
			b = v2 - v1;
			b.Cross(a).Normalise();

			// add face normal to the vec positions list in vec0, vec1, vec2
			found = false;
			for (VecVec::iterator vnIt = vertNorms[vec0].begin(); vnIt != vertNorms[vec0].end(); vnIt++ )
			{
				if ( *vnIt == b )
				{
					found = true;
					break;
				}
			}

			if ( !found )
			{
				vertNorms[vec0].push_back(b);
			}

			found = false;
			for (VecVec::iterator vnIt = vertNorms[vec1].begin(); vnIt != vertNorms[vec1].end(); vnIt++ )
			{
				if ( *vnIt == b )
				{
					found = true;
					break;
				}
			}

			if ( !found )
			{
				vertNorms[vec1].push_back(b);
			}

			found = false;
			for (VecVec::iterator vnIt = vertNorms[vec2].begin(); vnIt != vertNorms[vec2].end(); vnIt++ )
			{
				if ( *vnIt == b )
				{
					found = true;
					break;
				}
			}

			if ( !found )
			{
				vertNorms[vec2].push_back(b);
			}
		}

		// Calculate the average for all of the face normals for each of the vectors
		for ( std::vector<VecVec>::iterator vnIt = vertNorms.begin(); vnIt != vertNorms.end(); vnIt++ )
		{
			faceNormal = Vector3();

			for (VecVec::iterator vnFnIt = (*vnIt).begin(); vnFnIt != (*vnIt).end(); vnFnIt++ )
			{
				faceNormal += *vnFnIt;
			}
			faceNormal.Normalise();
			norms.push_back(faceNormal);
		}

		return norms;

		 /*normals = list( MeshUtilities.face_normal(vertices, face) for face in faces )

            # Normal Generation

            # Calculate the normals once for each unique vertex and after calcs done,
            # rebuild the vertex list
            vertex_normals = []

            # for each vertex
            for v_inc in range(len(vertices)):
                face_normals = []
                vert_norm = Vector3()

                # for each face
                for f_inc in range(len(faces)):

                    # if the face contains the current vertex
                    if v_inc in faces[f_inc]:
                        # and the faces normal isn't already stored i.e. ignore co-planar face normals
                        if normals[f_inc] not in face_normals:
                            # Store it
                            face_normals.append(normals[f_inc])

                # Calculate the average for all of the face normals found

                for f_norm in face_normals:
                    vert_norm += f_norm
                vert_norm.normalize()

                vertex_normals.append(vert_norm)

            for vi in range(len(self._vert_index)):
                v_ind = self._vert_index[vi]
                self._normals.append(vertex_normals[v_ind])*/
	}

}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/Real_timer.h>

#include <CGAL/compute_average_spacing.h>
#include <CGAL/grid_simplify_point_set.h>
#include <CGAL/jet_smooth_point_set.h>

#include <boost/lexical_cast.hpp>

#include <vector>
#include <fstream>

// Types
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::FT FT;
typedef Kernel::Point_3 Point;
typedef CGAL::Random_points_on_sphere_3<Point> Generator;

// Concurrency
#ifdef CGAL_LINKED_WITH_TBB
typedef CGAL::Parallel_tag Concurrency_tag;
#else
typedef CGAL::Sequential_tag Concurrency_tag;
#endif

// instance of std::function<bool(double)>
struct Progress_to_std_cerr_callback
{
  mutable std::size_t nb;
  CGAL::Real_timer timer;
  double t_start;
  mutable double t_latest;
  const std::string name;

  Progress_to_std_cerr_callback (const char* name)
    : name (name)
  {
    timer.start();
    t_start = timer.time();
    t_latest = t_start;
  }

  bool operator()(double advancement) const
  {
    // Avoid calling time() at every single iteration, which could
    // impact performances very badly
    ++ nb;
    if (advancement != 1 && nb % 100 != 0)
      return true;

    double t = timer.time();
    if (advancement == 1 || (t - t_latest) > 0.1) // Update every 1/10th of second
    {
      std::cerr << "\r" // Return at the beginning of same line and overwrite
                << name << ": " << int(advancement * 100) << "%";

      if (advancement == 1)
        std::cerr << std::endl;
      t_latest = t;
    }

    return true;
  }
};

int main (int argc, char* argv[])
{
  int N = (argc > 1) ? boost::lexical_cast<int>(argv[1]) : 1000;

  // Generate N points on a sphere of radius 100.
  std::vector<Point> points;
  points.reserve (N);
  Generator generator(100.);
  std::copy_n (generator, N, std::back_inserter(points));

  // Compute average spacing
  FT average_spacing = CGAL::compute_average_spacing<Concurrency_tag>
    (points, 6,
     CGAL::parameters::callback
     (Progress_to_std_cerr_callback("Computing average spacing")));

  // Simplify on a grid with a size of twice the average spacing
  points.erase(CGAL::grid_simplify_point_set
               (points, 2. * average_spacing,
                CGAL::parameters::callback
                (Progress_to_std_cerr_callback("Grid simplification"))),
               points.end());

  // Smooth simplified point set
  CGAL::jet_smooth_point_set<Concurrency_tag>
    (points, 6,
     CGAL::parameters::callback
     (Progress_to_std_cerr_callback("Jet smoothing")));

  return EXIT_SUCCESS;
}


#include "penrose.h"
#include <Eigen/Dense>
#include "../../external_libs/quickhull/QuickHull.hpp"

namespace quacry{
using Vec2 = glm::vec2;
using Vec3 = glm::vec3;
using Vec4 = glm::vec4;
using Mat4 = glm::mat4;
using Vec5f = Eigen::Matrix<float,5,1>;
using Mat5f = Eigen::Matrix<float,5,5>;
using namespace kipod::MeshModels;

auto PenroseRotation() -> Mat5f
{
    Mat5f g;
    double c = std::sqrt(2./5);
    double t = 2./5 * 3.1415926535;
    double u = 1./std::sqrt(2.);
    auto si = [=](int i){ return (float)sin(i*t); };
    auto co = [=](int i){ return (float)cos(i*t); };
    for(int j = 0; j<5; ++j)
        g.col(j) = c*Vec5f(co(j), si(j), co(2*j), si(2*j), u);

    return g;
}

auto PenroseInternalPolytope(Mat5f g, const Vec5f& gamma) -> std::pair<std::vector<Vec3>,std::vector<unsigned int>>
{
    LOG_INFO("Creating Internal Penrose Polytope from data g={} and gamma={}", g,gamma);

    using namespace quickhull;
    QuickHull<float> qh;
    auto vertices = std::vector<Vec3>();
    auto g_penrose = PenroseRotation();
    g = g*g_penrose;
    auto unit_cube = std::vector<Vec5f>();
    auto s = [](int i, int j){ return (i & (1 << j)) == 0 ? -0.5f : 0.5f; };
    LOG_INFO("Creating 5-dim Unitcube:");
    for(int i = 0; i<32; ++i){
       Vec5f v = { s(i,0), s(i,1),s(i,2),s(i,3),s(i,4)};
       unit_cube.push_back(v);
       LOG_INFO("{}th vector = {}", i, v.transpose());
    }
    for(auto& v : unit_cube) v = g*(v + gamma);
    for(const auto& v : unit_cube) vertices.emplace_back(v[2],v[3],v[4]);
    auto hull = qh.getConvexHull((float*)vertices.data(), vertices.size(), true, false);
    auto hull_vertices = std::vector<glm::vec3>();
    auto indices = std::vector<unsigned int>();
    for(auto& v : hull.getVertexBuffer()) hull_vertices.emplace_back(v.x,v.y,v.z);
    for(auto& v : hull.getIndexBuffer()) indices.push_back(v);
    return std::make_pair(hull_vertices, indices);
}

auto Penrose() -> Quasicrystal23
{
    auto [v,i] = PenroseInternalPolytope(Mat5f::Identity(), Vec5f::Constant(0.25f));
    auto penrose = Quasicrystal23("Penrose", PenroseRotation(), MeshModel(v,i), {-2,2, -2,2, -2,2, -2,2, -2,2});

    return penrose;
}
}


#include "entropy.hpp"
#include <armadillo>
#include <stdlib.h>
using namespace arma;
using namespace std;

// von Neumann entropy of the first k qubits
double vNentropy(unsigned int k, cx_dvec &psi)
{
  int d = psi.size();
  cx_dmat sqrtrho = reshape(psi, 1<<k, d/(1<<k));
  vec s = svd(sqrtrho);
  psi.reshape(size(psi));
  double sum = 0.0;
  for (int i=0; i<(1<<k); i++)
    {
      double ent_sq = norm(s(i));
      sum -= log2(ent_sq) * ent_sq;
    }
  return sum;
}

// Diagonal entropy of the first k qubits
double diagentropy(unsigned int k, cx_dvec &psi)
{
  int d = psi.size();
  double sum = 0.0;
  for (int i=0; i<(1<<k); i++)
    {
      double ent_sq = 0.0;
      for (int j=0; j<(d/(1<<k)); j++)
	{
	  ent_sq += norm(psi(i + (j<<k)));
	}
      sum -= log2(ent_sq) * ent_sq;
    }
  return sum;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/FluidEigenMappings.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <Eigen/SVD>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class PCA
{
public:
  using MatrixXd = Eigen::MatrixXd;
  using VectorXd = Eigen::VectorXd;
  using ArrayXd = Eigen::ArrayXd;

  void init(RealMatrixView in)
  {
    using namespace Eigen;
    using namespace _impl;
    MatrixXd input = asEigen<Matrix>(in);
    mNumDataPoints = input.rows();
    mMean = input.colwise().mean();
    MatrixXd         X = (input.rowwise() - mMean.transpose());
    BDCSVD<MatrixXd> svd(X.matrix(), ComputeThinV | ComputeThinU);
    mBases = svd.matrixV();
    mValues = svd.singularValues();
    mExplainedVariance = mValues.array().square() / (mNumDataPoints - 1);
    mInitialized = true;
  }

  void init(RealMatrixView bases, RealVectorView values, RealVectorView mean,
            index numDataPoints = 2)
  {
    mBases = _impl::asEigen<Eigen::Matrix>(bases);
    mValues = _impl::asEigen<Eigen::Matrix>(values);
    mMean = _impl::asEigen<Eigen::Matrix>(mean);
    mNumDataPoints = numDataPoints;
    mExplainedVariance = mValues.array().square() / (mNumDataPoints - 1);
    mInitialized = true;
  }

  void processFrame(const RealVectorView in, RealVectorView out, index k,
                    bool whiten = false) const
  {
    using namespace Eigen;
    using namespace _impl;
    if (k > mBases.cols()) return;
    VectorXd input = asEigen<Matrix>(in);
    input = input - mMean;
    VectorXd result = input.transpose() * mBases.block(0, 0, mBases.rows(), k);

    if (whiten)
    {
      ArrayXd norm = mExplainedVariance.segment(0, k).max(epsilon).rsqrt();
      result.array() *= norm;
    }
     out <<= _impl::asFluid(result);
  }

  void inverseProcessFrame(RealVectorView in, RealVectorView out, bool whiten = false) const
  {
    using namespace Eigen;
    using namespace _impl;

    if(!whiten)
    {
      asEigen<Matrix>(out) =
          mMean +
          (asEigen<Matrix>(in).transpose() * mBases.transpose()).transpose();
    }
    else
    {
      asEigen<Matrix>(out) = mMean +  (asEigen<Matrix>(in).transpose() *
            (mExplainedVariance.sqrt().matrix().asDiagonal() * mBases.transpose())).transpose();
    }
  }

  double process(const RealMatrixView in, RealMatrixView out, index k,
                 bool whiten = false) const
  {
    using namespace Eigen;
    using namespace _impl;

    if (k > mBases.cols()) return 0;
    MatrixXd input = asEigen<Matrix>(in);
    MatrixXd result = (input.rowwise() - mMean.transpose()) *
                      mBases.block(0, 0, mBases.rows(), k);
    if (whiten)
    {
      ArrayXd norm = mExplainedVariance.segment(0, k).max(epsilon).rsqrt();
      result = result.array().rowwise() * norm.transpose().max(epsilon);
    }
    double variance = 0;

    double total = mExplainedVariance.sum();
    for (index i = 0; i < k; i++) variance += mExplainedVariance[i];
    out <<= _impl::asFluid(result);

    return variance / total;
  }

  void inverseProcess(RealMatrixView in, RealMatrixView out, bool whiten = false) const
  {
    using namespace Eigen;

    if (in.cols() > dims()) return;
    if (out.cols() < in.cols()) return;

    if (!whiten)
      _impl::asEigen<Matrix>(out) =
          (_impl::asEigen<Matrix>(in) * mBases.transpose()).rowwise() +
          mMean.transpose();

    else
    {
      _impl::asEigen<Matrix>(out) =
          (_impl::asEigen<Matrix>(in) *
           (mExplainedVariance.sqrt().matrix().asDiagonal() *
            mBases.transpose()))
              .rowwise() +
          mMean.transpose();
    }
  }

  bool  initialized() const { return mInitialized; }

  void  getBases(RealMatrixView out) const { out <<= _impl::asFluid(mBases); }
  void  getValues(RealVectorView out) const { out <<= _impl::asFluid(mValues); }
  void  getMean(RealVectorView out) const { out <<= _impl::asFluid(mMean); }
  index getNumDataPoints() const { return mNumDataPoints; }

  index dims() const { return mBases.rows(); }
  index size() const { return mBases.cols(); }
  void  clear()
  {
    mBases.setZero();
    mMean.setZero();
    mInitialized = false;
  }

  MatrixXd mBases;
  VectorXd mValues;
  ArrayXd  mExplainedVariance;
  VectorXd mMean;
  index    mNumDataPoints;
  bool     mInitialized{false};
};
}// namespace algorithm
}// namespace fluid


#include "stats/asphericity.h"
#include <Eigen/Core>
#include <Eigen/SVD>
#include <xmd/utils/convert.h>

namespace xmd {
    void compute_asphericity::operator()() const {
        vec3r center_of_mass = vec3r::Zero();
        real total_mass = 0.0;
        for (int idx = 0; idx < num_particles; ++idx) {
            center_of_mass += mass[idx] * r[idx];
            total_mass += mass[idx];
        }
        center_of_mass /= total_mass;

        using matrix_t = Eigen::Matrix<real, 3, Eigen::Dynamic>;
        matrix_t R = matrix_t::Zero(3, num_particles);
        for (int idx = 0; idx < num_particles; ++idx) {
            R.col(idx) = convert(r[idx] - center_of_mass);
        }

        auto lambda = Eigen::JacobiSVD<decltype(R)>(R).singularValues();
        *asphericity = (real)1.5 * pow(lambda.z(), 2.0)
            - (real)0.5 * lambda.squaredNorm();
    }
}


//test for PS12 problem 1
#include <Eigen/Dense>
#include <iostream>

int main(){

    unsigned int s = 3;
	Eigen::VectorXd c_(s);
    Eigen::MatrixXd A_(s,s);
    Eigen::VectorXd b_(s);
    A_ << 0,      0,      0,
         1./3.,  0,      0,
         0,      2./3.,  0;
    b_ << 1./4.,  0,      3./4.;

    short n=A_.rows();
	assert(n==b_.size());
	c_.resize(n);
	std::cout << n;
	for (int i=0; i<n;i++){
		c_(i)=0;
		for (int j=0;j<i+1;j++){
				c_(i)+=A_(i,j);
			}
		}
	std::cout << A_ << std::endl;
	std::cout << c_ << std::endl;

}

Nummerical Methods for CSE/PS12/test.cpp


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2015 Andres Hernandez

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

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 license for more details.
*/

/*! \file particleswarmoptimization.hpp
\brief Implementation based on:
Clerc, M., Kennedy, J. (2002) The particle swarm-explosion, stability and
convergence in a multidimensional complex space. IEEE Transactions on Evolutionary
Computation, 6(2): 58–73.
*/

#ifndef quantlib_optimization_particleswarmoptimization_hpp
#define quantlib_optimization_particleswarmoptimization_hpp

#include <ql/qldefines.hpp>

#if BOOST_VERSION >= 104700

#include <ql/math/optimization/problem.hpp>
#include <ql/math/optimization/constraint.hpp>
#include <ql/math/randomnumbers/mt19937uniformrng.hpp>
#include <ql/experimental/math/isotropicrandomwalk.hpp>
#include <ql/experimental/math/levyflightdistribution.hpp>

#include <boost/random/mersenne_twister.hpp>
typedef boost::mt19937 base_generator_type;

#include <boost/random/uniform_int_distribution.hpp>
typedef boost::random::uniform_int_distribution<QuantLib::Size> uniform_integer;

namespace QuantLib {

    /*! The process is as follows:
    M individuals are used to explore the N-dimensional parameter space
    X_{i}^k = (X_{i, 1}^k, X_{i, 2}^k, \ldots, X_{i, N}^k) is the kth-iteration for the ith-individual
    X is updated via the rule
    X_{i, j}^{k+1} = X_{i, j}^k + V_{i, j}^{k+1}
    with V being the "velocity" that updates the position:
    V_{i, j}^{k+1} = \chi\left(V_{i, j}^k + c_1 r_{i, j}^k (P_{i, j}^k - X_{i, j}^k)
    + c_2 R_{i, j}^k (G_{i, j}^k - X_{i, j}^k)\right)
    where c are constants, r and R are uniformly distributed random numbers in the range [0, 1], and
    P_{i, j} is the personal best parameter set for individual i up to iteration k
    G_{i, j} is the global best parameter set for the swarm up to iteration k.
    c_1 is the self recognition coefficient
    c_2 is the social recognition coefficient

    This version is known as the PSO with constriction factor (PSO-Co).
    PSO with inertia factor (PSO-In) updates the velocity according to:
    V_{i, j}^{k+1} = \omega V_{i, j}^k + \hat{c}_1 r_{i, j}^k (P_{i, j}^k - X_{i, j}^k)
    + \hat{c}_2 R_{i, j}^k (G_{i, j}^k - X_{i, j}^k)
    and is accessible from PSO-Co by setting \omega = \chi, and \hat{c}_{1,2} = \chi c_{1,2}

    These two versions of PSO are normally referred to as canonical PSO.

    Convergence of PSO-Co is improved if \chi is chosen as
    \chi = \frac{2}{\vert 2-\phi-\sqrt{\phi^2 - 4\phi}\vert}, with \phi = c_1 + c_2
    Stable convergence is achieved if \phi >= 4. Clerc and Kennedy recommend
    c_1 = c_2 = 2.05 and \phi = 4.1

    Different topologies can be chosen for G, e.g. instead of it being the best
    of the swarm, it is the best of the nearest neighbours, or some other form.

    In the canonical PSO, the inertia function is trivial. It is simply a
    constant (the inertia) multiplying the previous iteration's velocity. The
    value of the inertia constant determines the weight of a global search over
    local search. Like in the case of the topology, other possibilities for the
    inertia function are also possible, e.g. a function that interpolates between a
    high inertia at the beginning of the optimization (hence prioritizing a global
    search) and a low inertia towards the end of the optimization (hence prioritizing
    a local search).

    The optimization stops either because the number of iterations has been reached
    or because the stationary function value limit has been reached.
    */
    class ParticleSwarmOptimization : public OptimizationMethod {
      public:
        class Inertia;
        class Topology;
        friend class Inertia;
        friend class Topology;
        ParticleSwarmOptimization(Size M,
            boost::shared_ptr<Topology> topology,
            boost::shared_ptr<Inertia> inertia,
            Real c1 = 2.05, Real c2 = 2.05,
            unsigned long seed = 0);
        ParticleSwarmOptimization(const Size M,
            boost::shared_ptr<Topology> topology,
            boost::shared_ptr<Inertia> inertia,
            Real omega, Real c1, Real c2,
            unsigned long seed = 0);
        void startState(Problem &P, const EndCriteria &endCriteria);
        EndCriteria::Type minimize(Problem &P, const EndCriteria &endCriteria);

      protected:
        std::vector<Array> X_, V_, pBX_, gBX_;
        Array pBF_, gBF_;
        Array lX_, uX_;
        Size M_, N_;
        Real c0_, c1_, c2_;
        MersenneTwisterUniformRng rng_;
        boost::shared_ptr<Topology> topology_;
        boost::shared_ptr<Inertia> inertia_;
    };

    //! Base inertia class used to alter the PSO state
    /*! This pure virtual base class provides the access to the PSO state
    which the particular inertia algorithm will change upon each iteration.
    */
    class ParticleSwarmOptimization::Inertia {
        friend class ParticleSwarmOptimization;
      public:
        virtual ~Inertia() {}
        //! initialize state for current problem
        virtual void setSize(Size M, Size N, Real c0, const EndCriteria &endCriteria) = 0;
        //! produce changes to PSO state for current iteration
        virtual void setValues() = 0;
      protected:
        ParticleSwarmOptimization *pso_;
        std::vector<Array> *X_, *V_, *pBX_, *gBX_;
        Array *pBF_, *gBF_;
        Array *lX_, *uX_;
      private:
        void init(ParticleSwarmOptimization *pso) {
            pso_ = pso;
            X_ = &pso_->X_;
            V_ = &pso_->V_;
            pBX_ = &pso_->pBX_;
            gBX_ = &pso_->gBX_;
            pBF_ = &pso_->pBF_;
            gBF_ = &pso_->gBF_;
            lX_ = &pso_->lX_;
            uX_ = &pso_->uX_;
        }
    };

    //! Trivial Inertia
    /*     Inertia is a static value
    */
    class TrivialInertia : public ParticleSwarmOptimization::Inertia {
      public:
        inline void setSize(Size M, Size N, Real c0, const EndCriteria &endCriteria) {
            c0_ = c0;
            M_ = M;
        }
        inline void setValues() {
            for (Size i = 0; i < M_; i++) {
                (*V_)[i] *= c0_;
            }
        }
      private:
        Real c0_;
        Size M_;
    };

    //! Simple Random Inertia
    /*     Inertia value gets multiplied with a random number
    between (threshhold, 1)
    */
    class SimpleRandomInertia : public ParticleSwarmOptimization::Inertia {
      public:
        SimpleRandomInertia(Real threshhold = 0.5, unsigned long seed = 0)
            : threshhold_(threshhold), rng_(seed) {
            QL_REQUIRE(threshhold_ >= 0.0 && threshhold_ < 1.0, "Threshhold must be a Real in [0, 1)");
        }
        inline void setSize(Size M, Size N, Real c0, const EndCriteria &endCriteria) {
            M_ = M;
            c0_ = c0;
        }
        inline void setValues() {
            for (Size i = 0; i < M_; i++) {
                Real val = c0_*(threshhold_ + (1.0 - threshhold_)*rng_.nextReal());
                (*V_)[i] *= val;
            }
        }
      private:
        Real c0_, threshhold_;
        Size M_;
        MersenneTwisterUniformRng rng_;
    };

    //! Decreasing Inertia
    /*     Inertia value gets decreased every iteration until it reaches
    a value of threshhold when iteration reaches the maximum level
    */
    class DecreasingInertia : public ParticleSwarmOptimization::Inertia {
      public:
        DecreasingInertia(Real threshhold = 0.5)
            : threshhold_(threshhold) {
            QL_REQUIRE(threshhold_ >= 0.0 && threshhold_ < 1.0, "Threshhold must be a Real in [0, 1)");
        }
        inline void setSize(Size M, Size N, Real c0, const EndCriteria &endCriteria) {
            N_ = N;
            c0_ = c0;
            iteration_ = 0;
            maxIterations_ = endCriteria.maxIterations();
        }
        inline void setValues() {
            Real c0 = c0_*(threshhold_ + (1.0 - threshhold_)*(maxIterations_ - iteration_) / maxIterations_);
            for (Size i = 0; i < M_; i++) {
                (*V_)[i] *= c0;
            }
        }
      private:
        Real c0_, threshhold_;
        Size M_, N_, maxIterations_, iteration_;
    };

    //! AdaptiveInertia
    /*    Alen Lukic, Approximating Kinetic Parameters Using Particle
    Swarm Optimization.
    */
    class AdaptiveInertia : public ParticleSwarmOptimization::Inertia {
      public:
        AdaptiveInertia(Real minInertia, Real maxInertia, Size sh = 5, Size sl = 2)
            :minInertia_(minInertia), maxInertia_(maxInertia),
            sh_(sh), sl_(sl) {};
        inline void setSize(Size M, Size N, Real c0, const EndCriteria &endCriteria) {
            M_ = M;
            c0_ = c0;
            adaptiveCounter = 0;
            best_ = QL_MAX_REAL;
            started_ = false;
        }
        void setValues();
      private:
        Real c0_, best_;
        Real minInertia_, maxInertia_;
        Size M_;
        Size sh_, sl_;
        Size adaptiveCounter;
        bool started_;
    };

    //! Levy Flight Inertia
    /*    As long as the particle keeps getting frequent updates to its
    personal best value, the inertia behaves like a SimpleRandomInertia,
    but after a number of iterations without improvement, the behaviour
    changes to that of a Levy flight ~ u^{-1/\alpha}
    */
    class LevyFlightInertia : public ParticleSwarmOptimization::Inertia {
      public:
        typedef IsotropicRandomWalk<LevyFlightDistribution, base_generator_type> IsotropicLevyFlight;
        LevyFlightInertia(Real alpha, Size threshhold,
                          unsigned long seed = 0)
            :flight_(base_generator_type(seed), LevyFlightDistribution(1.0, alpha),
                1, Array(1, 1.0), seed),
            threshhold_(threshhold) {};
        inline void setSize(Size M, Size N, Real c0, const EndCriteria &endCriteria) {
            M_ = M;
            N_ = N;
            c0_ = c0;
            personalBestF_ = *pBF_;
            adaptiveCounter_ = std::vector<Size>(M_, 0);
            flight_.setDimension(N_, *lX_, *uX_);
        }
        inline void setValues() {
            for (Size i = 0; i < M_; i++) {
                if ((*pBF_)[i] < personalBestF_[i]) {
                    personalBestF_[i] = (*pBF_)[i];
                    adaptiveCounter_[i] = 0;
                }
                else {
                    adaptiveCounter_[i]++;
                }
                if (adaptiveCounter_[i] <= threshhold_) {
                    //Simple Random Inertia
                    (*V_)[i] *= c0_*(0.5 + 0.5*rng_.nextReal());
                }
                else {
                    //If particle has not found a new personal best after threshhold_ iterations
                    //then trigger a Levy flight pattern for the speed
                    flight_.nextReal<Real *>(&(*V_)[i][0]);
                }
            }
        }
      private:
        MersenneTwisterUniformRng rng_;
        IsotropicLevyFlight flight_;
        Array personalBestF_;
        std::vector<Size> adaptiveCounter_;
        Real c0_;
        Size M_, N_;
        Size threshhold_;
    };

    //! Base topology class used to determine the personal and global best
    /*! This pure virtual base class provides the access to the PSO state
    which the particular topology algorithm will change upon each iteration.
    */
    class ParticleSwarmOptimization::Topology {
        friend class ParticleSwarmOptimization;
      public:
        virtual ~Topology() {}
        //! initialize state for current problem
        virtual void setSize(Size M) = 0;
        //! produce changes to PSO state for current iteration
        virtual void findSocialBest() = 0;
      protected:
        ParticleSwarmOptimization *pso_;
        std::vector<Array> *X_, *V_, *pBX_, *gBX_;
        Array *pBF_, *gBF_;
      private:
        void init(ParticleSwarmOptimization *pso) {
            pso_ = pso;
            X_ = &pso_->X_;
            V_ = &pso_->V_;
            pBX_ = &pso_->pBX_;
            gBX_ = &pso_->gBX_;
            pBF_ = &pso_->pBF_;
            gBF_ = &pso_->gBF_;
        }
    };

    //! Global Topology
    /*  The global best as seen by each particle is the best from amongst
    all particles
    */
    class GlobalTopology : public ParticleSwarmOptimization::Topology {
      public:
        inline void setSize(Size M) { M_ = M; }
        inline void findSocialBest() {
            Real bestF = (*pBF_)[0];
            Size bestP = 0;
            for (Size i = 1; i < M_; i++) {
                if (bestF < (*pBF_)[i]) {
                    bestF = (*pBF_)[i];
                    bestP = i;
                }
            }
            Array& x = (*pBX_)[bestP];
            for (Size i = 0; i < M_; i++) {
                if (i != bestP) {
                    (*gBX_)[i] = x;
                    (*gBF_)[i] = bestF;
                }
            }
        }

      private:
        Size M_;
    };

    //! K-Neighbor Topology
    /*  The global best as seen by each particle is the best from amongst
    the previous K and next K neighbors. For particle I, the best is
    then taken from amongst the [I - K, I + K] particles.
    */
    class KNeighbors : public ParticleSwarmOptimization::Topology {
      public:
        KNeighbors(Size K = 1) :K_(K) {
            QL_REQUIRE(K > 0, "Neighbors need to be larger than 0");
        }
        inline void setSize(Size M) {
            M_ = M;
            if (M_ < 2 * K_ + 1)
                K_ = (M_ - 1) / 2;
        }
        void findSocialBest();

      private:
        Size K_, M_;
    };

    //! Clubs Topology
    /*  H.M. Emara,  Adaptive Clubs-based Particle Swarm Optimization
    Each particle is originally assigned to a default number of clubs
    from among the total set. The best as seen by each particle is the
    best from amongst the clubs to which the particle belongs.
    Underperforming particles join more clubs randomly (up to a maximum
    number) to widen the particles that influence them, while
    overperforming particles leave clubs randomly (down to a minimum
    number) to avoid early convergence to local minima.
    */
    class ClubsTopology : public ParticleSwarmOptimization::Topology {
      public:
        ClubsTopology(Size defaultClubs, Size totalClubs,
            Size maxClubs, Size minClubs,
            Size resetIteration, unsigned long seed = 0);
        void setSize(Size M);
        void findSocialBest();

      private:
        Size totalClubs_, maxClubs_, minClubs_, defaultClubs_;
        Size iteration_, resetIteration_;
        Size M_;
        std::vector<std::vector<bool> > clubs4particles_;
        std::vector<std::vector<bool> > particles4clubs_;
        std::vector<Size> bestByClub_;
        std::vector<Size> worstByClub_;
        base_generator_type generator_;
        uniform_integer distribution_;

        void leaveRandomClub(Size particle, Size currentClubs);
        void joinRandomClub(Size particle, Size currentClubs);
    };

}

#endif

#endif


//==================================================================================================
/*!
  @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_CONSTANT_GOLD_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_GOLD_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Gold Gold (function template)

  Generates the Golden Ratio \f$\phi

  @headerref{<boost/simd/constant/gold.hpp>}

  @par Description

  1.  @code
      template<typename T> T Gold();
      @endcode

  2.  @code
      template<typename T> T Gold( boost::simd::as_<T> const& target );
      @endcode

  Generates a constant that evaluate to the [Golden Ratio](http://mathworld.wolfram.com/GoldenRatio.html)
  defined as \f$\frac{1}{2}(1 + \sqrt{5})\f$.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `T(1.61803398874989484820458683436563811772)`.

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/gold.hpp>
#include <boost/simd/constant/simd/gold.hpp>

#endif


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/optimization/optimization_compute_index_sets.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

template <typename T>
class BoundFunction
{
public:

  typedef OpenTissue::math::ValueTraits<T> value_traits;

  bool m_is_lower;

public:

  BoundFunction(bool const & is_lower)
    : m_is_lower(is_lower)
  {}

  template<typename vector_type>
  T operator()(vector_type const & x, size_t const & i) const
  {
    return m_is_lower ?  -value_traits::one() : value_traits::one();
  }

};

BOOST_AUTO_TEST_SUITE(opentissue_math_big_compute_index_sets);

BOOST_AUTO_TEST_CASE(test_case)
{
  typedef double                    real_type;
  typedef ublas::vector<real_type>  vector_type;
  typedef ublas::vector<size_t>     idx_vector_type;

  vector_type x;
  vector_type y;

  x.resize(10,false);
  y.resize(10,false);

  BoundFunction<double> l(true);
  BoundFunction<double> u(false);

  x(0) =  0.0;   y(0) =  0.0;  // active
  x(1) = -1.0;   y(1) =  1.0;  // lower
  x(2) =  1.0;   y(2) = -1.0;  // upper
  x(3) = -2.0;   y(3) = -1.0;  // active
  x(4) =  2.0;   y(4) =  1.0;  // active
  x(5) =  3.0;   y(5) =  0.0;  // upper
  x(6) = -3.0;   y(6) =  0.0;  // lower
  x(7) =  0.5;   y(7) =  0.5;  // active
  x(8) = -0.5;   y(8) = -0.5;  // active
  x(9) =  0.25;   y(9) = 0.25; // active

  idx_vector_type bitmask;
  size_t cnt_active;
  size_t cnt_inactive;
  OpenTissue::math::optimization::compute_index_sets( y, x, l, u, bitmask, cnt_active, cnt_inactive );

  BOOST_CHECK( cnt_active == 6 );
  BOOST_CHECK( cnt_inactive == 4 );

  BOOST_CHECK( bitmask(0) == OpenTissue::math::optimization::IN_ACTIVE );
  BOOST_CHECK( bitmask(1) == OpenTissue::math::optimization::IN_LOWER );
  BOOST_CHECK( bitmask(2) == OpenTissue::math::optimization::IN_UPPER );
  BOOST_CHECK( bitmask(3) == OpenTissue::math::optimization::IN_ACTIVE );
  BOOST_CHECK( bitmask(4) == OpenTissue::math::optimization::IN_ACTIVE );
  BOOST_CHECK( bitmask(5) == OpenTissue::math::optimization::IN_UPPER );
  BOOST_CHECK( bitmask(6) == OpenTissue::math::optimization::IN_LOWER );
  BOOST_CHECK( bitmask(7) == OpenTissue::math::optimization::IN_ACTIVE );
  BOOST_CHECK( bitmask(8) == OpenTissue::math::optimization::IN_ACTIVE );
  BOOST_CHECK( bitmask(9) == OpenTissue::math::optimization::IN_ACTIVE );
}

BOOST_AUTO_TEST_SUITE_END();


// Author(s): Jan Friso Groote
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file linear_inequalities_test.cpp
/// \brief Test the linear_inequality functionality.

#define BOOST_TEST_MODULE linear_inequalities_test
#include "mcrl2/data/fourier_motzkin.h"
#include "mcrl2/data/join.h"
#include "mcrl2/data/linear_inequalities.h"
#include "mcrl2/data/parse.h"
#include <boost/test/included/unit_test_framework.hpp>

using namespace mcrl2;
using namespace mcrl2::core;
using namespace mcrl2::data;

void check(bool result, std::string message)
{
  if(!result) {
    std::cout << message << std::endl;
  }
  BOOST_CHECK(result);
}

void test_linear_inequality()
{
  data_specification data_spec;
  data_spec.add_context_sort(sort_real::real_());
  rewriter rewr(data_spec);

  variable vx("x", sort_real::real_());
  variable vy("y", sort_real::real_());
  linear_inequality li;
  data_expression expr;

  expr = less(real_zero(), real_zero());
  li = linear_inequality(expr, rewr);
  check(li.is_false(rewr), "Expected " + pp(expr) + "' to be false");
  check(li.lhs().empty(), "Expected left hand side of '" + pp(expr) + "' to be empty " + pp(li.lhs()));

  expr = less_equal(real_zero(), real_zero());
  li = linear_inequality(expr, rewr);
  check(li.is_true(rewr), "Expected '" + pp(expr) + "' to be true");
  check(li.lhs().empty(), "Expected left hand side of '" + pp(expr) + "' to be empty");

  expr = less_equal(sort_real::minus(vx, vx), real_one());
  li = linear_inequality(expr, rewr);
  check(li.is_true(rewr), "Expected '" + pp(expr) + "' to be true");
  check(li.lhs().empty(), "Expected left hand side of '" + pp(expr) + "' to be empty");

  expr = less_equal(sort_real::minus(vx, vx), real_one());
  li = linear_inequality(expr, rewr);
  check(li.is_true(rewr), "Expected '" + pp(expr) + "' to be true");
  check(li.lhs().empty(), "Expected left hand side of '" + pp(expr) + "' to be empty");

  expr = less_equal(sort_real::plus(vx, vy), vx);
  li = linear_inequality(expr, rewr);
  check(!li.is_true(rewr), "Expected '" + pp(expr) + "' to not be true");
  check(!li.lhs().empty(), "Expected left hand side of '" + pp(expr) + "' not to be empty");
  check(li.transform_to_data_expression() == less_equal(sort_real::times(real_one(), vy), real_zero()),
    "Expression '" + pp(expr) + "' parsing/output problem " + pp(li.transform_to_data_expression()));

  bool got_exception = false;
  try
  {
    expr = not_equal_to(vx, vy);
    li = linear_inequality(expr, rewr);
  }
  catch(const mcrl2::runtime_error&)
  {
    got_exception = true;
  }
  check(got_exception, "Expected an exception while parsing x != y.");
}

void split_conjunction_of_inequalities_set(const data_expression& e, std::vector < linear_inequality >& v, const rewriter& r)
{
  if (sort_bool::is_and_application(e))
  {
    split_conjunction_of_inequalities_set(application(e)[0],v,r);
    split_conjunction_of_inequalities_set(application(e)[1],v,r);
  }
  else
  {
    v.push_back(linear_inequality(e,r));
  }
}

bool test_consistency_of_inequalities(const std::string& vars,
                                      const std::string& inequalities,
                                      const bool expect_consistent)
{
  // Take care that reals are part of the data type.
  data_specification data_spec;
  variable_list variables=parse_variables(vars);
  data_spec.add_context_sort(sort_real::real_());
  const data_expression e=parse_data_expression(inequalities,variables,data_spec);

  rewriter r(data_spec);
  std::vector < linear_inequality > v_inequalities;
  split_conjunction_of_inequalities_set(e,v_inequalities,r);

  if (is_inconsistent(v_inequalities,r))
  {
    if (expect_consistent)
    {
      std::cout << "Expected consistent, found inconsistent\n";
      std::cout << variables << ": " << inequalities << "\n";
      std::cout << "Internal inequalities: " << pp_vector(v_inequalities) << "\n";
      return false;
    }
  }
  else
  {
    if (!expect_consistent)
    {
      std::cout << "Expected inconsistent, found consistent\n";
      std::cout << variables << ": " << inequalities << "\n";
      std::cout << "Internal inequalities: " << pp_vector(v_inequalities) << "\n";
      return false;
    }
  }
  return true;
}

bool test_application_of_Fourier_Motzkin(const std::string& vars,
                                         const std::string& variables_to_be_eliminated,
                                         const std::string& inequalities,
                                         const std::string& inconsistent_with,
                                         bool check_consistent = false)
{
  // Take care that reals are part of the data type.
  data_specification data_spec;
  data_spec.add_context_sort(sort_real::real_());
  const variable_list variables=parse_variables(vars);
  const data_expression e_in=parse_data_expression(inequalities,variables,data_spec);
  const variable_list v_elim= data::detail::parse_variables(variables_to_be_eliminated);

  rewriter r(data_spec);
  std::vector < linear_inequality > v_inequalities;
  split_conjunction_of_inequalities_set(e_in,v_inequalities,r);

  std::vector < linear_inequality> resulting_inequalities;
  fourier_motzkin(v_inequalities, v_elim.begin(), v_elim.end(), resulting_inequalities, r);

  std::vector < linear_inequality> inconsistent_inequalities=resulting_inequalities;
  inconsistent_inequalities.push_back(linear_inequality(parse_data_expression(inconsistent_with,variables,data_spec),r));
  if (!(check_consistent ^ is_inconsistent(inconsistent_inequalities,r, false)))
  {
    std::cout << "Expected set of inequations to be " << (check_consistent ? "" : "in") << "consistent with given inequality after applying Fourier-Motzkin elimination\n";
    std::cout << "Input: " << variables << ": " << inequalities << "\n";
    std::cout << "Parsed input : " << pp_vector(v_inequalities) << "\n";
    std::cout << "Variables to be eliminated: " << v_elim << "\n";
    std::cout << "Input after applying Fourier Motzkin: " << pp_vector(resulting_inequalities) << "\n";
    std::cout << "Should be " << (check_consistent ? "" : "in") << "consistent with " << inconsistent_with << "\n";
    std::cout << (check_consistent ? "Consistent" : "Inconsistent")  << " inequality after parsing " << pp(linear_inequality(parse_data_expression(inconsistent_with,variables,data_spec),r)) << "\n";
    return false;
  }
  return true;
}

void test_high_level_fourier_motzkin()
{
  data_specification data_spec;
  data_spec.add_context_sort(sort_real::real_());
  rewriter rewr(data_spec);
  variable vx("x", sort_real::real_());
  variable vy("y", sort_real::real_());

  data_expression expr = sort_bool::and_(equal_to(vx, vy), less(vy, sort_real::real_(2)));
  variable_list elim_vars({variable("y", sort_real::real_())});
  data_expression out;
  variable_list vars_out;
  fourier_motzkin(expr, elim_vars, out, vars_out, rewr);

  BOOST_CHECK(vars_out.empty());
  BOOST_CHECK(out == less(sort_real::times(real_one(), vx), sort_real::real_(2)));
}

void test_high_level_fourier_motzkin_non_linear()
{
  data_specification data_spec;
  data_spec.add_context_sort(sort_real::real_());
  rewriter rewr(data_spec);
  variable vx("x", sort_real::real_());

  data_expression expr = less(sort_real::times(vx,vx), sort_real::real_(0));
  variable_list elim_vars({vx});
  data_expression out;
  variable_list vars_out;
  fourier_motzkin(expr, elim_vars, out, vars_out, rewr);

  // Fourier Motzkin should fail, because the expression is not linear
  // Original result should be returned
  BOOST_CHECK(vars_out == elim_vars);
  BOOST_CHECK(out == expr);
}

void split_conditions_helper(const std::string& vars,
                             const std::string& expr,
                             std::vector< data_expression_list >& real_conditions,
                             std::vector< data_expression >& non_real_conditions)
{
  data_specification data_spec;
  data_spec.add_context_sort(sort_real::real_());
  const variable_list variables=parse_variables(vars);
  const data_expression e_in=parse_data_expression(expr,variables,data_spec);

  data::detail::split_condition(e_in, real_conditions, non_real_conditions);
}

void test_split_conditions()
{
  std::vector < data_expression_list > real_conditions;
  std::vector < data_expression > non_real_conditions;

  split_conditions_helper("x,y:Real, a,b,c:Bool;", "a && b", real_conditions, non_real_conditions);
  BOOST_CHECK(real_conditions.size() == 1);
  BOOST_CHECK(non_real_conditions.size() == 1);
  BOOST_CHECK(real_conditions[0].size() == 0);
  BOOST_CHECK(sort_bool::is_and_application(non_real_conditions[0]));
  real_conditions.clear(); non_real_conditions.clear();

  split_conditions_helper("x,y:Real, a,b,c:Bool;", "(a || b) && c", real_conditions, non_real_conditions);
  BOOST_CHECK(real_conditions.size() == 1);
  BOOST_CHECK(non_real_conditions.size() == 1);
  BOOST_CHECK(real_conditions[0].size() == 0);
  BOOST_CHECK(sort_bool::is_and_application(non_real_conditions[0]));
  BOOST_CHECK(sort_bool::is_or_application(sort_bool::left(non_real_conditions[0])));
  real_conditions.clear(); non_real_conditions.clear();

  split_conditions_helper("x,y:Real, a,b,c:Bool;", "(a || b) && x < 5", real_conditions, non_real_conditions);
  BOOST_CHECK(real_conditions.size() == 1);
  BOOST_CHECK(non_real_conditions.size() == 1);
  BOOST_CHECK(real_conditions[0].size() == 1);
  BOOST_CHECK(is_less_application(real_conditions[0].front()));
  BOOST_CHECK(sort_bool::is_or_application(non_real_conditions[0]));
  real_conditions.clear(); non_real_conditions.clear();

  split_conditions_helper("x,y:Real, a,b,c:Bool;", "(a || b) && (x == 3 || y > 4)", real_conditions, non_real_conditions);
  BOOST_CHECK(real_conditions.size() == 2);
  BOOST_CHECK(non_real_conditions.size() == 2);
  BOOST_CHECK(real_conditions[0].size() == 1);
  BOOST_CHECK(real_conditions[1].size() == 1);
  BOOST_CHECK((is_equal_to_application(real_conditions[0].front()) && is_greater_application(real_conditions[1].front())) ||
              (is_equal_to_application(real_conditions[1].front()) && is_greater_application(real_conditions[0].front())));
  BOOST_CHECK(non_real_conditions[0] == non_real_conditions[1]);
  real_conditions.clear(); non_real_conditions.clear();

  split_conditions_helper("x,y:Real, a,b,c:Bool;", "(x == y || y < 0) && (x == 3 || y > 4)", real_conditions, non_real_conditions);
  BOOST_CHECK(real_conditions.size() == 4);
  BOOST_CHECK(non_real_conditions.size() == 4);
  for(int i = 0; i < 4; i++)
  {
    BOOST_CHECK(real_conditions[i].size() == 2);
    BOOST_CHECK(non_real_conditions[i] == sort_bool::true_());
  }
  real_conditions.clear(); non_real_conditions.clear();

  split_conditions_helper("x,y:Real, a,b,c:Bool;", "(x == y || a) && (x == 3 || b)", real_conditions, non_real_conditions);
  BOOST_CHECK(real_conditions.size() == 4);
  BOOST_CHECK(non_real_conditions.size() == 4);
  for(int i = 0; i < 4; i++)
  {
    std::set< data_expression > split_without_true = split_and(non_real_conditions[i]);
    split_without_true.erase(sort_bool::true_());
    BOOST_CHECK(real_conditions[i].size() + split_without_true.size() == 2);
  }
  real_conditions.clear(); non_real_conditions.clear();
}

BOOST_AUTO_TEST_CASE(test_main)
{
  test_linear_inequality();
  test_split_conditions();

  BOOST_CHECK(test_consistency_of_inequalities("x:Real;", "x<3  && x>=4", false));
  BOOST_CHECK(test_consistency_of_inequalities("x:Real;", "x<3  && x>=2", true));
  BOOST_CHECK(test_consistency_of_inequalities("x:Real;", "x<3  && x>=3", false));
  BOOST_CHECK(test_consistency_of_inequalities("x:Real;", "x<=3  && x>=3", true));
  BOOST_CHECK(test_consistency_of_inequalities("u:Real;","0 <= u && -u <= -4 && -u < 0",true));
  BOOST_CHECK(test_consistency_of_inequalities("u,t:Real;","u + -t <= 1 && -u <= -4 && t < u && -u < 0 && -t <= 0 ",true));
  BOOST_CHECK(test_consistency_of_inequalities("u,t,l:Real;","u + -t <= 1 && -u <= -4 && -u + l < 0 && -u < 0 && -t <= 0 && -l + t <= 0",true));

  BOOST_CHECK(test_application_of_Fourier_Motzkin("x,y:Real;", "y:Real;", "-y + x < 0 &&  y < 2", "x>=2"));
  BOOST_CHECK(test_application_of_Fourier_Motzkin("cup1,cup2,add:Real;", "add:Real;", "add <= 2 && 0 <= add && cup2 + 2 - add <= cup1 + add && 3 < cup1 + add - 2", "cup2 - cup1 >= 2", true));
  test_high_level_fourier_motzkin();
  test_high_level_fourier_motzkin_non_linear();
}


//include <scottgs/Timing.hpp>

#include <iostream>
#include <cstdlib>
#include <algorithm>

#include "FloatMatrix.hpp"
#include "MatrixMultiply.hpp"

#include <boost/numeric/ublas/io.hpp>

void initRandomMatrix(scottgs::FloatMatrix& m);

int main(int argc, char * argv[])
{
	std::cout << "Homework 1" << std::endl
		  << "Grant Scott (scottgs)" << std::endl;  // CHANGE TO YOUR name AND pawprint

	// ---------------------------------------------
	// BEGIN: Self Test Portion
	// ---------------------------------------------
	std::cout << "Running Self Test" << std::endl
		  << "-----------------" << std::endl;
	scottgs::FloatMatrix l42(4, 2);
	l42(0,0) = 2; l42(0,1) = 3;
	l42(1,0) = 4; l42(1,1) = 5;
	l42(2,0) = 6; l42(2,1) = 7;
	l42(3,0) = 8; l42(3,1) = 9;

	scottgs::FloatMatrix r23(2,3);
	r23(0,0) = 2; r23(0,1) = 3; r23(0,2) = 4;
	r23(1,0) = 5; r23(1,1) = 6; r23(1,2) = 7;

	// (19,24,29),(33,42,51),(47,60,73),(61,78,95)
	scottgs::FloatMatrix er(4,3);
	er(0,0) = 19; er(0,1) = 24; er(0,2) = 29;
	er(1,0) = 33; er(1,1) = 42; er(1,2) = 51;
	er(2,0) = 47; er(2,1) = 60; er(2,2) = 73;
	er(3,0) = 61; er(3,1) = 78; er(3,2) = 95;

	scottgs::MatrixMultiply mm;
	scottgs::FloatMatrix result = mm(l42,r23);
	std::cout << "Result of " << std::endl << l42 << std::endl
		  << "   times  " << std::endl << r23 << std::endl
		  << "  equals  " << std::endl << result << std::endl;

	if ( ! std::equal( er.data().begin(), er.data().end(), result.data().begin() ) )
	{
		std::cerr << "Self Test Expected Result: " << er << std::endl
			  << "           ... but found : " << result << std::endl;
		return 1;
	}

	scottgs::FloatMatrix result2 = mm.multiply(l42,r23);
	std::cout << "Result of " << std::endl << l42 << std::endl
		  << "   times  " << std::endl << r23 << std::endl
		  << "  equals  " << std::endl << result2 << std::endl;

	if ( ! std::equal( er.data().begin(), er.data().end(), result2.data().begin() ) )
	{
		std::cerr << "Self Test Expected Result: " << er << std::endl
			  << "           ... but found : " << result2 << std::endl;
		return 1;
	}

	std::cout << "Self Test Complete" << std::endl
		  << "==================" << std::endl;

	// ---------------------------------------------
	// END: Self Test Portion
	// ---------------------------------------------

	return 0;
}


/*
This file covers basics about using eigen such as initializing a matrix, operations, and functions.

How to Install / Use Eigen:
1. go to desired install folder
2. git clone https://gitlab.com/libeigen/eigen
3. add path/to/eigen to include directories: Project >> PROJECT Properties >>
    VC++ Directories >> Include Directories >> (add path here)
4. start using Eigen

things to add:
  * append new row(s)
  * append new col(s)

*/

#include <iostream>
#include <Eigen/Dense>
using namespace std;
int main() {
  namespace eig = Eigen;
  // create a matrix: can either be a dynamic or static matrix:

  //dynamic matrix (note the "X" in the name)
  eig::MatrixX<double> x1(2, 2);
  x1 << 0, 1, 2, 3;

  // static matrix (note the delimiting of dimensions in the template)
  eig::Matrix<double, 2, 2> x2;
  x2 << 5,2,3,1;

  eig::MatrixX<double> x3(2, 5);
  x3 << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9;

  // alternative to matrices are arrays, which function a little bit more like numpy arrays
  eig::ArrayXX<double> y1(2, 2); // having two 'X's is correct
  y1 << 2, 3, 4, 5;
  eig::Array<double, 2, 2> y2; // also has a static option
  y2 << 5, 6, 7, 8;

  // conversion between the two:
  eig::MatrixX<double> xtemp = y1.matrix();

  // note there are also vectors, but used to a far lesser extent
  eig::VectorX<double> v(6);
  v << 1, 2, 3, 4, 5, 6;
  cout << "vector: " << v << endl;

  // basic per-element operations:
  cout << "scalar addition \n" << (x1.array() + 10).matrix() << endl; // note: need to be in array
  cout << "scalar subtraction \n" << (x1.array() - 10).matrix() << endl;
  cout << "scalar multiplication \n" << x1 * 10 << endl;
  cout << "scalar division \n" << x1 / 10 << endl;

  // matrix operations
  cout << "matrix addition \n" << x1 + x2 << endl;
  cout << "matrix subtraction \n" << x1 - x2 << endl;
  cout << "transpose \n" << x1.transpose() << endl;
  cout << "inverse \n" << x1.inverse() << endl;
  cout << "matmult \n" << x1 * x2 << endl;
  cout << "conjugation \n" << x1.conjugate() << endl;

  // WARNING: AVOID TRANSPOSITION ISSUES, WHICH OCCUR WHEN TRANSPOSING IN-PLACE:
  // a=a.transpose() // DO NOT DO THIS
  x1.transposeInPlace();
  x1.transposeInPlace();

  // arithmetic reduction operations:
  cout << "sum the matrix: " << x1.sum() << endl;
  cout << "mult the matrix: " << x1.prod() << endl;
  cout << "matrix mean: " << x1.mean() << endl;
  cout << "matrix min: " << x1.minCoeff() << endl;
  cout << "matrix max: " << x1.maxCoeff() << endl;

  // matrix properties
  cout << "diagonal: " << x1.diagonal().transpose() << endl;
  cout << "shape (should be 2,5): " << x3.rows() << ',' << x3.cols() << endl;

  // matrix manipulation
  eig::Map < eig::MatrixX<double>> x4(x1.data(), 1,x1.size());
  cout << "reshape: " << x4 << endl;
  // append a row
  eig::MatrixX<double> x6(2,2);
  x6 << 0, 1, 2, 3;
  x6.conservativeResize(x6.cols() + 1, eig::NoChange); // shortcut
  x6(2, 0) = 5;
  x6(2, 1) = 6;
  cout << "append row: \n" << x6 << endl;

  // taking part of matrix:
  eig::MatrixX<double> x5(5, 5);
  for (int i = 0; i < x5.size(); i++) x5(i) = i;
  cout << "initial \n" << x5 << endl;

  cout << "arbitrary block \n" << x5.block(1, 1, 2, 2) << endl;
  cout << "column1 \n" << x5.block(0, 1, x5.rows(), 1) << endl;
  cout << "row3 \n" << x5.block(3, 0, 1,x5.cols()) << endl;

  // special matrices. dynamic or static, can be whatever dimensions needed
  cout << "identity matrix \n" << eig::Matrix<double,3,3>::Identity() << endl;
  cout << "zeros \n" << eig::Matrix<double, 3, 3>::Zero() << endl;
  cout << "ones\n" << eig::Matrix<double, 3, 3>::Ones() << endl;
  cout << "random\n" << eig::MatrixX<double>::Random(3,3) << endl;

}


/**
 * Munkres.hpp
 * @author : koide
 * 14/11/03
 **/
#ifndef KKL_MUNKRES_HPP
#define KKL_MUNKRES_HPP

#include <cfloat>
#include <vector>
#include <iostream>
#include <Eigen/Dense>

namespace kkl {
	namespace alg {

template<typename T>
bool isZero(T value) {
	return value == 0;
}
template<>
bool isZero(float value) {
	return abs(value) <= FLT_EPSILON;
}
template<>
bool isZero(double value) {
	return abs(value) <= DBL_EPSILON;
}

/************************************************
 * Munkres
 * http://csclab.murraystate.edu/bob.pilgrim/445/munkres.html
 *
 * Munkres<int> munkres
 * auto ans = munkres.solve( cost )
************************************************/
template<typename T>
class Munkres {
public:
	/*********************************************************
	 * solve
   * cost : cost matrix (cost.rows <= cost.cols)
   * ret : solution
	********************************************************/
	Eigen::VectorXi solve(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& cost_) {
		assert( cost_.rows() <= cost_.cols() );
		cost = cost_;
		K = std::min(cost.rows(), cost.cols());

		starred = Eigen::MatrixXi::Zero(cost.rows(), cost.cols());
		primed = Eigen::MatrixXi::Zero(cost.rows(), cost.cols());
		covered_rows = Eigen::VectorXi::Zero(cost.rows());
		covered_cols = Eigen::VectorXi::Zero(cost.cols());

		// step 1
		cost.colwise() -= cost.rowwise().minCoeff();

		// step 2
		for (int i = 0; i < cost.rows(); i++) {
			for (int j = 0; j < cost.cols(); j++) {
				if (isZero(cost(i, j)) && starred.row(i).count() == 0 && starred.col(j).count() == 0){
					starred(i, j) = 1;
				}
			}
		}

		// step3 ~ step 6
		step3();

		// done, find the starred zeros
		Eigen::VectorXi ret(K);
		for (int i = 0; i < K; i++) {
			for (int j = 0; j < cost.cols(); j++) {
				if (starred(i, j)) {
					ret(i) = j;
					break;
				}
			}
		}

		return ret;
	}

	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
	/*****************************
	 * step3
	*****************************/
	void step3() {
		for (int i = 0; i < cost.cols(); i++) {
			if (starred.col(i).count() != 0) {
				covered_cols[i] = 1;
			}
		}
		// if K columns are covered, go to done
		if (covered_cols.count() == K) {
			return;
		}
		// otherwise, go to step 4
		return step4();
	}

	/*****************************
	 * step4
	*****************************/
	void step4() {
		for (int i = 0; i < cost.rows(); i++) {
			for (int j = 0; j < cost.cols(); j++){
				// find a noncovered zero and prime it
				if (!covered_rows[i] && !covered_cols[j] && isZero(cost(i, j))){
					primed(i, j) = 1;

					// if there is no starred zero in the row, go to step 5
					if (starred.row(i).count() == 0) {
						return step5(i, j);
					}
					// otherwise, cover this row and uncover the column containing the starred zero
					covered_rows[i] = 1;
					for (int k = 0; k < cost.cols(); k++) {
						if (starred(i, k)) {
							covered_cols[k] = 0;
						}
					}
				}
			}
		}

		T smallest = -1;
		// continue in this manner until there are no uncovered zeros left
		// save the smallest uncovered value and go to step 6
		for (int i = 0; i < cost.rows(); i++){
			for (int j = 0; j < cost.cols(); j++) {
				if (!covered_rows[i] && !covered_cols[j] && isZero(cost(i, j))){
					return step4();
				}

				if (!covered_rows[i] && !covered_cols[j] &&
					(smallest > cost(i, j) || smallest < 0.0)) {
					smallest = cost(i, j);
				}
			}
		}

		return step6(smallest);
	}

	/*****************************
	 * step5
	*****************************/
	void step5(int z0_row, int z0_col) {
		std::vector<std::pair<int, int>> series(1);
		series[0] = std::make_pair(z0_row, z0_col);

		bool done = false;
		while (!done) {
			// z1 : the starred zero in the column of z0
			bool added = false;
			const auto& z0 = series.back();
			for (int i = 0; i < cost.rows(); i++) {
				if (starred(i, z0.second)) {
					added = true;
					series.push_back(std::make_pair(i, z0.second));
					break;
				}
			}
			if (!added) {
				break;
			}

			const auto& z1 = series.back();
			// z2 : the primed zero in the rows of z1
			for (int i = 0; i < cost.cols(); i++) {
				if (primed(z1.first, i)){
					series.push_back(std::make_pair(z1.first, i));
					break;
				}
			}

			done = true;
			const auto& z2 = series.back();
			for (int i = 0; i < cost.rows(); i++){
				if (starred(i, z2.second)) {
					done = false;
				}
			}
		}

		for (int i = 0; i < series.size(); i++) {
			if (i % 2 == 0) {
				starred(series[i].first, series[i].second) = 1;
			}
			else {
				starred(series[i].first, series[i].second) = 0;
			}
		}

		primed.setZero();
		covered_rows.setZero();
		covered_cols.setZero();

		return step3();
	}

	/*****************************
	 * step6
	*****************************/
	void step6(T smallest) {
		for (int i = 0; i < cost.rows(); i++) {
			if (covered_rows[i]) {
				cost.row(i).array() += smallest;
			}
		}
		for (int i = 0; i < cost.cols(); i++) {
			if (!covered_cols[i]) {
				cost.col(i).array() -= smallest;
			}
		}

		return step4();
	}

	/*****************************
	 * showState
	 * show all matrices
	*****************************/
	void showState() const {
		std::cout << "--- cost ---" << std::endl << cost << std::endl;
		std::cout << "--- starred ---" << std::endl << starred << std::endl;
		std::cout << "--- primed ---" << std::endl << primed << std::endl;

		Eigen::MatrixXi covered(covered_rows.size(), covered_cols.size());
		for (int i = 0; i < covered.rows(); i++) {
			for (int j = 0; j < covered.cols(); j++) {
				covered(i, j) = covered_rows[i] + covered_cols[j];
			}
		}
		std::cout << "--- covered ---" << std::endl << covered << std::endl;
	}

private:
	int K;
	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> cost;

	Eigen::MatrixXi starred;
	Eigen::MatrixXi primed;

	Eigen::VectorXi covered_rows;
	Eigen::VectorXi covered_cols;
};

	}
}

#endif


//------------------------------------------------------------------------------
/// \file WriterMonad_tests.cpp
/// \ref Ivan Čukić, Functional Programming in C++,  Manning Publications;
/// 1st edition (November 19, 2018). ISBN-13: 978-1617293818
//------------------------------------------------------------------------------
#include "Categories/Monads/WriterMonad.h"

#include <boost/test/unit_test.hpp>
#include <string>

using Categories::Monads::WriterMonad::WriterMonadEndomorphism;
using Categories::Monads::WriterMonad::unit;
using Categories::Monads::WriterMonad::bind;

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(Monads)
BOOST_AUTO_TEST_SUITE(WriterMonad_tests)

using LogEndomorphism = WriterMonadEndomorphism<int, std::string>;

// Test morphisms.

template <int N = 0>
WriterMonadEndomorphism<int, std::string> add_N(const int x)
{
  return WriterMonadEndomorphism<int, std::string>{
    x + N,
    "added " + std::to_string(N) + " "};
}

template <int N = 0>
WriterMonadEndomorphism<int, std::string> subtract_N(const int x)
{
  return WriterMonadEndomorphism<int, std::string>{
    x - N,
    "subtracted " + std::to_string(N) + " "};
}

BOOST_AUTO_TEST_SUITE(Construction_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(CartesianProductExplicitlyConstructs)
{
  const WriterMonadEndomorphism<int, std::string> tx {42};
  BOOST_TEST(tx.value() == 42);
  BOOST_TEST(tx.log().empty());
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(CartesianProductConstructsWithCartesianProduct)
{
  const std::string start_log {"Start"};
  const WriterMonadEndomorphism<int, std::string> tx {42, start_log};
  BOOST_TEST(tx.value() == 42);
  BOOST_TEST((tx.log() == start_log));
}

BOOST_AUTO_TEST_SUITE_END() // Construction_tests

BOOST_AUTO_TEST_SUITE(Morphisms_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(AddNIsAMorphism)
{
  const auto result = add_N<42>(69);
  BOOST_TEST(result.value() == 42 + 69);
  BOOST_TEST(result.log() == "added 42 ");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SubtractNIsAMorphism)
{
  const auto result = subtract_N<42>(69);
  BOOST_TEST(result.value() == 69 - 42);
  BOOST_TEST(result.log() == "subtracted 42 ");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(UnitActsAsAUnitAndIsVeryGeneral)
{
  const auto result = unit<int, LogEndomorphism>(1608);
  BOOST_TEST(result.value() == 1608);
  BOOST_TEST(result.log().empty());
}

BOOST_AUTO_TEST_SUITE_END() // Morphisms_tests

BOOST_AUTO_TEST_SUITE(Bind_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BindIsComposition)
{
  const LogEndomorphism previous_actions {42, "Started at 42,"};

  const auto result = bind(previous_actions, add_N<69>);

  BOOST_TEST(result.value() == 42 + 69);
  BOOST_TEST(result.log() == "Started at 42,added 69 ");

  const auto result2 = bind(result, subtract_N<10>);

  BOOST_TEST(result2.value() == 42 + 69 - 10);
  BOOST_TEST(result2.log() == "Started at 42,added 69 subtracted 10 ");
}

BOOST_AUTO_TEST_SUITE_END() // Morphisms_tests

BOOST_AUTO_TEST_SUITE_END() // WriterMonad_tests
BOOST_AUTO_TEST_SUITE_END() // Monads
BOOST_AUTO_TEST_SUITE_END() // Categories


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

void test_log_diff_exp(double a, double b) {
  using stan::math::log_diff_exp;
  using std::exp;
  using std::log;
  EXPECT_FLOAT_EQ(log(exp(a) - exp(b)), log_diff_exp(a, b));
}

TEST(MathFunctions, log_diff_exp) {
  using stan::math::log_diff_exp;
  test_log_diff_exp(3.0, 2.0);
  test_log_diff_exp(4.0, 1.0);
  test_log_diff_exp(3.0, 2.0);
  test_log_diff_exp(0, -2.1);
  test_log_diff_exp(-20.0, -23);
  test_log_diff_exp(-21.2, -32.1);
  EXPECT_NO_THROW(log_diff_exp(-20.0, 12));
  EXPECT_NO_THROW(log_diff_exp(-20.0, -12.1));
  EXPECT_NO_THROW(log_diff_exp(120.0, 120.10));
  EXPECT_NO_THROW(log_diff_exp(-20.0, 10.2));
  EXPECT_NO_THROW(log_diff_exp(10, 11));
  EXPECT_NO_THROW(log_diff_exp(10, 10));
  EXPECT_NO_THROW(log_diff_exp(-10.21, -10.21));

  // exp(10000.0) overflows
  EXPECT_FLOAT_EQ(10000.0, log_diff_exp(10000.0, 0.0));
  EXPECT_FLOAT_EQ(0.0, log_diff_exp(0.0, -10000.0));
}

TEST(MathFunctions, log_diff_exp_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::log_diff_exp(3.0, nan));

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::log_diff_exp(nan, 2.0));

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::log_diff_exp(nan, nan));
}


/**
 * @file
 * @brief Test the order of quadrature rules by testing it with monomials
 * @author Raffael Casagrande
 * @date   2018-08-19 06:54:02
 * @copyright MIT License
 */

#include <gtest/gtest.h>
#include <lf/quad/quad.h>
#include <boost/math/special_functions/factorials.hpp>

namespace lf::quad::test {

double integrate(QuadRule qr, std::vector<int> monomialCoefficients) {
  double result = 0;
  for (int i = 0; i < qr.Points().cols(); ++i) {
    double temp = 1;
    for (int j = 0; j < monomialCoefficients.size(); ++j) {
      temp *= std::pow(qr.Points()(j, i), monomialCoefficients[j]);
    }
    result += temp * qr.Weights()(i);
  }
  return result;
}

void checkQuadRule(QuadRule qr, double precision = 1e-12,
                   bool check_order_exact = true) {
  EXPECT_EQ(qr.Points().cols(), qr.Weights().size());
  EXPECT_EQ(qr.Points().rows(), qr.RefEl().Dimension());

  auto order = qr.Degree();
  if (qr.RefEl() == base::RefEl::kSegment()) {
    for (int i = 0; i <= order; ++i) {
      // integrate x^i
      EXPECT_DOUBLE_EQ(integrate(qr, {i}), 1. / (1. + i))
          << "Failure for i = " << i;
    }
    // try integrate one order too high:
    EXPECT_GT(std::abs(integrate(qr, {static_cast<int>(order) + 1}) -
                       1. / (2. + order)),
              1e-10);
  } else if (qr.RefEl() == base::RefEl::kTria()) {
    // TRIA
    ///////////////////////////////////////////////////////////////////////////
    auto exact_value = [](int i, int j) {
      return boost::math::factorial<double>(i) *
             boost::math::factorial<double>(j + 1) /
             ((1 + j) * boost::math::factorial<double>(2 + i + j));
    };
    for (int i = 0; i <= order; ++i) {
      for (int j = 0; j <= order - i; ++j) {
        // integrate x^i y^j
        double qr_val = integrate(qr, {i, j});
        EXPECT_NEAR(qr_val / exact_value(i, j), 1, precision)
            << "Failure for x^" << i << "*y^" << j << ": " << qr_val << " <-> "
            << exact_value(i, j);
      }
    }
    if (check_order_exact) {
      // Make sure that at least on of the order+1 polynomials is not integrated
      // correctly
      bool one_fails = false;
      for (int i = -1; i <= static_cast<int>(order); ++i) {
        if (std::abs(integrate(qr, {i + 1, static_cast<int>(order - i)}) -
                     exact_value(i + 1, order - i)) > 1e-12) {
          one_fails = true;
          break;
        }
      }
      EXPECT_TRUE(one_fails) << "order = " << (int)order;
    }

  } else if (qr.RefEl() == base::RefEl::kQuad()) {
    // QUAD
    ///////////////////////////////////////////////////////////////////////////
    for (int i = 0; i <= order; ++i) {
      for (int j = 0; j <= order; ++j) {
        // integrate x^i y^j
        double qr_val = integrate(qr, {i, j});
        double ext_val = 1. / ((1. + i) * (1. + j));
        EXPECT_DOUBLE_EQ(qr_val, ext_val)
            << "Failure for x^" << i << "*y^" << j << ": " << qr_val << " <-> "
            << ext_val;
      }
    }

    // make sure that not all of the higher polynomials integrate correctly:
    bool atLeastOneFails = false;
    for (int i = 0; i <= order + 1; ++i) {
      if (std::abs(integrate(qr, {static_cast<int>(order + 1), i}) -
                   1. / ((2. + order) * (1. + i))) > 1e-10) {
        atLeastOneFails = true;
        break;
      }
      if (std::abs(integrate(qr, {i, static_cast<int>(order + 1)}) -
                   1. / ((2. + order) * (1. + i))) > 1e-10) {
        atLeastOneFails = true;
        break;
      }
    }
    EXPECT_TRUE(atLeastOneFails);
  }
}

TEST(qr_IntegrationTest, Segment) {
  for (int i = 1; i < 10; ++i) {
    checkQuadRule(make_QuadRule(base::RefEl::kSegment(), i));
  }
}

TEST(qr_IntegrationTest, Quad) {
  checkQuadRule(make_QuadRule(base::RefEl::kQuad(), 1));
  checkQuadRule(make_QuadRule(base::RefEl::kQuad(), 2));
  checkQuadRule(make_QuadRule(base::RefEl::kQuad(), 3));
}

TEST(qr_IntegrationTest, Tria) {
  // make sure that also the tensor product versions are tested.
  for (int i = 1; i < 55; ++i) {
    checkQuadRule(make_QuadRule(base::RefEl::kTria(), i), 1e-12, i < 10);
  }
}

// Test midpoint quadrature rule for triangles
TEST(qr_IntegrationTest, mp) {
  checkQuadRule(make_TriaQR_EdgeMidpointRule(), 1e-12, true);
  checkQuadRule(make_QuadQR_EdgeMidpointRule(), 1e-12, true);
}

TEST(qr_IntegrationTest, P6O4) {
  checkQuadRule(make_TriaQR_P6O4(), 1e-12, true);
}

}  // namespace lf::quad::test


/**
 * @file linear_regression_test.cpp
 *
 * Test for linear regression.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/linear_regression/linear_regression.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::regression;

BOOST_AUTO_TEST_SUITE(LinearRegressionTest);

/**
 * Creates two 10x3 random matrices and one 10x1 "results" matrix.
 * Finds B in y=BX with one matrix, then predicts against the other.
 */
BOOST_AUTO_TEST_CASE(LinearRegressionTestCase)
{
  // Predictors and points are 10x3 matrices.
  arma::mat predictors(3, 10);
  arma::mat points(3, 10);

  // Responses is the "correct" value for each point in predictors and points.
  arma::vec responses(10);

  // The values we get back when we predict for points.
  arma::vec predictions(10);

  // We'll randomly select some coefficients for the linear response.
  arma::vec coeffs;
  coeffs.randu(4);

  // Now generate each point.
  for (size_t row = 0; row < 3; row++)
    predictors.row(row) = arma::linspace<arma::rowvec>(0, 9, 10);

  points = predictors;

  // Now add a small amount of noise to each point.
  for (size_t elem = 0; elem < points.n_elem; elem++)
  {
    // Max added noise is 0.02.
    points[elem] += math::Random() / 50.0;
    predictors[elem] += math::Random() / 50.0;
  }

  // Generate responses.
  for (size_t elem = 0; elem < responses.n_elem; elem++)
    responses[elem] = coeffs[0] +
        dot(coeffs.rows(1, 3), arma::ones<arma::rowvec>(3) * elem);

  // Initialize and predict.
  LinearRegression lr(predictors, responses);
  lr.Predict(points, predictions);

  // Output result and verify we have less than 5% error from "correct" value
  // for each point.
  for (size_t i = 0; i < predictions.n_cols; ++i)
    BOOST_REQUIRE_SMALL(predictions(i) - responses(i), .05);
}

/**
 * Check the functionality of ComputeError().
 */
BOOST_AUTO_TEST_CASE(ComputeErrorTest)
{
  arma::mat predictors;
  predictors << 0 << 1 << 2 << 4 << 8 << 16 << arma::endr
             << 16 << 8 << 4 << 2 << 1 << 0 << arma::endr;
  arma::vec responses = "0 2 4 3 8 8";

  // http://www.mlpack.org/trac/ticket/298
  // This dataset gives a cost of 1.189500337 (as calculated in Octave).
  LinearRegression lr(predictors, responses);

  BOOST_REQUIRE_CLOSE(lr.ComputeError(predictors, responses), 1.189500337,
      1e-3);
}

/**
 * Ensure that the cost is 0 when a perfectly-fitting dataset is given.
 */
BOOST_AUTO_TEST_CASE(ComputeErrorPerfectFitTest)
{
  // Linear regression should perfectly model this dataset.
  arma::mat predictors;
  predictors << 0 << 1 << 2 << 1 << 6 << 2 << arma::endr
             << 0 << 1 << 2 << 2 << 2 << 6 << arma::endr;
  arma::vec responses = "0 2 4 3 8 8";

  LinearRegression lr(predictors, responses);

  BOOST_REQUIRE_SMALL(lr.ComputeError(predictors, responses), 1e-25);
}

/**
 * Test ridge regression using an empty dataset, which is not invertible.  But
 * the ridge regression part should make it invertible.
 */
BOOST_AUTO_TEST_CASE(RidgeRegressionTest)
{
  // Create empty dataset.
  arma::mat data;
  data.zeros(10, 5000); // 10-dimensional, 5000 points.
  arma::vec responses;
  responses.zeros(5000); // 5000 points.

  // Any lambda greater than 0 works to make the predictors covariance matrix
  // invertible.  If ridge regression is not working correctly, then the matrix
  // will not be invertible and the test should segfault (or something else
  // ugly).
  LinearRegression lr(data, responses, 0.0001);

  // Now just make sure that it predicts some more zeros.
  arma::vec predictedResponses;
  lr.Predict(data, predictedResponses);

  for (size_t i = 0; i < 5000; ++i)
    BOOST_REQUIRE_SMALL((double) predictedResponses[i], 1e-20);
}

/**
 * Creates two 10x3 random matrices and one 10x1 "results" matrix.
 * Finds B in y=BX with one matrix, then predicts against the other, but uses
 * ridge regression with an extremely small lambda value.
 */
BOOST_AUTO_TEST_CASE(RidgeRegressionTestCase)
{
  // Predictors and points are 10x3 matrices.
  arma::mat predictors(3, 10);
  arma::mat points(3, 10);

  // Responses is the "correct" value for each point in predictors and points.
  arma::vec responses(10);

  // The values we get back when we predict for points.
  arma::vec predictions(10);

  // We'll randomly select some coefficients for the linear response.
  arma::vec coeffs;
  coeffs.randu(4);

  // Now generate each point.
  for (size_t row = 0; row < 3; row++)
    predictors.row(row) = arma::linspace<arma::rowvec>(0, 9, 10);

  points = predictors;

  // Now add a small amount of noise to each point.
  for (size_t elem = 0; elem < points.n_elem; elem++)
  {
    // Max added noise is 0.02.
    points[elem] += math::Random() / 50.0;
    predictors[elem] += math::Random() / 50.0;
  }

  // Generate responses.
  for (size_t elem = 0; elem < responses.n_elem; elem++)
    responses[elem] = coeffs[0] +
        dot(coeffs.rows(1, 3), arma::ones<arma::rowvec>(3) * elem);

  // Initialize and predict with very small lambda.
  LinearRegression lr(predictors, responses, 0.001);
  lr.Predict(points, predictions);

  // Output result and verify we have less than 5% error from "correct" value
  // for each point.
  for (size_t i = 0; i < predictions.n_cols; ++i)
    BOOST_REQUIRE_SMALL(predictions(i) - responses(i), .05);
}

/**
 * Test that a LinearRegression model trained in the constructor and trained in
 * the Train() method give the same model.
 */
BOOST_AUTO_TEST_CASE(LinearRegressionTrainTest)
{
  // Random dataset.
  arma::mat dataset = arma::randu<arma::mat>(5, 1000);
  arma::vec responses = arma::randu<arma::vec>(1000);

  LinearRegression lr(dataset, responses, 0.3);
  LinearRegression lrTrain;
  lrTrain.Lambda() = 0.3;

  lrTrain.Train(dataset, responses);

  BOOST_REQUIRE_EQUAL(lr.Parameters().n_elem, lrTrain.Parameters().n_elem);
  for (size_t i = 0; i < lr.Parameters().n_elem; ++i)
    BOOST_REQUIRE_CLOSE(lr.Parameters()[i], lrTrain.Parameters()[i], 1e-5);
}

BOOST_AUTO_TEST_SUITE_END();


// Implementing the class that is defined in the header file: Option.hpp
//
// (c) Sudhansh Dua

#include "Option.hpp"
#include <cmath>
#include <boost/math/distributions.hpp>

using namespace std;
using namespace boost::math;

//	Gaussian functions using boost libraries
double Option::N(double x) const
{
	normal_distribution<> Standard_normal(0.0, 1.0);
	return cdf(Standard_normal, x);
}

double Option::n(double x) const
{
	normal_distribution<> Standard_normal(0.0, 1.0);
	return pdf(Standard_normal, x);
}

//	Kernel functions
double Option::CallPrice() const
{
	return ::CallPrice(S, K, T, r, sig, b);
}

double Option::PutPrice() const
{
	return ::PutPrice(S, K, T, r, sig, b);
}

//	Initialising all the default values
void Option::init()
{
	//	Default values
	r = 0.03;
	sig = 0.2;
	K = 100;
	S = 95;				//	Default stock price
	T = 1;
	b = r;				//	Black - Scholes(1973) stock option model : b = r
	type = "C";			//	Call option as the default
}

void Option::copy(const Option& option)
{
	S = option.S;
	K = option.K;
	T = option.T;
	r = option.r;
	sig = option.sig;
	b = option.b;
	type = option.type;
}

//	Constructors and destructor
Option::Option()						//	Default constructor
{
	init();
}

Option::Option(const Option& option)				//	Copy constructor
{
	copy(option);
}

//	Constructor that accepts values
Option::Option(const double& S1, const double& K1, const double& T1, const double& r1,
	const double& sig1, const double& b1, const string type1) : S(S1), K(K1), T(T1), r(r1), sig(sig1), b(b1), type(type1) {}

Option::~Option() {}						//	Destructor

//	Assignment operator
Option& Option::operator = (const Option& option)
{
	if (this == &option)
	{
		return *this;
	}
	copy(option);
	return *this;
}

//	Functions that calculate option price and sensitivities
double Option::Price() const
{
	if (type == "C")
	{
		return CallPrice();

	}
	else
	{
		return PutPrice();
	}
}

// Modifier functions
void Option::toggle()				//	Change the option type
{
	type = ((type == "C") ? "P" : "C");
}

//	Global Functions
double CallPrice(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);

	return (S * exp((b - r) * T) * cdf(standard_normal, d1)) - (K * exp(-r * T) * cdf(standard_normal, d2));
}

double PutPrice(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);

	return (K * exp(-r * T) * cdf(standard_normal, -d2)) - (S * exp((b - r) * T) * cdf(standard_normal, -d1));
}


#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace boost::numeric;

template<class type>
inline static void rotate(type c, type s, type &r, type &h)
{
    type tmp = c * r + s * h;
    h        = c * h - s * r;
    r        = tmp;
}

template<class type>
int gmres(ublas::vector<type> &y, ublas::matrix<type> &A, ublas::vector<type> &x, type tol, size_t m)
{
    using namespace ublas;

    typedef vector<type> Vec;
    typedef matrix<type> Mat;

    int size = y.size();
    unit_vector<type> e_1(size + 1, 0);

    m = (m > size) ?size :m;

    // Lower case name for vector
    Vec x_i(x);
    Vec r_i = y - prod(A, x);
    Vec v_i = r_i / norm_2(r_i);
    Vec e_i = e_1 * norm_2(r_i);

    // Givens rotation args
    Vec c(m + 1, 0);
    Vec s(m + 1, 0);

    // Upper case name for matrix, V = V^t, R = Q^t * H
    Mat V(m, size, 0);
    Mat R(size + 1, m, 0);

    for(int i = 0; i < size; i++)
    {
        type beta = norm_2(r_i);
        Vec  v_i  = r_i / beta;
        Vec  e_i  = e_1 * beta;

        int dim = 0;
        V.clear();
        for(int j = 0; j < m; j++)
        {
            row(V, j) = v_i;
            v_i       = prod(A, v_i);
            for(int k = 0; k <= j; k++)
            {
                R(k, j) = inner_prod(v_i, row(V, k));
                v_i    -= R(k, j) * row(V, k);
            }

            // Re-orthogonalization
            #pragma omp parallel for
            for(int k = 0; k <= j; k++)
            {
                type tmp   = inner_prod(v_i, row(V, k));
                row(V, k) -= tmp * v_i;
            }

            R(j + 1, j) = norm_2(v_i);

            dim++;
            if(R(j + 1, j) > tol)
            {
                v_i /= R(j + 1, j);
            }
            else
            {
                R(j + 1, j) = (type) 0;
                v_i.clear();
                break;
            }
        }

        // Apply givens rotation
        for(int j = 0; j < m; j++)
        {
            type r = std::sqrt(R(j, j) * R(j, j) + R(j + 1, j) * R(j + 1, j));
            if(r > tol)
            {
                c(j)   = R(j, j)     / r;
                s(j)   = R(j + 1, j) / r;
            }
            else
            {
                c(j) = (type) 1;
                s(j) = (type) 0;
            }

            rotate(c(j), s(j), R(j, j), R(j + 1, j));
            rotate(c(j), s(j), e_i(j), e_i(j + 1));
        }

        // Solve for y_i
        Vec y_i(solve(subrange(R, 0, dim, 0, dim), subrange(e_i, 0, dim), upper_tag()));

        // Update x
        x_i += prod(y_i, subrange(V, 0, dim, 0, size));
        r_i  = y - prod(A, x_i);

        if(norm_2(r_i) < tol) break;
    }

    x = x_i;
    return 0;
}

int main(int argc, char **argv)
{
    std::cout.precision(15);

    int size, nnz;
    std::string filename;
    if(argc == 2) filename = argv[1];
    else filename = "prob.mtx";

    std::ifstream prob(filename);
    if(!prob.is_open()) exit(0);
    prob >> size;
    prob >> size >> nnz;

    std::cout << "Problem size : " << size << std::endl;

    ublas::vector<double> x(size, 0.0);
    ublas::vector<double> y(size, 1.0);
    ublas::matrix<double> A(size, size, 0.0);

    // Set up problem
    for(int i = 0; i < nnz; i++)
    {
        int tmpRow, tmpCol;
        double tmpVal;

        prob >> tmpRow >> tmpCol >> tmpVal;
        A(tmpRow - 1, tmpCol - 1) = tmpVal;
    }

    // GMRES
    if(gmres(y, A, x, 1e-12, y.size()) != 0)
    {
        std::cout << "GMRES not converged" << std::endl;
    }
    else
    {
        double r = norm_2(y - prod(A, x));
        std::cout << "r   = " << r << std::endl;
        std::cout << "err = " << r / norm_2(y) << std::endl;
    }

    std::cout << x << std::endl;

    return 0;
}


#include <minres.hpp>

#include <armadillo>

bool test_minres()
{
    // finite difference helmholtz: -u''(x) - k*u(x) == x^2, u(-1) == u(1) == 0 {k is chosen so the system is indefinite}
    int n = 20;
    double k = 2;
    arma::vec x = arma::linspace(-1, 1, n);
    double h = x[1] - x[0];
    arma::vec D2 = {-1.0, 2.0, -1.0};
    D2 /= h*h;
    auto L = [&D2,k](const arma::vec& u) -> arma::vec
    {
        return arma::conv(u, D2, "same") - k*u;
    };
    arma::vec f = x%x;

    arma::vec u = arma::zeros(n);
    double tol = 1e-5;
    auto rslts = minres(u, L, f, arma::dot<arma::vec,arma::vec>, IdentityPreconditioner{}, n, tol);

    bool success = true;
    if (arma::norm(L(u) - f) > tol * arma::norm(f)) {
        std::cout << "pcg returned bad residual\n";
        success = false;
    }

    return success;
}