#include "order_facets_around_edge.h"
#include <Eigen/Geometry>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <stdexcept>

// adj_faces contains signed index starting from +- 1.
template<
  typename DerivedV,
  typename DerivedF,
  typename DerivedI >
void igl::copyleft::cgal::order_facets_around_edge(
    const Eigen::PlainObjectBase<DerivedV>& V,
    const Eigen::PlainObjectBase<DerivedF>& F,
    size_t s,
    size_t d,
    const std::vector<int>& adj_faces,
    Eigen::PlainObjectBase<DerivedI>& order, bool debug)
{
  // Although we only need exact predicates in the algorithm,
  // exact constructions are needed to avoid degeneracies due to
  // casting to double.
  typedef CGAL::Exact_predicates_exact_constructions_kernel K;
  typedef K::Point_3 Point_3;
  typedef K::Plane_3 Plane_3;

  auto get_face_index = [&](int adj_f)->size_t
  {
    return abs(adj_f) - 1;
  };

  auto get_opposite_vertex = [&](size_t fid)->size_t
  {
    typedef typename DerivedF::Scalar Index;
    if (F(fid, 0) != (Index)s && F(fid, 0) != (Index)d) return F(fid, 0);
    if (F(fid, 1) != (Index)s && F(fid, 1) != (Index)d) return F(fid, 1);
    if (F(fid, 2) != (Index)s && F(fid, 2) != (Index)d) return F(fid, 2);
    assert(false);
    return -1;
  };

  // Handle base cases
  if (adj_faces.size() == 0)
  {
    order.resize(0, 1);
    return;
  } else if (adj_faces.size() == 1)
  {
    order.resize(1, 1);
    order(0, 0) = 0;
    return;
  } else if (adj_faces.size() == 2)
  {
    const size_t o1 = get_opposite_vertex(get_face_index(adj_faces[0]));
    const size_t o2 = get_opposite_vertex(get_face_index(adj_faces[1]));
    const Point_3 ps(V(s, 0), V(s, 1), V(s, 2));
    const Point_3 pd(V(d, 0), V(d, 1), V(d, 2));
    const Point_3 p1(V(o1, 0), V(o1, 1), V(o1, 2));
    const Point_3 p2(V(o2, 0), V(o2, 1), V(o2, 2));
    order.resize(2, 1);
    switch (CGAL::orientation(ps, pd, p1, p2))
    {
      case CGAL::POSITIVE:
        order(0, 0) = 1;
        order(1, 0) = 0;
        break;
      case CGAL::NEGATIVE:
        order(0, 0) = 0;
        order(1, 0) = 1;
        break;
      case CGAL::COPLANAR:
        {
          switch (CGAL::coplanar_orientation(ps, pd, p1, p2)) {
            case CGAL::POSITIVE:
              // Duplicated face, use index to break tie.
              order(0, 0) = adj_faces[0] < adj_faces[1] ? 0:1;
              order(1, 0) = adj_faces[0] < adj_faces[1] ? 1:0;
              break;
            case CGAL::NEGATIVE:
              // Coplanar faces, one on each side of the edge.
              // It is equally valid to order them (0, 1) or (1, 0).
              // I cannot think of any reason to prefer one to the
              // other.  So just use (0, 1) ordering by default.
              order(0, 0) = 0;
              order(1, 0) = 1;
              break;
            case CGAL::COLLINEAR:
              std::cerr << "Degenerated triangle detected." <<
                std::endl;
              assert(false);
              break;
            default:
              assert(false);
          }
        }
        break;
      default:
        assert(false);
    }
    return;
  }

  const size_t num_adj_faces = adj_faces.size();
  const size_t o = get_opposite_vertex( get_face_index(adj_faces[0]));
  const Point_3 p_s(V(s, 0), V(s, 1), V(s, 2));
  const Point_3 p_d(V(d, 0), V(d, 1), V(d, 2));
  const Point_3 p_o(V(o, 0), V(o, 1), V(o, 2));
  const Plane_3 separator(p_s, p_d, p_o);
  if (separator.is_degenerate()) {
    throw std::runtime_error(
        "Cannot order facets around edge due to degenerated facets");
  }

  std::vector<Point_3> opposite_vertices;
  for (size_t i=0; i<num_adj_faces; i++)
  {
    const size_t o = get_opposite_vertex( get_face_index(adj_faces[i]));
    opposite_vertices.emplace_back(
        V(o, 0), V(o, 1), V(o, 2));
  }

  std::vector<int> positive_side;
  std::vector<int> negative_side;
  std::vector<int> tie_positive_oriented;
  std::vector<int> tie_negative_oriented;

  std::vector<size_t> positive_side_index;
  std::vector<size_t> negative_side_index;
  std::vector<size_t> tie_positive_oriented_index;
  std::vector<size_t> tie_negative_oriented_index;

  for (size_t i=0; i<num_adj_faces; i++)
  {
    const int f = adj_faces[i];
    const Point_3& p_a = opposite_vertices[i];
    auto orientation = separator.oriented_side(p_a);
    switch (orientation) {
      case CGAL::ON_POSITIVE_SIDE:
        positive_side.push_back(f);
        positive_side_index.push_back(i);
        break;
      case CGAL::ON_NEGATIVE_SIDE:
        negative_side.push_back(f);
        negative_side_index.push_back(i);
        break;
      case CGAL::ON_ORIENTED_BOUNDARY:
        {
          auto inplane_orientation = CGAL::coplanar_orientation(
              p_s, p_d, p_o, p_a);
          switch (inplane_orientation) {
            case CGAL::POSITIVE:
              tie_positive_oriented.push_back(f);
              tie_positive_oriented_index.push_back(i);
              break;
            case CGAL::NEGATIVE:
              tie_negative_oriented.push_back(f);
              tie_negative_oriented_index.push_back(i);
              break;
            case CGAL::COLLINEAR:
            default:
              throw std::runtime_error(
                  "Degenerated facet detected.");
              break;
          }
        }
        break;
      default:
        // Should not be here.
        throw std::runtime_error("Unknown CGAL state detected.");
    }
  }
  if (debug) {
    std::cout << "tie positive: " << std::endl;
    for (auto& f : tie_positive_oriented) {
      std::cout << get_face_index(f) << " ";
    }
    std::cout << std::endl;
    std::cout << "positive side: " << std::endl;
    for (auto& f : positive_side) {
      std::cout << get_face_index(f) << " ";
    }
    std::cout << std::endl;
    std::cout << "tie negative: " << std::endl;
    for (auto& f : tie_negative_oriented) {
      std::cout << get_face_index(f) << " ";
    }
    std::cout << std::endl;
    std::cout << "negative side: " << std::endl;
    for (auto& f : negative_side) {
      std::cout << get_face_index(f) << " ";
    }
    std::cout << std::endl;
  }

  auto index_sort = [](std::vector<int>& data) -> std::vector<size_t>{
    const size_t len = data.size();
    std::vector<size_t> order(len);
    for (size_t i=0; i<len; i++) { order[i] = i; }
    auto comp = [&](size_t i, size_t j) { return data[i] < data[j]; };
    std::sort(order.begin(), order.end(), comp);
    return order;
  };

  DerivedI positive_order, negative_order;
  order_facets_around_edge(V, F, s, d, positive_side, positive_order, debug);
  order_facets_around_edge(V, F, s, d, negative_side, negative_order, debug);
  std::vector<size_t> tie_positive_order = index_sort(tie_positive_oriented);
  std::vector<size_t> tie_negative_order = index_sort(tie_negative_oriented);

  // Copy results into order vector.
  const size_t tie_positive_size = tie_positive_oriented.size();
  const size_t tie_negative_size = tie_negative_oriented.size();
  const size_t positive_size = positive_order.size();
  const size_t negative_size = negative_order.size();

  order.resize(
      tie_positive_size + positive_size + tie_negative_size + negative_size,1);

  size_t count=0;
  for (size_t i=0; i<tie_positive_size; i++)
  {
    order(count+i, 0) = tie_positive_oriented_index[tie_positive_order[i]];
  }
  count += tie_positive_size;

  for (size_t i=0; i<negative_size; i++)
  {
    order(count+i, 0) = negative_side_index[negative_order(i, 0)];
  }
  count += negative_size;

  for (size_t i=0; i<tie_negative_size; i++)
  {
    order(count+i, 0) = tie_negative_oriented_index[tie_negative_order[i]];
  }
  count += tie_negative_size;

  for (size_t i=0; i<positive_size; i++)
  {
    order(count+i, 0) = positive_side_index[positive_order(i, 0)];
  }
  count += positive_size;
  assert(count == num_adj_faces);

  // Find the correct start point.
  size_t start_idx = 0;
  for (size_t i=0; i<num_adj_faces; i++)
  {
    const Point_3& p_a = opposite_vertices[order(i, 0)];
    const Point_3& p_b =
      opposite_vertices[order((i+1)%num_adj_faces, 0)];
    auto orientation = CGAL::orientation(p_s, p_d, p_a, p_b);
    if (orientation == CGAL::POSITIVE)
    {
      // Angle between triangle (p_s, p_d, p_a) and (p_s, p_d, p_b) is
      // more than 180 degrees.
      start_idx = (i+1)%num_adj_faces;
      break;
    } else if (orientation == CGAL::COPLANAR &&
        Plane_3(p_s, p_d, p_a).orthogonal_direction() !=
        Plane_3(p_s, p_d, p_b).orthogonal_direction())
    {
      // All 4 points are coplanar, but p_a and p_b are on each side of
      // the edge (p_s, p_d).  This means the angle between triangle
      // (p_s, p_d, p_a) and (p_s, p_d, p_b) is exactly 180 degrees.
      start_idx = (i+1)%num_adj_faces;
      break;
    }
  }
  DerivedI circular_order = order;
  for (size_t i=0; i<num_adj_faces; i++)
  {
    order(i, 0) = circular_order((start_idx + i)%num_adj_faces, 0);
  }
}

template<
  typename DerivedV,
  typename DerivedF,
  typename DerivedI>
IGL_INLINE
void igl::copyleft::cgal::order_facets_around_edge(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedF>& F,
  size_t s,
  size_t d,
  const std::vector<int>& adj_faces,
  const Eigen::PlainObjectBase<DerivedV>& pivot_point,
  Eigen::PlainObjectBase<DerivedI>& order)
{
  assert(V.cols() == 3);
  assert(F.cols() == 3);
  assert(pivot_point.cols() == 3);
  auto signed_index_to_index = [&](int signed_idx)
  {
      return abs(signed_idx) -1;
  };
  auto get_opposite_vertex_index = [&](size_t fid) -> typename DerivedF::Scalar
  {
      typedef typename DerivedF::Scalar Index;
      if (F(fid, 0) != (Index)s && F(fid, 0) != (Index)d) return F(fid, 0);
      if (F(fid, 1) != (Index)s && F(fid, 1) != (Index)d) return F(fid, 1);
      if (F(fid, 2) != (Index)s && F(fid, 2) != (Index)d) return F(fid, 2);
      assert(false);
      // avoid warning
      return -1;
  };

  {
    // Check if s, d and pivot are collinear.
    typedef CGAL::Exact_predicates_exact_constructions_kernel K;
    K::Point_3 ps(V(s,0), V(s,1), V(s,2));
    K::Point_3 pd(V(d,0), V(d,1), V(d,2));
    K::Point_3 pp(pivot_point(0,0), pivot_point(0,1), pivot_point(0,2));
    if (CGAL::collinear(ps, pd, pp)) {
        throw std::runtime_error(
                "Pivot point is collinear with the outer edge!");
    }
  }

  const size_t N = adj_faces.size();
  const size_t num_faces = N + 1; // N adj faces + 1 pivot face

  // Because face indices are used for tie breaking, the original face indices
  // in the new faces array must be ascending.
  auto comp = [&](int i, int j)
  {
    return signed_index_to_index(adj_faces[i]) <
      signed_index_to_index(adj_faces[j]);
  };
  std::vector<size_t> adj_order(N);
  for (size_t i=0; i<N; i++) adj_order[i] = i;
  std::sort(adj_order.begin(), adj_order.end(), comp);

  DerivedV vertices(num_faces + 2, 3);
  for (size_t i=0; i<N; i++)
  {
    const size_t fid = signed_index_to_index(adj_faces[adj_order[i]]);
    vertices.row(i) = V.row(get_opposite_vertex_index(fid));
  }
  vertices.row(N  ) = pivot_point;
  vertices.row(N+1) = V.row(s);
  vertices.row(N+2) = V.row(d);

  DerivedF faces(num_faces, 3);
  for (size_t i=0; i<N; i++)
  {
    if (adj_faces[adj_order[i]] < 0)
    {
      faces(i,0) = N+1; // s
      faces(i,1) = N+2; // d
      faces(i,2) = i  ;
    } else
    {
      faces(i,0) = N+2; // d
      faces(i,1) = N+1; // s
      faces(i,2) = i  ;
    }
  }
  // Last face is the pivot face.
  faces(N, 0) = N+1;
  faces(N, 1) = N+2;
  faces(N, 2) = N;

  std::vector<int> adj_faces_with_pivot(num_faces);
  for (size_t i=0; i<num_faces; i++)
  {
    if ((size_t)faces(i,0) == N+1 && (size_t)faces(i,1) == N+2)
    {
        adj_faces_with_pivot[i] = int(i+1) * -1;
    } else
    {
        adj_faces_with_pivot[i] = int(i+1);
    }
  }

  DerivedI order_with_pivot;
  order_facets_around_edge(
    vertices, faces, N+1, N+2, adj_faces_with_pivot, order_with_pivot);

  assert((size_t)order_with_pivot.size() == num_faces);
  order.resize(N);
  size_t pivot_index = num_faces + 1;
  for (size_t i=0; i<num_faces; i++)
  {
    if ((size_t)order_with_pivot[i] == N)
    {
      pivot_index = i;
      break;
    }
  }
  assert(pivot_index < num_faces);

  for (size_t i=0; i<N; i++)
  {
    order[i] = adj_order[order_with_pivot[(pivot_index+i+1)%num_faces]];
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::copyleft::cgal::order_facets_around_edge<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, unsigned long, unsigned long, std::vector<int, std::allocator<int> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, bool);
template void igl::copyleft::cgal::order_facets_around_edge<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, unsigned long, unsigned long, std::vector<int, std::allocator<int> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, bool);
template void igl::copyleft::cgal::order_facets_around_edge<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, unsigned long, unsigned long, std::vector<int, std::allocator<int> > const&, Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template void igl::copyleft::cgal::order_facets_around_edge<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, unsigned long, unsigned long, std::vector<int, std::allocator<int> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template void igl::copyleft::cgal::order_facets_around_edge<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, unsigned long, unsigned long, std::vector<int, std::allocator<int> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template void igl::copyleft::cgal::order_facets_around_edge<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, unsigned long, unsigned long, std::vector<int, std::allocator<int> > const&, Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
#endif


#include "mex.h"
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <Eigen/Core>

template <int _N>
void psd_project_rows(Eigen::MatrixXd & H)
{
  const Eigen::Index m = H.rows();
  const int N = _N==Eigen::Dynamic ? round(sqrt(H.cols())) : _N;
  static_assert( _N == Eigen::Dynamic || _N == N,"");
  for(Eigen::Index r = 0;r<m;r++)
  {
    typedef Eigen::Matrix<double,_N,_N> Matrix;
    Matrix Hr(N,N);
    for(int i=0;i<N;i++)
    {
      for(int j=i;j<N;j++)
      {
        Hr(i,j) = H(r,i+N*j);
        Hr(j,i) = H(r,i+N*j);
      }
    }
    Eigen::SelfAdjointEigenSolver<Matrix> es(Hr);
    Hr =
      (es.eigenvectors()*
      es.eigenvalues().cwiseMax(0).asDiagonal() *
      es.eigenvectors().transpose()).eval();
    for(int i=0;i<N;i++)
    {
      for(int j=i;j<N;j++)
      {
        H(r,i+N*j) = Hr(i,j);
        H(r,j+N*i) = Hr(i,j);
      }
    }

  }
}

// https://stackoverflow.com/a/1549960/148668
bool is_perfect_square(int n) {
    if (n < 0)
        return false;
    int root(round(sqrt(n)));
    return n == root * root;
}

void mexFunction(
  int          nlhs,
  mxArray      *plhs[],
  int          nrhs,
  const mxArray *prhs[]
  )
{
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  mexErrMsgTxt(nrhs>=1,"nrhs should be == 1");
  Eigen::MatrixXd H;
  parse_rhs_double(prhs+0,H);

  mexErrMsgTxt(is_perfect_square(H.cols()),"size(H,2) must be perfect square");

  const int N(round(sqrt(H.cols())));
  switch(N)
  {
    case  2: psd_project_rows< 2>(H); break;
    case  3: psd_project_rows< 3>(H); break;
    case  4: psd_project_rows< 4>(H); break;
    case  5: psd_project_rows< 5>(H); break;
    case  6: psd_project_rows< 6>(H); break;
    case  7: psd_project_rows< 7>(H); break;
    case  8: psd_project_rows< 8>(H); break;
    case  9: psd_project_rows< 9>(H); break;
    case 10: psd_project_rows<10>(H); break;
    case 11: psd_project_rows<11>(H); break;
    case 12: psd_project_rows<12>(H); break;
    default:
      psd_project_rows<Eigen::Dynamic>(H);
      break;
  }

  switch(nlhs)
  {
    case 1:
      prepare_lhs_double(H,plhs+0);
    default:break;
  }
  std::cout.rdbuf(outbuf);
}


#define _USE_MATH_DEFINES

#include <stdlib.h>
#include <time.h>
#include <math.h>

#include <climits>
#include <cfloat>
#include <cmath>

#include <boost/multiprecision/cpp_int.hpp>

#include "externals.h"
#include "typedValue.h"
#include "stack.h"
#include "word.h"
#include "context.h"
#include "mathMacro.h"

const BigInt kBigInt_FLT_MAX(FLT_MAX);
const BigInt kBigInt_Minus_FLT_MAX(-FLT_MAX);
const BigInt kBigInt_DBL_MAX(DBL_MAX);
const BigInt kBigInt_Minus_DBL_MAX(-DBL_MAX);

static double deg2rad(double inTheta) { return inTheta/180.0*M_PI; }
static BigFloat deg2rad(BigFloat inTheta) { return inTheta/180.0*M_PI; }
static double rad2deg(double inTheta) { return inTheta/M_PI*180.0; }
static BigFloat rad2deg(BigFloat inTheta) { return inTheta/M_PI*180.0; }

void InitDict_Math() {
	Install(new Word("true",WORD_FUNC {
		inContext.DS.emplace_back(true);
		NEXT;
	}));

	Install(new Word("false",WORD_FUNC {
		inContext.DS.emplace_back(false);
		NEXT;
	}));

	Install(new Word("+",WORD_FUNC { TwoOp(+); }));
	Install(new Word("-",WORD_FUNC { TwoOp(-); }));

	Install(new Word("*",WORD_FUNC { TwoOp(*); }));
	Install(new Word("/",WORD_FUNC { TwoOp(/); }));

	Install(new Word("%",WORD_FUNC {
		Stack& ds=inContext.DS;
		if(ds.size()<2) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue tos=Pop(ds);
		TypedValue& second=ds.back();

		// second %= tos;
		ModAssign(second,tos);
		NEXT;
   }));

	Install(new Word("&",WORD_FUNC { BitOp(&); }));
	Install(new Word("|",WORD_FUNC { BitOp(|); }));
	Install(new Word("^",WORD_FUNC { BitOp(^); }));
	Install(new Word(">>",WORD_FUNC { BitShiftOp(>>); }));
	Install(new Word("<<",WORD_FUNC { BitShiftOp(<<); }));

	// bitwise NOT.
	Install(new Word("~",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt: 		tos.intValue=~tos.intValue;		break;
			case kTypeLong:		tos.longValue=~tos.longValue;	break;
			case kTypeBigInt:	*tos.bigIntPtr=~*tos.bigIntPtr;	break;
			default:
				return inContext.Error_InvalidType(E_TOS_INT_OR_LONG_OR_BIGINT,tos);
		}
		NEXT;
	}));

	Install(new Word(">",WORD_FUNC { CmpOp(>); }));
	Install(new Word("<",WORD_FUNC { CmpOp(<); }));

	Install(new Word(">=",WORD_FUNC { CmpOp(>=); }));
	Install(new Word("<=",WORD_FUNC { CmpOp(<=); }));

	Install(new Word("==",WORD_FUNC { CmpOp(==); }));
	Install(new Word("!=",WORD_FUNC { CmpOp(!=); }));

	Install(new Word("and",WORD_FUNC { BoolOp(&&); }));
	Install(new Word("or",WORD_FUNC { BoolOp(||); }));
	Install(new Word("xor",WORD_FUNC { BoolOp(!=); }));
	Install(new Word("not",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_AT_LEAST_2); }
		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType!=kTypeBool) {
			return inContext.Error_InvalidType(E_TOS_BOOL,tos);
		}
		tos.boolValue = tos.boolValue != true;
		NEXT;
	}));

	Install(new Word("&&",WORD_FUNC {
		if(inContext.DS.size()<2) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue tos=Pop(inContext.DS);
		if(tos.dataType!=kTypeBool && tos.dataType!=kTypeWord) {
			return inContext.Error_InvalidType(E_TOS_BOOL_OR_WP,tos);
		}

		TypedValue second=Pop(inContext.DS);
		if(second.dataType!=kTypeBool && second.dataType!=kTypeWord) {
			return inContext.Error_InvalidType(E_SECOND_BOOL_OR_WP,second);
		}

		if(tos.dataType==kTypeBool) {
			if(tos.boolValue==false) {
				inContext.DS.emplace_back(false);
				goto next;
			}
		} else {
			if(inContext.Exec(tos)==false) { return false; }
			tos=Pop(inContext.DS);
			if(tos.dataType!=kTypeBool) {
				return inContext.Error(E_WP_AT_TOS_SHOULD_PUSH_A_BOOL);
			}
			if(tos.boolValue==false) {
				inContext.DS.emplace_back(false);
				goto next;
			}
		}

		if(second.dataType==kTypeBool) {
			if(second.boolValue==false) {
				inContext.DS.emplace_back(false);
				goto next;
			}
		} else {
			if(inContext.Exec(second)==false) {
				return false;
			}
			second=Pop(inContext.DS);
			if(second.dataType!=kTypeBool) {
				return inContext.Error(E_WP_AT_SECOND_SHOULD_PUSH_A_BOOL);
			}
			if(second.boolValue==false) {
				inContext.DS.emplace_back(false);
				goto next;
			}
		}
		inContext.DS.emplace_back(true);
next:
		NEXT;
	}));
	Install(new Word("||",WORD_FUNC {
		if(inContext.DS.size()<2) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue tos=Pop(inContext.DS);
		if(tos.dataType!=kTypeBool && tos.dataType!=kTypeWord) {
			return inContext.Error_InvalidType(E_TOS_BOOL_OR_WP,tos);
		}

		TypedValue second=Pop(inContext.DS);
		if(second.dataType!=kTypeBool && second.dataType!=kTypeWord) {
			return inContext.Error_InvalidType(E_SECOND_BOOL_OR_WP,tos);
		}

		if(tos.dataType==kTypeBool) {
			if(tos.boolValue==true) {
				inContext.DS.emplace_back(true);
				goto next;
			}
		} else {
			if(inContext.Exec(tos)==false) { return false; }
			tos=Pop(inContext.DS);
			if(tos.dataType!=kTypeBool) {
				return inContext.Error(E_WP_AT_TOS_SHOULD_PUSH_A_BOOL);
			}
			if(tos.boolValue==true) {
				inContext.DS.emplace_back(true);
				goto next;
			}
		}

		if(second.dataType==kTypeBool) {
			if(second.boolValue==true) {
				inContext.DS.emplace_back(true);
				goto next;
			}
		} else {
			if(inContext.Exec(second)==false) {
				return false;
			}
			second=Pop(inContext.DS);
			if(second.dataType!=kTypeBool) {
				return inContext.Error(E_WP_AT_SECOND_SHOULD_PUSH_A_BOOL);
			}
			if(second.boolValue==true) {
				inContext.DS.emplace_back(true);
				goto next;
			}
		}
		inContext.DS.emplace_back(false);
next:
		NEXT;
	}));

	Install(new Word("sqrt",WORD_FUNC { OneArgFloatingFunc(sqrt); }));
	Install(new Word("exp", WORD_FUNC { OneParamFunc(exp);  }));
	Install(new Word("log", WORD_FUNC { OneParamFunc(log);  }));
	Install(new Word("log10", WORD_FUNC { OneParamFunc(log10);  }));

	Install(new Word("sin",WORD_FUNC { OneParamFunc(sin); }));
	Install(new Word("cos",WORD_FUNC { OneParamFunc(cos); }));
	Install(new Word("tan",WORD_FUNC { OneParamFunc(tan); }));

	Install(new Word("asin",WORD_FUNC { OneParamFunc(asin); }));
	Install(new Word("acos",WORD_FUNC { OneParamFunc(acos); }));
	Install(new Word("atan",WORD_FUNC { OneParamFunc(atan); }));

	Install(new Word("floor",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		FloorOrCeil(floor,tos);
		NEXT;
	}));

	Install(new Word("ceil",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		FloorOrCeil(ceil,tos);
		NEXT;
	}));

	Install(new Word("deg-to-rad",WORD_FUNC { OneParamFunc(deg2rad); }));
	Install(new Word("rad-to-deg",WORD_FUNC { OneParamFunc(rad2deg); }));

	Install(new Word("pow",WORD_FUNC {
		using namespace boost::multiprecision;

		if(inContext.DS.size()<2) { return inContext.Error(E_DS_AT_LEAST_2); }

		TypedValue tos=Pop(inContext.DS);
		TypedValue& second=ReadTOS(inContext.DS);
		if(second.dataType==kTypeDouble) {
			switch(tos.dataType) {
				case kTypeInt: /* double x int -> double */
					second.doubleValue=pow(second.doubleValue,(double)tos.intValue);
					break;
				case kTypeLong: /* double x long -> double */
					second.doubleValue=pow(second.doubleValue,(double)tos.longValue);
					break;
				case kTypeFloat: /* double x float -> double */
					second.doubleValue=pow(second.doubleValue,(double)tos.floatValue);
					break;
				case kTypeDouble: /* double x double -> dobule */
					second.doubleValue=pow(second.doubleValue,tos.doubleValue);
					break;
				case kTypeBigInt: { /* double x bigInt -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.doubleValue),
									  BigFloat(*tos.bigIntPtr));
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				case kTypeBigFloat: { /* double x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.doubleValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeBigInt) {
			switch(tos.dataType) {
				case kTypeInt:	/* bigInt x int -> bigInt */
					*second.bigIntPtr=pow(*second.bigIntPtr,tos.intValue);
					break;
				case kTypeLong:	/* bigInt x long -> bigInt */
					*second.bigIntPtr=pow(*second.bigIntPtr,tos.longValue);
					break;
				case kTypeFloat: {	/* bigInt x float -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(*second.bigIntPtr),
									  BigFloat(tos.floatValue));
						delete(second.bigIntPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				case kTypeDouble: {	/* bigInt x double -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(*second.bigIntPtr),
									  BigFloat(tos.doubleValue));
						delete(second.bigIntPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				case kTypeBigInt: /* bigInt x bigInt -> bigInt */
					return inContext.Error_InvalidType(E_OUT_OF_SUPPORT_TOS_SECOND,
													   tos,second);
				case kTypeBigFloat: { /* bigInt x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(*second.bigIntPtr),*tos.bigFloatPtr);
						delete(second.bigIntPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeBigFloat) {
			switch(tos.dataType) {
				case kTypeInt: /* bigFloat x int -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(tos.intValue));
					break;
				case kTypeLong: /* bigFloat x long -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											 BigFloat(tos.longValue));
					break;
				case kTypeFloat: /* bigFloat x float -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(tos.floatValue));
					break;
				case kTypeDouble: /* bigFloat x double -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(tos.doubleValue));
					break;
				case kTypeBigInt: /* bigFloat x bigInt -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(*tos.bigIntPtr));
					break;
				case kTypeBigFloat: /* bigFloat x bigFloat -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,*tos.bigFloatPtr);
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeInt) {
			switch(tos.dataType) {
				case kTypeInt:	/* int x int -> int */
					second.intValue=(int)pow(second.intValue,tos.intValue);
					break;
				case kTypeLong:	/* int x long -> long */
					second.longValue=(long)pow((long)second.intValue,tos.longValue);
					second.dataType=kTypeLong;
					break;
				case kTypeFloat: /* int x float -> float */
					second.floatValue=(float)pow((float)second.intValue,
												 tos.floatValue);
					second.dataType=kTypeFloat;
					break;
				case kTypeDouble: /* int x double -> double */
					second.doubleValue=(double)pow((double)second.intValue,
												   tos.doubleValue);
					second.dataType=kTypeDouble;
					break;
				case kTypeBigInt: /* int x bigInt -> OutOfSupport */
					return inContext.Error_InvalidType(E_OUT_OF_SUPPORT_TOS_SECOND,
													   tos,second);
				case kTypeBigFloat: { /* int x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.intValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeLong) {
			switch(tos.dataType) {
				case kTypeInt:	/* long x int -> long */
					second.longValue=(long)pow(second.longValue,(long)tos.intValue);
					break;
				case kTypeLong:	/* long x long -> long */
					second.longValue=(long)pow(second.longValue,tos.longValue);
					break;
				case kTypeFloat:	/* long x float -> float */
					second.floatValue=(float)pow((float)second.longValue,
												 tos.floatValue);
					second.dataType=kTypeFloat;
					break;
				case kTypeDouble:	/* long x double -> double */
					second.doubleValue=(double)pow((double)second.longValue,
												   tos.doubleValue);
					second.dataType=kTypeDouble;
					break;
				case kTypeBigInt: /* long x bigInt -> OutOfSupport */
					return inContext.Error_InvalidType(E_OUT_OF_SUPPORT_TOS_SECOND,
													   tos,second);
				case kTypeBigFloat: { /* long x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.longValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==kTypeFloat) {
			switch(tos.dataType) {
				case kTypeInt: /* float x int -> float */
					second.floatValue=(float)pow((double)second.floatValue,
												 (double)tos.intValue);
					break;
				case kTypeLong: /* float x long -> float*/
					second.floatValue=(float)pow((double)second.floatValue,
												 (double)tos.longValue);
					break;
				case kTypeFloat: /* float x float -> float */
					second.floatValue=(float)pow((double)second.floatValue,
												 (double)tos.floatValue);
					break;
				case kTypeDouble: /* float x double -> dobule */
					second.doubleValue=pow((double)second.floatValue,tos.doubleValue);
					second.dataType=kTypeDouble;
					break;
				case kTypeBigInt: { /* float x bigInt -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.floatValue),
									  BigFloat(*tos.bigIntPtr));
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				case kTypeBigFloat: { /* float x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.floatValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else {
onError:
			return inContext.Error_InvalidType(E_INVALID_DATA_TYPE_TOS_SECOND,
											   tos,second);
		}
		NEXT;
	}));

	Install(new Word("0?",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:
				tos.dataType=kTypeBool;
				tos.boolValue=tos.intValue==0;
				break;
			case kTypeLong:
				tos.dataType=kTypeBool;
				tos.boolValue=tos.longValue==0;
				break;
			case kTypeBigInt: {
					BigInt *biPtr=tos.bigIntPtr;
					tos.dataType=kTypeBool;
					tos.boolValue=*biPtr==0;
					delete(biPtr);
				}
				break;
			case kTypeFloat:
				tos.dataType=kTypeBool;
				tos.boolValue=tos.floatValue==0;
				break;
			case kTypeDouble:
				tos.dataType=kTypeBool;
				tos.boolValue=tos.doubleValue==0;
				break;
			case kTypeBigFloat: {
					BigFloat *bfPtr=tos.bigFloatPtr;
					tos.dataType=kTypeBool;
					tos.boolValue=*bfPtr==0;
					delete(bfPtr);
				}
				break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	// equivalent to '1 +'.
	Install(new Word("1+",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:		tos.intValue+=1;		break;
			case kTypeFloat:	tos.floatValue+=1;		break;
			case kTypeDouble:	tos.doubleValue+=1;		break;
			case kTypeBigInt:	*tos.bigIntPtr+=1;		break;
			case kTypeBigFloat:	*tos.bigFloatPtr+=1;	break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	// equivalent to '1 -'.
	Install(new Word("1-",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:		tos.intValue-=1;	break;
			case kTypeFloat:	tos.floatValue-=1;	break;
			case kTypeDouble:	tos.doubleValue-=1;	break;
			case kTypeBigInt:	*tos.bigIntPtr-=1;	break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word("2/",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:		tos.intValue/=2;		break;
			case kTypeFloat:	tos.floatValue/=2.0f;	break;
			case kTypeDouble:	tos.doubleValue/=2.0;	break;
			case kTypeBigInt:	*tos.bigIntPtr/=2;		break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word("rand-max",WORD_FUNC {
		inContext.DS.emplace_back(RAND_MAX);
		NEXT;
	}));

	// [0,RAND_MAX]
	Install(new Word("rand",WORD_FUNC {
		inContext.DS.emplace_back(rand());
		NEXT;
	}));

	// [0,1]
	Install(new Word("random",WORD_FUNC {
		inContext.DS.emplace_back((float)rand()/(float)RAND_MAX);
		NEXT;
	}));

	Install(new Word("randomize",WORD_FUNC {
		srand((unsigned int)time(NULL));
		NEXT;
	}));

	// 整数 --- 数値
	// n --- t
	// s.t. t is an integer and t in [0,n).
	Install(new Word("rand-to",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue tos=Pop(inContext.DS);
		if(tos.dataType!=kTypeInt) {
			return inContext.Error_InvalidType(E_TOS_INT,tos);
		}

		const int n=tos.intValue;
		if(n<0 || RAND_MAX<n) { return inContext.Error(E_TOS_POSITIVE_INT,n); }
		int t=(int)(rand()/((float)RAND_MAX+1)*n);
		inContext.DS.emplace_back(t);
		NEXT;
	}));

	Install(new Word("pi",WORD_FUNC {
		inContext.DS.emplace_back(M_PI);
		NEXT;
	}));

	Install(new Word(">int",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }

		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType==kTypeFloat) {
			if(tos.floatValue<INT_MIN || INT_MAX<tos.floatValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=(int)tos.floatValue;
		} else if(tos.dataType==kTypeDouble) {
			if(tos.doubleValue<INT_MIN || INT_MAX<tos.doubleValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=(int)tos.doubleValue;
		} else if(tos.dataType==kTypeLong) {
			if(tos.longValue<(long)INT_MIN || (long)INT_MAX<tos.longValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=(int)tos.longValue;
		} else if(tos.dataType==kTypeBigInt) {
			if(*tos.bigIntPtr<INT_MIN || INT_MAX<*tos.bigIntPtr) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=static_cast<int>(*tos.bigIntPtr);
		} else if(tos.dataType==kTypeAddress) {
			tos.dataType=kTypeInt;
		} else if(tos.dataType!=kTypeInt) {
			return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		tos.dataType=kTypeInt;
		NEXT;
	}));

	Install(new Word(">long",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType==kTypeFloat) {
			if(tos.floatValue<LONG_MIN || LONG_MAX<tos.floatValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_LONG_DUE_TO_OVERFLOW);
			}
			tos.longValue=(long)tos.floatValue;
		} else if(tos.dataType==kTypeDouble) {
			if(tos.doubleValue<LONG_MIN || LONG_MAX<tos.doubleValue) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_LONG_DUE_TO_OVERFLOW);
			}
			tos.longValue=(long)tos.doubleValue;
		} else if(tos.dataType==kTypeInt) {
			tos.longValue=(long)tos.intValue;
		} else if(tos.dataType==kTypeBigInt) {
			if(*tos.bigIntPtr<LONG_MIN || LONG_MAX<*tos.bigIntPtr) {
				return inContext.Error(E_CAN_NOT_CONVERT_TO_LONG_DUE_TO_OVERFLOW);
			}
			tos.longValue=static_cast<long>(*tos.bigIntPtr);
		} else if(tos.dataType!=kTypeLong) {
			return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		tos.dataType=kTypeLong;
		NEXT;
	}));

	Install(new Word(">INT",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue tos=Pop(inContext.DS);
		BigInt bigInt;
		switch(tos.dataType) {
			case kTypeInt:	  bigInt=tos.intValue;		break;
			case kTypeLong:	  bigInt=tos.longValue;		break;
			case kTypeFloat:  bigInt=static_cast<BigInt>(tos.floatValue);	break;
			case kTypeDouble: bigInt=static_cast<BigInt>(tos.doubleValue);	break;
			case kTypeString: bigInt=BigInt(*tos.stringPtr); break;
			case kTypeBigInt:
				// do nothing
				break;
			case kTypeBigFloat: bigInt=static_cast<BigInt>(*tos.bigFloatPtr); break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER_OR_STRING,tos);
		}
		if(tos.dataType!=kTypeBigInt) {
			inContext.DS.emplace_back(bigInt);
		} else {
			inContext.DS.emplace_back(*tos.bigIntPtr);
		}
		NEXT;
	}));

	Install(new Word(">float",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:
				tos.floatValue=(float)tos.intValue;
				tos.dataType=kTypeFloat;
				break;
			case kTypeLong:
				tos.floatValue=(float)tos.longValue;
				tos.dataType=kTypeFloat;
				break;
			case kTypeBigInt: {
					if(*tos.bigIntPtr>kBigInt_FLT_MAX
					  || *tos.bigIntPtr<kBigInt_Minus_FLT_MAX) {
						return inContext.Error(
								E_CAN_NOT_CONVERT_TO_FLOAT_DUE_TO_OVERFLOW);
					}
					float f=tos.ToFloat(inContext);
					delete tos.bigIntPtr;
					tos.floatValue=f;
					tos.dataType=kTypeFloat;
				}
				break;
			case kTypeFloat:
				// do nothing
				break;
			case kTypeDouble:
				if(tos.doubleValue>FLT_MAX || tos.doubleValue<-FLT_MAX) {
					return inContext.Error(E_CAN_NOT_CONVERT_TO_FLOAT_DUE_TO_OVERFLOW);
				}
				tos.floatValue=(float)tos.doubleValue;
				tos.dataType=kTypeFloat;
				break;
			case kTypeBigFloat: {
					if(*tos.bigFloatPtr>FLT_MAX || *tos.bigFloatPtr<-FLT_MAX) {
						return inContext.Error(
								E_CAN_NOT_CONVERT_TO_FLOAT_DUE_TO_OVERFLOW);
					}
					float f=tos.ToFloat(inContext);
					delete tos.bigFloatPtr;
					tos.floatValue=f;
					tos.dataType=kTypeFloat;
				}
				break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word(">double",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case kTypeInt:
				tos.doubleValue=(double)tos.intValue;
				tos.dataType=kTypeDouble;
				break;
			case kTypeLong:
				tos.doubleValue=(double)tos.longValue;
				tos.dataType=kTypeDouble;
				break;
			case kTypeBigInt: {
					if(*tos.bigIntPtr>kBigInt_DBL_MAX
					  || *tos.bigIntPtr<kBigInt_Minus_DBL_MAX) {
						return inContext.Error(
								E_CAN_NOT_CONVERT_TO_DOUBLE_DUE_TO_OVERFLOW);
					}
					double t=tos.ToDouble(inContext);
					delete tos.bigIntPtr;
					tos.doubleValue=t;
					tos.dataType=kTypeDouble;
				}
				break;
			case kTypeFloat:
				tos.doubleValue=(double)tos.floatValue;
				tos.dataType=kTypeDouble;
				break;
			case kTypeDouble:
				// do nothing
				break;
			case kTypeBigFloat: {
					if(*tos.bigFloatPtr>DBL_MAX || *tos.bigFloatPtr<-DBL_MAX) {
						return inContext.Error(
								E_CAN_NOT_CONVERT_TO_DOUBLE_DUE_TO_OVERFLOW);
					}
					double t=tos.ToDouble(inContext);
					delete tos.bigFloatPtr;
					tos.doubleValue=t;
					tos.dataType=kTypeDouble;
				}
				break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word(">FLOAT",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue tos=Pop(inContext.DS);
		BigFloat bigFloat;
		switch(tos.dataType) {
			case kTypeInt:	  bigFloat=tos.intValue;	break;
			case kTypeLong:	  bigFloat=tos.longValue;	break;
			case kTypeFloat:  bigFloat=tos.floatValue;	break;
			case kTypeDouble: bigFloat=tos.doubleValue;	break;
			case kTypeString: bigFloat=BigFloat(*tos.stringPtr); break;
			case kTypeBigInt: bigFloat=static_cast<BigFloat>(*tos.bigIntPtr); break;
			case kTypeBigFloat: /* do nothing */ 		break;
			default:
				return inContext.Error_InvalidType(E_TOS_NUMBER_OR_STRING,tos);
		}
		if(tos.dataType!=kTypeBigFloat) {
			inContext.DS.emplace_back(bigFloat);
		} else {
			inContext.DS.emplace_back(*tos.bigFloatPtr);
		}
		NEXT;
	}));

	Install(new Word(">address",WORD_FUNC {
		if(inContext.DS.size()<1) { return inContext.Error(E_DS_IS_EMPTY); }
		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType!=kTypeInt && tos.dataType!=kTypeAddress) {
			return inContext.Error_InvalidType(E_TOS_INT,tos);
		}
		tos.dataType=kTypeAddress;
		NEXT;
	}));
}


#include <iostream>
#include <iomanip>
#include <fstream>
#include <cmath>
#include <vector>
#include <chrono>
#include <string>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include "../src/markovchains/MarkovChain.h"

using namespace Eigen;
using namespace std;
using namespace MarkovChain;

// Eigen Flags
//#define EIGEN_NDEBUG
//#define EIGEN_NO_STATIC_ASSERT

//// ENUMERATED CLASS FOR POLICY TYPE
enum class Policies
{
    IDLE,
    HIRE_LOW_WAGE,
    HIRE_HIGH_WAGE,
    LAYOFF_LOW_WAGE,
    LAYOFF_HIGH_WAGE,
    EXIT,
};

//// FUNCTION PHI THAT RETURNS SCALING OF HIRING ENTRY BY FIRM SIZE (PART OF THE PARAMETERS OF THE MODEL!!!)
double phi(int total_employment){
    return total_employment + 1;
}

//// MAIN PROGRAM
int main()
{
    const bool verbose = true;
    const bool graphs = true;
    const bool tables = false;
    const bool load_guess_from_file = false;
    const bool save_guess_to_file = false;

    // Greetings
    if (verbose) {
        cout <<"----------------------------------------------\n";
        cout <<"------| SOLVING THE PROBLEM OF THE FIRM |-----\n";
        cout <<"----------------------------------------------\n\n";
    }

    // Start Clock
    auto t0 = std::chrono::high_resolution_clock::now();

    // Endogenous Variables (should be passed as arguments of the function)
    double hiring_rate_low = 0.1;
    double hiring_rate_high = 3.0;
    double quit_rate_low = 0.07;

    // Parameters of the state space
    const int nw = 2;       // ASSUME N_w = 2 for the time being
    const int nz = 4;       // For now fix it at nz = 4
    const int nbar = 60;
    const double wl = 1.0;
    const double wh = 1.25;

    // Parameters of the model (read from file)
    const double decreasing_returns = 0.666666666;
    const double real_rate = 0.01;
    const double discount_rate = log(1.0 + real_rate);
    const double fixed_cost = 1.5;
    const double exit_rate = 1e5;
    const double layoff_rate = 13.0;
    const double layoff_cost = 1.0;
    const double exogenous_separations_rate = 1.0/16.0;
    const double aggregate_productivity = 1.0;

    // Parameters for HJB
    const double hjb_criterion = 1e-10;
    const int hjb_maxit = 100;
    const double hjb_step_size = 1e8;
    const double hjb_criterion_BiCGSTAB = 1e-6; // Must be relative to criterion_hjb

    // Parameters for KFE
    const double kfe_criterion = 1e-10;
//    const int kfe_maxit = 100;
    const double kfe_step_size = 1e8;
    const double criterion_kfe_BiCGSTAB = 1e-6; // Must be relative to criterion_hjb

    // Infinitesimal Generator of Poisson Process for Productivity and set \mathcal{Z}.
    Matrix4d Q;
    Q(0,0) = -0.00; Q(0,1) =  0.00; Q(0,2) =  0.00; Q(0,3) =  0.00;
    Q(1,0) =  0.10; Q(1,1) = -0.55; Q(1,2) =  0.30; Q(1,3) =  0.15;
    Q(2,0) =  0.10; Q(2,1) =  0.25; Q(2,2) = -0.60; Q(2,3) =  0.25;
    Q(3,0) =  0.10; Q(3,1) =  0.15; Q(3,2) =  0.30; Q(3,3) = -0.55;

    Vector4d productivity;
    productivity(0) = 0.00;
    productivity(1) = 2.00;
    productivity(2) = 2.75;
    productivity(3) = 3.50;

    // Entrants
    Vector4d distribution_of_entrants;
    distribution_of_entrants(0) = 0.00;
    distribution_of_entrants(1) = 0.25;
    distribution_of_entrants(2) = 0.50;
    distribution_of_entrants(3) = 0.25;

    // ADD PRINTING OF PARAMETERS
    if (verbose){
        // PRINT PARAMETERS HERE
    }

    // Type Definitions for Clarity
    typedef Triplet<double> T;
    typedef SparseMatrix<double, ColMajor> SM;
    typedef SparseMatrix<double, RowMajor> SMr;

    //// STEP 1: CREATE SPARSE MATRICES & PROFIT VECTORS
    // NOTE: formulation removes for the first time all states such that n_l(omega) + n_h(omega) > n_bar.

    if (verbose){
        cout <<"--------| CREATING SPARSE MATRICES... |-------\n";
    }

    const int num_endog_states = (nbar + 1) * (nbar + 2) / 2;
    const int num_tot_states = nz * num_endog_states;

    // Create matrix for lookup between idx and labor force (not trivial mapping, will try to fix this in the future)
    MatrixX3i table_of_states(num_endog_states, 3);
    MatrixXi table_of_states_inv = MatrixXi::Zero(nbar+1, nbar+1);
    int k = 0;
    for (int j = 0; j <= nbar; j += 1) {
        for (int i = 0; i <= nbar; i += 1) {
            if (i + j <= nbar) {
                table_of_states(k,0) = k;
                table_of_states(k,1) = i;
                table_of_states(k,2) = j;
                table_of_states_inv(i,j) = k;
                k += 1;
            }
        }
    }

    // Define triplets + sparse matrices, and vectors for profits and severance pay
    vector<T> triplet_stay; triplet_stay.reserve(num_tot_states * (2 * nz + 6));
//    vector<T> triplet_layoff_low; triplet_layoff_low.reserve(2 * num_tot_states);
//    vector<T> triplet_layoff_high; triplet_layoff_high.reserve(2 * num_tot_states);
//    vector<T> triplet_hire_low; triplet_hire_low.reserve(2 * num_tot_states);
//    vector<T> triplet_hire_high; triplet_hire_high.reserve(2 * num_tot_states);
//    vector<T> triplet_exit; triplet_exit.reserve(num_tot_states);
//    SM stay(num_tot_states,num_tot_states);
//    SM endog_sep(num_tot_states,num_tot_states);
//    SM layoff_low(num_tot_states,num_tot_states);
//    SM layoff_high(num_tot_states,num_tot_states);
//    SM hire_low(num_tot_states,num_tot_states);
//    SM hire_high(num_tot_states,num_tot_states);
//    SM exit(num_tot_states,num_tot_states);
    VectorXd profits(num_tot_states);
    VectorXd severance_pay_low(num_tot_states);
    VectorXd severance_pay_high(num_tot_states);

    // Populate objects by looping over all states, endogenous and exogenous.
    for (int i = 0; i < num_tot_states; i += 1) {
        int idx_endog = i % num_endog_states;
        int nl = table_of_states(idx_endog, 1);
        int nh = table_of_states(idx_endog, 2);
        int n = nl + nh;
        int z = i / num_endog_states;
        if ( nl != 0) {
            triplet_stay.emplace_back(T(i, i, - (exogenous_separations_rate + quit_rate_low) * nl));
            triplet_stay.emplace_back(T(i, i - 1, (exogenous_separations_rate + quit_rate_low) * nl));
//            triplet_layoff_low[2 * i] = T(i, i, -layoff_rate * nl);
//            triplet_layoff_low[2 * i + 1] = T(i, i - 1, layoff_rate * nl);
        }
        if (nh != 0) {
            triplet_stay.emplace_back(T(i, i, - exogenous_separations_rate * nh));
            triplet_stay.emplace_back(T(i, i - (nbar - nh + 2), exogenous_separations_rate * nh));
//            triplet_layoff_high[2 * i] = T(i, i  , -layoff_rate * nh);
//            triplet_layoff_high[2 * i + 1] = T(i, i - (nbar - nh + 2), layoff_rate * nh);
        }
        if (n != nbar) {
//            triplet_hire_low[2 * i] = T(i, i, - hiring_rate_low * phi(n));
//            triplet_hire_low[2 * i + 1] = T(i, i + 1, hiring_rate_low * phi(n));
//            triplet_hire_high[2 * i] = T(i, i, - hiring_rate_high * phi(n));
//            triplet_hire_high[2 * i + 1] = T(i, i + (nbar - nh + 1), hiring_rate_high * phi(n));
        }
//        triplet_exit[i] = T(i, i, - exit_rate);
        for (int k = 0; k < nz; k += 1) {
            triplet_stay.emplace_back(T(i, i + (k - z) * num_endog_states, Q(z,k)));
        }
        profits(i) = productivity(z) * pow(n, decreasing_returns) - wl * nl - wh * nh - fixed_cost;
        severance_pay_low(i) = -layoff_cost * layoff_rate * wl * nl;
        severance_pay_high(i) = -layoff_cost * layoff_rate * wh * nh;
    }
//    triplet_layoff_low.insert(triplet_layoff_low.end(), triplet_stay.begin(), triplet_stay.end());
//    triplet_layoff_high.insert(triplet_layoff_high.end(), triplet_stay.begin(), triplet_stay.end());
//    triplet_hire_low.insert(triplet_hire_low.end(), triplet_stay.begin(), triplet_stay.end());
//    triplet_hire_high.insert(triplet_hire_high.end(), triplet_stay.begin(), triplet_stay.end());
//    triplet_exit.insert(triplet_exit.end(), triplet_stay.begin(), triplet_stay.end());
//    stay.setFromTriplets(triplet_stay.begin(), triplet_stay.end());
//    endog_sep.setFromTriplets(triplet_endog_sep.begin(),triplet_endog_sep.end());
//    layoff_low.setFromTriplets(triplet_layoff_low.begin(), triplet_layoff_low.end());
//    layoff_high.setFromTriplets(triplet_layoff_high.begin(), triplet_layoff_high.end());
//    hire_low.setFromTriplets(triplet_hire_low.begin(), triplet_hire_low.end());
//    hire_high.setFromTriplets(triplet_hire_high.begin(), triplet_hire_high.end());
//    exit.setFromTriplets(triplet_exit.begin(), triplet_exit.end());

    // Matrices for Hiring are h * matrix!
//    hire_low = hiring_rate_low * hire_low;
//    hire_high = hiring_rate_high * hire_high;
//    endog_sep = quit_rate_low * endog_sep;
//    for (int i = 0; i < triplet_endog_sep.size(); i += 1){
//        triplet_endog_sep[i].value() = quit_rate_low * triplet_endog_sep[i].value();
//    }

    // Time
    auto t1 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_sparse = t1 - t0;
    if (verbose) {
        cout <<"Sparse Matrices Computed in: " << elapsed_sparse.count() << "\n\n";
        cout <<"--------| SOLVING THE HJB EQUATION... |-------\n";
    }

    //// STEP 2: SOLVE HJB EQUATION

    // Set guess of value function or try loading it from file.
    VectorXd value = VectorXd::LinSpaced(num_tot_states, 0, 1);
    if (load_guess_from_file) {
        ifstream inf("value_guess.dat", ios::binary);
        if (!inf) {
            ;
        } else {
            int nbar_file;
            int nz_file;
            inf >> nbar_file;
            inf >> nz_file;
            if (nbar_file == nbar && nz_file == nz){
                int i = 0;
                while (inf && i < num_tot_states) {
                    inf >> value(i);
                    i += 1;
                }
            }
        }
    }

    // Preallocate objects for HJB equation loop
    VectorXd value_next(num_tot_states);
    vector<Policies> policy;
    policy.reserve(num_tot_states);
//    VectorXd profits_vector_for_hjb(num_tot_states);
//    MatrixXi::Index maxRow, maxCol;
//    Matrix<double, Dynamic, Dynamic> alternatives(num_tot_states, 6);

    // Identity Matrix
    SMr speye(num_tot_states,num_tot_states);
    speye.setIdentity();

    // Auxiliary Matrix for Linear System

    vector<T> triplet_sparse_matrix_for_hjb;
//    triplet_sparse_matrix_for_hjb.reserve(num_tot_states * (nz * 2 + 6));

    // Solver Definition
    // BiCGSTAB
    BiCGSTAB<SMr> hjb_solver;
    hjb_solver.setTolerance(hjb_criterion_BiCGSTAB * hjb_criterion);

    // SparseLU
//    SparseLU<SM, COLAMDOrdering<int>> hjb_solver;

    // While Loop
    double max_diff = -1;
    double diff = -1;
    int iteration_hjb = 0;
    double supnorm_hjb = 1e100;
    while (iteration_hjb < hjb_maxit && supnorm_hjb > hjb_criterion) {

//        profits_vector_for_hjb = profits;
        VectorXd profits_vector_for_hjb = profits;

        vector<T> triplet_update_infinitesimal_generator;
        triplet_update_infinitesimal_generator.insert(triplet_update_infinitesimal_generator.end(),
                                                      triplet_stay.begin(),
                                                      triplet_stay.end());

        // Infinitesimal Generator (no entry)
        SMr infinitesimal_generator(num_tot_states,num_tot_states);
//        infinitesimal_generator.reserve(VectorXi::Constant(num_tot_states, 3 + nz));

        SMr A(num_tot_states,num_tot_states);

        // Populate objects by looping over all states, endogenous and exogenous.
        for (int i = 0; i < num_tot_states; i += 1) {
            int idx_endog = i % num_endog_states;
            int nl = table_of_states(idx_endog, 1);
            int nh = table_of_states(idx_endog, 2);
            int n = nl + nh;
            int z = i / num_endog_states;

            ArrayXd alternatives = ArrayXd::Constant(6, -1);

            alternatives(0) = 0;
            if (nl != 0) {
                alternatives(3) = layoff_rate * nl * (value(i - 1) - layoff_cost * wl - value(i));
            }
            if (nh != 0) {
                alternatives(4) = layoff_rate * nh * (value(i - (nbar - nh + 2)) - layoff_cost * wh - value(i));
            }
            if (n != nbar) {
                alternatives(1) = hiring_rate_low * phi(n) * (value(i + 1) - value(i));
                alternatives(2) = hiring_rate_high * phi(n) * (value(i + (nbar - nh + 1)) - value(i));
            }
            alternatives(5) = -exit_rate * value(i);

            ArrayXd::Index maxIdx;
            alternatives.maxCoeff(&maxIdx);
            policy[i] = static_cast<Policies>(maxIdx);

            switch (policy[i])
            {
                case Policies::IDLE:
                    break;

                case Policies::HIRE_LOW_WAGE:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -hiring_rate_low * phi(n)));
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i + 1, hiring_rate_low * phi(n)));
                    break;

                case Policies::HIRE_HIGH_WAGE:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -hiring_rate_high * phi(n)));
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i + (nbar - nh + 1), hiring_rate_high * phi(n)));
                    break;

                case Policies::LAYOFF_LOW_WAGE:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -layoff_rate * nl));
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i - 1, layoff_rate * nl));
                    profits_vector_for_hjb(i) += severance_pay_low(i);
                    break;

                case Policies::LAYOFF_HIGH_WAGE:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -layoff_rate * nh));
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i - (nbar - nh + 2), layoff_rate * nh));
                    profits_vector_for_hjb(i) += severance_pay_high(i);
                    break;

                case Policies::EXIT:
                    triplet_update_infinitesimal_generator.emplace_back(T(i, i, -exit_rate));
                    break;
            }
        }

        VectorXd b(num_tot_states);

        b = profits_vector_for_hjb + (1 / hjb_step_size) * value;
        infinitesimal_generator.setFromTriplets(triplet_update_infinitesimal_generator.begin(),
                                                triplet_update_infinitesimal_generator.end());
        A = (1 / hjb_step_size + discount_rate) * speye - infinitesimal_generator;

//        cout << A;
//        cout <<"\n"<< b;
//        for (int i = 0; i < num_tot_states; i += 1){
//            cout << "\n" << static_cast<int>(policy[i]);
//        }

        // BiCGSTAB
        hjb_solver.compute(A);
        value_next = hjb_solver.solveWithGuess(b,value);

        // SparseLU
//        hjb_solver.analyzePattern(A);
//        hjb_solver.factorize(A);
//        value_next = hjb_solver.solve(b);

        // Compute Supnorm
        max_diff = -1;
        diff = -1;
        for (int i = 0; i < num_tot_states; i += 1){
            diff = abs(value(i) - value_next(i));
            if (diff > max_diff) {
                max_diff = diff;
            }
        }
        supnorm_hjb = max_diff;

        // Update value function
        value = value_next;

        // Update iteration count and display advancement
        iteration_hjb += 1;
        if (verbose) {
            if (iteration_hjb % 5 == 0 || iteration_hjb ==1){
                cout <<"Iteration = "<<iteration_hjb<<", SupNorm = "<<supnorm_hjb<<"\n";
            }
        }

        // Save vector of triplets for fast creation of final sparse matrix
        if (supnorm_hjb < hjb_criterion) {
            triplet_sparse_matrix_for_hjb.insert(triplet_sparse_matrix_for_hjb.end(),
                                                 triplet_update_infinitesimal_generator.begin(),
                                                 triplet_update_infinitesimal_generator.end());
        }
    }

    // Add entrants to the Sparse Infinitesimal Generator
    for (int i = 0; i < num_tot_states; i += 1) {
        switch (policy[i])
        {
            case Policies::IDLE:break;
            case Policies::HIRE_LOW_WAGE:break;
            case Policies::HIRE_HIGH_WAGE:break;
            case Policies::LAYOFF_LOW_WAGE:break;
            case Policies::LAYOFF_HIGH_WAGE:break;
            case Policies::EXIT:
                for (int k = 0; k < nz; k += 1) {
                    triplet_sparse_matrix_for_hjb.emplace_back(T(i, num_endog_states * k, exit_rate * distribution_of_entrants(k)));
                }
                break;
        }
    }
//    sparse_matrix_for_hjb.setZero()
    SMr sparse_matrix_for_hjb(num_tot_states,num_tot_states);
    sparse_matrix_for_hjb.setFromTriplets(triplet_sparse_matrix_for_hjb.begin(), triplet_sparse_matrix_for_hjb.end());

    // Save Value Function to File
    if (save_guess_to_file) {
        ofstream outf("value_guess.dat", ios::binary);
        if (!outf) {
            ;
        } else {
            std::streamsize ss = std::cout.precision();
            outf.precision(16);
            outf << nbar << endl;
            outf << nz << endl;
            for(int i =0; i < num_tot_states; i += 1) {
                outf << value(i) << endl;
            }
            outf.precision(ss);
        }
    }

    // Time
    auto t2 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_hjb = t2 - t1;
    if (verbose) {
        if (iteration_hjb % 5 != 0) {
            cout << "Iteration = " << iteration_hjb << ", SupNorm = " << supnorm_hjb << "\n";
        }
        cout <<"----------------------------------------------\n";
        cout <<"HJB Equations solved in: " << elapsed_hjb.count() << "\n\n";
        cout <<"--------| SOLVING THE KF EQUATIONS... |-------\n";
    }

    //// STEP 3: SOLVE KF EQUATIONS

    // Solve KF Equations using Sparse CTMC Class (REMEMBER!! A is the infinitesimal generator, SparseCTMC takes care
    // of getting the adjoint).
    SparseCTMC endogenous_ctmc = SparseCTMC(sparse_matrix_for_hjb);
    VectorXd distribution;
    distribution = endogenous_ctmc.getStationaryDistribution(kfe_step_size);
    cout << "Check Sum to 1:" << distribution.sum() << "\n";

    // Polish distribution trimming low values
    double cumsum = 0.0;
    for (int i = 0; i < num_tot_states; i += 1){
        if (abs(distribution(i)) < kfe_criterion) {
            distribution(i) = 0.0;
        }
        cumsum += distribution(i);
    }
    distribution = distribution / cumsum;

    // Time
    auto t3 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_kfe = t3 - t2;
    if (verbose) {
        cout <<"KF Equations solved in: " << elapsed_kfe.count() << "\n\n";
        cout <<"--------|   COMPUTING AGGREGATES...   |-------\n";
    }

    //// STEP 4: COMPUTE AGGREGATES
    //TODO: Add aggregates
    // Aggregates: Demand Intensities and Employment Shares
    double agg_hiring_flow_low_wage = 0;
    double agg_hiring_flow_high_wage = 0;
    double agg_demand_intensity_low_wage = 0;
    double agg_demand_intensity_high_wage = 0;
    double agg_employment_low_wage = 0;
    double agg_employment_high_wage = 0;

    // Statistics by Firm Size
    VectorXd firm_size_distribution = VectorXd::Zero(nbar + 1);
    VectorXd mean_wage_by_firm_size_temp = VectorXd::Zero(nbar + 1);
    VectorXd layoffs_low_wage_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd layoffs_high_wage_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd layoffs_total_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd mass_layoffs_low_wage_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd mass_layoffs_high_wage_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd mass_layoffs_total_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd endogenous_separations_by_firm_size = VectorXd::Zero(nbar + 1);
    VectorXd poaching_by_firm_size = VectorXd::Zero(nbar + 1);

    // Poaching
    VectorXd poaching_by_state(num_tot_states);
    double agg_fraction_poached;
    double agg_flow_of_quits;
    // TODO ADD POACHING

    // NL - NH graph unconditional on productivity level
    VectorXd unconditional_distribution = VectorXd::Zero(num_endog_states);

    for (int i = 0; i < num_tot_states; i += 1){
        int idx_endog = i % num_endog_states;
        int nl = table_of_states(idx_endog, 1);
        int nh = table_of_states(idx_endog, 2);
        int n = nl + nh;
        int z = i / num_endog_states;

        agg_employment_low_wage += nl * distribution(i);
        agg_employment_high_wage += nh * distribution(i);

        unconditional_distribution(idx_endog) += distribution(i);

        firm_size_distribution(n) += distribution(i);

        mean_wage_by_firm_size_temp(n) += (wl * nl + wh * nh) * distribution(i) / n;

        endogenous_separations_by_firm_size(n) += quit_rate_low * nl * distribution(i);

        switch (policy[i]){
            case Policies::HIRE_HIGH_WAGE:
                agg_demand_intensity_high_wage += distribution(i) * phi(n) * hiring_rate_high;
                break;
            case Policies::HIRE_LOW_WAGE:
                agg_demand_intensity_low_wage += distribution(i) * phi(n) * hiring_rate_low;
                break;
            case Policies::IDLE:break;
            case Policies::LAYOFF_LOW_WAGE:
                layoffs_low_wage_by_firm_size(n) += distribution(i) * nl * layoff_rate;
                layoffs_total_by_firm_size(n) += distribution(i) * nl * layoff_rate;
                break;
            case Policies::LAYOFF_HIGH_WAGE:
                layoffs_high_wage_by_firm_size(n) += distribution(i) * nh * layoff_rate;
                layoffs_total_by_firm_size(n) += distribution(i) * nh * layoff_rate;
                break;
            case Policies::EXIT:
                mass_layoffs_low_wage_by_firm_size(n) += distribution(i) * nl * exit_rate;
                mass_layoffs_high_wage_by_firm_size(n) += distribution(i) * nh * exit_rate;
                break;
        }

    }
    mass_layoffs_total_by_firm_size = mass_layoffs_low_wage_by_firm_size + mass_layoffs_high_wage_by_firm_size;
    agg_demand_intensity_low_wage = agg_hiring_flow_high_wage / hiring_rate_high;
    agg_demand_intensity_low_wage = agg_hiring_flow_low_wage / hiring_rate_low;

    int max_firm_size = 0;
    for (int i = 0; i <= nbar; i += 1){
        if (firm_size_distribution(i) > kfe_criterion) {
            max_firm_size = i;
        }
    }
    VectorXd mean_wage_by_firm_size(max_firm_size + 1);
    mean_wage_by_firm_size = mean_wage_by_firm_size_temp.head(max_firm_size + 1);
    for (int i = 0; i <= max_firm_size; i += 1){
        mean_wage_by_firm_size(i) = mean_wage_by_firm_size(i) / firm_size_distribution(i);
    }

    // Time
    auto t4 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_agg = t4 - t3;
    if (verbose) {
        cout <<"Aggregates computed in: " << elapsed_agg.count() << "\n\n";
    }

    //// TOTAL TIME ELAPSED
    auto t_end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_tot = t_end - t0;
    if (verbose) {
        cout <<"--------|            DONE!            |-------\n";
        cout << "Total Execution Time = " << elapsed_tot.count() << endl;
        cout <<" \n\n";
    }

    //// DISPLAY RESULTS FOR CLARITY

    //// TABLES
    if (tables) {
        cout << "----------------------------------------------\n";
        cout << "--------|    STATISTICS OF INTEREST   |-------\n";
        cout << "----------------------------------------------\n\n";

        cout << "Aggregate Employment:\n";
        cout << "Low Wage\t" << agg_employment_low_wage << "\n";
        cout << "High Wage\t" << agg_employment_high_wage << "\n";

        cout << "\n\n";
    }

    //// GRAPHS
    if (graphs) {
        cout << "----------------------------------------------\n";
        cout << "--------|            PLOTS            |-------\n";
        cout << "----------------------------------------------\n\n";

        //// POLICY FUNCTIONS
        cout << "--------|       POLICY FUNCTIONS      |-------\n\n";
        cout << "\033[1mLegend\033[0m\n";
        cout << "Color\t\tCode\tPolicy\n";
        cout << "\033[1;34mBlue\t\t2\t\tHire at High Wage\033[0m\n";
        cout << "\033[1;36mCyan\t\t1\t\tHire at Low Wage\033[0m\n";
        cout << "\033[1;32mGreen\t\t0\t\tStay\033[0m\n";
        cout << "\033[1;35mMagenta\t\t3\t\tLay Off at Low Wage\033[0m\n";
        cout << "\033[1;31mRed\t\t\t4\t\tLay Off at High Wage\033[0m\n";
        cout << "\033[1;30mWhite\t\t5\t\tExit\033[0m\n";

        for (int z = 0; z < nz; z += 1){
            cout << "\n\n";
            cout << "Productivity is: " << productivity(z) << "\n";
            for (int row = nbar; row >= 0; row -= 1){
                if (row == nbar) {
                    cout << "NH\t" << row << "\t";
                } else {
                    cout << "\n\t" << row <<"\t";
                }
                for (int col = 0; col <= nbar; col += 1){
                    if (row + col <= nbar) {
                        int index_endog = table_of_states_inv(col,row);
                        int i = z * num_endog_states + index_endog;
                        switch (policy[i]) {
                            case Policies::IDLE:
                                cout << "\033[1;32m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::HIRE_LOW_WAGE:
                                cout << "\033[1;36m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::HIRE_HIGH_WAGE:
                                cout << "\033[1;34m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::LAYOFF_LOW_WAGE:
                                cout << "\033[1;35m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::LAYOFF_HIGH_WAGE:
                                cout << "\033[1;31m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                            case Policies::EXIT:
                                cout << "\033[1;30m" << static_cast<int>(policy[i]) << "\033[0m\t"; break;
                        }
                    } else if (col < nbar){
                        cout << "\t";
                    }
                }
            }
            cout << "\n\t\t";
            for (int col = 0; col <= nbar; col += 1){
                cout << col << "\t";
            }
            cout << "\n\t\tNL\n";
        }

        //// UNCONDITIONAL DISTRIBUTION
        cout << "\n\n";

        cout << "--------| UNCONDITIONAL DISTRIBUTION  |-------\n\n";
        double graph_max = unconditional_distribution.maxCoeff();
        VectorXd graph_cutoffs = VectorXd::LinSpaced(7,kfe_criterion,graph_max+1e-6);
        string colors[7] = {"35", "31", "33", "32", "36", "34", "30"};

        cout << "\033[1mLegend\033[0m\n";
        cout << "Color\t\tInterval\n";
        cout << "\033[1;34mBlue\t\t[ " << graph_cutoffs(5) << ", " << graph_cutoffs(6) << " ]\033[0m\n";
        cout << "\033[1;36mCyan\t\t[ " << graph_cutoffs(4) << ", " << graph_cutoffs(5) << " )\033[0m\n";
        cout << "\033[1;32mGreen\t\t[ " << graph_cutoffs(3) << ", " << graph_cutoffs(4) << " )\033[0m\n";
        cout << "\033[1;33mYellow\t\t[ " << graph_cutoffs(2) << ", " << graph_cutoffs(3) << " )\033[0m\n";
        cout << "\033[1;31mRed\t\t\t[ " << graph_cutoffs(1) << ", " << graph_cutoffs(2) << " )\033[0m\n";
        cout << "\033[1;35mMagenta\t\t( " << "0" << ", " << graph_cutoffs(1) << " )\033[0m\n";
        cout << "\033[1;30mWhite\t\tLess than tolerance (essentially zero).\033[0m\n\n";

        for (int row = nbar; row >= 0; row -= 1){
            if (row == nbar) {
                cout << "NH\t" << row << "\t";
            } else {
                cout << "\n\t" << row << "\t";
            }
            for (int col = 0; col <= nbar; col += 1){
                if (row + col <= nbar) {
                    int index_endog = table_of_states_inv(col,row);
                    for (int j = 1; j < 7; j += 1){
                        if (unconditional_distribution(index_endog) >= kfe_criterion && unconditional_distribution(index_endog) < graph_cutoffs(j)){
                            cout << "\033[1;"<< colors[j-1] <<"mx\033[0m\t";
                            break;
                        } else if (unconditional_distribution(index_endog) < kfe_criterion){
                            cout << "\033[1;"<< colors[6] <<"mx\033[0m\t";
                            break;
                        }
                    }
                } else if (col < nbar){
                    cout << "\t";
                }
            }
        }
        cout << "\n\t\t";
        for (int col = 0; col <= nbar; col += 1){
            cout << col << "\t";
        }
        cout << "\n\t\tNL\n";

        //// FIRM SIZE DISTRIBUTION
        cout << "\n\n";
        cout << "--------|    FIRM SIZE DISTRIBUTION   |-------\n\n";

        graph_max = firm_size_distribution.maxCoeff();
        graph_cutoffs = VectorXd::LinSpaced(21,kfe_criterion,graph_max);
        for (int row = 21; row > 0; row -= 1){
            std::streamsize ss = std::cout.precision();
            if (row == 21) {
                cout.precision(2);
                cout << "f(x)" << std::fixed << graph_cutoffs(20) << "\t";
                cout.precision(ss);
            } else {
                cout.precision(2);
                cout << "\n\t" << std::fixed << graph_cutoffs(row-1) <<"\t";
                cout.precision(ss);
            }
            for (int col = 0; col <= nbar; col += 1){
                if (firm_size_distribution(col) < graph_cutoffs(row-1)){
                    cout << "\t";
                } else {
                    cout << "\033[1;34mx\033[0m\t";
                }
            }
        }
        cout << "\n\t\t\t";
        for (int col = 0; col <= nbar; col += 1){
            cout << col << "\t";
        }
        cout << "\n\t\t\tTotal Employees\n";

        //// MEAN WAGE BY FIRM SIZE
        cout << "\n\n";
        cout << "--------|    MEAN WAGE BY FIRM SIZE   |-------\n\n";

        graph_cutoffs = VectorXd::LinSpaced(21,wl,wh);
        for (int row = 21; row > 1; row -= 1){
            std::streamsize ss = std::cout.precision();
            if (row == 21) {
                cout.precision(2);
                cout << "f(x)" << std::fixed << graph_cutoffs(20) << "\t";
                cout.precision(ss);
            } else {
                cout.precision(2);
                cout << "\n\t" << std::fixed << graph_cutoffs(row-1) <<"\t";
                cout.precision(ss);
            }
            for (int col = 1; col <= max_firm_size; col += 1){
                if (mean_wage_by_firm_size(col) > graph_cutoffs(row-1) || mean_wage_by_firm_size(col) <= graph_cutoffs(row-2)){
                    cout << "\t";
                } else {
                    cout << "\033[1;34mx\033[0m\t";
                }
            }
        }
        cout << "\n\t\t\t";
        for (int col = 1; col <= max_firm_size; col += 1){
            cout << col << "\t";
        }
        cout << "\n\t\t\tTotal Employees\n";

        //// TODO ADD OTHER GRAPHS

    }
}


/**
 * \file PedalToneStackFilter.cpp
 */

#include <boost/math/tools/polynomial.hpp>

#include "PedalToneStackFilter.h"
#include "IIRFilter.h"

namespace ATK
{
  template<typename DataType>
  SD1ToneCoefficients<DataType>::SD1ToneCoefficients(int nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels), R1(10e3), R2(22e3), R3(470), R4(10e3),
    C1(static_cast<DataType>(0.018e-6)), C2(static_cast<DataType>(0.027e-6)), C3(static_cast<DataType>(0.01e-6)), alpha(1)
  {
  }

  template<typename DataType>
  void SD1ToneCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    DataType tempm[2] = {static_cast<DataType>(-2 * input_sampling_rate), static_cast<DataType>(2 * input_sampling_rate)};
    DataType tempp[2] = {static_cast<DataType>(1), static_cast<DataType>(1)};
    boost::math::tools::polynomial<DataType> poly1(tempm, 1);
    boost::math::tools::polynomial<DataType> poly2(tempp, 1);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    b += poly2 * poly2;
    b += poly2 * poly1 * (C2*R3+R4*C3+alpha*(1-alpha)*R2*C2+alpha*C2*R4);
    b += poly1 * poly1 * (C3*R4*(R3*C2+alpha*(1-alpha)*R2*C2));

    a += poly2 * poly2;
    a += poly2 * poly1 * (C2*R3+R1*C1+alpha*(1-alpha)*R2*C2+(1-alpha)*C2*R1);
    a += poly1 * poly1 * (C1*R1*(R3*C2+alpha*(1-alpha)*R2*C2));

    for(int i = 0; i < in_order + 1; ++i)
    {
      coefficients_in[i] = b[i] / a[out_order];
    }
    for(int i = 0; i < out_order; ++i)
    {
      coefficients_out[i] = -a[i] / a[out_order];
    }
  }

  template<typename DataType_>
  void SD1ToneCoefficients<DataType_>::set_tone(DataType_ alpha)
  {
    if(alpha < 0 || alpha > 1)
    {
      throw std::out_of_range("Tone is outside the interval [0,1]");
    }
    this->alpha = alpha;

    setup();
  }

  template<typename DataType_>
  DataType_ SD1ToneCoefficients<DataType_>::get_tone() const
  {
    return alpha;
  }

  template<typename DataType>
  TS9ToneCoefficients<DataType>::TS9ToneCoefficients(int nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels), R1(1e3), R2(10e3), R3(1e3), R4(220), P(22e3),
  C1(static_cast<DataType>(0.022e-6)), C2(static_cast<DataType>(0.022e-6)), alpha(1)
  {
  }

  template<typename DataType>
  void TS9ToneCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    DataType tempm[2] = {static_cast<DataType>(-2 * input_sampling_rate), static_cast<DataType>(2 * input_sampling_rate)};
    DataType tempp[2] = {static_cast<DataType>(1), static_cast<DataType>(1)};
    boost::math::tools::polynomial<DataType> poly1(tempm, 1);
    boost::math::tools::polynomial<DataType> poly2(tempp, 1);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    b += R2 * poly2 * poly2;
    b += poly2 * poly1 * (alpha * C2 * R2 * R3 + alpha * (1-alpha) * C2 * P * R2 + R2 * R4 * C2);

    a += (R2 + R1) * poly2 * poly2;
    a += poly2 * poly1 * ((1-alpha) * C2 * (alpha * P * R2 + R1 * alpha * P + R1 * R2) + R4 * C2 * (R2 + R1) + R1 * C1 * R2);
    a += poly1 * poly1 * (C2 * R4 * C1 * R2 * R1 + (1-alpha) * C2 * R1 * P * C1 * R2);

    for(int i = 0; i < in_order + 1; ++i)
    {
      coefficients_in[i] = b[i] / a[out_order];
    }
    for(int i = 0; i < out_order; ++i)
    {
      coefficients_out[i] = -a[i] / a[out_order];
    }
  }

  template<typename DataType_>
  void TS9ToneCoefficients<DataType_>::set_tone(DataType_ alpha)
  {
    if(alpha < 0 || alpha > 1)
    {
      throw std::out_of_range("Tone is outside the interval [0,1]");
    }
    this->alpha = alpha;

    setup();
  }

  template<typename DataType_>
  DataType_ TS9ToneCoefficients<DataType_>::get_tone() const
  {
    return alpha;
  }

  template class SD1ToneCoefficients<float>;
  template class SD1ToneCoefficients<double>;

  template class IIRFilter<SD1ToneCoefficients<float> >;
  template class IIRFilter<SD1ToneCoefficients<double> >;

  template class TS9ToneCoefficients<float>;
  template class TS9ToneCoefficients<double>;

  template class IIRFilter<TS9ToneCoefficients<float> >;
  template class IIRFilter<TS9ToneCoefficients<double> >;
}


/*
 * MIT License
 *
 * Copyright (c) 2019 Camille Schreck
 *
 * 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.
 *
 * definition.hpp
 */

#ifndef DEFINITIONS_HPP
#define DEFINITIONS_HPP

#include <Eigen/Core>

using namespace Eigen;

//#define DOUBLE_PRECISION

#ifdef DOUBLE_PRECISION

#define FLOAT double
#define VEC2 Vector2d
#define VEC3 Vector3d
#define VEC4 Vector4d
#define VECX VectorXd
#define MAT2 Matrix2d
#define MAT3 Matrix3d
#define MAT4 Matrix4d
#define MATX MatrixXd
#define ANGLE_AXIS AngleAxisd
#define QUATERNION Quaterniond
#define COMPLEX std::complex<double>
#define VEC2C Vector2cd
#define VECXC VectorXcd
#define MATXC MatrixXcd

#else

#define FLOAT float
#define VEC2 Vector2f
#define VEC3 Vector3f
#define VEC4 Vector4f
#define VECX VectorXf
#define MAT2 Matrix2f
#define MAT3 Matrix3f
#define MAT4 Matrix4f
#define MATX MatrixXf
#define ANGLE_AXIS AngleAxisf
#define QUATERNION Quaternionf
#define COMPLEX std::complex<float>
#define VEC2C Vector2cf
#define VECXC VectorXcf
#define MATXC MatrixXcf

#endif

inline FLOAT damping(FLOAT d_coef, FLOAT x, FLOAT k) {
  return exp(-d_coef*k*k*x);
}

inline COMPLEX fund_solution(FLOAT x) {
  return COMPLEX(0, -1)/(FLOAT)4.0*sqrtf(2.0f/(M_PI*x))*exp(COMPLEX(0, 1)*(x - (FLOAT)M_PI/4.0f));
}

inline FLOAT omega(FLOAT k) {
  return sqrtf(9.81*k + 0.074/1000*pow(k, 3));
}

inline FLOAT velocity(FLOAT k) {
  return 0.5*omega(k)/k;
}
inline FLOAT velocity(FLOAT k, FLOAT omega) {
  return 0.5*omega/k;
}

// linear interpolation 1 if x = p1, 0 if x = p2
inline FLOAT interpolation(float x, float p1, float p2) {
  float d = p2 - p1;
  float dx = x - p1;
  return 1-dx/d;
}

#endif


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

#ifndef SMOOTH__INTERNAL__TN_HPP_
#define SMOOTH__INTERNAL__TN_HPP_

#include <Eigen/Core>

#include "common.hpp"

namespace smooth {

/**
 * @brief T(n) Lie Group represented as R^n
 *
 * Memory layout
 * -------------
 * Group:    x1 x2 ... xn
 * Tangent:  v1 v2 ... vn
 *
 * Lie group Matrix form
 * ---------------------
 * [ I T ]
 * [ 0 1 ]
 *
 * where T = [x1 ... xn]'
 *
 * Lie algebra Matrix form
 * -----------------------
 * [ 0 V ]
 * [ 0 0 ]
 *
 * where V = [v1 ... vn]'
 */
template<int N, typename _Scalar>
struct TnImpl
{
  using Scalar = _Scalar;

  static constexpr Eigen::Index Dim     = N + 1;
  static constexpr Eigen::Index Dof     = N;
  static constexpr Eigen::Index RepSize = N;
  static constexpr bool IsCommutative   = true;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out) { g_out.setZero(); }

  static void setRandom(GRefOut g_out) { g_out.setRandom(); }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    m_out.setIdentity();
    m_out.template topRightCorner<Dof, 1>() = g_in;
  }
  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out) { g_out = g_in1 + g_in2; }

  static void inverse(GRefIn g_in, GRefOut g_out) { g_out = -g_in; }

  static void log(GRefIn g_in, TRefOut a_out) { a_out = g_in; }

  static void exp(TRefIn a_in, GRefOut g_out) { g_out = a_in; }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out.setZero();
    A_out.template topRightCorner<N, 1>() = a_in;
  }

  static void vee(MRefIn A_in, TRefOut a_out) { a_out = A_in.template topRightCorner<N, 1>(); }

  static void ad(TRefIn, TMapRefOut A_out) { A_out.setZero(); }
};

}  // namespace smooth

#endif  // SMOOTH__INTERNAL__TN_HPP_


// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2018 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef OPENMVG_SFM_STELLAR_STELLAR_DEFINITIONS_HPP
#define OPENMVG_SFM_STELLAR_STELLAR_DEFINITIONS_HPP

#include "openMVG/sfm/pipelines/stellar/relative_scale.hpp"
#include "openMVG/numeric/l1_solver_admm.hpp"

#include <Eigen/Sparse>

namespace openMVG{
namespace sfm{

/// Mode that can be used to solve to rescale the relative translation of a stellar pod
enum class Stellar_Translation_Averaging_Solver_Type
{
  SCALING_SOLVER_L1,
  SCALING_SOLVER_L2,
  SCALING_SOLVER_L2_FULL
};

/// Check if a stellar pod defined by some relative scale is one CC or not
inline bool Relative_scales_are_one_cc
(
  const std::vector<Relative_Scale> & relative_scales
)
{
  // Iterate along the pairs:
  // - check if everytime at least one of the pair id was listed before
  std::set<uint32_t> previous_ids;
  const Pair_Set used_pairs = Relative_Scale::Get_pairs(relative_scales);
  for (const Pair & pair_it : used_pairs)
  {
    if (previous_ids.empty())
    {
      previous_ids = {pair_it.first, pair_it.second};
    }
    else
      if (previous_ids.count(pair_it.first) == 0 &&
          previous_ids.count(pair_it.second) == 0)
        return false;
  }
  return !relative_scales.empty();
}

/// Solve the relative scale to a common coordinate system
/// Since ratios of depth are used, the found solution is an approximation
/// Solving equation (4) from “Global Structure-from-Motion by Similarity Averaging"
/// Zhaopeng Cui and Ping Tan. (ICCV 2015).”
///
inline
bool
Solve_stellar_translation_scales_averaging
(
  const size_t node_id, // Central node
  const std::vector<Relative_Scale> & vec_relative_scales,
  const Hash_Map<Pair, geometry::Pose3> & relative_poses,
  Hash_Map<IndexT, geometry::Pose3> & triplet_pose,
  const Stellar_Translation_Averaging_Solver_Type e_used_solver =
    Stellar_Translation_Averaging_Solver_Type::SCALING_SOLVER_L2_FULL
)
{
  const Pair_Set used_pairs = Relative_Scale::Get_pairs(vec_relative_scales);
  std::cout << "Stellar reconstruction with center node: " << node_id << "\n"
    << "#relative scales: " << vec_relative_scales.size() << "\n"
    << "#pairs : " << used_pairs.size() << std::endl;

  // Assert that the relative scales pose ids defined a unique connected component
  if (!Relative_scales_are_one_cc(vec_relative_scales))
  {
    std::cerr << "The stellar edges are not giving a single connected component." << std::endl;
    return false;
  }

  // Compute a contiguous indexes mapping
  std::map<Pair, unsigned int> pair_to_index;
  {
    unsigned int i = 0;
    for (const Pair & pair_it: used_pairs)
    {
      pair_to_index[pair_it] = i++;
    }
  }

  // Solve the scale to put all the pair in a common global coordinate system
  Vec x_scales;
  switch (e_used_solver)
  {
    case Stellar_Translation_Averaging_Solver_Type::SCALING_SOLVER_L2_FULL:
    {
      // Setup the linear system: lhx X = rhs
      std::vector< Eigen::Triplet<double> > vec_triplets;
      Vec rhs(vec_relative_scales.size());
      unsigned int i = 0;
      for (const Relative_Scale & relative_scales : vec_relative_scales)
      {
        // Write:
        // S_ij / S_il = factor_ijl
        // as the following linear constraint:
        // S_ij - S_il = log(factor_ijl)
        vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[0]), 1); // row, col, value
        vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[1]), -1);
        rhs[i] = std::log(relative_scales.ratio);
        ++i;
      }

      sMat lhs;
      lhs.resize(vec_relative_scales.size(), pair_to_index.size());
      lhs.setFromTriplets(vec_triplets.cbegin(), vec_triplets.cend());

      // Use QR
      Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int> > solver(lhs);
      if (solver.info() != Eigen::Success)
      {
        std::cerr << "Sparse matrix cannot be factorized" << std::endl;
        return false;
      }
      const Vec x = solver.solve(rhs);
      if (solver.info() != Eigen::Success)
      {
        std::cerr << "Sparse system cannot be solved" << std::endl;
        return false;
      }

      // Make the log of the distances negative, such that all distances are <= 1
      const double maxLogDistance = x.maxCoeff();
      x_scales = (x.array() - maxLogDistance).array().exp();
    }
    break;

    case Stellar_Translation_Averaging_Solver_Type::SCALING_SOLVER_L2:
    {
      // Setup the linear system: lhx X = rhs
      std::vector< Eigen::Triplet<double> > vec_triplets;
      Vec rhs(vec_relative_scales.size());
      unsigned int i = 0;
      for (const Relative_Scale & relative_scales : vec_relative_scales)
      {
        // Write:
        // S_ij / S_il = factor_ijl
        // as the following linear constraint:
        // S_ij - S_il = log(factor_ijl)
        if (pair_to_index[relative_scales.pairs[0]]!=0)
          vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[0])-1, 1); // row, col, value
        if (pair_to_index[relative_scales.pairs[1]]!=0)
          vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[1])-1, -1);
        rhs[i] = std::log(relative_scales.ratio);
        ++i;
      }

      sMat lhs;
      lhs.resize(vec_relative_scales.size(), pair_to_index.size()-1);
      lhs.setFromTriplets(vec_triplets.cbegin(), vec_triplets.cend());

      // Use QR
      Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int> > solver(lhs);
      if (solver.info() != Eigen::Success)
      {
        std::cerr << "Sparse matrix cannot be factorized" << std::endl;
        return false;
      }
      const Vec x = solver.solve(rhs);
      if (solver.info() != Eigen::Success)
      {
        std::cerr << "Sparse system cannot be solved" << std::endl;
        return false;
      }

      // Make the log of the distances negative, such that all distances are <= 1
      Vec xCpy(x.size()+1);
      xCpy << 0.0, x;
      const double maxLogDistance = xCpy.maxCoeff();
      x_scales = (xCpy.array() - maxLogDistance).array().exp();
    }
      break;

    case Stellar_Translation_Averaging_Solver_Type::SCALING_SOLVER_L1:
    {
      // Setup the linear system: lhx X = rhs
      std::vector< Eigen::Triplet<double> > vec_triplets;
      Vec rhs(vec_relative_scales.size());
      unsigned int i = 0;
      for (const Relative_Scale & relative_scales : vec_relative_scales)
      {
        // Write:
        // S_ij / S_il = factor_ijl
        // as the following linear constraint:
        // S_ij - S_il = log(factor_ijl)
        if (pair_to_index[relative_scales.pairs[0]]!=0)
          vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[0])-1, 1); // row, col, value
        if (pair_to_index[relative_scales.pairs[1]]!=0)
          vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[1])-1, -1);
        rhs[i] = std::log(relative_scales.ratio);
        ++i;
      }

      sMat lhs;
      lhs.resize(vec_relative_scales.size(), pair_to_index.size()-1);
      lhs.setFromTriplets(vec_triplets.cbegin(), vec_triplets.cend());

      // Try to solve under L1 norm
      openMVG::L1Solver<sMat>::Options opt;
      openMVG::L1Solver<sMat> l1solver(opt, lhs);

      Vec l1solution_vec(pair_to_index.size());
      l1solution_vec.setZero();
      if (l1solver.Solve(rhs, &l1solution_vec))
      {
        // Make the log of the distances negative, such that all distances are <= 1
        Vec x(l1solution_vec.size()+1);
        x << 0.0, l1solution_vec;
        const double maxLogDistance = x.maxCoeff();
        x_scales = (x.array() - maxLogDistance).array().exp();
      }
      else
      {
        std::cerr << "Sparse system cannot be solved" << std::endl;
        return false;
      }
    }
    break;
    default:
      std::cerr << "Unsupported solver" << std::endl;
      return false;
  }

  if (x_scales.size() == 0)
  {
    return false;
  }

  // Upgrade the found stellar reconstruction
  triplet_pose.clear();
  triplet_pose[node_id] = geometry::Pose3(); // Identity

  std::set<unsigned int> stellar_ids;
  for (unsigned int i = 0 ; i < x_scales.size(); ++i)
  {
    // Retrieve the original pair (inverse mapping)
    Pair selected_pair;
    for (const Pair & pair_it: used_pairs)
    {
      if (pair_to_index[pair_it] == i)
      {
        selected_pair = pair_it;
        break;
      }
    }
    std::cout << "Pair: " << selected_pair.first << "," << selected_pair.second << "; scaling: " << x_scales[i] << std::endl;
    stellar_ids.insert(selected_pair.first);
    stellar_ids.insert(selected_pair.second);

    if (relative_poses.count(selected_pair) == 0)
    {
      return false;
    }

    if (selected_pair.first == node_id)
    {
      geometry::Pose3 relative_pose = relative_poses.at(selected_pair);
      relative_pose.center() /= x_scales[i];
      triplet_pose[selected_pair.second] = relative_pose;
    }
    else
    {
      geometry::Pose3 relative_pose = relative_poses.at(selected_pair).inverse();
      relative_pose.center() /= x_scales[i];
      triplet_pose[selected_pair.first] = relative_pose;
    }
  }
  return true;
}

} // namespace sfm
} // namespace openMVG

#endif // OPENMVG_SFM_STELLAR_STELLAR_DEFINITIONS_HPP


//#include "ps/ps.h"
#include <iostream>
#include <vector>
#include "softmax.h"
#include "prox_l1.h"
#include "prox_l2.h"

#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXi;
using Eigen::ArrayXd;

using namespace std;
//using namespace ps;

int main() {
  MatrixXd X(3, 4);
  VectorXi y(3);
  vector<double> a = {1, -1, 1};
  X << 1,2,3,4,5,6,7,8,9,10,11,12;
  X = X / 10;
  for (int i = 0; i < a.size(); i++) {
    y(i) = a[i];
  }
  cout << X << endl;
  cout << y << endl;

  // test data copy
  vector<double> vecx(X.rows()*X.cols());
  for (int i = 0; i < X.cols(); i++) {
    VectorXd::Map(&vecx[i*X.rows()], X.rows()) = X.col(i);
  }
  cout << "---------" << endl;
  for (int i = 0; i < vecx.size(); i++) {
    cout << vecx[i] << " ";
  }
  cout << endl;

  // test data copy back
  cout << "---------" << endl;
  for (int i = 0; i < vecx.size(); i++) {
    vecx[i] = vecx[i] * 2;
  }
  for (int i = 0; i < X.cols(); i++) {
    X.col(i) = VectorXd::Map(&vecx[i*X.rows()], X.rows());
  }
  cout << X << endl;

  // test replicate
  cout << "---------" << endl;
  cout << X.rowwise().sum() << endl;
  cout << X.rowwise().sum().replicate(1, X.cols()) << endl;

  // test transpose
  cout << "---------" << endl;
  cout << X.transpose() << endl;
  cout << "---------" << endl;
  cout << X << endl;

  // test softmax
  lrprox::SOFTMAX softmax = lrprox::SOFTMAX(4, 2);
  // one-hot
  MatrixXi y_onehot = softmax.onehot_encoder(y);
//  y_onehot << 0, 1, 0, 1, 0, 1;
//  y_onehot.transposeInPlace();
  cout << "---------" << endl;
  X << 1,2,3,4,5,6,7,8,9,10,11,12;
  X = X / 10;
  cout << X << endl;
  cout << y_onehot << endl;
  for (int i = 1; i <= 3; i++) {
    cout << "--" << endl;
    MatrixXd w(4, 2);
    w.col(0) << 1, 2, 3, 4;
    w.col(1) << 5, 6, 7, 8;
    w = (-w.eval().array()/10 *i).matrix();
    softmax.updateWeight(w);
    cout << softmax.getWeight() << endl;
    cout << softmax.cost(X, y_onehot) << endl;
    cout << softmax.grad(X, y_onehot) << endl;
  }

  // test proximal
  lrprox::prox_l1 prox_op1 = lrprox::prox_l1(0.1);
  lrprox::prox_l2 prox_op2 = lrprox::prox_l2(0.1);
  double gamma = 0.3;
  cout << "---------" << endl;
  VectorXd x(Eigen::Map<VectorXd>(X.data(), X.cols()*X.rows()));
  cout << prox_op1.cost(X) << endl;
  cout << prox_op1.cost(x) << endl;
  cout << prox_op1.proximal(X, gamma) << endl;
  cout << prox_op1.proximal(x, gamma) << endl;
  cout << prox_op2.cost(X) << endl;
  cout << prox_op2.cost(x) << endl;
  cout << prox_op2.proximal(X, gamma) << endl;
  cout << prox_op2.proximal(x, gamma) << endl;

  // test outputWeight
  vector<double> w_vec;
  cout << "---------" << endl;
  softmax.outputWeight(*(&w_vec));
  cout << softmax.getWeight() << endl;
  for (int i = 0; i < w_vec.size(); i++) {
    cout << w_vec[i] << " ";
  }
  cout << endl;
  w_vec[2] = 100;
  softmax.updateWeight(w_vec);
  cout << softmax.getWeight() << endl;
  for (int i = 0; i < w_vec.size(); i++) {
    cout << w_vec[i] << " ";
  }
  cout << endl;
  MatrixXd xtmp = softmax.getWeight();
  xtmp.data()[0] = -100;
  for (int i = 0; i < w_vec.size(); i++) {
    cout << xtmp.data()[i] << " ";
  }
  cout << endl;

  return 0;
}


/*
 *  Simulate the real boundary of the region of attraction (ROA)
 *  for the reversed VDP example.
 *
 *  Created by Yinan Li on May 19, 2018.
 *
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

#include "src/definitions.h"
#include "src/matlabio.h"

/* user defined dynamics */
void vdp(const rocs::Rn &x, rocs::Rn &dxdt, double t) {
    dxdt[0] = -x[1];
    dxdt[1] = x[0] + x[1]*(x[0]*x[0]-1);
}

int main()
{
    double xlb[] = {-4, -4};
    double xub[] = {4, 4};
    double eta[] = {0.001, 0.001};
    double T = 10.0;
    double dt = 0.01;

    rocs::grid xg(2, eta, xlb, xub);
    xg.gridding();

    /* ROA computation by running simulations */
    boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;
    std::vector<bool> roa(xg._nv);
    rocs::Rn x;
    bool stable;
    for (int i = 0; i < xg._nv; ++i) {

	x = xg._data[i];

	// boost::numeric::odeint::integrate(vdp, x, 0.0, T);
	// boost::numeric::odeint::integrate_adaptive(make_controlled(1E-12, 1E-12, dopri5()), vdp, x, 0.0, T, dt);
	boost::numeric::odeint::integrate_const(rk45, vdp, x, 0.0, T, dt);

	stable = true;
	for (int j = 0; j < xg._dim; ++j) {
	    // std::cout << x[j] << ", ";
	    stable = stable & (x[j] < 0.5) & (x[j] > -0.5);
	}
	// std::cout <<'\n';

	if (stable)
	    roa[i] = true;
    }

    /* write roa boundary to a mat file */
    rocs::matWriter wtr("data_roavdp_real.mat");
    wtr.open();

    wtr.write_boundary(xg, roa, "bd0");
    wtr.close();

    return 0;
}