/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <cstddef>
#include <cmath>
#include <boost/array.hpp>
#include <boost/call_traits.hpp>
#include <boost/numeric/ublas/matrix.hpp>

namespace sprig {
	namespace ublas {
		//
		// make_scaling_matrix_2d
		//
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> make_scaling_matrix_2d(T const sx, T const sy) {
			boost::numeric::ublas::matrix<T> mat = boost::numeric::ublas::identity_matrix<T>(3);
			mat(0, 0) = sx;
			mat(1, 1) = sy;
			return mat;
		}
		//
		// make_rotation_matrix_2d
		//
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> make_rotation_matrix_2d(T const a) {
			boost::numeric::ublas::matrix<T> mat = boost::numeric::ublas::identity_matrix<T>(3);
			mat(0, 0) = std::cos(a);
			mat(0, 1) = std::sin(a);
			mat(1, 0) = -mat(0, 1);
			mat(1, 1) = mat(0, 0);
			return mat;
		}
		//
		// make_translation_matrix_2d
		//
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> make_translation_matrix_2d(T const tx, T const ty) {
			boost::numeric::ublas::matrix<T> mat = boost::numeric::ublas::identity_matrix<T>(3);
			mat(2, 0) = tx;
			mat(2, 1) = ty;
			return mat;
		}
		//
		// matrix_prod
		//
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> matrix_prod(
			boost::numeric::ublas::matrix<T> const& m1,
			boost::numeric::ublas::matrix<T> const& m2
			)
		{
			return boost::numeric::ublas::prod(m1, m2);
		}
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> matrix_prod(
			boost::numeric::ublas::matrix<T> const& m1,
			boost::numeric::ublas::matrix<T> const& m2,
			boost::numeric::ublas::matrix<T> const& m3
			)
		{
			return boost::numeric::ublas::prod(matrix_prod(m1, m2), m3);
		}
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> matrix_prod(
			boost::numeric::ublas::matrix<T> const& m1,
			boost::numeric::ublas::matrix<T> const& m2,
			boost::numeric::ublas::matrix<T> const& m3,
			boost::numeric::ublas::matrix<T> const& m4
			)
		{
			return boost::numeric::ublas::prod(matrix_prod(m1, m2, m3), m4);
		}
		template<typename T>
		SPRIG_INLINE boost::numeric::ublas::matrix<T> matrix_prod(
			boost::numeric::ublas::matrix<T> const& m1,
			boost::numeric::ublas::matrix<T> const& m2,
			boost::numeric::ublas::matrix<T> const& m3,
			boost::numeric::ublas::matrix<T> const& m4,
			boost::numeric::ublas::matrix<T> const& m5
			)
		{
			return boost::numeric::ublas::prod(matrix_prod(m1, m2, m3, m4), m5);
		}
		//
		// scaling_2d
		//
		template<typename T>
		class scaling_2d {
		public:
			typedef T value_type;
			typedef boost::numeric::ublas::matrix<value_type> matrix_type;
		private:
			boost::array<value_type, 2> elems_;
		public:
			scaling_2d() {
				elems_[0] = value_type();
				elems_[1] = value_type();
			}
			scaling_2d(
				typename boost::call_traits<value_type>::param_type v1,
				typename boost::call_traits<value_type>::param_type v2
				)
			{
				elems_[0] = v1;
				elems_[1] = v2;
			}
			matrix_type matrix() const{
				return make_scaling_matrix_2d(elems_[0], elems_[1]);
			}
			template<std::size_t N>
			value_type const& get() const {
				return elems_[N];
			}
			template<std::size_t N>
			value_type& get() {
				return elems_[N];
			}
		};
		//
		// rotation_2d
		//
		template<typename T>
		class rotation_2d {
		public:
			typedef T value_type;
			typedef boost::numeric::ublas::matrix<value_type> matrix_type;
		private:
			boost::array<value_type, 1> elems_;
		public:
			rotation_2d() {
				elems_[0] = value_type();
			}
			rotation_2d(
				typename boost::call_traits<value_type>::param_type v1
				)
			{
				elems_[0] = v1;
			}
			matrix_type matrix() const{
				return make_rotation_matrix_2d(elems_[0]);
			}
			template<std::size_t N>
			value_type const& get() const {
				return elems_[N];
			}
			template<std::size_t N>
			value_type& get() {
				return elems_[N];
			}
		};
		//
		// translation_2d
		//
		template<typename T>
		class translation_2d {
		public:
			typedef T value_type;
			typedef boost::numeric::ublas::matrix<value_type> matrix_type;
		private:
			boost::array<value_type, 2> elems_;
		public:
			translation_2d() {
				elems_[0] = value_type();
				elems_[1] = value_type();
			}
			translation_2d(
				typename boost::call_traits<value_type>::param_type v1,
				typename boost::call_traits<value_type>::param_type v2
				)
			{
				elems_[0] = v1;
				elems_[1] = v2;
			}
			matrix_type matrix() const{
				return make_translation_matrix_2d(elems_[0], elems_[1]);
			}
			template<std::size_t N>
			value_type const& get() const {
				return elems_[N];
			}
			template<std::size_t N>
			value_type& get() {
				return elems_[N];
			}
		};
	}	// namespace ublas
}	// namespace sprig

#endif	// #ifndef SPRIG_UBLAS_MATRIX_HPP


#include <utility>
#include <vector>
#include <stack>
#include <algorithm>

#include <boost/geometry/geometry.hpp>

using namespace boost::geometry;

template
<
    typename Point
>
struct triangle {
    Point p1,p2,p3;
    std::vector<std::size_t> adjacent_indices;
};

template
<
    typename Area,
    typename Point
>
Area triangle_double_area(const Point& p1, const Point& p2, const Point& p3) {
    return std::abs(boost::numeric_cast<Area>(get<0>(p1))*boost::numeric_cast<Area>(get<1>(p2))-boost::numeric_cast<Area>(get<0>(p1))*boost::numeric_cast<Area>(get<1>(p3))+boost::numeric_cast<Area>(get<0>(p2))*boost::numeric_cast<Area>(get<1>(p3))-boost::numeric_cast<Area>(get<0>(p2))*boost::numeric_cast<Area>(get<1>(p1))+boost::numeric_cast<Area>(get<0>(p3))*boost::numeric_cast<Area>(get<1>(p1))-boost::numeric_cast<Area>(get<0>(p3))*boost::numeric_cast<Area>(get<1>(p2)));
}

template
<
    typename Point
>
double circumcircle_diameter(const Point& p1, const Point& p2, const Point& p3)
{
    return distance(p1,p2)*distance(p1,p3)*distance(p2,p3)/triangle_double_area<double,Point>(p1,p2,p3);
}

template
<
    typename Point
>
Point circumcircle_center(const Point& p1, const Point& p2, const Point& p3)
{
    auto ax = get<0>(p1);
    auto ay = get<1>(p1);
    auto bx = get<0>(p2);
    auto by = get<1>(p2);
    auto cx = get<0>(p3);
    auto cy = get<1>(p3);
    auto d = 2*(ax*(by-cy)+bx*(cy-ay)+cx*(ay-by));
    auto x = ((ax*ax+ay*ay)*(by-cy)+(bx*bx+by*by)*(cy-ay)+(cx*cx+cy*cy)*(ay-by))/d;
    auto y = ((ax*ax+ay*ay)*(cx-bx)+(bx*bx+by*by)*(ax-cx)+(cx*cx+cy*cy)*(bx-ax))/d;
    return make<Point>(x,y);
}

template
<
    typename Segment
>
double angle(const Segment& s)
{
    return std::atan2(get<1,1>(s)-get<0,1>(s),get<1,0>(s)-get<0,0>(s));
}

template
<
    typename Point,
    typename Segment
>
bool is_left_of(const Point& p, const Segment& s)
{
    return (get<0>(p)-get<0,0>(s))*(get<1,1>(s)-get<0,1>(s))-(get<1>(p)-get<0,1>(s))*(get<1,0>(s)-get<0,0>(s))<0;
}

template
<
    typename Point
>
bool adjacent_at_edge(const triangle<Point>& t1, const triangle<Point>& t2, const Point& p1, const Point& p2)
{
    //Very naive check, not necessary with proper triangulation data structure
    return (equals(p1,t1.p1) || equals(p1,t1.p2) || equals(p1,t1.p3))
        && (equals(p2,t1.p1) || equals(p2,t1.p2) || equals(p2,t1.p3))
        && (equals(p1,t2.p1) || equals(p1,t2.p2) || equals(p1,t2.p3))
        && (equals(p2,t2.p1) || equals(p2,t2.p2) || equals(p2,t2.p3));
}

template
<
    typename Point
>
std::vector<std::pair<std::size_t,std::size_t>> flip_if_not_locally_delaunay(std::vector<triangle<Point>>& triangles, std::size_t ti1, std::size_t ti2)
{
    triangle<Point>& t1 = triangles[ti1];
    if(std::find(t1.adjacent_indices.begin(),t1.adjacent_indices.end(),ti2)==t1.adjacent_indices.end())
        return {};
    triangle<Point>& t2 = triangles[ti2];
    Point *p1, *p2, *p3, *p4;
    if(!equals(t1.p1,t2.p1) && !equals(t1.p1,t2.p2) && !equals(t1.p1, t2.p3)) {
        p4 = &t1.p1;
        p1 = &t1.p2;
        p3 = &t1.p3;
    }
    else if(!equals(t1.p2,t2.p1) && !equals(t1.p2,t2.p2) && !equals(t1.p2, t2.p3)) {
        p4 = &t1.p2;
        p1 = &t1.p3;
        p3 = &t1.p1;
    }
    else {
        p4 = &t1.p3;
        p1 = &t1.p1;
        p3 = &t1.p2;
    }
    if(!equals(t2.p1,*p1) && !equals(t2.p1,*p3))
        p2 = &t2.p1;
    else if(!equals(t2.p2,*p1) && !equals(t2.p2,*p3))
        p2 = &t2.p2;
    else
        p2 = &t2.p3;
    bool loc_delaunay = distance(circumcircle_center(*p1,*p2,*p3),*p4)>circumcircle_diameter(*p1,*p2,*p3)/2;
    if(loc_delaunay)
        return {};
    std::size_t t1a1=-1, t2a1=-1;
    for(const auto& i : t1.adjacent_indices) {
        if(i!=ti2)
            if(adjacent_at_edge(t1, triangles[i], *p3, *p4))
                t1a1 = i;
    }
    for(const auto& i : t2.adjacent_indices) {
        if(i!=ti1)
            if(adjacent_at_edge(t2, triangles[i], *p1, *p2))
                t2a1 = i;
    }
    Point temp;
    assign(temp,*p3);
    assign(*p3,*p2);
    if(equals(t2.p1,temp))
        assign(t2.p2,*p4);
    else if(equals(t2.p2,temp))
        assign(t2.p3,*p4);
    else
        assign(t2.p1,*p4);
    if(t1a1!=-1 && t2a1!=-1) {
        std::replace(triangles[t1a1].adjacent_indices.begin(),triangles[t1a1].adjacent_indices.end(),ti1,ti2);
        std::replace(t1.adjacent_indices.begin(),t1.adjacent_indices.end(),t1a1,t2a1);
        std::replace(triangles[t2a1].adjacent_indices.begin(),triangles[t2a1].adjacent_indices.end(),ti2,ti1);
        std::replace(t2.adjacent_indices.begin(),t2.adjacent_indices.end(),t2a1,t1a1);
    }
    else if(t1a1!=-1) {
        std::replace(triangles[t1a1].adjacent_indices.begin(),triangles[t1a1].adjacent_indices.end(),ti1,ti2);
        t1.adjacent_indices.erase(std::find(t1.adjacent_indices.begin(),t1.adjacent_indices.end(),t1a1));
        t2.adjacent_indices.push_back(t1a1);
    }
    else if(t2a1!=-1) {
        std::replace(triangles[t2a1].adjacent_indices.begin(),triangles[t2a1].adjacent_indices.end(),ti2,ti1);
        t2.adjacent_indices.erase(std::find(t2.adjacent_indices.begin(),t2.adjacent_indices.end(),t2a1));
        t1.adjacent_indices.push_back(t2a1);
    }
    std::vector<std::pair<std::size_t,std::size_t>> outer_edges;
    for(const auto& t : {ti1,ti2}) {
        for(const auto& ta : triangles[t].adjacent_indices)
            if(ta != ti1 && ta != ti2)
                outer_edges.push_back(std::pair<std::size_t,std::size_t>(t,ta));
    }
    return outer_edges;
}

template
<
    typename MultiPoint
>
std::pair<std::vector<triangle<typename boost::range_value<MultiPoint>::type>>,std::vector<model::segment<typename boost::range_value<MultiPoint>::type>>> shull(const MultiPoint& input, bool skip_delaunay = false, double eps=0.0001)
{
    typedef typename boost::range_value<MultiPoint>::type Point;
    typedef triangle<Point> Triangle;
    std::vector<Point> points;
    points.reserve(boost::size(input));
    for(const auto& in : input)
        points.push_back(in);
    //Step 2
    std::sort(points.begin()+1, points.end(), [&points](const Point& p1, const Point& p2) {
       auto d1 = comparable_distance(points[0],p1);
       auto d2 = comparable_distance(points[0],p2);
       if(d1<d2)
           return true;
       else if(d2<d1)
           return false;
       if(get<0>(p1)<get<0>(p2) || (get<0>(p1)==get<0>(p2) && get<1>(p1)<get<1>(p2) ))
           return true;
       return false;});
    for(auto it=points.end()-1;it!=points.begin();--it) //remove almost-duplicates
        if(distance(*it,*(it-1))<=eps)
            it = points.erase(it);
    //Step 4
    double min_circumcircle_diameter = circumcircle_diameter(points[0],points[1],points[2]);
    std::size_t mcd_index = 2;
    for(std::size_t i=3;i<points.size();++i) {
        auto d = circumcircle_diameter(points[0],points[1],points[i]);
        if(d<min_circumcircle_diameter) {
            mcd_index=i;
            min_circumcircle_diameter=d;
        }
        if(distance(points[0],points[i])>min_circumcircle_diameter)
            break;
    }
    std::swap(points[2],points[mcd_index]);
    Point C = circumcircle_center(points[0], points[1], points[2]);

    //Step 5
    typedef typename model::segment<Point> Segment;
    typedef typename std::pair<Segment,std::size_t> indexed_segment;
    if(!is_left_of(points[2], Segment{points[0],points[1]}))
        std::swap(points[1],points[2]);
    std::sort(points.begin()+2, points.end(), [&C](const Point& p1, const Point& p2) {
       return comparable_distance(C,p1) < comparable_distance(C,p2);
    });

    std::vector<indexed_segment> outer_hull = {indexed_segment(Segment{points[0],points[1]},0),indexed_segment(Segment{points[1],points[2]},0),indexed_segment(Segment{points[2],points[0]},0)};
    std::vector<Triangle> triangles = {Triangle{points[0],points[1],points[2],{}}};

    auto outer_hull_pred = [](const indexed_segment& s1, const indexed_segment& s2) { return angle(s1.first)<angle(s2.first); };
    //Step 6
    std::sort(outer_hull.begin(), outer_hull.end(),outer_hull_pred);

    //Step 7
    for(std::size_t i = 3; i<points.size(); ++i) {
        const Point& p = points[i];
        std::size_t end_tri = triangles.size();
        //The following could (probably) be done faster with binary searches (for large hulls)
        auto begin = std::find_if(outer_hull.begin(), outer_hull.end(), [&p](const indexed_segment& s) {
            return !is_left_of(p,s.first);
        });
        auto end = std::find_if(begin, outer_hull.end(), [&p](const indexed_segment& s) {
            return is_left_of(p,s.first);
        });
        for(auto it = begin; it != end; ++it) {
            triangles.push_back(Triangle{it->first.first, p, it->first.second,{it->second}});
            triangles[it->second].adjacent_indices.push_back(triangles.size()-1);
            if(it!=begin) {
                triangles[triangles.size()-1].adjacent_indices.push_back(triangles.size()-2);
                triangles[triangles.size()-2].adjacent_indices.push_back(triangles.size()-1);
            }
        }
        indexed_segment new_outer1(Segment(begin->first.first,p),end_tri), new_outer2(Segment(p,(end-1)->first.second),triangles.size()-1);
        if(begin==outer_hull.begin()) {
            auto begin2 = std::find_if(end, outer_hull.end(), [&p](const indexed_segment& s) {
                return !is_left_of(p,s.first);
            });

            if(begin2 != outer_hull.end()) {
                std::size_t end_tri2 = triangles.size();
                for(auto it = begin2; it != outer_hull.end(); ++it) {
                    triangles.push_back(Triangle{it->first.first, p, it->first.second,{it->second}});
                    triangles[it->second].adjacent_indices.push_back(triangles.size()-1);
                    if(it!=begin2) {
                        triangles[triangles.size()-1].adjacent_indices.push_back(triangles.size()-2);
                        triangles[triangles.size()-2].adjacent_indices.push_back(triangles.size()-1);
                    }
                }
                triangles.back().adjacent_indices.push_back(end_tri);
                triangles[end_tri].adjacent_indices.push_back(triangles.size()-1);
                new_outer1 = indexed_segment(Segment(begin2->first.first,p), end_tri2);
                outer_hull.erase(begin2, outer_hull.end());
            }
        }
        outer_hull.erase(begin, end);
        for(const auto& s : {new_outer1, new_outer2})
            outer_hull.insert(std::upper_bound(outer_hull.begin(), outer_hull.end(), s, outer_hull_pred),s);
    }

    //Step 9
    if(!skip_delaunay) {
        std::stack<std::pair<std::size_t,std::size_t>> L;
        for(std::size_t i=0; i < triangles.size(); ++i)
            for(const auto& a : triangles[i].adjacent_indices)
                if(i<a)
                    L.emplace(i,a);
        while(!L.empty()) {
            auto edge = L.top();
            L.pop();
            auto new_edges = flip_if_not_locally_delaunay<Point>(triangles, edge.first, edge.second);
            for(const auto& ne : new_edges)
                L.push(ne);
        }
    }
    std::vector<Segment> no_index_hull;
    no_index_hull.reserve(outer_hull.size());
    for(const auto& si : outer_hull)
        no_index_hull.push_back(si.first);
    return std::make_pair(triangles,no_index_hull);
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  int rows=5, cols=5;
MatrixXf m(rows,cols);
m << (Matrix3f() << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished(),
     MatrixXf::Zero(3,cols-3),
     MatrixXf::Zero(rows-3,3),
     MatrixXf::Identity(rows-3,cols-3);
cout << m;

  return 0;
}


/**
 * @file softmax_regression_test.cpp
 * @author Siddharth Agrawal
 *
 * Test the SoftmaxRegression class.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/softmax_regression/softmax_regression.hpp>

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

using namespace mlpack;
using namespace mlpack::regression;
using namespace mlpack::distribution;
using namespace mlpack::optimization;

BOOST_AUTO_TEST_SUITE(SoftmaxRegressionTest);

BOOST_AUTO_TEST_CASE(SoftmaxRegressionFunctionEvaluate)
{
  const size_t points = 1000;
  const size_t trials = 50;
  const size_t inputSize = 10;
  const size_t numClasses = 5;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for(size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  // Create a SoftmaxRegressionFunction. Regularization term ignored.
  SoftmaxRegressionFunction srf(data, labels, numClasses, 0);

  // Run a number of trials.
  for(size_t i = 0; i < trials; i++)
  {
    // Create a random set of parameters.
    arma::mat parameters;
    parameters.randu(numClasses, inputSize);

    double logLikelihood = 0;

    // Compute error for each training example.
    for(size_t j = 0; j < points; j++)
    {
      arma::mat hypothesis, probabilities;

      hypothesis = arma::exp(parameters * data.col(j));
      probabilities = hypothesis / arma::accu(hypothesis);

      logLikelihood += log(probabilities(labels(j), 0));
    }
    logLikelihood /= points;

    // Compare with the value returned by the function.
    BOOST_REQUIRE_CLOSE(srf.Evaluate(parameters), -logLikelihood, 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(SoftmaxRegressionFunctionRegularizationEvaluate)
{
  const size_t points = 1000;
  const size_t trials = 50;
  const size_t inputSize = 10;
  const size_t numClasses = 5;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for(size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  // 3 objects for comparing regularization costs.
  SoftmaxRegressionFunction srfNoReg(data, labels, numClasses, 0);
  SoftmaxRegressionFunction srfSmallReg(data, labels, numClasses, 1);
  SoftmaxRegressionFunction srfBigReg(data, labels, numClasses, 20);

  // Run a number of trials.
  for (size_t i = 0; i < trials; i++)
  {
    // Create a random set of parameters.
    arma::mat parameters;
    parameters.randu(numClasses, inputSize);

    double wL2SquaredNorm;
    wL2SquaredNorm = arma::accu(parameters % parameters);

    // Calculate regularization terms.
    const double smallRegTerm = 0.5 * wL2SquaredNorm;
    const double bigRegTerm = 10 * wL2SquaredNorm;

    BOOST_REQUIRE_CLOSE(srfNoReg.Evaluate(parameters) + smallRegTerm,
        srfSmallReg.Evaluate(parameters), 1e-5);
    BOOST_REQUIRE_CLOSE(srfNoReg.Evaluate(parameters) + bigRegTerm,
        srfBigReg.Evaluate(parameters), 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(SoftmaxRegressionFunctionGradient)
{
  const size_t points = 1000;
  const size_t inputSize = 10;
  const size_t numClasses = 5;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for(size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  // 2 objects for 2 terms in the cost function. Each term contributes towards
  // the gradient and thus need to be checked independently.
  SoftmaxRegressionFunction srf1(data, labels, numClasses, 0);
  SoftmaxRegressionFunction srf2(data, labels, numClasses, 20);

  // Create a random set of parameters.
  arma::mat parameters;
  parameters.randu(numClasses, inputSize);

  // Get gradients for the current parameters.
  arma::mat gradient1, gradient2;
  srf1.Gradient(parameters, gradient1);
  srf2.Gradient(parameters, gradient2);

  // Perturbation constant.
  const double epsilon = 0.0001;
  double costPlus1, costMinus1, numGradient1;
  double costPlus2, costMinus2, numGradient2;

  // For each parameter.
  for (size_t i = 0; i < numClasses; i++)
  {
    for (size_t j = 0; j < inputSize; j++)
    {
      // Perturb parameter with a positive constant and get costs.
      parameters(i, j) += epsilon;
      costPlus1 = srf1.Evaluate(parameters);
      costPlus2 = srf2.Evaluate(parameters);

      // Perturb parameter with a negative constant and get costs.
      parameters(i, j) -= 2 * epsilon;
      costMinus1 = srf1.Evaluate(parameters);
      costMinus2 = srf2.Evaluate(parameters);

      // Compute numerical gradients using the costs calculated above.
      numGradient1 = (costPlus1 - costMinus1) / (2 * epsilon);
      numGradient2 = (costPlus2 - costMinus2) / (2 * epsilon);

      // Restore the parameter value.
      parameters(i, j) += epsilon;

      // Compare numerical and backpropagation gradient values.
      BOOST_REQUIRE_CLOSE(numGradient1, gradient1(i, j), 1e-2);
      BOOST_REQUIRE_CLOSE(numGradient2, gradient2(i, j), 1e-2);
    }
  }
}

BOOST_AUTO_TEST_CASE(SoftmaxRegressionTwoClasses)
{
  const size_t points = 1000;
  const size_t inputSize = 3;
  const size_t numClasses = 2;
  const double lambda = 0.5;

  // Generate two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);

  for (size_t i = 0; i < points / 2; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 2; i < points; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }

  // Train softmax regression object.
  SoftmaxRegression<> sr(data, labels, numClasses, lambda);

  // Compare training accuracy to 100.
  const double acc = sr.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(acc, 100.0, 0.5);

  // Create test dataset.
  for (size_t i = 0; i < points / 2; i++)
  {
    data.col(i) = g1.Random();
    labels(i) =  0;
  }
  for (size_t i = points / 2; i < points; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }

  // Compare test accuracy to 100.
  const double testAcc = sr.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(testAcc, 100.0, 0.6);
}

BOOST_AUTO_TEST_CASE(SoftmaxRegressionFitIntercept)
{
  // Generate a two-Gaussian dataset,
  // which can't be separated without adding the intercept term.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(3, 1000);
  arma::Row<size_t> responses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Now train a logistic regression object on it.
  SoftmaxRegression<> lr(data, responses, 2, 0.01, true);

  // Ensure that the error is close to zero.
  const double acc = lr.ComputeAccuracy(data, responses);
  BOOST_REQUIRE_CLOSE(acc, 100.0, 2.0);

  // Create a test set.
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Ensure that the error is close to zero.
  const double testAcc = lr.ComputeAccuracy(data, responses);
  BOOST_REQUIRE_CLOSE(testAcc, 100.0, 2.0);
}

BOOST_AUTO_TEST_CASE(SoftmaxRegressionMultipleClasses)
{
  const size_t points = 5000;
  const size_t inputSize = 5;
  const size_t numClasses = 5;
  const double lambda = 0.5;

  // Generate five-Gaussian dataset.
  arma::mat identity = arma::eye<arma::mat>(5, 5);
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0 2.0 2.0"), identity);
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0 2.0 2.0"), identity);
  GaussianDistribution g3(arma::vec("3.0 2.0 7.0 0.0 5.0"), identity);
  GaussianDistribution g4(arma::vec("4.0 1.0 1.0 2.0 7.0"), identity);
  GaussianDistribution g5(arma::vec("1.0 0.0 1.0 8.0 3.0"), identity);

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);

  for (size_t i = 0; i < points / 5; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 5; i < (2 * points) / 5; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }
  for (size_t i = (2 * points) / 5; i < (3 * points) / 5; i++)
  {
    data.col(i) = g3.Random();
    labels(i) = 2;
  }
  for (size_t i = (3 * points) / 5; i < (4 * points) / 5; i++)
  {
    data.col(i) = g4.Random();
    labels(i) = 3;
  }
  for (size_t i = (4 * points) / 5; i < points; i++)
  {
    data.col(i) = g5.Random();
    labels(i) = 4;
  }

  // Train softmax regression object.
  SoftmaxRegression<> sr(data, labels, numClasses, lambda);

  // Compare training accuracy to 100.
  const double acc = sr.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(acc, 100.0, 2.0);

  // Create test dataset.
  for (size_t i = 0; i < points / 5; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 5; i < (2 * points) / 5; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }
  for (size_t i = (2 * points) / 5; i < (3 * points) / 5; i++)
  {
    data.col(i) = g3.Random();
    labels(i) = 2;
  }
  for (size_t i = (3 * points) / 5; i < (4 * points) / 5; i++)
  {
    data.col(i) = g4.Random();
    labels(i) = 3;
  }
  for (size_t i = (4 * points) / 5; i < points; i++)
  {
    data.col(i) = g5.Random();
    labels(i) = 4;
  }

  // Compare test accuracy to 100.
  const double testAcc = sr.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(testAcc, 100.0, 2.0);
}

BOOST_AUTO_TEST_CASE(SoftmaxRegressionTrainTest)
{
  // Make sure a SoftmaxRegression object trained with Train() operates the same
  // as a SoftmaxRegression object trained in the constructor.
  arma::mat dataset = arma::randu<arma::mat>(5, 1000);
  arma::Row<size_t> labels(1000);
  for (size_t i = 0; i < 500; ++i)
    labels[i] = size_t(0.0);
  for (size_t i = 500; i < 1000; ++i)
    labels[i] = size_t(1.0);

  // This should be the same as the default parameters given by
  // SoftmaxRegression.
  SoftmaxRegressionFunction srf(dataset, labels, 2, 0.0001, false);
  L_BFGS<SoftmaxRegressionFunction> lbfgs(srf);
  SoftmaxRegression<> sr(lbfgs);

  SoftmaxRegression<> sr2(dataset.n_rows, 2);
  sr2.Parameters() = srf.GetInitialPoint(); // Start from the same place.
  sr2.Train(dataset, labels, 2);

  // Ensure that the parameters are the same.
  BOOST_REQUIRE_EQUAL(sr.Parameters().n_rows, sr2.Parameters().n_rows);
  BOOST_REQUIRE_EQUAL(sr.Parameters().n_cols, sr2.Parameters().n_cols);
  for (size_t i = 0; i < sr.Parameters().n_elem; ++i)
  {
    if (std::abs(sr.Parameters()[i]) < 1e-4)
      BOOST_REQUIRE_SMALL(sr2.Parameters()[i], 1e-4);
    else
      BOOST_REQUIRE_CLOSE(sr.Parameters()[i], sr2.Parameters()[i], 1e-4);
  }
}

BOOST_AUTO_TEST_CASE(SoftmaxRegressionOptimizerTrainTest)
{
  // The same as the previous test, just passing in an instantiated optimizer.
  arma::mat dataset = arma::randu<arma::mat>(5, 1000);
  arma::Row<size_t> labels(1000);
  for (size_t i = 0; i < 500; ++i)
    labels[i] = size_t(0.0);
  for (size_t i = 500; i < 1000; ++i)
    labels[i] = size_t(1.0);

  SoftmaxRegressionFunction srf(dataset, labels, 2, 0.01, true);
  L_BFGS<SoftmaxRegressionFunction> lbfgs(srf);
  SoftmaxRegression<> sr(lbfgs);

  SoftmaxRegression<> sr2(dataset.n_rows, 2, true);
  L_BFGS<SoftmaxRegressionFunction> lbfgs2(srf);
  sr2.Parameters() = srf.GetInitialPoint();
  sr2.Train(lbfgs2);

  // Ensure that the parameters are the same.
  BOOST_REQUIRE_EQUAL(sr.Parameters().n_rows, sr2.Parameters().n_rows);
  BOOST_REQUIRE_EQUAL(sr.Parameters().n_cols, sr2.Parameters().n_cols);
  for (size_t i = 0; i < sr.Parameters().n_elem; ++i)
  {
    if (std::abs(sr.Parameters()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(sr2.Parameters()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(sr.Parameters()[i], sr2.Parameters()[i], 1e-5);
  }
}

BOOST_AUTO_TEST_SUITE_END();


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

// Modified Jonker-Volgenant Algorithm
// DF Crouse. On implementing 2D rectangular assignment algorithms.
// IEEE Transactions on Aerospace and Electronic Systems, 52(4):1679-1696, August 2016,

#pragma once

#include "AlgorithmUtils.hpp"
#include "FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class Assign2D
{
public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXXd = Eigen::ArrayXXd;
  using ArrayXi = Eigen::ArrayXi;
  static const index UNASSIGNED = -1;
  bool process(Eigen::Ref<const Eigen::ArrayXXd> costMatrix,
               Eigen::Ref<Eigen::ArrayXi>        result)
  {
    using namespace std;
    using namespace Eigen;
    double minCost = costMatrix.minCoeff();
    if(minCost > 0) minCost = 0;
    mCost = costMatrix - minCost;
    if(mCost.cols() > mCost.rows()){
      mCost.transposeInPlace();
      mTransposed = true;
    }
    else mTransposed = false;

    mRow2Col = ArrayXi::Constant(mCost.rows(), UNASSIGNED);
    mCol2Row = ArrayXi::Constant(mCost.cols(), UNASSIGNED);
    mV = ArrayXd::Zero(mCost.rows());
    mU = ArrayXd::Zero(mCost.cols());
    for (index c = 0; c < mCost.cols(); c++){
      index sink = shortestPath(c);
      if(sink == UNASSIGNED) return false;
      index j = sink;
      index h;
      while(true){
        index i = mPred(j);
        mRow2Col(j) = i;
        h = mCol2Row(i);
        mCol2Row(i) = j;
        j = h;
        if(i == c) break;
      }
    }
    result = mTransposed?mCol2Row:mRow2Col;
    return true;
  }

  index shortestPath(index c){
    using namespace std;
    mPred = ArrayXi::Zero(mCost.rows());
    ArrayXi scannedCols = ArrayXi::Zero(mCost.cols());
    ArrayXi scannedRows = ArrayXi::Zero(mCost.rows());
    vector<int> row2Scan(mCost.rows());
    iota(row2Scan.begin(), row2Scan.end(), 0);
    index nRows2Scan = mCost.rows();
    index sink = UNASSIGNED;
    double delta = 0;
    index currentCol = c;
    index currentRow, currentClosestRow, closestRow;
    double minVal, reducedCost;
    ArrayXd shortestPathCost = ArrayXd::Constant(mCost.rows(), infinity);
    while(sink == UNASSIGNED){
      scannedCols(currentCol) = 1;
      minVal = infinity;
      for(index r = 0; r < nRows2Scan; r++){
        currentRow = row2Scan[r];
        reducedCost = delta
                    + mCost(currentRow, currentCol)
                    - mU(currentCol) - mV(currentRow);
        if(reducedCost < shortestPathCost(currentRow)){
          mPred(currentRow) = currentCol;
          shortestPathCost(currentRow) = reducedCost;
        }
        if(shortestPathCost(currentRow) < minVal){
          minVal = shortestPathCost(currentRow);
          currentClosestRow = r;
        }
      }
      if(isinf(minVal)) return UNASSIGNED;
      closestRow = row2Scan[currentClosestRow];
      scannedRows(closestRow) = 1;
      nRows2Scan--;
      row2Scan.erase(row2Scan.begin() + currentClosestRow);
      delta = shortestPathCost(closestRow);
      if(mRow2Col(closestRow) == UNASSIGNED) sink = closestRow;
      else currentCol = mRow2Col(closestRow);
    }
    mU(c) = mU(c) + delta;
    for(index i = 0; i < mCost.cols(); i++){
      if(i !=c && scannedCols(i) != 0){
        mU(i) = mU(i) + delta - shortestPathCost(mCol2Row(i));
      }
    }

    for(index i = 0; i < mCost.rows(); i++){
      if(scannedRows(i) != 0){
        mV(i) = mV(i) - delta + shortestPathCost(i);
      }
    }
    return sink;
}
private:
  ArrayXXd mCost;
  ArrayXi  mRow2Col;
  ArrayXi  mCol2Row;
  ArrayXd  mU;
  ArrayXd  mV;
  ArrayXi mPred;
  bool mTransposed{false};
};
} // namespace algorithm
} // namespace fluid


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <string>

using namespace Eigen;
using namespace std;

double amips_param;
double c1_param;
double c2_param;
double d1_param;

Vector2d energy_derivative(int type, Vector2d S)
{

    Vector2d es;
    double J;
    double l1;
    double l2;

    switch(type)
    {
        case 0: //arap
            es[0] = 2 * (S[0] - 1);
            es[1] = 2 * (S[1] - 1);
            break;

        case 1: // mips
            es[0] = 1.0 / S[1] - S[1] / (S[0] * S[0]);
            es[1] = 1.0 / S[0] - S[0] / (S[1] * S[1]);
            //es[0] *= 100;
            //es[1] *= 100;
            break;

        case 2: // iso
            es[0] = 2 * S[0] - 2.0 / (S[0] * S[0] * S[0]);
            es[1] = 2 * S[1] - 2.0 / (S[1] * S[1] * S[1]);
            break;

        case 3: // amips
            es[0] = amips_param * exp(amips_param * (S[0] / S[1] + S[1] / S[0])) * (1.0 / S[1] - S[1] / (S[0] * S[0]));
            es[1] = amips_param * exp(amips_param * (S[0] / S[1] + S[1] / S[0])) * (1.0 / S[0] - S[0] / (S[1] * S[1]));
            break;

        case 4: // conf
            es[0] = 2 * S[0] / (S[1] * S[1]);
            es[1] = -2 * S[0] * S[0] / (S[1] * S[1] * S[1]);
            break;

        case 5: // gmr
						//            J = S[0] * S[1];
						//            l1 = S[0] * S[0] + S[1] * S[1];
						//            l2 = S[0] * S[0] * S[1] * S[1];
						//
						//            es[0] = c1_param * (-2.0 / 3.0 * pow(J, -5.0 / 3.0) * S[1] * l1 + pow(J, -2.0 / 3.0) * 2 * S[0])
						//                    + c2_param * (-4.0 / 3.0 * pow(J, -7.0 / 3.0) * S[1] * l2 + pow(J, -4.0 / 3.0) * 2 * S[1] * S[1] * S[0])
						//                    + d1_param * 2 * (J - 1) * S[1];
						//
						//            es[1] = c1_param * (-2.0 / 3.0 * pow(J, -5.0 / 3.0) * S[0] * l1 + pow(J, -2.0 / 3.0) * 2 * S[1])
						//                    + c2_param * (-4.0 / 3.0 * pow(J, -7.0 / 3.0) * S[0] * l2 + pow(J, -4.0 / 3.0) * 2 * S[0] * S[0] * S[1])
						//                    + d1_param * 2 * (J - 1) * S[0];

						es[0] = c1_param * (1.0 / S[1] - S[1] / (S[0] * S[0])) - d1_param *2.*S[1]*(1.0 - S[0]*S[1]);
						es[1] = c1_param * (1.0 / S[0] - S[0] / (S[1] * S[1])) - d1_param *2.*S[0]*(1.0 - S[0]*S[1]);

						break;

        case 6: // olg

            if(S[0] > 1.0 / S[1])
            {
                es[0] = 1;
                es[1] = 0;
            }else{
                es[0] = 0;
                es[1] = -1.0 / (S[1] * S[1]);
            }

            break;
    }

    return es;

}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *output_mex, *J_value_mex, *JT_value_mex, *wu_mex, *bu_mex;
    const int *dims;
    double *tri_num, *X_g_inv, *tri_areas, *obj_tri, *q_target, *type, *amips_s, *F_dot, *ver_num, *c1_g, *c2_g, *d1_g, *J_index, *JT_index, *JTJ_info, *x2u;
    double *output, *J_value, *JT_value, *wu, *bu;

    tri_num = mxGetPr(prhs[0]);
    X_g_inv = mxGetPr(prhs[1]);
    tri_areas = mxGetPr(prhs[2]);
    obj_tri = mxGetPr(prhs[3]);
    q_target = mxGetPr(prhs[4]);
    type = mxGetPr(prhs[5]);
    amips_s = mxGetPr(prhs[6]);
    F_dot = mxGetPr(prhs[7]);
    ver_num = mxGetPr(prhs[8]);
    c1_g = mxGetPr(prhs[9]);
    c2_g = mxGetPr(prhs[10]);
    d1_g = mxGetPr(prhs[11]);
    J_index = mxGetPr(prhs[12]);
    JT_index = mxGetPr(prhs[13]);
    JTJ_info = mxGetPr(prhs[14]);
    x2u = mxGetPr(prhs[15]);

    int tri_n = tri_num[0];
    int ver_n = ver_num[0];
    int energy_type = type[0];
    amips_param = amips_s[0];
    c1_param = c1_g[0];
    c2_param = c2_g[0];
    d1_param = d1_g[0];
    int J_rn = JTJ_info[0];
    int J_cn = JTJ_info[1];
    int JT_rn = JTJ_info[2];
    int JT_cn = JTJ_info[3];

    output_mex = plhs[0] = mxCreateDoubleMatrix(2 * ver_n, 1, mxREAL);
    J_value_mex = plhs[1] = mxCreateDoubleMatrix(J_rn * J_cn, 1, mxREAL);
    JT_value_mex = plhs[2] = mxCreateDoubleMatrix(JT_rn * JT_cn, 1, mxREAL);
    wu_mex = plhs[3] = mxCreateDoubleMatrix(tri_n, 1, mxREAL);
    bu_mex = plhs[4] = mxCreateDoubleMatrix(tri_n, 1, mxREAL);

    output = mxGetPr(output_mex);
    J_value = mxGetPr(J_value_mex);
    JT_value = mxGetPr(JT_value_mex);
    wu = mxGetPr(wu_mex);
    bu = mxGetPr(bu_mex);

    memset(J_value, 0, J_rn * J_cn * sizeof(double));
    memset(JT_value, 0, JT_rn * JT_cn * sizeof(double));
    memset(output, 0, 2 * ver_n * sizeof(double));

    vector<int> offset;

    offset.resize(2 * ver_n, 0);

    int tri[3];
    double tri_area;

    Matrix2d B, X_f, A, F_d;
    Vector2d S, u0, u1, v0, v1;
    Matrix2d U, V;

    Vector2d es;
    Vector2d cs;

    double es1, es2;
    double d1, d2;
    double dphi;

    for(int i = 0; i < tri_n; i++)
    {
        tri[0] = obj_tri[i] - 1;
        tri[1] = obj_tri[i + tri_n] - 1;
        tri[2] = obj_tri[i + 2 * tri_n] - 1;
        tri_area = tri_areas[i];

        B(0, 0) = X_g_inv[i];
        B(0, 1) = X_g_inv[i + tri_n * 2];
        B(1, 0) = X_g_inv[i + tri_n];
        B(1, 1) = X_g_inv[i + tri_n * 3];

        X_f(0, 0) = q_target[2 * tri[1]] - q_target[2 * tri[0]];
        X_f(1, 0) = q_target[2 * tri[1] + 1] - q_target[2 * tri[0] + 1];
        X_f(0, 1) = q_target[2 * tri[2]] - q_target[2 * tri[0]];
        X_f(1, 1) = q_target[2 * tri[2] + 1] - q_target[2 * tri[0] + 1];

        A = X_f * B;

        JacobiSVD<Matrix2d> svd(A, ComputeFullU | ComputeFullV);

        S = svd.singularValues();
        U = svd.matrixU();
        V = svd.matrixV();

        if(A.determinant() <= 0)
        {
            //mexPrintf("inverted\n");
        }

        bu[i] = S[0] * S[1];

        es = energy_derivative(energy_type, S);

        cs = S;

        es1 = es[0];
        es2 = es[1];

        ////////////////////////////////////////////

        wu[i] = 0;

        u0 = U.col(0);
        u1 = U.col(1);
        v0 = V.col(0);
        v1 = V.col(1);

        for(int j = 0; j < 6; j++)
        {

            F_d(0, 0) = F_dot[i + j * tri_n];
            F_d(0, 1) = F_dot[i + j * tri_n + 6 * 2 * tri_n];
            F_d(1, 0) = F_dot[i + j * tri_n + 6 * tri_n];
            F_d(1, 1) = F_dot[i + j * tri_n + 6 * 3 * tri_n];

            d1 = u0.transpose() * F_d * v0;
            d2 = u1.transpose() * F_d * v1;

            dphi = d1 * es1 + d2 * es2;

            int cache_1 = 2 * tri[j / 2] + (j % 2);
            int cache_2 = x2u[cache_1];

            output[cache_1] += tri_area * dphi;

            if(cache_2 > 0)
            {
                int idx = cache_2 - 1;

                double value = d1 * cs[1] + d2 * cs[0];

                J_value[idx * J_cn + offset[cache_1]] = value;

                offset[cache_1]++;

                JT_value[i * JT_cn + j] = value;

                wu[i] += value * value;
            }
        }
    }

    return;

}


#include "Kinematics.h"
#include <Eigen/SVD>

using namespace MotionControl;

Matrix<double,3,3> Kinematics::Rodrigues(Matrix<double,3,1> a, double q)
{
	return AngleAxisd(q,a).toRotationMatrix();
}

Matrix<double,3,1> Kinematics::rot2omega(Matrix<double,3,3> R)
{
	double alpha = (R(0,0)+R(1,1)+R(2,2)-1)/2;
	double th;
	Matrix<double,3,1> vector_R(Matrix<double,3,1>::Zero());

	if(fabs(alpha-1) < eps)
		return Matrix<double,3,1>::Zero();

	th = std::cos(alpha);
	vector_R << R(2,1)-R(1,2), R(0,2)-R(2,0), R(1,0)-R(0,1);
	return 0.5*th/std::sin(th)*vector_R;
}

Eigen::Matrix<double,3,3> Kinematics::computeMatrixFromAngles(double roll, double pitch, double yaw)
{
	Eigen::Matrix<double,3,3> R;

	R(0,0) = std::cos(pitch) * std::cos(yaw) - std::sin(roll) * std::sin(pitch) * std::sin(yaw);
	R(0,1) = -std::cos(roll) * std::sin(yaw);
	R(0,2) = std::sin(roll) * std::cos(yaw) + std::sin(roll) * std::cos(pitch) * std::sin(yaw);
	R(1,0) = std::cos(pitch) * std::sin(yaw) + std::sin(roll) * std::sin(pitch) * std::cos(yaw);
	R(1,1) = std::cos(roll) * std::cos(yaw);
	R(1,2) = std::sin(pitch) * std::sin(yaw) - std::sin(roll) * std::cos(pitch) * std::cos(yaw);
	R(2,0) = -std::cos(roll) * std::sin(pitch);
	R(2,1) = std::sin(roll);
	R(2,2) = std::cos(roll) * std::cos(pitch);

	return R;
}

void Kinematics::computeAnglesFromMatrix(Eigen::Matrix<double,3,3> R, double &roll, double &pitch, double &yaw)
{
	float threshold = 0.001;
	if(abs(R(2,1) - 1.0) < threshold){ // R(2,1) = std::sin(x) = 1の時
		roll 	= M_PI / 2;
		pitch = 0;
		yaw 	= std::atan2(R(1,0), R(0,0));
	}else if(abs(R(2,1) + 1.0) < threshold){ // R(2,1) = std::sin(x) = -1の時
		roll	= - M_PI / 2;
		pitch = 0;
		yaw		= std::atan2(R(1,0), R(0,0));
	}else{
		roll	= std::sin(R(2,1));
		pitch	= std::atan2(-R(2,0), R(2,2));
		yaw		= std::atan2(-R(0,1), R(1,1));
	}
}

vector<int> Kinematics::FindRoute(int to)
{
	vector<int> idx;
	int link_num = to;

	while(link_num != 0)
	{
		idx.push_back(link_num);
		link_num = ulink[link_num].parent;
	}
	reverse(idx.begin(), idx.end());
	return idx;
}

Matrix<double,6,1> Kinematics::calcVWerr(Link Cref, Link Cnow)
{
	Matrix<double,3,1> perr = Cref.p - Cnow.p;
	Matrix<double,3,3> Rerr = Cref.R - Cnow.R;
	Matrix<double,3,1> werr = Cnow.R * rot2omega(Rerr);
	Matrix<double,6,1> err;

	err << perr,werr;
	return err;
}

void Kinematics::calcForwardKinematics(int rootlink)
{
	if(rootlink == -1) return;
	if(rootlink != 0)
	{
		int parent = ulink[rootlink].parent;
		ulink[rootlink].p = ulink[parent].R * ulink[rootlink].b + ulink[parent].p;
		ulink[rootlink].R = ulink[parent].R * Rodrigues(ulink[rootlink].a, ulink[rootlink].q);
	}
	calcForwardKinematics(ulink[rootlink].sister);
	calcForwardKinematics(ulink[rootlink].child);
}

// 逆運動学
bool Kinematics::calcInverseKinematics(int to, Link target)
{
	Matrix<double,6,6> J;
	Matrix<double,6,1> dq, err;

	ColPivHouseholderQR<MatrixXd> QR; //QR分解?
	const double lambda = 0.5;
	const int iteration = 100;

	calcForwardKinematics(CC);
	vector<int> idx = FindRoute(to);

	for(int n=0;n<iteration;n++){
		J = calcJacobian(ulink, idx);
		err = calcVWerr(target, ulink[to]);
		if(err.norm() < eps) return true;
		dq = lambda * (J.inverse() * err);
		for(size_t nn=0;nn<idx.size();nn++){
			int j = idx[nn];
			ulink[j].q += dq(nn);
		}
		calcForwardKinematics(CC);
	}
	return false;
}

void Kinematics::MoveJoints( vector<int> idx, MatrixXd dq )
{
	for(int n=0; n<idx.size(); n++)
	{
		int j = idx[n];
		ulink[j].q += dq(n);
	}
}

bool Kinematics::calcLMInverseKinematics(int to, Link target)
{
	MatrixXd J, Jh, dq;
	vector<int> idx = FindRoute(to);
	double wn_pos = 1/0.3;
	double wn_ang = 1/(2*pi);
	double lambda, Ek, Ek2;
	Matrix< double, 6,1 > we, err, gerr;
	we << wn_pos, wn_pos, wn_pos, wn_ang, wn_ang, wn_ang;
	Matrix< double, 6,6 >We = we.array().matrix().asDiagonal();
	MatrixXd Wn = MatrixXd::Identity(idx.size(),idx.size());

	calcForwardKinematics(CC);
	err = calcVWerr( target, ulink[to]);
	Ek = err.transpose() * We * err;

	for(int i = 0; i < 10; i++)
	{

		J = calcJacobian( ulink, idx );
		lambda = Ek + 0.002;
		Jh   = J.transpose() * We * J + Wn * lambda; //Hk + wn
		gerr = J.transpose() * We * err; // gk

		dq = Jh.inverse() * gerr; // new qk
		MoveJoints(idx, dq);

		Ek2 = gerr.transpose() * We * gerr;

		if(Ek2 < 1E-12)
		{
			return true;
		}
		else if( Ek2 < Ek ) Ek = Ek2;
		else
		{
			MoveJoints(idx, -dq);
			return true;
		}
	}
	return false;
}


#include <Eigen/Dense>
#include <iostream>
#include <set>
#include <time.h>
#include <fstream>
#include <vector>
#include <math.h>

using namespace std;
using namespace Eigen;

// N * E = B * T
#define N 128*150	// number of training data samples
#define D 784	// number of features
#define T 150	// total number of iterations per epoch
#define B_size 128	// size of mini-batch
#define ALPHA 128	// 2^7
#define epoch 1
#define SCALING_FACTOR 8192

typedef Matrix<unsigned long long int, Dynamic, Dynamic> MATRIX;
typedef Matrix<double, Dynamic, Dynamic> MATRIXd;
typedef Matrix<unsigned long long int, Dynamic, 1, ColMajor> ColVectorXi64;
typedef Matrix<double, Dynamic, 1, ColMajor> ColVectorXd;
typedef Matrix<double, 1, Dynamic, RowMajor> RowVectorXd;
typedef Matrix<unsigned long long int, 1, Dynamic, RowMajor> RowVectorXi64;

// Simulates dditive shares created by the clients and distributed to the two servers.
void genDataShares(MATRIX& X0, MATRIX& X1, MATRIX& Y0, MATRIX& Y1);

// Initialize W0 and W1 to be random
void initializeW(MATRIX& W0, MATRIX& W1);

// Simulates generation of U, V, Z multiplication triplets
void genUVZmultTriplets(MATRIX& U0, MATRIX& U1, MATRIX& V0, MATRIX& V1, MATRIX& Z0, MATRIX& Z1);

// Simulates generation of V', Z' multiplication triplets
void genVZprimeMultTriplets(MATRIX& UB0, MATRIX& UB1, MATRIX& Vp0, MATRIX& Vp1, MATRIX& Zp0, MATRIX& Zp1);

// Returns set of random indices corresponding to training data for current minibatch(iteration)
set<int> createMiniBatch(set<int>& usedIndices);

// Reconstructs M, by adding MO and M1
void reconstruct(MATRIX M0, MATRIX M1, MATRIX& M);

void maskMatrix(MATRIX Data, MATRIX Mask, MATRIX& Final);

void computeDifference(MATRIX& Dif, MATRIX Y_, MATRIX Y);

int reverse_int(int i);

void read_MNIST_data(bool train, vector<vector<unsigned long long int>>& vec, int& number_of_images, int& number_of_features);

void read_MNIST_labels(bool train, vector<unsigned long long int>& vec);

void read_MNIST_data_test(bool train, vector<vector<double>>& vec, int& number_of_images, int& number_of_features);

void read_MNIST_labels_test(bool train, vector<double>& vec);

void secretShareData(MATRIX& X, MATRIX& Y, MATRIX& X0, MATRIX& X1, MATRIX& Y0, MATRIX& Y1);

int main() {

	cout << unsigned long int(-28145) % 4294967296 << endl;

	// READ TRAINING DATA
	vector<vector<unsigned long long int>> training_data;
	int param_n= N; int param_d= D;
	read_MNIST_data(true, training_data, param_n, param_d);
	MATRIX X(param_n, param_d);
	for(int i = 0; i < training_data.size(); i++){
        X.row(i) << Map<RowVectorXi64>(training_data[i].data(), training_data[i].size());
    }
	X *= SCALING_FACTOR;
	X /= 255;
	//cout << X << endl;
	//cout << X.rows() << " " << X.cols() << endl;
	/*for (int i = 0 ; i < 784 ; ++i) {
		cout << training_data.at(0).at(i) << " ";
	}
	cout << endl<<  X.row(0) << endl;*/

	// READ TRAINING LABEL
	vector<unsigned long long int> training_labels;
	read_MNIST_labels(true, training_labels);
	MATRIX Y(N, 1);
	//for (int i = 0; i < training_labels.size(); ++i) {
	//	Y(i) = training_labels.at(i);
	//}
	Y << Map<ColVectorXi64>(training_labels.data(), training_labels.size());
	Y *= SCALING_FACTOR;
	Y /= 10;

	cout << sizeof(unsigned long long int) << endl;
	cout << sizeof(uint64_t) << endl;

	// READ TESTING DATA
	vector<vector<double> > testing_data;
	int n_;
	//param_n= N; param_d= D;
	read_MNIST_data_test(false, testing_data, n_, param_d);
	MATRIXd Xt(n_, param_d);
	for(int i = 0; i < testing_data.size(); i++){
        Xt.row(i) << Map<RowVectorXd>(testing_data[i].data(), testing_data[i].size());
    }
	//Xt *= SCALING_FACTOR;
	Xt /= 255;

	// READING TESTING LABEL
	vector<double> testing_labels;
	read_MNIST_labels_test(false, testing_labels);
	MATRIXd Yt(n_, 1);
	//for (int i = 0; i < training_labels.size(); ++i) {
	//	Y(i) = training_labels.at(i);
	//}
	Yt << Map<ColVectorXd>(testing_labels.data(), testing_labels.size());
	//Yt *= SCALING_FACTOR;
	//Yt /= 10;

	/*for (long int n : training_labels) {
		cout << n << endl;
	}
	cout << training_labels.size() << endl;*/
	//cout << Yt << endl;
	//cout << Y.rows() << " " << Y.cols() << endl;

	//print out numbers
	/*int c = 0;
	for (int i = 0; i < 784; ++i) {
		if (c % 28 == 0) {
			cout << endl;
		}
		cout << X(2, i) << " ";
		++c;
	}
	cout << training_labels.at(2) << endl;*/
	//cout << Xt.row(0) << endl;

	/*START OF PROTOCOL*/

	// shares of training data
	clock_t start = clock();
	MATRIX X0(N, D);	MATRIX X1(N, D);
	MATRIX Y0(N, 1);	MATRIX Y1(N, 1);
	//genDataShares(X0, X1, Y0, Y1);
	secretShareData(X, Y, X0, X1, Y0, Y1);

	// tests:
	/*cout << "X0 MATRIX:\n" << X0 << endl;
	cout << "X1 MATRIX:\n" << X1 << endl;
	cout << "<X>:\n" << X0 + X1 << endl;*/
	/*cout << "Y0 MATRIX:\n" << Y0 << endl;
	cout << "Y1 MATRIX:\n" << Y1 << endl;
	cout << "<Y>:\n" << Y0 + Y1 << endl;*/

	// coefficient vertor w
	MATRIX W0(D, 1);	MATRIX W1(D, 1);
	initializeW(W0, W1);

	// tests:
	/*cout << "W0:\n" << W0 << endl;
	cout << "W1:\n" << W1 << endl;*/

	// generate shared U, V, Z multiplication triplets
	MATRIX U0(N, D);	MATRIX U1(N, D);
	MATRIX V0(D, T);	MATRIX V1(D, T);
	MATRIX Z0(N, T);	MATRIX Z1(N, T);
	genUVZmultTriplets(U0, U1, V0, V1, Z0, Z1);

	// tests:
	/*cout << "MATRIX U0: \n" << U0 << endl;
	cout << "MATRIX U1: \n" << U1 << endl;
	cout << "MATRIX V0: \n" << V0 << endl;
	cout << "MATRIX V1: \n" << V1 << endl;
	cout << "MATRIX Z0: \n " << Z0 << endl;
	cout << "MATRIX Z1: \n " << Z1 << endl;*/

	// * STEP 1: each party runs Ei = Xi - Ui masking
	MATRIX E0(N, D);	MATRIX E1(N, D);	MATRIX E(N, D);
	maskMatrix(X0, U0, E0);
	maskMatrix(X1, U1, E1);
	reconstruct(E0, E1, E);		// |?| how to code how each party does this independently

	// tests:
	/*cout << "MATRIX E0:\n" << E0 << endl;
	cout << "MATRIX E1:\n" << E1 << endl;
	cout << "MATRIX E: \n" << E << endl;*/

	// * STEP 2: start of each iteration
	set<int> usedIndices;
	for (int j = 0; j < T; ++j) {

		//cout << "Iteration " << j+1 << "..." << endl;

		// * STEP 3: select minibatch
		set<int> minibatch = createMiniBatch(usedIndices); // select minibatch indices

		// tests:
		/*cout << "minibatch:" << endl;
		for (int n : minibatch) {
			cout << n << " ";
		}
		cout << endl;*/

		// submatrices size B
		MATRIX EB(B_size, D);
		MATRIX UB0(B_size, D);	MATRIX UB1(B_size, D);
		MATRIX XB0(B_size, D);	MATRIX XB1(B_size, D);
		MATRIX YB0(B_size, 1);	MATRIX YB1(B_size, 1);
		MATRIX ZB0(B_size, T);	MATRIX ZB1(B_size, T);

		int r = 0;	int k = 0;
		for (int n : minibatch) {
			k = n - 1;
			EB.row(r) = E.row(k);
			UB0.row(r) = U0.row(k);
			UB1.row(r) = U1.row(k);
			XB0.row(r) = X0.row(k);
			XB1.row(r) = X1.row(k);
			YB0.row(r) = Y0.row(k);
			YB1.row(r) = Y1.row(k);
			ZB0.row(r) = Z0.row(k);
			ZB1.row(r) = Z1.row(k);
			r++;
		}

		// tests:
		/*cout << "MATRIX EB:\n" << EB << endl;
		cout << "MATRIX UB0:\n" << UB0 << endl;
		cout << "MATRIX UB1:\n" << UB1 << endl;
		cout << "MATRIX XB0:\n" << XB0 << endl;
		cout << "MATRIX XB1:\n" << XB1 << endl;
		cout << "MATRIX YB0:\n" << YB0 << endl;
		cout << "MATRIX YB1:\n" << YB1 << endl;
		cout << "MATRIX ZB0:\n" << ZB0 << endl;
		cout << "MATRIX ZB1:\n" << ZB1 << endl;*/

		// generate V' and Zp multiplication triplets based on minibatch indices
		MATRIX Vp0(B_size, T);	MATRIX Vp1(B_size, T);
		MATRIX Zp0(D, T);		MATRIX Zp1(D, T);
		genVZprimeMultTriplets(UB0, UB1, Vp0, Vp1, Zp0, Zp1);

		// tests:
	/*	cout << "matrix Vp0: \n" << Vp0 << endl;
		cout << "matrix Vp1: \n" << Vp1 << endl;
		cout << "matrix Zp0: \n" << Zp0 << endl;
		cout << "matrix Zp1: \n" << Zp1 << endl;*/

		//// * STEP 4: each party computes Fji = wi - V[j]i	jth iteration
		MATRIX F0(D, 1);	MATRIX F1(D, 1);	MATRIX F(D, 1);
		MATRIX V0j = V0.col(j);
		MATRIX V1j = V1.col(j);
		maskMatrix(W0, V0j, F0);
		maskMatrix(W1, V1j, F1);
		reconstruct(F0, F1, F);

		// tests:
		/*cout << "MATRIX V0j: \n" << V0j << endl;
		cout << "MATRIX F0:\n" << F0 << endl;
		cout << "MATRIX V1j: \n" << V1j << endl;
		cout << "MATRIX F1:\n" << F1 << endl;
		cout << "MATRIX F:\n" << F << endl;*/

		// * STEP 5: calculate predicted output
		MATRIX Ypred_0 = -0 * (EB * F) + (XB0 * F) + (EB * W0) + ZB0.col(j);	// replace with j
		MATRIX Ypred_1 = -1 * (EB * F) + (XB1 * F) + (EB * W1) + ZB1.col(j);	// replace with j

		// tests:
		/*cout << "EB\n" << EB << endl;
		cout << "XB0\n" << XB0<< endl;
		cout << "XB1\n" << XB1<< endl;
		cout << "ZB0.col(0)\n" << ZB0.col(0) << endl;
		cout << "ZB1.col(0)\n" << ZB1.col(0) << endl;
		cout << "Ypred_0:\n" << Ypred_0 << endl;
		cout << "Ypred_1:\n" << Ypred_1 << endl;*/

		// * STEP 6:
		MATRIX D0; MATRIX D1;
		computeDifference(D0, Ypred_0, YB0);
		computeDifference(D1, Ypred_1, YB1);

		// tests:
		/*cout << "Ypred_0:\n" << Ypred_0 << endl;
		cout << "YB0 \n" << YB0 << endl;
		cout << "D0:\n" <<D0 << endl;
		cout << "Ypred_1:\n" << Ypred_1 << endl;
		cout << "YB1 \n" << YB1 << endl;
		cout << "D1:\n" <<D1 << endl;*/

		// * STEP 7: mask D0, and D1
		MATRIX Fp0;	MATRIX Fp1; MATRIX Fp;
		MATRIX Vp0j = Vp0.col(j);
		MATRIX Vp1j = Vp1.col(j);
		maskMatrix(D0, Vp0j, Fp0);
		maskMatrix(D1, Vp1j, Fp1);
		reconstruct(Fp0, Fp1, Fp);

		// tests:
		/*cout << "MATRIX Fp0: \n" << Fp0 << endl;
		cout << "MATRIX Fp1: \n" << Fp1 << endl;
		cout << "MATRIX Fp: \n" << Fp << endl;*/

		// * STEP 8: compute shares of delta
		MATRIX delta0 = -0 * (EB.transpose() * Fp) + (XB0.transpose() * Fp) + (EB.transpose() * D0) + Zp0.col(j);
		MATRIX delta1 = -1 * (EB.transpose() * Fp) + (XB1.transpose() * Fp) + (EB.transpose() * D1) + Zp1.col(j);

		// tests:
		/*cout << "Before truncation:" << endl;
		cout << "delta0:\n" << delta0 << endl;
		cout << "delta1:\n" << delta1 << endl;*/

		// *STEP 9: truncation
		int l = 64;	int x = 32;	int d = 13;

		for (int i = 0; i < D; ++i) {
			delta0(i) = delta0(i) >> d;
			delta1(i) = delta1(i) >> d;
		}

		// tests:
		/*cout << "After truncation:" << endl;
		cout << "delta0:\n" << delta0 << endl;
		cout << "delta1:\n" << delta1 << endl;*/

		// *STEP 10:
		/*cout << "W0:\n" << W0 << endl;
		cout << "W1:\n" << W1 << endl;*/
		//cout << (B_size * ALPHA) << endl;

		W0 = W0 - (delta0 / (B_size * ALPHA));
		W1 = W1 - (delta1 / (B_size * ALPHA));

		/*cout << "W0:\n" << W0 << endl;
		cout << "W1:\n" << W1 << endl;*/

	}

	// *STEP 11: Reconstruct W
	MATRIX W(D,1);
    reconstruct(W0, W1, W);

	// tests:
	//cout << "FINAL W:\n" << W << endl;

	clock_t end = clock();
	double elapsed = (double(end) - double(start)) / CLOCKS_PER_SEC;
	//cout << "Time measured : " << elapsed << "seconds.\n";

	ColVectorXd Wd;
	W /= SCALING_FACTOR;
	Wd = W.cast <double>();

	//cout << Yt << endl;

	/*cout << "Xt size: " << Xt.rows() << " " << Xt.cols() << endl;
	cout << "Wd size: " << Wd.rows() << " " << Wd.cols() << endl;*/
	// Xt will always be 10000 x 784, make sure Wd is also 784

	ColVectorXd prediction = Xt * Wd;

	prediction /= SCALING_FACTOR;
	prediction *= 10;
    n_ = Yt.rows();	// 10000
	double temp1;
	long int temp2, temp3;
    prediction = round(prediction.array());

	cout << "SIZE: " << n_ << endl;

	cout << prediction << endl;
	int count = 0; int count_zero = 0;
    for (int i = 0; i < n_ ; i++){
		temp3 = (long long int)prediction(i);

		//temp1 = temp3/(double)pow(2,20+8);
		temp2 = (long long int)Yt(i);
		//if (temp2 == 0) {
		//	count_zero++;
		//}

		//if(temp3>0.5 && temp2 == 1){
		//	count++;
		//}
		//else if (temp3 < 0.5 && temp2 == 0) {
		//	count++;
		//}

		if(prediction(i) == Yt(i))
            count++;

		//cout << "pred: " << prediction(i) << "	test label: " << Yt(i) << endl;
  //
    }

	cout << "num zeros: " << count_zero << endl;

	cout << "num correct" << count << endl;
	double accuracy = count/((double) n_);
	cout << "Accuracy on testing the trained model is " << accuracy * 100 << endl;

} // end of main()

void secretShareData(MATRIX& X, MATRIX& Y, MATRIX& X0, MATRIX& X1, MATRIX& Y0, MATRIX& Y1) {
	unsigned long long int n, n0;

	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < D; j++) {
			n = X(i, j);
			n0 = rand();
			X0(i, j) = n0;
			X1(i, j) = (n - n0);
		}
	}

	for (int i = 0; i < N; ++i) {
		n = Y(i);
		n0 = rand();
		Y0(i) = n0;
		Y1(i) = (n - n0)  ;	// add mod 2l ?
	}
}

void genDataShares(MATRIX& X0, MATRIX& X1, MATRIX& Y0, MATRIX& Y1) {
	unsigned long long int n, n0, n1;
	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < D; j++) {
			n = rand();
			n0 = rand();
			n1 = (n - n0); 		// add mod 2l ?
			X0(i, j) = n0;
			X1(i, j) = n1;
		}
	}

	for (int i = 0; i < N; ++i) {
		n = rand();
		n0 = rand();
		n1 = (n - n0) ;		// add mod 2l ?
		Y0(i) = n0;
		Y1(i) = n1;
	}
}

void initializeW(MATRIX& W0, MATRIX& W1) {
	for (int i = 0; i < D; ++i) {
		W0(i) = rand();
		W1(i) = rand();
	}
}

void genUVZmultTriplets(MATRIX& U0, MATRIX& U1, MATRIX& V0, MATRIX& V1, MATRIX& Z0, MATRIX& Z1) {

	unsigned long long int n, n0;
	for (int i = 0; i < N; i++) {
		for (int j = 0; j < D; j++) {
			n = rand();
			n0 = rand();
			U0(i, j) = n0;
			U1(i, j) = (n - n0);
		}
	}

	for (int i = 0; i < D; i++) {
		for (int j = 0; j < T; j++) {
			n = rand();
			n0 = rand();
			V0(i, j) = n0;
			V1(i, j) = (n - n0);
		}
	}

	Z0 = U0 * V0;
	Z1 = U1 * V1;
}

void genVZprimeMultTriplets(MATRIX& UB0, MATRIX& UB1, MATRIX& Vp0, MATRIX& Vp1, MATRIX& Zp0, MATRIX& Zp1) {
	MATRIX UBT0 = UB0.transpose();
	MATRIX UBT1 = UB1.transpose();

	unsigned long long int n, n0;
	for (int i = 0; i < B_size; i++) {
		for (int j = 0; j < T; j++) {
			n = rand();
			n0 = rand();
			Vp0(i, j) = n0;
			Vp1(i, j) = n - n0;
		}
	}

	Zp0 = UBT0 * Vp0;
	Zp1 = UBT1 * Vp1;
	/*cout << "MATRIX UBT0:\n" << UBT0 << endl;
	cout << "MATRIX UBT1:\n" << UBT1 << endl;*/
}

void reconstruct(MATRIX M0, MATRIX M1, MATRIX& M) {	M = M0 + M1;}

void maskMatrix(MATRIX Data, MATRIX Mask, MATRIX& Final) {	Final = Data - Mask;}

set<int> createMiniBatch(set<int>& usedIndices) {
	set<int> minibatch;
	int num = 0;
	//srand(time(NULL));
	while (minibatch.size() != B_size) {
		num = (rand() % (N - 1 + 1)) + 1;
		if (usedIndices.count(num) == 0) {
			usedIndices.insert(num);
			minibatch.insert(num);
		}
	}
	return minibatch;
}

void computeDifference(MATRIX& Dif, MATRIX Y_, MATRIX Y) {    Dif = Y_ - Y;}

int reverse_int(int i){
    unsigned char ch1, ch2, ch3, ch4;
    ch1 = i & 255;
    ch2 = (i >> 8) & 255;
    ch3 = (i >> 16) & 255;
    ch4 = (i >> 24) & 255;
    return ((int) ch1 << 24) + ((int) ch2 << 16) + ((int) ch3 << 8) + ch4;
}

void read_MNIST_data(bool train, vector<vector<unsigned long long int>> &vec, int& number_of_images, int& number_of_features){
    std::ifstream file;
	if (train == true)
		file.open("/Users/ariad/Documents/train-images-idx3-ubyte", std::ios::binary);
	else
		file.open("/Users/ariad/Documents/t10k-images-idx3-ubyte", std::ios::binary);

    if(!file){
        std::cout<<"Unable to open file";
        return;
    }
    if (file.is_open()){
        int magic_number = 0;
        int n_rows = 0;
        int n_cols = 0;
        file.read((char*) &magic_number, sizeof(magic_number));
        magic_number = reverse_int(magic_number);
        file.read((char*) &number_of_images, sizeof(number_of_images));
        number_of_images = reverse_int(number_of_images);
        if(train == true)
            number_of_images = N;
        file.read((char*) &n_rows, sizeof(n_rows));
        n_rows = reverse_int(n_rows);
        file.read((char*) &n_cols, sizeof(n_cols));
        n_cols = reverse_int(n_cols);
        number_of_features = n_rows * n_cols;
        std::cout << "Number of Images: " << number_of_images << std::endl;
        std::cout << "Number of Features: " << number_of_features << std::endl;
        for(int i = 0; i < number_of_images; ++i){
            std::vector<unsigned long long int> tp;
            for(int r = 0; r < n_rows; ++r)
                for(int c = 0; c < n_cols; ++c){
                    unsigned char temp = 0;
                    file.read((char*) &temp, sizeof(temp));
                    tp.push_back(((unsigned long long int) temp));
                }
            vec.push_back(tp);
        }
    }
}

void read_MNIST_labels(bool train, vector<unsigned long long int> &vec){
    std::ifstream file;
	if (train == true) {
		file.open("/Users/ariad/Documents/train-labels-idx1-ubyte", std::ios::binary);
	}
	else {
		file.open("/Users/ariad/Documents/t10k-labels-idx1-ubyte", std::ios::binary);
	}
    if(!file){
        std::cout << "Unable to open file";
        return;
    }
    if (file.is_open()){
        int magic_number = 0;
        int number_of_images = 0;
        file.read((char*) &magic_number, sizeof(magic_number));
        magic_number = reverse_int(magic_number);
        file.read((char*) &number_of_images, sizeof(number_of_images));
        number_of_images = reverse_int(number_of_images);
		cout << "number of images inside MNIST LAbels: " << number_of_images << endl;

        if(train == true)
            number_of_images = N;
        for(int i = 0; i < number_of_images; ++i){
            unsigned char temp = 0;
            file.read((char*) &temp, sizeof(temp));
			//vec.push_back((unsigned long int) temp);

            if((unsigned long long int) temp == 0)
                vec.push_back((unsigned long long int) 0);
            else
                vec.push_back((unsigned long long int) 1);
        }
    }
}

void read_MNIST_data_test(bool train, vector<vector<double>> &vec, int& number_of_images, int& number_of_features){
    std::ifstream file;
	if (train == true)
		file.open("/Users/ariad/Documents/train-images-idx3-ubyte", std::ios::binary);
	else
		file.open("/Users/ariad/Documents/t10k-images-idx3-ubyte", std::ios::binary);

    if(!file){
        std::cout<<"Unable to open file";
        return;
    }
    if (file.is_open()){
        int magic_number = 0;
        int n_rows = 0;
        int n_cols = 0;
        file.read((char*) &magic_number, sizeof(magic_number));
        magic_number = reverse_int(magic_number);
        file.read((char*) &number_of_images, sizeof(number_of_images));
        number_of_images = reverse_int(number_of_images);
        if(train == true)
            number_of_images = N;
        file.read((char*) &n_rows, sizeof(n_rows));
        n_rows = reverse_int(n_rows);
        file.read((char*) &n_cols, sizeof(n_cols));
        n_cols = reverse_int(n_cols);
        number_of_features = n_rows * n_cols;
        std::cout << "Number of Images: " << number_of_images << std::endl;
        std::cout << "Number of Features: " << number_of_features << std::endl;
        for(int i = 0; i < number_of_images; ++i){
            std::vector<double> tp;
            for(int r = 0; r < n_rows; ++r)
                for(int c = 0; c < n_cols; ++c){
                    unsigned char temp = 0;
                    file.read((char*) &temp, sizeof(temp));
                    tp.push_back(((double) temp));
                }
            vec.push_back(tp);
        }
    }
}

void read_MNIST_labels_test(bool train, vector<double> &vec){
    std::ifstream file;
	if (train == true) {
		file.open("/Users/ariad/Documents/train-labels-idx1-ubyte", std::ios::binary);
	}
	else {
		file.open("/Users/ariad/Documents/t10k-labels-idx1-ubyte", std::ios::binary);
	}
    if(!file){
        std::cout << "Unable to open file";
        return;
    }
    if (file.is_open()){
        int magic_number = 0;
        int number_of_images = 0;
        file.read((char*) &magic_number, sizeof(magic_number));
        magic_number = reverse_int(magic_number);
        file.read((char*) &number_of_images, sizeof(number_of_images));
        number_of_images = reverse_int(number_of_images);
		cout << "number of images inside MNIST LAbels: " << number_of_images << endl;

        if(train == true)
            number_of_images = N;
        for(int i = 0; i < number_of_images; ++i){
            unsigned char temp = 0;
            file.read((char*) &temp, sizeof(temp));
			//vec.push_back((double) temp);

            if((double) temp == 0)
                vec.push_back((double) 0);
            else
                vec.push_back((double) 1);
        }
    }
}

/*
	Modulus
	Epochs, testing accuracy
*/


//////////////////////////////////////////////////////////////////////////////
// accumulator::zscore.cpp                                                  //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#include <algorithm>
#include <vector>
#include <boost/ref.hpp>
#include <boost/range.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/statistics/detail/accumulator/statistics/zscore.hpp>

void example_accumulator_zscore(std::ostream& os)
{

	os << "-> example_accumulator_zscore" << std::endl;

	typedef double val_;
    namespace stat = boost::statistics::detail;
    typedef stat::accumulator::tag::zscore tag_z_;

    typedef boost::accumulators::accumulator_set<
    	val_,
    	boost::accumulators::stats<tag_z_>
    > acc_;

	typedef boost::normal_distribution<val_> r_;
    typedef boost::mt19937 urng_;
	typedef boost::variate_generator<urng_&,r_> vg_;
	typedef std::vector<val_> vec_;

	const val_ m = -1;
    const val_ s = 2;
	r_ r(m,s);
    urng_ urng;
	vg_ vg(urng,r);
    vec_ vec;

	std::generate_n(std::back_inserter(vec),10000,vg);
    acc_ acc;
    acc = std::for_each(
    	boost::begin(vec),
        boost::end(vec),
       	acc
    );

	// should be close -1.96, 0, 1.96
    val_ a = stat::accumulator::extract::zscore(acc,m-1.96 * s);
    val_ b = stat::accumulator::extract::zscore(acc,m);
    val_ c = stat::accumulator::extract::zscore(acc,m+1.96 * s);
	os << "a = " << a << std::endl;
	os << "b = " << b << std::endl;
	os << "c = " << c << std::endl;

    os << std::endl;

}


#include <igl/read_triangle_mesh.h>
#include <igl/hessian_energy.h>
#include <igl/massmatrix.h>
#include <igl/cotmatrix.h>
#include <igl/jet.h>
#include <igl/edges.h>
#include <igl/vertex_components.h>
#include <igl/remove_unreferenced.h>
#include <igl/opengl/glfw/Viewer.h>

#include <Eigen/Core>
#include <Eigen/SparseCholesky>

#include <iostream>
#include <set>
#include <limits>
#include <stdlib.h>

#include "tutorial_shared_path.h"

#include <igl/isolines.h>

int main(int argc, char * argv[])
{
    typedef Eigen::SparseMatrix<double> SparseMat;

    //Read our mesh
    Eigen::MatrixXd V;
    Eigen::MatrixXi F, E;
    if(!igl::read_triangle_mesh(
        argc>1?argv[1]: TUTORIAL_SHARED_PATH "/beetle.off",V,F)) {
        std::cout << "Failed to load mesh." << std::endl;
    }
    igl::edges(F,E);

    //Constructing an exact function to smooth
    Eigen::VectorXd zexact = V.block(0,2,V.rows(),1).array()
        + 0.5*V.block(0,1,V.rows(),1).array()
        + V.block(0,1,V.rows(),1).array().pow(2)
        + V.block(0,2,V.rows(),1).array().pow(3);

    //Make the exact function noisy
    srand(5);
    const double s = 0.2*(zexact.maxCoeff() - zexact.minCoeff());
    Eigen::VectorXd znoisy = zexact + s*Eigen::VectorXd::Random(zexact.size());

    //Constructing the squared Laplacian and squared Hessian energy
    SparseMat L, M;
    igl::cotmatrix(V, F, L);
    igl::massmatrix(V, F, igl::MASSMATRIX_TYPE_BARYCENTRIC, M);
    Eigen::SimplicialLDLT<SparseMat> solver(M);
    SparseMat MinvL = solver.solve(L);
    SparseMat QL = L.transpose()*MinvL;
    SparseMat QH;
    igl::hessian_energy(V, F, QH);

    //Solve to find Laplacian-smoothed and Hessian-smoothed solutions
    const double al = 8e-4;
    Eigen::SimplicialLDLT<SparseMat> lapSolver(al*QL + (1.-al)*M);
    Eigen::VectorXd zl = lapSolver.solve(al*M*znoisy);
    const double ah = 5e-6;
    Eigen::SimplicialLDLT<SparseMat> hessSolver(ah*QH + (1.-ah)*M);
    Eigen::VectorXd zh = hessSolver.solve(ah*M*znoisy);

    //Viewer that shows all functions: zexact, znoisy, zl, zh
    igl::opengl::glfw::Viewer viewer;
    viewer.data().set_mesh(V,F);
    viewer.data().show_lines = false;
    viewer.callback_key_down =
      [&](igl::opengl::glfw::Viewer & viewer, unsigned char key, int mod)->bool
      {
        //Graduate result to show isolines, then compute color matrix
        const Eigen::VectorXd* z;
        switch(key) {
            case '1':
                z = &zexact;
                break;
            case '2':
                z = &znoisy;
                break;
            case '3':
                z = &zl;
                break;
            case '4':
                z = &zh;
                break;
            default:
                return false;
        }
        Eigen::MatrixXd isoV;
        Eigen::MatrixXi isoE;
        if(key!='2')
            igl::isolines(V, F, *z, 30, isoV, isoE);
        viewer.data().set_edges(isoV,isoE,Eigen::RowVector3d(0,0,0));
        Eigen::MatrixXd colors;
        igl::jet(*z, true, colors);
        viewer.data().set_colors(colors);
        return true;
    };
    std::cout << R"(Usage:
1  Show original function
2  Show noisy function
3  Biharmonic smoothing (zero Neumann boundary)
4  Biharmonic smoothing (natural Hessian boundary)

)";
    viewer.launch();

    return 0;
}


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

  @copyright 2012-2015 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_GOLDBAR_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_GOLDBAR_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the conjugate golden ratio that is \f$\bar\phi = \frac{1-\sqrt5}{2}\f$

    @par Semantic:

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

    is similar for floating types to:

    @code
    T r = (1-sqrt(T(5)))/2;
    @endcode

    @return The Goldbar constant for the proper type
  **/
  template<typename T> T Goldbar();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant goldbar.

      @return The Goldbar constant for the proper type
    **/
    const boost::dispatch::functor<tag::goldbar_> goldbar = {};
  }
} }
#endif

#include <boost/simd/constant/definition/goldbar.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[dimension
//` Examine the number of coordinates making up the points in a linestring type

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian);

int main()
{
    int dim = boost::geometry::dimension
        <
            boost::geometry::model::linestring
                <
                    boost::tuple<float, float, float>
                >
        >::value;

    std::cout << "dimensions: " << dim << std::endl;

    return 0;
}

//]

//[dimension_output
/*`
Output:
[pre
dimensions: 3
]
*/
//]


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

/*
 Copyright (C) 2018 Klaus Spanderen

 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 cevrndcalculator.cpp */

#include <ql/errors.hpp>
#include <ql/math/functional.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/distributions/normaldistribution.hpp>

#include <ql/methods/finitedifferences/utilities/cevrndcalculator.hpp>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>

namespace QuantLib {

    CEVRNDCalculator::CEVRNDCalculator(Real f0, Real alpha, Real beta)
    : f0_(f0),
      alpha_(alpha),
      beta_(beta),
      delta_((1.0-2.0*beta)/(1.0-beta)),
      x0_(X(f0)) {
        QL_REQUIRE(beta != 1.0, "beta can not be one");
    }

    Real CEVRNDCalculator::massAtZero(Time t) const {
        if (delta_ < 2.0)
            return 1.0-boost::math::gamma_p(-0.5*delta_+1.0,x0_/(2.0*t));
        else
            return 0.0;
    }

    Real CEVRNDCalculator::X(Real f) const {
        return std::pow(f, 2.0*(1.0-beta_))/square<Real>()(alpha_*(1.0-beta_));
    }

    Real CEVRNDCalculator::invX(Real x) const {
        return std::pow(x*square<Real>()(alpha_*(1.0-beta_)),
                        1.0/(2.0*(1.0-beta_)));
    }

    Real CEVRNDCalculator::pdf(Real f, Time t) const {
        const Real y = X(f);

        if (delta_ < 2.0) {
            return boost::math::pdf(
                boost::math::non_central_chi_squared_distribution<Real>(
                    4.0-delta_, y/t), x0_/t)/t * 2.0*(1.0-beta_)*y/f;
        }
        else {
            return boost::math::pdf(
                boost::math::non_central_chi_squared_distribution<Real>(
                    delta_, x0_/t), y/t)/t * 2.0*(beta_-1.0)*y/f;
        }
    }

    Real CEVRNDCalculator::cdf(Real f, Time t) const {
        const Real y = X(f);

        if (delta_ < 2.0)
            return 1.0 - boost::math::cdf(
                boost::math::non_central_chi_squared_distribution<Real>(
                    2.0-delta_, y/t), x0_/t);
        else
            return 1.0 - boost::math::cdf(
                boost::math::non_central_chi_squared_distribution<Real>(
                    delta_, x0_/t), y/t);
    }

    Real CEVRNDCalculator::sankaranApprox(Real c, Time t, Real x) const {
        const Real a = x0_/t;
        const Real b = 2.0 - delta_;

        c = std::max(c, -0.45*b);

        const Real h = 1 - 2*(b+c)*(b+3*c)/(3*square<Real>()(b+2*c));
        const Real p = (b+2*c)/square<Real>()(b+c);
        const Real m = (h-1)*(1-3*h);

        const Real u = (std::pow(a/(b+c), h) - (1 + h*p*(h-1-0.5*(2-h)*m*p)))/
                (h*std::sqrt(2*p)*(1+0.5*m*p));

        return u - x;
    }

    Real CEVRNDCalculator::invcdf(Real q, Time t) const {
        if (delta_ < 2.0) {
            if (f0_ < QL_EPSILON || q < massAtZero(t))
                return 0.0;

            const Real x = InverseCumulativeNormal()(1-q);

            auto cdfApprox = [&](Real _c){ return sankaranApprox(_c, t, x); };

            const Real y0 = X(f0_)/t;

            try {
                Brent brent;
                brent.setMaxEvaluations(20);
                const Real guess =
                    invX(brent.solve(cdfApprox, 1e-8, y0, 0.02*y0) * t);

                return InvCDFHelper(this, guess, 1e-8, 100).inverseCDF(q, t);
            }
            catch (...) {
                return InvCDFHelper(this, f0_, 1e-8, 100).inverseCDF(q, t);
            }
        }
        else {
            const Real x = t * boost::math::quantile(
                boost::math::non_central_chi_squared_distribution<Real>(
                    delta_, x0_/t), 1-q);
            return invX(x);
        }
    }
}


#include <iostream>
#include <fstream>
#include "spaND.h"
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>
#include "mmio.hpp"

using namespace Eigen;
using namespace std;
using namespace spaND;

int main(int argc, char* argv[]) {
    assert(argc == 5);
    string folder(argv[1]);
    int nlevels = atoi(argv[2]);
    double tol = atof(argv[3]);
    int skip = atoi(argv[4]);
    cout << "Inputs folder " << folder << " nlevels = " << nlevels << " tol = " << tol << " skip = " << skip << endl;
    string file  = folder + "/jac4.mm";
    string xfile = folder + "/xCoords.mm";
    string yfile = folder + "/yCoords.mm";
    cout << "Reading from " << file << " " << xfile << " " << yfile << endl;
    SpMat A = mmio::sp_mmread<double,int>(file);
    int N = A.rows();
    cout << "A has " << N << " rows" << endl;
    VectorXd xcoords = mmio::dense_mmread<double>(xfile);
    VectorXd ycoords = mmio::dense_mmread<double>(yfile);
    // Create X coordinate matrix
    MatrixXd X(2, N);
    assert(xcoords.size() == N/2);
    assert(ycoords.size() == N/2);
    for(int i = 0; i < N; i++) {
        X(0,i) = xcoords(i/2);
        X(1,i) = ycoords(i/2);
    }
    Tree t = Tree(nlevels);
    t.set_tol(tol);
    t.set_skip(skip);
    t.set_use_geo(true);
    t.set_Xcoo(&X);
    // Run algo
    timer start = wctime();
    t.partition(A);
    // Rest
    timer part = wctime();
    t.assemble(A);
    timer ass = wctime();
    try {
        t.factorize();
    } catch (exception& ex) {
        cout << ex.what();
        exit(1);
    }
    timer end = wctime();
    t.print_log();
    cout << "Timings:" << endl << "  Partition: " << elapsed(start,part) << " s." << endl << "  Assembly: " << elapsed(part,ass) << " s." << endl << "  Factorization: " << elapsed(ass, end) << " s." << endl;
    // Use CG
    VectorXd x = VectorXd::Zero(N);
    VectorXd b = VectorXd::Random(N);
    timer cg0 = wctime();
    int iter = cg(A, b, x, t, 500, 1e-12, true);
    timer cg1 = wctime();
    cout << "CG: #iterations: " << iter << ", residual |Ax-b|/|b|: " << (A*x-b).norm() / b.norm() << endl;
    cout << "  CG: " << elapsed(cg0, cg1) << " s." << endl;
    return 0;
}


#include <iostream>
#include <Eigen/dense>
#include "file_manage.h"
#include "gaussian_mixture.h"
using namespace std;
using namespace Eigen;

int main()
{
	MatrixXd features;
	VectorXd labels;

	read_csv("data/iris.csv", features, labels, 150, 5);
	//read_csv("data/seeds.csv", features, labels, 210, 8);

	GMM model(3);
	model.fit(features, 1);

	return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include "../simple_lib/include/simple_layer.h"

using namespace Eigen;

int main()
{
    using std::cout;
    using std::endl;

    int batch_size = 3;
    int input_size = 3;
    int output_size = 2;

    MatrixXd X = MatrixXd::Zero(batch_size, input_size);
    MatrixXd Y, dX;
    MatrixXd W = MatrixXd::Zero(input_size, output_size);
    VectorXd b = VectorXd::Zero(output_size);
    MatrixXd dout = MatrixXd::Ones(batch_size, output_size);
    X << 1, 2, 3,
         4, 5, 6,
         7, 8, 9;
    W << 1, 2,
         2, 3,
         3, 4;
    b << 1, 2;

    MyDL::Affine affine(W, b);
    Y = affine.forward(X);
    cout << "forward: " << Y << endl;
    dX = affine.backward(dout);
    cout << "backward: " << dX << endl;

    return 0;
}


// Group B - Perpetual American Option
//
// by Scott Sidoli
//
// 6-10-19
//
// Main.cpp
//
// We program the formula for pricing perptual American Options (part a). We test the data.
// Again, we take the input of an array to compute prices for a varying parameter. We then
// code a matrix pricer that allows us to vary two parameters. The structure of this project
// mirrors the structure of group A. We have defined types for the option parameters in
// Types.hpp. We have the struct in OptionData.hpp and we have our pricing functions in
// OptionPricing.hpp. The perpetual American Option Class is in PerpetualAmericanOption.hpp
// We have MeshArray.hpp to use the method for creating an array of parameter values.

#include "PerpetualAmericanOption.hpp"
#include "MeshArray.hpp"
#include <boost/tuple/tuple_io.hpp>

int main()
{	// 2 b)
	// We test our perpetual American option class. First we create the option and set the data.
	PerpetualAmericanOption Amer_Option;
	Strike_Price K = (Strike_Price)100.0;
	Volatility sig = (Volatility) 0.1;
	rate r = (rate)0.1;
	cost_of_carry b = (cost_of_carry)0.02;
	curr_stock_price S = (curr_stock_price)110;
	Amer_Option.SetOption(K,sig,r,b,S);

	// Now we print the call and put prices.
	cout << "Perpetual American Option Call Price: " << Amer_Option.CallPriceAmer() << endl;
	cout << "Perpetual American Option Put Price: " <<Amer_Option.PutPriceAmer() << endl;

	cout << endl;
	//============================================================================================== =
	// 2 c) We create a mesh array of current stock prices and determine the option prices.
	curr_stock_price start = (curr_stock_price) 10.0;
	curr_stock_price end = (curr_stock_price) 50.0;
	curr_stock_price interval = (curr_stock_price) 1.0;

	vector<curr_stock_price> S_array = MeshArray(start, end, interval);

	// Create vector of Calls and puts
	vector<double> CallPrices = Amer_Option.CallPriceAmer(S_array);
	vector<double> PutPrices = Amer_Option.PutPriceAmer(S_array);

	// Print the result
	cout << "S - value, Call, Put " << endl;
	PrintArray(S_array, CallPrices, PutPrices);

	// Reset the Option
	Amer_Option.SetOption(K, sig, r, b, S);
	cout << endl;
	//============================================================================================== =
	// 2 d) We create a matrix of option parameters and output a matrix of option prices.
	// For the parameters, we use K and sig. We create two vectors.
	Strike_Price K_start = (Strike_Price)80.0;
	Strike_Price K_end = (Strike_Price)120.0;
	Strike_Price K_interval = (Strike_Price)1.0;
	vector<Strike_Price> K_array = MeshArray(K_start, K_end, K_interval);

	Volatility sig_start = (Volatility) 0.01;
	Volatility sig_end = (Volatility) 0.20;
	Volatility sig_interval = (Volatility)0.01;
	vector<Volatility> sig_array = MeshArray(sig_start, sig_end, sig_interval);

	// Create a Matrix to store the call prices and put prices.
	typedef boost::tuple<double, double> CallPrice_PutPrice;
	const int num_rows = 50;
	const int num_cols = 50;
	CallPrice_PutPrice PriceMatrix[num_rows][num_cols]; // Price matrix stores call and put price when parameter I takes value x and parameter J takes value y.
														   // In this example we use expiry time and volatility.

	cout << "(Strike Price, Volatility, (Call Price Put Price))" << endl;

	for (int i = 0; i < K_array.size(); ++i)
	{
		Amer_Option.SetOption(K_array[i]);

		for (int j = 0; j < sig_array.size(); ++j)
		{
			Amer_Option.SetOption(sig_array[j]);
			PriceMatrix[i][j] = CallPrice_PutPrice(Amer_Option.CallPriceAmer(), Amer_Option.PutPriceAmer());

			cout << "(" << K_array[i] << ", " << sig_array[j] << ", " << PriceMatrix[i][j] << ")" << endl;
		}
	}
	cout << endl;

	// reset the option
	Amer_Option.SetOption(K, sig, r, b, S);
	return 0;
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial: Calculation of the eigenvalue with largest modulus using the power iteration method
*             (power-iter.cpp and power-iter.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

// include necessary system headers
#include <iostream>

#ifndef NDEBUG
  #define NDEBUG
#endif

#define VIENNACL_WITH_UBLAS

//include basic scalar and vector types of ViennaCL
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"

#include "viennacl/linalg/power_iter.hpp"
#include "viennacl/io/matrix_market.hpp"
// Some helper functions for this tutorial:
#include <iostream>
#include <fstream>
#include <limits>
#include <string>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>

int main()
{
  typedef double     ScalarType;

  boost::numeric::ublas::compressed_matrix<ScalarType> ublas_A;

  if (!viennacl::io::read_matrix_market_file(ublas_A, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }

  viennacl::compressed_matrix<double>  vcl_A(ublas_A.size1(), ublas_A.size2());
  viennacl::copy(ublas_A, vcl_A);

  viennacl::linalg::power_iter_tag ptag(1e-6);

  std::cout << "Starting computation of eigenvalue with largest modulus (might take about a minute)..." << std::endl;
  std::cout << "Result of power iteration with ublas matrix (single-threaded): " << viennacl::linalg::eig(ublas_A, ptag) << std::endl;
  std::cout << "Result of power iteration with ViennaCL (OpenCL accelerated): " << viennacl::linalg::eig(vcl_A, ptag) << std::endl;

}


#include <algorithm>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include "nv_core.h"
#include "nv_ml.h"
#include "nv_num.h"

// k-means++
int nv_kmeans(nv_matrix_t *means_mat,  // k
		  nv_matrix_t *count_mat,  // k
		  nv_matrix_t *labels_mat, // data->m
		  const nv_matrix_t *data_mat,
		  const int k,
		  const int max_epoch)
{
	cv::Mat means(means_mat->m, means_mat->n, CV_32F, means_mat->v);
	cv::Mat data(data_mat->m, data_mat->n, CV_32F, data_mat->v);
	cv::Mat labels(data_mat->m, 1, CV_32S);

	Eigen::Map<Eigen::VectorXf> count(count_mat->v, count_mat->m);
	count.setZero();

	cv::kmeans(data, k, labels, cv::TermCriteria(cv::TermCriteria::MAX_ITER, max_epoch, 0), 1, cv::KMEANS_PP_CENTERS, means);

	for (int m = 0; m < data_mat->m; ++m) {
		NV_MAT_V(labels_mat, m, 0) = labels.at<int>(m, 0);
		++count(labels.at<int>(m, 0));
	}
	return k;
}


#pragma once
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/register/box.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <cmath>
#include <list>
#include <tuple>

#include "geohash/math.hpp"

namespace geohash {

// Geographic point
struct Point {
  double lng{};
  double lat{};
};

// A box made of two describing points
class Box {
 public:
  // Default constructor
  Box() = default;

  // Constructor taking the minimum corner point and the maximum corner point
  Box(const Point& min_corner, const Point& max_corner)
      : min_corner_(min_corner), max_corner_(max_corner) {}

  // Returns the center of the box.
  [[nodiscard]] inline constexpr auto center() const -> Point {
    return {(min_corner_.lng + max_corner_.lng) * 0.5,
            (min_corner_.lat + max_corner_.lat) * 0.5};
  }

  // Returns the delta of the box in latitude and longitude.
  [[nodiscard]] inline auto delta(bool round) const
      -> std::tuple<double, double> {
    auto x = max_corner_.lng - min_corner_.lng;
    auto y = max_corner_.lat - min_corner_.lat;
    if (round) {
      x = Box::max_decimal_power(x);
      y = Box::max_decimal_power(y);
    }
    return std::make_tuple(x, y);
  }

  // Returns a point inside the box, making an effort to round to minimal
  // precision.
  [[nodiscard]] inline auto round() const -> Point {
    double x;
    double y;
    std::tie(x, y) = delta(true);
    return {std::ceil(min_corner_.lng / x) * x,
            std::ceil(min_corner_.lat / y) * y};
  }

  // Returns the box, or the two boxes on either side of the dateline if the
  // defined box wraps around the globe (i.e. the longitude of the min corner
  // is greater than the longitude of the max corner.)
  [[nodiscard]] auto split() const -> std::list<Box> {
    // box wraps around the globe ?
    if (min_corner_.lng > max_corner_.lng) {
      return {Box({min_corner_.lng, min_corner_.lat}, {180, max_corner_.lat}),
              Box({-180, min_corner_.lat}, {max_corner_.lng, max_corner_.lat})};
    }
    return {*this};
  }

  // Returns true if the geographic point is within the box
  [[nodiscard]] auto contains(const Point& point) const -> bool {
    // box wraps around the globe ?
    if (min_corner_.lng > max_corner_.lng) {
      for (const auto& item : split()) {
        if (!item.contains(point)) {
          return false;
        }
      }
      return true;
    }
    return (min_corner_.lat <= point.lat && point.lat <= max_corner_.lat &&
            min_corner_.lng <= point.lng && point.lng <= max_corner_.lng);
  }

  // Returns the minimum corner point
  [[nodiscard]] auto min_corner() const -> const Point& { return min_corner_; }

  // Returns the minimum corner point
  [[nodiscard]] auto min_corner() -> Point& { return min_corner_; }

  // Returns the maximum corner point
  [[nodiscard]] auto max_corner() const -> const Point& { return max_corner_; }

  // Returns the maximum corner point
  [[nodiscard]] auto max_corner() -> Point& { return max_corner_; }

 private:
  Point min_corner_{};
  Point max_corner_{};

  // Returns the maximum power of 10 from a number (x > 0)
  static auto max_decimal_power(const double x) -> double {
    auto m = static_cast<int32_t>(std::floor(std::log10(x)));
    return power10(m);
  }
};

}  // namespace geohash

BOOST_GEOMETRY_REGISTER_POINT_2D(
    geohash::Point, double,
    boost::geometry::cs::geographic<boost::geometry::degree>, lng, lat)

BOOST_GEOMETRY_REGISTER_BOX(geohash::Box, geohash::Point, min_corner(),
                            max_corner())

namespace geohash {

using Polygon = boost::geometry::model::polygon<Point>;

}  // namespace geohash


// Vec2.cpp
#include "Vec2.h"

#include <armadillo>

namespace ugps
{
    num_ug Vec2::length() const
    {
        return sqrt(length2());
    }

    num_ug Vec2::length2() const
    {
        return x*x + y*y;
    }

    arma::Col<num_ug> Vec2::col() const
    {
        arma::Col<num_ug> c = {x,y};
        return c;
    }

    arma::Row<num_ug> Vec2::row() const
    {
        arma::Row<num_ug> r = {x,y};
        return r;
    }

    Vec2 Vec2::rotate(const num_ug& angle) const
    {
        return Vec2(
                cos(angle)*x - sin(angle)*y,
                sin(angle)*y + cos(angle)*x
                );
    }

    num_ug Vec2::angle() const
    {
        return atan2(y,x);
    }
}


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

#ifndef CRYPTO3_MATH_XGCD_HPP
#define CRYPTO3_MATH_XGCD_HPP

#include <algorithm>
#include <vector>

#include <boost/math/tools/polynomial_gcd.hpp>
#include <boost/integer/extended_euclidean.hpp>

#include <nil/crypto3/math/polynomial/basic_operations.hpp>

namespace nil {
    namespace crypto3 {
        namespace math {

            /*!
             * @brief Perform the standard Extended Euclidean Division algorithm.
             * Input: Polynomial A, Polynomial B.
             * Output: Polynomial G, Polynomial U, Polynomial V, such that G = (A * U) + (B * V).
             */
            template<typename Range1, typename Range2, typename Range3, typename Range4,
                     typename Range5>
            void extended_euclidean(const Range1 &a, const Range2 &b, Range3 &g, Range4 &u, Range5 &v) {

                typedef
                    typename std::iterator_traits<decltype(std::begin(std::declval<Range1>()))>::value_type value_type;

                if (is_zero(b)) {
                    g = a;
                    u = std::vector<value_type>(1, value_type::one());
                    v = std::vector<value_type>(1, value_type::zero());
                    return;
                }

                std::vector<value_type> U(1, value_type::one());
                std::vector<value_type> V1(1, value_type::zero());
                std::vector<value_type> G(a);
                std::vector<value_type> V3(b);

                std::vector<value_type> Q(1, value_type::zero());
                std::vector<value_type> R(1, value_type::zero());
                std::vector<value_type> T(1, value_type::zero());

                while (!is_zero(V3)) {
                    division(Q, R, G, V3);
                    multiplication(G, V1, Q);
                    subtraction(T, U, G);

                    U = V1;
                    G = V3;
                    V1 = T;
                    V3 = R;
                }

                multiplication(V3, a, U);
                subtraction(V3, G, V3);
                division(V1, R, V3, b);

                value_type lead_coeff = G.back().inversed();
                std::transform(G.begin(), G.end(), G.begin(),
                               std::bind(std::multiplies<value_type>(), lead_coeff, std::placeholders::_1));
                std::transform(U.begin(), U.end(), U.begin(),
                               std::bind(std::multiplies<value_type>(), lead_coeff, std::placeholders::_1));
                std::transform(V1.begin(), V1.end(), V1.begin(),
                               std::bind(std::multiplies<value_type>(), lead_coeff, std::placeholders::_1));

                g = G;
                u = U;
                v = V1;
            }
        }    // namespace math
    }        // namespace crypto3
}    // namespace nil

#endif    // ALGEBRA_FFT_XGCD_HPP