#include "mm/ug_bitext.h"
#include <boost/format.hpp>
using namespace std;
using namespace Moses;
using namespace ugdiss;
using namespace sapt;

typedef L2R_Token<SimpleWordId> Token;
typedef mmTtrack<Token> ttrack_t;
typedef mmTSA<Token>     tsa_t;

TokenIndex V1,V2;
boost::shared_ptr<ttrack_t> T1,T2;
tsa_t I1,I2;

float lbop_level = .05;
#define smooth 1
namespace stats
{
  using namespace Moses;
  using namespace sapt;
  float
  pmi(size_t j,size_t m1, size_t m2, size_t N)
  {
#if smooth
    float p1 = lbop(N,m1,lbop_level);
    float p2 = lbop(N,m2,lbop_level);
    float p12 = lbop(N,j,lbop_level);
    return log(p12) - log(p1) - log(p2);
#else
    return log(j) + log(N) - log(m1) - log(m2);
#endif
  }

  float
  npmi(size_t j,size_t m1, size_t m2, size_t N)
  {
#if smooth
    float p1 = lbop(N,m1,lbop_level);
    float p2 = lbop(N,m2,lbop_level);
    float p12 = lbop(N,j,lbop_level);
    return (log(p12) - log(p1) - log(p2)) / -log(p12);
#else
    return pmi(j,m1,m2,N) / (log(N) - log(j));
#endif
  }

  float
  mi(size_t j,size_t m1, size_t m2, size_t N)
  {
    float ret = 0;
    if (j)    ret += float(j)/N * pmi(j,m1,m2,N);
    if (m1>j) ret += float(m1-j)/N * pmi(m1-j,m1,N-m2,N);
    if (m2>j) ret += float(m2-j)/N * pmi(m2-j,N-m1,m2,N);
    if (N>m1+m2-j) ret += float(N-m1-m2+j)/N * pmi(N-m1-m2+j,N-m1,N-m2,N);
    return ret;
  }
}

struct SinglePhrase
{
  typedef map<uint64_t,SPTR<SinglePhrase> > cache_t;
  uint64_t pid; // phrase id
  vector<ttrack::Position> occs; // occurrences
};

struct PPair
{
  struct score_t;
  uint64_t p1,p2;
  ushort s1,e1,s2,e2;
  int parent;

  struct stats_t
  {
    typedef map<pair<uint64_t,uint64_t>, SPTR<stats_t> > cache_t;
    size_t m1,m2,j;
    float  npmi; // normalized point-wise mutual information
    float   pmi; // point-wise mutual information
    float    mi; // mutual information
    float score;

    void
    set(vector<ttrack::Position> const& o1,
	vector<ttrack::Position> const& o2,
	size_t const N)
    {
      m1 = m2 = j = 0;
      size_t i1=0,i2=0;
      while (i1 < o1.size() && i2 < o2.size())
	{
	  if (i1 && o1[i1].sid == o1[i1-1].sid) { ++i1; continue; }
	  if (i2 && o2[i2].sid == o2[i2-1].sid) { ++i2; continue; }

	  if (o1[i1].sid == o2[i2].sid) { ++j; ++i1; ++i2; ++m1; ++m2; }
	  else if (o1[i1].sid < o2[i2].sid) { ++i1; ++m1; }
	  else { ++i2; ++m2; }
	}
      // for (++i1; i1 < o1.size(); ++i1)
      // 	if (o1[i1-1].sid != o1[i1].sid) ++m1;
      // for (++i2; i2 < o2.size(); ++i2)
      // 	if (o2[i2-1].sid != o2[i2].sid) ++m2;

      m1 = 1; m2 = 1;
      for (i1=1; i1 < o1.size(); ++i1)
       	if (o1[i1-1].sid != o1[i1].sid) ++m1;
      for (i2=1; i2 < o2.size(); ++i2)
	if (o2[i2-1].sid != o2[i2].sid) ++m2;

      this->mi = stats::mi(j,m1,m2,N);
      this->pmi = stats::pmi(j,m1,m2,N);
      this->npmi = stats::npmi(j,m1,m2,N);
      // float z = float(m1)/N * float(m2)/N;
      // float hmean = 2.*j/(m1+m2);
      this->score = npmi; // npmi; // hmean; // /sqrt(z);
    }
  } stats;

  PPair(ushort s1_=0, ushort e1_=0, ushort s2_=0, ushort e2_=0)
    : s1(s1_), e1(e1_), s2(s2_), e2(e2_), parent(-1) { }

  bool
  operator<(PPair const& other) const
  {
    return (this->stats.score == other.stats.score
	    ? (e1-s1 + e2-s2 > other.e1-other.s1 + other.e2-other.s2)
	    : (this->stats.score > other.stats.score));
  }

  size_t len1() const { return e1 - s1; }
  size_t len2() const { return e2 - s2; }
  bool includes(PPair const& o) const
  {
    return s1 <= o.s1 && e1 >= o.e1 && s2 <= o.s2 && e2 >= o.e2;
  }

};

SinglePhrase::cache_t cache1,cache2;
PPair::stats_t::cache_t ppcache;

struct SortByPositionInCorpus
{
  bool
  operator()(ttrack::Position const& a,
	     ttrack::Position const& b) const
  {
    return a.sid != b.sid ? a.sid < b.sid : a.offset < b.offset;
  }
};

void
getoccs(tsa_t::tree_iterator const& m,
	vector<ttrack::Position>& occs)
{
  occs.clear();
  occs.reserve(m.approxOccurrenceCount()+10);
  tsa::ArrayEntry I(m.lower_bound(-1));
  char const* stop = m.upper_bound(-1);
  do {
    m.root->readEntry(I.next,I);
    occs.push_back(I);
  } while (I.next != stop);
  sort(occs.begin(),occs.end(),SortByPositionInCorpus());
}

void
lookup_phrases(vector<id_type> const& snt,
	       TokenIndex& V, ttrack_t const& T,
	       tsa_t const& I, SinglePhrase::cache_t& cache,
	       vector<vector<SPTR<SinglePhrase> > >& dest)
{
  dest.resize(snt.size());
  for (size_t i = 0; i < snt.size(); ++i)
    {
      tsa_t::tree_iterator m(&I);
      dest[i].clear();
      for (size_t k = i; k < snt.size() && m.extend(snt[k]); ++k)
	{
	  if (m.approxOccurrenceCount() < 3) break;
	  // if (k - i > 0) break;
	  SPTR<SinglePhrase>& o = cache[m.getPid()];
	  if (!o)
	    {
	      o.reset(new SinglePhrase());
	      o->pid = m.getPid();
	      getoccs(m,o->occs);
	    }
	  dest[i].push_back(o);
	}
    }
}

struct
RowIndexSorter
{
  vector<vector<float> > const& M;
  size_t const my_col;
  RowIndexSorter(vector<vector<float> > const& m, size_t const c)
    : M(m), my_col(c) { }

  template<typename T>
  bool
  operator()(T const& a, T const& b) const
  {
    return M.at(a).at(my_col) > M.at(b).at(my_col);
  }
};

struct
ColIndexSorter
{
  vector<vector<float> > const& M;
  size_t const my_row;
  ColIndexSorter(vector<vector<float> > const& m, size_t const r)
    : M(m), my_row(r) { }

  template<typename T>
  bool
  operator()(T const& a, T const& b) const
  {
    return M.at(my_row).at(a) > M[my_row].at(b);
  }

};

int main(int argc, char* argv[])
{
  string base = argv[1];
  string L1   = argv[2];
  string L2   = argv[3];

  T1.reset(new ttrack_t());
  T2.reset(new ttrack_t());

  V1.open(base + L1 + ".tdx");
  T1->open(base + L1 + ".mct");
  I1.open(base + L1 + ".sfa", T1);

  V2.open(base + L2 + ".tdx");
  T2->open(base + L2 + ".mct");
  I2.open(base + L2 + ".sfa", T2);

  tsa_t::tree_iterator m1(&I1);
  tsa_t::tree_iterator m2(&I1);
  string line1, line2;
  while (getline(cin,line1) and getline(cin,line2))
    {
      cout << "\n" << line1 << "\n" << line2 << endl;
      vector<vector<SPTR<SinglePhrase> > > M1,M2;
      vector<id_type> snt1,snt2;
      V1.fillIdSeq(line1,snt1);
      V2.fillIdSeq(line2,snt2);
      lookup_phrases(snt1,V1,*T1,I1,cache1,M1);
      lookup_phrases(snt2,V2,*T2,I2,cache2,M2);

      vector<PPair> pp_all, pp_good;
      vector<int> a1(snt1.size(),-1);
      vector<int> a2(snt2.size(),-1);

      vector<vector<int> > z1(snt1.size(),vector<int>(snt1.size(),-1));
      vector<vector<int> > z2(snt2.size(),vector<int>(snt2.size(),-1));
      vector<vector<vector<PPair> > >ppm1(M1.size()),ppm2(M2.size());
      vector<vector<float> >  M(snt1.size(), vector<float>(snt2.size(),0));
      vector<vector<size_t> > best1(snt1.size()), best2(snt2.size());
      for (size_t i1 = 0; i1 < M1.size(); ++i1)
	{
	  PPair pp;
	  pp.s1 = i1;
	  ppm1[i1].resize(M1[i1].size());
	  for (size_t i2 = 0; i2 < M2.size(); ++i2)
	    {
	      pp.s2 = i2;
	      pp.stats.j = 1;
	      ppm2[i2].resize(M2[i2].size());
	      for (size_t k1 = 0; k1 < M1[i1].size(); ++k1)
		{
		  pp.e1 = i1 + k1 + 1;
		  // if (pp.stats.j == 0) break;
		  for (size_t k2 = 0; k2 < M2[i2].size(); ++k2)
		    {
		      pp.e2 = i2 + k2 + 1;
		      SPTR<PPair::stats_t> & s
			= ppcache[make_pair(M1[i1][k1]->pid,M2[i2][k2]->pid)];
		      if (!s)
			{
			  s.reset(new PPair::stats_t());
			  s->set(M1[i1][k1]->occs,M2[i2][k2]->occs,T1->size());
			}
		      pp.stats = *s;
		      if (pp.stats.j == 0) break;
		      // ppm1[i1][k1].push_back(pp);
		      // ppm2[i2][k2].push_back(pp);
		      size_t J = pp.stats.j * 100;
		      if (pp.stats.score > 0
			  && J >= pp.stats.m1
			  && J > pp.stats.m2)
			{ pp_all.push_back(pp); }
		    }
		}
	    }
	}
      sort(pp_all.begin(),pp_all.end());
#if 0
      BOOST_FOREACH(PPair const& pp,pp_all)
	{
	  if (pp.stats.npmi < 0) continue;
	  for (size_t r = pp.s1; r < pp.e1; ++r)
	    for (size_t c = pp.s2; c < pp.e2; ++c)
	      {
		// M[r][c] += log(1-pp.stats.npmi);
		M[r][c] += log(1-pp.stats.mi);
	      }
	}
      for (size_t r = 0; r < M.size(); ++r)
	for (size_t c = 0; c < M[r].size(); ++c)
	  M[r][c] = 1.-exp(M[r][c]);
      for (size_t r = 0; r < best1.size(); ++r)
	{
	  best1[r].resize(snt2.size());
	  for (size_t c = 0; c < best1[r].size(); ++c)
	    best1[r][c] = c;
	  sort(best1[r].begin(),best1[r].end(),ColIndexSorter(M,r));
	}
      for (size_t c = 0; c < best2.size(); ++c)
	{
	  best2[c].resize(snt1.size());
	  for (size_t r = 0; r < best2[c].size(); ++r)
	    best2[c][r] = r;
	  sort(best2[c].begin(),best2[c].end(),RowIndexSorter(M,c));
	}
      for (size_t r = 0; r < best1.size(); ++r)
	{
	  cout << V1[snt1[r]] << ":";
	  for (size_t i = 0; i < min(3UL,M[r].size()); ++i)
	    {
	      size_t k = best1[r][i];
	      // if (M[r][k] >= M[best2[k][min(2UL,M.size())]][k])
	      cout << " " << k << ":" << V2[snt2[k]] << " " << M[r][k];
	    }
	  cout << endl;
	}
#endif
#if 0
      for (size_t k = 1; k < pp_all.size(); ++k)
	for (size_t i = k; i--;)
	  if (pp_all[i].s1 >= pp_all[k].s1 &&
	      pp_all[i].e1 <= pp_all[k].e1 &&
	      pp_all[i].s2 >= pp_all[k].s2 &&
	      pp_all[i].e2 <= pp_all[k].e2)
	    pp_all[i].stats.score += pp_all[k].stats.score;
      sort(pp_all.begin(),pp_all.end());
#endif

#if 1
      vector<int> assoc1(snt1.size(),-1), assoc2(snt2.size(),-1);
      for (size_t p = 0; p < pp_all.size(); ++p)
	{
	  PPair const& x = pp_all[p];
	  // if (x.stats.npmi < .7) break;
	  // if (z1[x.s1][x.e1-1] >= 0 || z2[x.s2][x.e2-1] >=0)
	  // continue;
	  for (size_t i = x.s1; i < x.e1; ++i)
	    {
	      if (assoc1[i] < 0)
		assoc1[i] = p;
	      else
		{
		  // PPair& y = pp_all[assoc1[i]];
		  // if (y.includes(x))
		  // assoc1[i] = p;
		}
	    }
	  for (size_t i = x.s2; i < x.e2; ++i)
	    {
	      if (assoc2[i] < 0)
		assoc2[i] = p;
	      else
		{
		  // PPair& y = pp_all[assoc2[i]];
		  // if (y.includes(x))
		    // assoc2[i] = p;
		}
	    }
	  z1[x.s1][x.e1-1] = p;
	  z2[x.s2][x.e2-1] = p;
	  continue;
	  cout << (boost::format("%.4f %.8f %.4f")
		   % x.stats.score
		   % x.stats.mi
		   % x.stats.npmi);
	  for (size_t z = x.s1; z < x.e1; ++z)
	    cout << " " << V1[snt1[z]];
	  cout << " :::";
	  for (size_t z = x.s2; z < x.e2; ++z)
	    cout << " " << V2[snt2[z]];
	  cout << " ["
	       << x.stats.m1 << "/" << x.stats.j << "/" << x.stats.m2
	       << "]" << endl;
	}
      vector<bool> done(pp_all.size(),false);
      for (size_t i = 0; i < snt1.size(); ++i)
	{
	  if (assoc1[i] < 0 || done[assoc1[i]])
	    continue;
	  // for (size_t k = 0; k < snt2.size(); ++k)
	    // if (assoc1[i] == assoc2[k])
	      {
		done[assoc1[i]] = true;
		PPair& p = pp_all[assoc1[i]];
		for (size_t j = p.s1; j < p.e1; ++j)
		  cout << j << ":" << V1[snt1[j]] << " ";
		cout << " ::: ";
		for (size_t j = p.s2; j < p.e2; ++j)
		  cout << j << ":" << V2[snt2[j]] << " ";
		cout << "["
		     << p.stats.m1 << "/" << p.stats.j << "/" << p.stats.m2
		     << "] "<< p.stats.score << endl;
		// break;
	      }
	}
      cout << endl;
      for (size_t i = 0; i < snt2.size(); ++i)
	{
	  if (assoc2[i] < 0 || done[assoc2[i]])
	    continue;
	  done[assoc2[i]] = true;
	  PPair& p = pp_all[assoc2[i]];
	  for (size_t j = p.s1; j < p.e1; ++j)
	    cout << j << ":" << V1[snt1[j]] << " ";
	  cout << " ::: ";
	  for (size_t j = p.s2; j < p.e2; ++j)
	    cout << j << ":" << V2[snt2[j]] << " ";
	  cout << "["
	       << p.stats.m1 << "/" << p.stats.j << "/" << p.stats.m2
	       << "] "<< p.stats.score << endl;
	}
#endif
      // sort(pp_all.begin(),pp_all.end());
      // BOOST_FOREACH(PPair const& pp, pp_all)
      // 	{
      // 	  while (ppm1[pp.s1].size() < pp.e1 - pp.s1)
      // 	    ppm1[pp.s1].push_back(vector<PPair>());
      // 	  vector<PPair>& v1 = ppm1[pp.s1][pp.e1-pp.s1-1];
      // 	  if (v1.size() && v1[0].stats.score > pp.stats.score)
      // 	    continue;
      // 	  while (ppm2[pp.s2].size() < pp.e2 - pp.s2)
      // 	    ppm2[pp.s2].push_back(vector<PPair>());
      // 	  vector<PPair>& v2 = ppm2[pp.s2][pp.e2-pp.s2-1];
      // 	  if (v2.size() && v2[0].stats.score > pp.stats.score)
      // 	    continue;
      // 	  v1.push_back(pp);
      // 	  v2.push_back(pp);
      // 	}

      // BOOST_FOREACH(vector<vector<PPair> >& vv, ppm1)
      // 	{
      // 	  BOOST_FOREACH(vector<PPair>& v, vv)
      // 	    {
      // 	      sort(v.begin(),v.end());
      // 	      if (v.size() > 1 && v[0].stats.score == v[1].stats.score)
      // 		v.clear();
      // 	    }
      // 	}
      // for (size_t i2 = 0; i2 < ppm2.size(); ++i2)
      // 	{
      // 	  for (size_t k2 = 0; k2 < ppm2[i2].size(); ++k2)
      // 	    {
      // 	      vector<PPair>& v2 = ppm2[i2][k2];
      // 	      sort(v2.begin(),v2.end());
      // 	      if (v2.size() > 1 && v2[0].stats.score == v2[1].stats.score)
      // 		{
      // 		  v2.clear();
      // 		  continue;
      // 		}
      // 	      ushort i1 = v2[0].s1;
      // 	      ushort k1 = v2[0].e1 - i1 -1;

      // 	      if (ppm1[i1][k1].size() == 0 ||
      // 		  ppm1[i1][k1][0].s2 != i2 ||
      // 		  ppm1[i1][k1][0].e2 != i2 + k2 + 1)
      // 		{ v2.clear(); }
      // 	      else pp_good.push_back(ppm2[i2][k2][0]);
      // 	    }
      // 	}
      // BOOST_FOREACH(PPair const& pp, pp_good)
      // 	{
      // 	  cout << pp.stats.mi << " ";
      // 	  for (size_t z = pp.s1; z < pp.e1; ++z)
      // 	    cout << V1[snt1[z]] << " ";
      // 	  cout << " ::: ";
      // 	  for (size_t z = pp.s2; z < pp.e2; ++z)
      // 	    cout << V2[snt2[z]] << " ";
      // 	  cout << pp.stats.m1 << "/" << pp.stats.j << "/" << pp.stats.m2 << endl;
      // 	}
      // // cout << string(80,'=') << endl;
      // // sort(pp_all.begin(),pp_all.end());
      // // BOOST_FOREACH(PPair const& pp, pp_all)
      // // 	{
      // // 	  cout << pp.mi << " ";
      // // 	  for (size_t z = pp.s1; z < pp.e1; ++z)
      // // 	    cout << V1[snt1[z]] << " ";
      // // 	  cout << " ::: ";
      // // 	  for (size_t z = pp.s2; z < pp.e2; ++z)
      // // 	    cout << V2[snt2[z]] << " ";
      // // 	  cout << pp.m1 << "/" << pp.j << "/" << pp.m2 << endl;
      // // 	}

    }
}


#ifndef STAN_MATH_TORSTEN_REFACTOR_LINODEMODEL_HPP
#define STAN_MATH_TORSTEN_REFACTOR_LINODEMODEL_HPP

#include <Eigen/Dense>
#include <stan/math/torsten/events_manager.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/torsten/event_solver.hpp>
#include <stan/math/torsten/pmx_linode_model.hpp>
#include <stan/math/torsten/PKModel/PKModel.hpp>
#include <stan/math/torsten/PKModel/Pred/Pred1_linOde.hpp>
#include <stan/math/torsten/PKModel/Pred/PredSS_linOde.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <vector>

namespace torsten {

/**
 * Computes the predicted amounts in each compartment at each event
 * for a compartment model, described by a linear system of ordinary
 * differential equations. Uses the stan::math::matrix_exp
 * function.
 *
 * @tparam T0 type of scalar for time of events.
 * @tparam T1 type of scalar for amount at each event.
 * @tparam T2 type of scalar for rate at each event.
 * @tparam T3 type of scalar for inter-dose inteveral at each event.
 * @tparam T4 type of scalar for matrix describing linear ODE system.
 * @tparam T5 type of scalars for bio-variability parameters.
 * @tparam T6 type of scalars for tlag parameters
 * @param[in] time times of events
 * @param[in] amt amount at each event
 * @param[in] rate rate at each event
 * @param[in] ii inter-dose interval at each event
 * @param[in] evid event identity:
 *                    (0) observation
 *                    (1) dosing
 *                    (2) other
 *                    (3) reset
 *                    (4) reset AND dosing
 * @param[in] cmt compartment number at each event
 * @param[in] addl additional dosing at each event
 * @param[in] ss steady state approximation at each event (0: no, 1: yes)
 * between time-points
 * @param[in] system square matrix describing the linear system of ODEs
 * @param[in] bio-variability at each event
 * @param[in] lag times at each event
 * @return a matrix with predicted amount in each compartment
 * at each event.
 */
template <typename T0, typename T1, typename T2, typename T3,
          typename T4, typename T5, typename T6>
Eigen::Matrix <typename torsten::return_t<T0, T1, T2, T3, T4, T5, T6>::type,
               Eigen::Dynamic, Eigen::Dynamic>
pmx_solve_linode(const std::vector<T0>& time,
            const std::vector<T1>& amt,
            const std::vector<T2>& rate,
            const std::vector<T3>& ii,
            const std::vector<int>& evid,
            const std::vector<int>& cmt,
            const std::vector<int>& addl,
            const std::vector<int>& ss,
            const std::vector< Eigen::Matrix<T4, Eigen::Dynamic, Eigen::Dynamic> >& system,
            const std::vector<std::vector<T5> >& biovar,
            const std::vector<std::vector<T6> >& tlag) {
  using std::vector;
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using boost::math::tools::promote_args;
  using refactor::PKRec;

  static const char* function("pmx_solve_linode");
  for (size_t i = 0; i < system.size(); i++)
    stan::math::check_square(function, "system matrix", system[i]);
  int nCmt = system[0].cols();

  std::vector<T4> parameters_dummy(0);
  std::vector<std::vector<T4> > pMatrix_dummy(1, parameters_dummy);
  torsten::pmx_check(time, amt, rate, ii, evid, cmt, addl, ss,
                pMatrix_dummy, biovar, tlag, function);

#ifdef OLD_TORSTEN
  return Pred(time, amt, rate, ii, evid, cmt, addl, ss,
              pMatrix_dummy, biovar, tlag, nCmt, system,
              Pred1_linOde(), PredSS_linOde());
#else
  using ER = NONMENEventsRecord<T0, T1, T2, T3, Eigen::Matrix<T4,-1,-1>, T5, T6>;
  using EM = EventsManager<ER>;
  const ER events_rec(nCmt, time, amt, rate, ii, evid, cmt, addl, ss, system, biovar, tlag);

  Matrix<typename EM::T_scalar, Dynamic, Dynamic> pred =
    Matrix<typename EM::T_scalar, Dynamic, Dynamic>::Zero(events_rec.num_event_times(), EM::nCmt(events_rec));

  using model_type = refactor::PMXLinODEModel<typename EM::T_time, typename EM::T_scalar, typename EM::T_rate, typename EM::T_par>;
  EventSolver<model_type> pr;
  pr.pred(0, events_rec, pred);
  return pred;

#endif
}

/**
 * Overload function to allow user to pass a matrix for
 * system.
 */
template <typename T0, typename T1, typename T2, typename T3,
          typename T4, typename T_biovar, typename T_tlag>
Eigen::Matrix <typename torsten::return_t<T0, T1, T2, T3, T4,
                                          typename torsten::value_type<T_biovar>::type,
                                          typename torsten::value_type<T_tlag>::type>::type,
               Eigen::Dynamic, Eigen::Dynamic>
pmx_solve_linode(const std::vector<T0>& time,
                 const std::vector<T1>& amt,
                 const std::vector<T2>& rate,
                 const std::vector<T3>& ii,
                 const std::vector<int>& evid,
                 const std::vector<int>& cmt,
                 const std::vector<int>& addl,
                 const std::vector<int>& ss,
                 const Eigen::Matrix<T4, -1, -1>& system,
                 const std::vector<T_biovar>& biovar,
                 const std::vector<T_tlag>& tlag) {
  std::vector<Eigen::Matrix<T4, -1, -1> > system_{system};
  auto biovar_ = torsten::to_array_2d(biovar);
  auto tlag_ = torsten::to_array_2d(tlag);

  return pmx_solve_linode(time, amt, rate, ii, evid, cmt, addl, ss,
                          system_, biovar_, tlag_);
}

/**
 * Overload function to allow user to pass a matrix for
 * system.
 */
template <typename T0, typename T1, typename T2, typename T3,
          typename T4, typename T_biovar, typename T_tlag,
          typename
          std::enable_if_t<
            !(torsten::is_std_vector<T_biovar>::value && torsten::is_std_vector<T_tlag>::value)>* = nullptr> //NOLINT
Eigen::Matrix <typename torsten::return_t<T0, T1, T2, T3, T4,
                                          typename torsten::value_type<T_biovar>::type,
                                          typename torsten::value_type<T_tlag>::type>::type,
               Eigen::Dynamic, Eigen::Dynamic>
pmx_solve_linode(const std::vector<T0>& time,
                 const std::vector<T1>& amt,
                 const std::vector<T2>& rate,
                 const std::vector<T3>& ii,
                 const std::vector<int>& evid,
                 const std::vector<int>& cmt,
                 const std::vector<int>& addl,
                 const std::vector<int>& ss,
                 const std::vector< Eigen::Matrix<T4, -1, -1> >& system,
                 const std::vector<T_biovar>& biovar,
                 const std::vector<T_tlag>& tlag) {
  auto biovar_ = torsten::to_array_2d(biovar);
  auto tlag_ = torsten::to_array_2d(tlag);

  return pmx_solve_linode(time, amt, rate, ii, evid, cmt, addl, ss,
                          system, biovar_, tlag_);
}

  // old version by using transpose
template <typename T0, typename T1, typename T2, typename T3,
  typename T4, typename T5, typename T6>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
                                                         typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
               Eigen::Dynamic, Eigen::Dynamic>
linOdeModel(const std::vector<T0>& time,
            const std::vector<T1>& amt,
            const std::vector<T2>& rate,
            const std::vector<T3>& ii,
            const std::vector<int>& evid,
            const std::vector<int>& cmt,
            const std::vector<int>& addl,
            const std::vector<int>& ss,
            const std::vector< Eigen::Matrix<T4, Eigen::Dynamic, Eigen::Dynamic> >& system,
            const std::vector<std::vector<T5> >& biovar,
            const std::vector<std::vector<T6> >& tlag) {
  auto x = pmx_solve_linode(time, amt, rate, ii, evid, cmt, addl, ss, system, biovar, tlag);
  return x.transpose();
}

template <typename T0, typename T1, typename T2, typename T3,
          typename T4, typename T_biovar, typename T_tlag>
Eigen::Matrix <typename torsten::return_t<T0, T1, T2, T3, T4,
                                          typename torsten::value_type<T_biovar>::type,
                                          typename torsten::value_type<T_tlag>::type>::type,
               Eigen::Dynamic, Eigen::Dynamic>
linOdeModel(const std::vector<T0>& time,
                 const std::vector<T1>& amt,
                 const std::vector<T2>& rate,
                 const std::vector<T3>& ii,
                 const std::vector<int>& evid,
                 const std::vector<int>& cmt,
                 const std::vector<int>& addl,
                 const std::vector<int>& ss,
                 const Eigen::Matrix<T4, -1, -1>& system,
                 const std::vector<T_biovar>& biovar,
                 const std::vector<T_tlag>& tlag) {
  auto x = pmx_solve_linode(time, amt, rate, ii, evid, cmt, addl, ss, system, biovar, tlag);
  return x.transpose();
}

  template <typename T0, typename T1, typename T2, typename T3,
            typename T4, typename T_biovar, typename T_tlag,
            typename
            std::enable_if_t<
              !(torsten::is_std_vector<T_biovar>::value && torsten::is_std_vector<T_tlag>::value)>* = nullptr> //NOLINT
  Eigen::Matrix <typename torsten::return_t<T0, T1, T2, T3, T4,
                                            typename torsten::value_type<T_biovar>::type,
                                            typename torsten::value_type<T_tlag>::type>::type,
                 Eigen::Dynamic, Eigen::Dynamic>
  linOdeModel(const std::vector<T0>& time,
              const std::vector<T1>& amt,
              const std::vector<T2>& rate,
              const std::vector<T3>& ii,
              const std::vector<int>& evid,
              const std::vector<int>& cmt,
              const std::vector<int>& addl,
              const std::vector<int>& ss,
              const std::vector< Eigen::Matrix<T4, -1, -1> >& system,
              const std::vector<T_biovar>& biovar,
              const std::vector<T_tlag>& tlag) {
    auto x = pmx_solve_linode(time, amt, rate, ii, evid, cmt, addl, ss,
                            system, biovar, tlag);
    return x.transpose();
  }

}
#endif


#define BOOST_TEST_MODULE GRE
#include <boost/test/unit_test.hpp>

#include <chrono>
#include <cmath>
#include <fstream>

#include <sycomore/magnetization.h>
#include <sycomore/como/Model.h>
#include <sycomore/Pulse.h>
#include <sycomore/sycomore.h>
#include <sycomore/units.h>

#include <iostream>

using namespace sycomore::units;

struct Fixture
{
    static sycomore::Species const species;
    static sycomore::Magnetization const m0;

    static sycomore::Quantity const flip_angle;
    static sycomore::Quantity const pulse_duration;
    static int const pulse_support_size = 101;
    static int const zero_crossings = 2;

    static sycomore::Quantity const TR;
    static sycomore::Quantity const slice_thickness;

    static size_t const TR_count = 10;
};
sycomore::Species const Fixture::species{1000_ms, 100_ms, 0.89_um*um/ms};
sycomore::Magnetization const Fixture::m0{0,0,1};
sycomore::Quantity const Fixture::flip_angle=40_deg;
sycomore::Quantity const Fixture::pulse_duration=1_ms;
sycomore::Quantity const Fixture::TR=500_ms;
sycomore::Quantity const Fixture::slice_thickness=1_mm;

BOOST_FIXTURE_TEST_CASE(Ideal, Fixture, *boost::unit_test::tolerance(1e-9))
{
    sycomore::como::Model model(species, m0, {{"echo", {TR/2.}}});

    std::vector<sycomore::Magnetization> magnetization;
    for(int i=0; i<TR_count; ++i)
    {
        model.apply_pulse({flip_angle, (M_PI/3+(i%2)*M_PI)*rad});
        model.apply_time_interval("echo");
        magnetization.push_back(model.isochromat());
        model.apply_time_interval("echo");
    }

    std::vector<double> baseline;
    std::string const root(getenv("SYCOMORE_TEST_DATA")?getenv("SYCOMORE_TEST_DATA"):"");
    if(root.empty())
    {
        throw std::runtime_error("SYCOMORE_TEST_DATA is undefined");
    }
    std::ifstream stream(root+"/baseline/GRE_ideal.dat", std::ios_base::binary);
    while(stream.good())
    {
        double value;
        stream.read(reinterpret_cast<char*>(&value), sizeof(value));
        if(stream.good())
        {
            baseline.push_back(value);
        }
    }

    BOOST_REQUIRE_EQUAL(baseline.size(), 3*TR_count);
    for(size_t i=0; i!=TR_count; ++i)
    {
        BOOST_TEST(magnetization[i][0] == baseline[3*i+0]);
        BOOST_TEST(magnetization[i][1] == baseline[3*i+1]);
        BOOST_TEST(magnetization[i][2] == baseline[3*i+2]);
    }
}

BOOST_FIXTURE_TEST_CASE(Real, Fixture, *boost::unit_test::tolerance(1e-9))
{
    auto const t0 = pulse_duration/(2*zero_crossings);
    sycomore::HardPulseApproximation sinc_pulse(
        {flip_angle, 0_rad},
        sycomore::linspace(pulse_duration, pulse_support_size),
        sycomore::sinc_envelope(t0), 1/t0, slice_thickness, "rf");

    sycomore::como::Model model(
        species, m0, {
            {"rf", sinc_pulse.get_time_interval()},
            {"half_echo", {
                (TR-pulse_duration)/2., -sinc_pulse.get_gradient_moment()/2}}
    });

    std::vector<sycomore::Magnetization> magnetization;

    // CoMoTk: 6.13 s
    // Total time: 0.85933 s
    // Pulse time: 0.334359 s
    // Time interval time: 0.521324 s
    // Unaccounted time: 0.00364655 s
    // Sycomore speedup: 7.13

    auto const start = std::chrono::high_resolution_clock::now();
    for(int i=0; i<TR_count; ++i)
    {
        sinc_pulse.set_phase((M_PI/3+(i%2)*M_PI)*rad);
        model.apply_pulse(sinc_pulse);
        model.apply_time_interval("half_echo");
        magnetization.push_back(model.isochromat());
        model.apply_time_interval("half_echo");
    }
    auto const total_time = std::chrono::duration<double, std::ratio<1>>(
        std::chrono::high_resolution_clock::now()-start).count();

    std::cout << "{\"total\": " << total_time;
    double accounted=0;
    for(auto && item: model.timers())
    {
        std::cout << ", \"" << item.first << "\": " << item.second;
        accounted += item.second;
    }
    std::cout << ", \"extra\": " << total_time-accounted << "}\n";

    std::vector<double> baseline;
    std::string const root(getenv("SYCOMORE_TEST_DATA")?getenv("SYCOMORE_TEST_DATA"):"");
    if(root.empty())
    {
        throw std::runtime_error("SYCOMORE_TEST_DATA is undefined");
    }
    std::ifstream stream(root+"/baseline/GRE_real.dat", std::ios_base::binary);
    while(stream.good())
    {
        double value;
        stream.read(reinterpret_cast<char*>(&value), sizeof(value));
        if(stream.good())
        {
            baseline.push_back(value);
        }
    }

    BOOST_REQUIRE_EQUAL(baseline.size(), 3*TR_count);
    for(size_t i=0; i!=TR_count; ++i)
    {
        BOOST_TEST(magnetization[i][0] == baseline[3*i+0]);
        BOOST_TEST(magnetization[i][1] == baseline[3*i+1]);
        BOOST_TEST(magnetization[i][2] == baseline[3*i+2]);
    }
}


#include "net.hpp"
#include "net_cpu.hpp"
#include "net_gpu.hpp"

#include <armadillo>
#include <iostream>

int main(int argc, char * argv[]) {
  arma::Mat<float> A;
  A.load("mnist.arma");
  A.shed_row(0); //remove header labels
  const int n_rows = 400;
  const int output_cols = 10;
  const int test_num = 200;
  arma::Mat<float> labels(n_rows, output_cols, arma::fill::zeros);
  arma::Mat<float> test_labels(test_num, output_cols, arma::fill::zeros);
  int i=0;
  int j=0;
  for (auto iter = A.begin_col(0); iter != A.end_col(0); ++iter) {
    if (i >= n_rows) {
      if (j >= test_num) {
        break;
      }
      test_labels(j, *iter) = 1;
      j += 1;
      continue;
    }
    labels(i, *iter) = 1;
    i += 1;
  }

  std::cout << labels << std::endl;
  A.shed_col(0);

  A /= 256;
  A -= .5;
  arma::Mat<float> test = A.rows(n_rows, n_rows + test_num - 1);

  A.shed_rows(n_rows, A.n_rows-1);
  std::cout << A.n_rows << "," << labels.n_rows << std::endl;
  arma::Mat<float> data = join_rows(labels, A);
  const int feature_size = 784;
  FeedForward_Network<> f({feature_size, 1000, 10});
  randomize(f);
  std::cout << "train" << std::endl;
  for (int i=0; i <80; i++) {
    std::cout << i << std::endl;
    gpu_train_batch(f, data.cols(output_cols, data.n_cols - 1).rows(0, n_rows-1), data.cols(0, output_cols-1), 10, 0.03f);
    std::cout << "start score" << std::endl;
    arma::Mat<float> result = gpu_predict(f, data.cols(output_cols, data.n_cols-1).rows(0, n_rows-1));
    std::cout << "Score: " << classify_percent_score(result, data.cols(0, output_cols-1)) << std::endl;

    result = gpu_predict(f, test);
    std::cout << "TestS: " << classify_percent_score(result, test_labels) << std::endl;
    std::cout << "Shuffling" << std::endl;
    data = shuffle(data);
  }

  return 0;
}


//
//  FrictionUtils.hpp
//  IPC
//
//  Created by Minchen Li on 9/26/19.
//

#ifndef FrictionUtils_hpp
#define FrictionUtils_hpp

#include "Types.hpp"

#include "MeshCollisionUtils.hpp"

#include <Eigen/Eigen>

#include <iostream>
#include <array>

namespace IPC {

// Point - Triangle

inline void computeTangentBasis_PT(
    const Eigen::RowVector3d& v0,
    const Eigen::RowVector3d& v1,
    const Eigen::RowVector3d& v2,
    const Eigen::RowVector3d& v3,
    Eigen::Matrix<double, 3, 2>& basis)
{
    Eigen::Vector3d v12 = (v2 - v1).transpose();
    basis.col(0) = v12.normalized();
    basis.col(1) = v12.cross((v3 - v1).transpose()).cross(v12).normalized();
}

inline void computeClosestPoint_PT(
    const Eigen::RowVector3d& v0,
    const Eigen::RowVector3d& v1,
    const Eigen::RowVector3d& v2,
    const Eigen::RowVector3d& v3,
    Eigen::Vector2d& beta)
{
    Eigen::Matrix<double, 2, 3> basis;
    basis.row(0) = v2 - v1;
    basis.row(1) = v3 - v1;
    beta = (basis * basis.transpose()).ldlt().solve(basis * (v0 - v1).transpose());
}

inline void computeRelDX_PT(
    const Eigen::RowVector3d& dx0,
    const Eigen::RowVector3d& dx1,
    const Eigen::RowVector3d& dx2,
    const Eigen::RowVector3d& dx3,
    double beta1, double beta2,
    Eigen::RowVector3d& relDX)
{
    relDX = dx0 - (dx1 + beta1 * (dx2 - dx1) + beta2 * (dx3 - dx1));
}

inline void liftRelDXTanToMesh_PT(
    const Eigen::Vector2d& relDXTan,
    const Eigen::Matrix<double, 3, 2>& basis,
    double beta1, double beta2,
    Eigen::Matrix<double, 12, 1>& TTTDX)
{
    TTTDX.template segment<3>(0) = basis * relDXTan;
    TTTDX.template segment<3>(3) = (-1 + beta1 + beta2) * TTTDX.template segment<3>(0);
    TTTDX.template segment<3>(6) = -beta1 * TTTDX.template segment<3>(0);
    TTTDX.template segment<3>(9) = -beta2 * TTTDX.template segment<3>(0);
}

inline void computeTTT_PT(
    const Eigen::Matrix<double, 3, 2>& basis,
    double beta1, double beta2,
    Eigen::Matrix<double, 12, 12>& TTT)
{
    Eigen::Matrix<double, 2, 12> TT;
    TT.template block<2, 3>(0, 0) = basis.transpose();
    TT.template block<2, 3>(0, 3) = (-1 + beta1 + beta2) * basis.transpose();
    TT.template block<2, 3>(0, 6) = -beta1 * basis.transpose();
    TT.template block<2, 3>(0, 9) = -beta2 * basis.transpose();
    TTT = TT.transpose() * TT;
}

// Edge - Edge

inline void computeTangentBasis_EE(
    const Eigen::RowVector3d& v0,
    const Eigen::RowVector3d& v1,
    const Eigen::RowVector3d& v2,
    const Eigen::RowVector3d& v3,
    Eigen::Matrix<double, 3, 2>& basis)
{
    Eigen::Vector3d v01 = (v1 - v0).transpose();
    basis.col(0) = v01.normalized();
    basis.col(1) = v01.cross((v3 - v2).transpose()).cross(v01).normalized();
}

inline void computeClosestPoint_EE(
    const Eigen::RowVector3d& v0,
    const Eigen::RowVector3d& v1,
    const Eigen::RowVector3d& v2,
    const Eigen::RowVector3d& v3,
    Eigen::Vector2d& gamma)
{
    Eigen::RowVector3d e20 = v0 - v2;
    Eigen::RowVector3d e01 = v1 - v0;
    Eigen::RowVector3d e23 = v3 - v2;

    Eigen::Matrix2d coefMtr;
    coefMtr(0, 0) = e01.squaredNorm();
    coefMtr(0, 1) = coefMtr(1, 0) = -e23.dot(e01);
    coefMtr(1, 1) = e23.squaredNorm();

    Eigen::Vector2d rhs;
    rhs[0] = -e20.dot(e01);
    rhs[1] = e20.dot(e23);

    gamma = coefMtr.ldlt().solve(rhs);
}

inline void computeRelDX_EE(
    const Eigen::RowVector3d& dx0,
    const Eigen::RowVector3d& dx1,
    const Eigen::RowVector3d& dx2,
    const Eigen::RowVector3d& dx3,
    double gamma1, double gamma2,
    Eigen::RowVector3d& relDX)
{
    relDX = dx0 + gamma1 * (dx1 - dx0) - (dx2 + gamma2 * (dx3 - dx2));
}

inline void liftRelDXTanToMesh_EE(
    const Eigen::Vector2d& relDXTan,
    const Eigen::Matrix<double, 3, 2>& basis,
    double gamma1, double gamma2,
    Eigen::Matrix<double, 12, 1>& TTTDX)
{
    Eigen::Vector3d relDXTan3D = basis * relDXTan;
    TTTDX.template segment<3>(0) = (1.0 - gamma1) * relDXTan3D;
    TTTDX.template segment<3>(3) = gamma1 * relDXTan3D;
    TTTDX.template segment<3>(6) = (gamma2 - 1.0) * relDXTan3D;
    TTTDX.template segment<3>(9) = -gamma2 * relDXTan3D;
}

inline void computeTTT_EE(
    const Eigen::Matrix<double, 3, 2>& basis,
    double gamma1, double gamma2,
    Eigen::Matrix<double, 12, 12>& TTT)
{
    Eigen::Matrix<double, 2, 12> TT;
    TT.template block<2, 3>(0, 0) = (1.0 - gamma1) * basis.transpose();
    TT.template block<2, 3>(0, 3) = gamma1 * basis.transpose();
    TT.template block<2, 3>(0, 6) = (gamma2 - 1.0) * basis.transpose();
    TT.template block<2, 3>(0, 9) = -gamma2 * basis.transpose();
    TTT = TT.transpose() * TT;
}

// Point - Edge

inline void computeTangentBasis_PE(
    const Eigen::RowVector3d& v0,
    const Eigen::RowVector3d& v1,
    const Eigen::RowVector3d& v2,
    Eigen::Matrix<double, 3, 2>& basis)
{
    Eigen::Vector3d v12 = (v2 - v1).transpose();
    basis.col(0) = v12.normalized();
    basis.col(1) = v12.cross((v0 - v1).transpose()).normalized();
}

inline void computeClosestPoint_PE(
    const Eigen::RowVector3d& v0,
    const Eigen::RowVector3d& v1,
    const Eigen::RowVector3d& v2,
    double& yita)
{
    Eigen::RowVector3d e12 = v2 - v1;
    yita = (v0 - v1).dot(e12) / e12.squaredNorm();
}

inline void computeRelDX_PE(
    const Eigen::RowVector3d& dx0,
    const Eigen::RowVector3d& dx1,
    const Eigen::RowVector3d& dx2,
    double yita,
    Eigen::RowVector3d& relDX)
{
    relDX = dx0 - (dx1 + yita * (dx2 - dx1));
}

inline void liftRelDXTanToMesh_PE(
    const Eigen::Vector2d& relDXTan,
    const Eigen::Matrix<double, 3, 2>& basis,
    double yita,
    Eigen::Matrix<double, 9, 1>& TTTDX)
{
    TTTDX.template segment<3>(0) = basis * relDXTan;
    TTTDX.template segment<3>(3) = (yita - 1.0) * TTTDX.template segment<3>(0);
    TTTDX.template segment<3>(6) = -yita * TTTDX.template segment<3>(0);
}

inline void computeTTT_PE(
    const Eigen::Matrix<double, 3, 2>& basis,
    double yita,
    Eigen::Matrix<double, 9, 9>& TTT)
{
    Eigen::Matrix<double, 2, 9> TT;
    TT.template block<2, 3>(0, 0) = basis.transpose();
    TT.template block<2, 3>(0, 3) = (yita - 1.0) * basis.transpose();
    TT.template block<2, 3>(0, 6) = -yita * basis.transpose();
    TTT = TT.transpose() * TT;
}

// Point - Point

inline void computeTangentBasis_PP(
    const Eigen::RowVector3d& v0,
    const Eigen::RowVector3d& v1,
    Eigen::Matrix<double, 3, 2>& basis)
{
    Eigen::RowVector3d v01 = v1 - v0;
    Eigen::RowVector3d xCross = Eigen::RowVector3d::UnitX().cross(v01);
    Eigen::RowVector3d yCross = Eigen::RowVector3d::UnitY().cross(v01);
    if (xCross.squaredNorm() > yCross.squaredNorm()) {
        basis.col(0) = xCross.normalized().transpose();
        basis.col(1) = v01.cross(xCross).normalized().transpose();
    }
    else {
        basis.col(0) = yCross.normalized().transpose();
        basis.col(1) = v01.cross(yCross).normalized().transpose();
    }
}

inline void computeRelDX_PP(
    const Eigen::RowVector3d& dx0,
    const Eigen::RowVector3d& dx1,
    Eigen::RowVector3d& relDX)
{
    relDX = dx0 - dx1;
}

inline void liftRelDXTanToMesh_PP(
    const Eigen::Vector2d& relDXTan,
    const Eigen::Matrix<double, 3, 2>& basis,
    Eigen::Matrix<double, 6, 1>& TTTDX)
{
    TTTDX.template segment<3>(0) = basis * relDXTan;
    TTTDX.template segment<3>(3) = -TTTDX.template segment<3>(0);
}

inline void computeTTT_PP(
    const Eigen::Matrix<double, 3, 2>& basis,
    Eigen::Matrix<double, 6, 6>& TTT)
{
    Eigen::Matrix<double, 2, 6> TT;
    TT.template block<2, 3>(0, 0) = basis.transpose();
    TT.template block<2, 3>(0, 3) = -basis.transpose();
    TTT = TT.transpose() * TT;
}

// static friction clamping model
// C0 clamping
inline void f0_SF_C0(double x2, double eps_f, double& f0)
{
    f0 = x2 / (2.0 * eps_f) + eps_f / 2.0;
}

inline void f1_SF_div_relDXNorm_C0(double eps_f, double& result)
{
    result = 1.0 / eps_f;
}

inline void f2_SF_C0(double eps_f, double& f2)
{
    f2 = 1.0 / eps_f;
}

// C1 clamping
inline void f0_SF_C1(double x2, double eps_f, double& f0)
{
    f0 = x2 * (-std::sqrt(x2) / 3.0 + eps_f) / (eps_f * eps_f) + eps_f / 3.0;
}

inline void f1_SF_div_relDXNorm_C1(double x2, double eps_f, double& result)
{
    result = (-std::sqrt(x2) + 2.0 * eps_f) / (eps_f * eps_f);
}

inline void f2_SF_C1(double x2, double eps_f, double& f2)
{
    f2 = 2.0 * (eps_f - std::sqrt(x2)) / (eps_f * eps_f);
}

// C2 clamping
inline void f0_SF_C2(double x2, double eps_f, double& f0)
{
    f0 = x2 * (0.25 * x2 - (std::sqrt(x2) - 1.5 * eps_f) * eps_f) / (eps_f * eps_f * eps_f) + eps_f / 4.0;
}

inline void f1_SF_div_relDXNorm_C2(double x2, double eps_f, double& result)
{
    result = (x2 - (3.0 * std::sqrt(x2) - 3.0 * eps_f) * eps_f) / (eps_f * eps_f * eps_f);
}

inline void f2_SF_C2(double x2, double eps_f, double& f2)
{
    f2 = 3.0 * (x2 - (2.0 * std::sqrt(x2) - eps_f) * eps_f) / (eps_f * eps_f * eps_f);
}

// interfaces
inline void f0_SF(double relDXSqNorm, double eps_f, double& f0)
{
#if (SFCLAMPING_ORDER == 0)
    f0_SF_C0(relDXSqNorm, eps_f, f0);
#elif (SFCLAMPING_ORDER == 1)
    f0_SF_C1(relDXSqNorm, eps_f, f0);
#elif (SFCLAMPING_ORDER == 2)
    f0_SF_C2(relDXSqNorm, eps_f, f0);
#endif
}

inline void f1_SF_div_relDXNorm(double relDXSqNorm, double eps_f, double& result)
{
#if (SFCLAMPING_ORDER == 0)
    f1_SF_div_relDXNorm_C0(eps_f, result);
#elif (SFCLAMPING_ORDER == 1)
    f1_SF_div_relDXNorm_C1(relDXSqNorm, eps_f, result);
#elif (SFCLAMPING_ORDER == 2)
    f1_SF_div_relDXNorm_C2(relDXSqNorm, eps_f, result);
#endif
}

inline void f2_SF(double relDXSqNorm, double eps_f, double& f2)
{
#if (SFCLAMPING_ORDER == 0)
    f2_SF_C0(eps_f, f2);
#elif (SFCLAMPING_ORDER == 1)
    f2_SF_C1(relDXSqNorm, eps_f, f2);
#elif (SFCLAMPING_ORDER == 2)
    f2_SF_C2(relDXSqNorm, eps_f, f2);
#endif
}

} // namespace IPC

#endif /* FrictionUtils_hpp */


#include "../include/ParticleFilter.h"

#include <boost/functional/hash.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <algorithm>
#include <cmath>
#include <iostream>
#include <numeric>
#include <unordered_set>

ParticleFilter::ParticleFilter(const cv::Mat& model,
                               const cv::Size& imSize,
                               const size_t numParticles,
                               const SimilarityMode simMode,
                               const double mseSigma,
                               const double sampleSigma,
                               const cv::Point2f& initModelPos,
                               const double alpha)
    : _modelPatch(model), _lastModelPatch(model), _modelPatchSize(model.size()), _imSize(imSize),
      _numParticles(numParticles), _simMode(simMode), _mseSigma(mseSigma),
      _sampleSigma(sampleSigma), _initModelPos(initModelPos), _alpha(alpha) {
    // If the similarity mode uses mean-squared error, then the model is just the input image patch
    if (simMode == SimilarityMode::MEAN_SHIFT_LT) {
        // Model is the histogram of the image patch
        _modelHist = calcHist(model);
    }
    std::cout << "Initialized" << std::endl;

    if (initModelPos.x == -1 && initModelPos.y == -1) {
        // No initial position, so generate uniform particles
        genParticles(0.f,
                     float(imSize.width),
                     0.f,
                     float(imSize.height),
                     numParticles,
                     GenParticleMode::UNIFORM,
                     _particles);
    } else {
        // Use initial position to generate particles
        float modelCenterX = initModelPos.x + float(model.cols) / 2.f;
        float modelCenterY = initModelPos.y + float(model.rows) / 2.f;
        genParticles(0.f,
                     float(imSize.width),
                     0.f,
                     float(imSize.height),
                     numParticles,
                     GenParticleMode::GAUSSIAN,
                     _particles,
                     _sampleSigma,
                     modelCenterX,
                     modelCenterY);
    }

    // Initialize N weights to 1/N
    _weights.assign(_particles.size(), 1.f / float(_particles.size()));
}

std::tuple<cv::Point2f, float, float> ParticleFilter::tick(const cv::Mat& frame) {
    // Predict the movement of all the particles
    displaceParticles();
    // Update weights with the movement of those particles
    updateParticles(frame);
    // Resample based on newly computed weights
    resampleMultinomial();
    // Predict the new state
    cv::Point2f bestEstPos;
    auto estimate = estimateState();
    // Update model
    std::tie(bestEstPos, std::ignore, std::ignore) = estimate;
    updateModel(frame, bestEstPos);

    return estimate;
}

const std::vector<cv::Point2f>& ParticleFilter::getParticles() const { return _particles; }

void ParticleFilter::drawParticles(cv::Mat& frame, const cv::Scalar& color) {
    for (const auto& p : _particles) {
        cv::circle(frame, p, 1, color, -1);
    }
}

void ParticleFilter::displaceParticles() {
    // Iterate over particles and displace
    cv::RNG rng;
    for (auto& p : _particles) {
        p.x += rng.gaussian(_sampleSigma);
        p.y += rng.gaussian(_sampleSigma);
    }
}

void ParticleFilter::updateParticles(const cv::Mat& frame) {
    const int rows = _modelPatchSize.height;
    const int cols = _modelPatchSize.width;

    const int xOffset = std::ceil(float(cols) / 2.f);
    const int yOffset = std::ceil(float(rows) / 2.f);

    cv::copyMakeBorder(
        frame, _paddedFrame, yOffset, yOffset, xOffset, xOffset, cv::BORDER_REPLICATE);

    // Iterate over particles and compute similarity
    double simSum = 0;
    for (int i = 0; i < _numParticles; i++) {
        const auto& p = _particles[i];

        // If the particle is outside the bounds of the image, then similarity can no longer be
        // computed. In that case just set the weight to 0.
        if (p.x < 0 || p.x >= _imSize.width || p.y < 0 || p.y >= _imSize.height) {
            _weights[i] = 0;
        } else {
            // Get a sample patch from the image
            cv::Mat candidate = _paddedFrame(cv::Rect(p, _modelPatchSize));

            double similarity = computeSimilarityToModel(candidate);
            simSum += similarity;
            _weights[i] = similarity;
        }
    }

    // Normalize weights
    std::transform(_weights.begin(), _weights.end(), _weights.begin(), [&simSum](float f) -> float {
        return f / simSum;
    });
}

void ParticleFilter::resampleMultinomial() {
    // Compute the cumulative sum of the weights to get a sorted array from 0.f to 1.f
    std::vector<float> cumulativeSum(_numParticles);
    if (_numParticles > 0) {
        cumulativeSum[0] = _weights[0];
    }

    for (int i = 1; i < _numParticles; i++) {
        cumulativeSum[i] = _weights[i] + cumulativeSum[i - 1];
    }

    // Generate N random points and see where they would fit in the array
    std::vector<cv::Point2f> partSampled(_numParticles);
    cv::RNG rng;
    for (int i = 0; i < _numParticles; i++) {
        float val = rng.uniform(0.f, 1.f);
        // Get an iterator to the first index that is greater than the random number
        auto iter = std::upper_bound(cumulativeSum.begin(), cumulativeSum.end(), val);
        int idx = iter - cumulativeSum.begin();
        // Copy that particle to a new vector of particles
        partSampled[i] = _particles[idx];
    }

    // Replace each particle with the particle corresponding with the index found previously
    _particles = partSampled;
}

double ParticleFilter::computeSimilarityToModel(const cv::Mat& candidate) {
    if (_simMode == SimilarityMode::MEAN_SQ_ERR) {
        double mse = calcMeanSqErr(candidate);
        // Compute the similarity with squared exponential
        return std::exp(-mse / (2 * _mseSigma * _mseSigma));
    } else /* if (_simMode == SimilarityMode::MEAN_SHIFT_LT) */ {
        std::vector<cv::Mat> candidateHist = calcHist(candidate);
        double comp = 0;
        for (int ch = 0; ch < candidateHist.size(); ch++) {
            comp += cv::compareHist(_modelHist[ch], candidateHist[ch], _histComp);
        }
        comp /= candidateHist.size();
        return std::exp(-comp);
    }
}

double ParticleFilter::calcMeanSqErr(const cv::Mat& candidate) {
    assert(candidate.rows == _modelPatch.rows && candidate.cols == _modelPatch.cols &&
           candidate.type() == _modelPatch.type());

    // Compute mean-squared error
    cv::Mat diff = _modelPatch - candidate;
    diff = diff.mul(diff);
    cv::Scalar sums = cv::sum(diff);
    double mse = 0;
    for (int ch = 0; ch < 4; ch++) {
        mse += sums[ch];
    }
    mse /= double(candidate.rows * candidate.cols);
}

std::vector<cv::Mat> ParticleFilter::calcHist(const cv::Mat& patch) {
    // Calculate histogram for each channel, then push that channel's hist back onto a vector
    std::vector<cv::Mat> hists;
    for (int i = 0; i < patch.channels(); i++) {
        cv::Mat hist;
        cv::calcHist(std::vector<cv::Mat>{patch},
                     std::vector<int>{i},
                     cv::Mat(),
                     hist,
                     std::vector<int>{_histBins},
                     std::vector<float>{0, 256});

        cv::normalize(hist, hist, 1, 0, cv::NORM_L2, -1);
        hists.push_back(hist);
    }

    return hists;
}

std::tuple<cv::Point2f, float, float> ParticleFilter::estimateState() {
    float xMean = 0;
    float yMean = 0;
    // X and Y variances
    float xVar = 0;
    float yVar = 0;

    // Iterate over particles and sum
    for (const auto& p : _particles) {
        xMean += p.x;
        yMean += p.y;
    }

    xMean /= float(_numParticles);
    yMean /= float(_numParticles);

    // Compute variance
    for (const auto& p : _particles) {
        xVar += (p.x - xMean) * (p.x - xMean);
        yVar += (p.y - yMean) * (p.y - yMean);
    }

    xVar /= float(_numParticles);
    yVar /= float(_numParticles);

    return std::make_tuple(cv::Point2f(xMean, yMean), xVar, yVar);
}

void ParticleFilter::updateModel(const cv::Mat& frame, const cv::Point2f& newModelPos) {
    // Padded frame was already computed by updateParticles()
    cv::Mat newPatch = _paddedFrame(cv::Rect(newModelPos, _modelPatchSize));
    if (_simMode == SimilarityMode::MEAN_SQ_ERR) {
        // Save the old model
        _lastModelPatch = _modelPatch;
        // Update the model
        _modelPatch = _alpha * newPatch + (1.0 - _alpha) * _lastModelPatch;
    } else {
        _lastModelPatch = _modelPatch;
        _lastModelHist = _modelHist;
        cv::Mat blended = _alpha * newPatch + (1.0 - _alpha) * _lastModelPatch;
        _modelHist = calcHist(blended);
    }
}

struct PairEqual {
    template <typename T1, typename T2>
    bool operator()(const std::pair<T1, T2>& lhs, const std::pair<T1, T2>& rhs) const {
        return lhs.first == rhs.first && lhs.second == rhs.second;
    }
};

template <typename T>
void ParticleFilter::genParticles(const T xMin,
                                  const T xMax,
                                  const T yMin,
                                  const T yMax,
                                  const size_t numParticles,
                                  const GenParticleMode mode,
                                  std::vector<cv::Point_<T>>& particles,
                                  const double sigma,
                                  const T xInit,
                                  const T yInit) {
    cv::RNG rng;

    // Hold points in a set with constant lookup so we can make sure the generated particles are
    // unique
    std::unordered_set<std::pair<T, T>, boost::hash<std::pair<T, T>>, PairEqual> particleMap;

    size_t count = 0;
    T xVal, yVal;
    while (count < numParticles) {
        if (mode == GenParticleMode::UNIFORM) {
            xVal = rng.uniform(xMin, xMax);
            yVal = rng.uniform(yMin, yMax);
        } else /* if (mode == GenParticleMode::GAUSSIAN) */ {
            xVal = rng.gaussian(sigma) + xInit;
            yVal = rng.gaussian(sigma) + yInit;
        }

        auto result = particleMap.emplace(xVal, yVal);
        // Make sure that emplacing was successful
        if (result.second) {
            // The point did not already exist in the map, so add it to the list of particles
            particles.emplace_back(xVal, yVal);
            count++;
        }
    }
}


#include <boost/math/distributions/non_central_t.hpp>


/* Copyright 2017 Battelle Energy Alliance, LLC

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

   http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
/*
 * distributionNDNormal.C
 * Created on Oct. 23, 2015
 * Author: @wangc
 * Extracted from @alfoa (Feb 6, 2014) distribution_base_ND.C
 *
 */

#include "distributionNDNormal.h"
#include "distributionNDBase.h"
#include "DistributionContainer.h"
#include <stdexcept>
#include <iostream>
#include "MDreader.h"
#include "distributionFunctions.h"
#include <cmath>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions/normal.hpp>
#include "distribution_1D.h"
using boost::math::normal;

#include <ctime>

#define _use_math_defines

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

//using namespace boost::numeric::ublas;

#define throwError(msg) { std::cerr << "\n\n" << msg << "\n\n"; throw std::runtime_error("Error"); }

#ifndef M_PI
//PI is not actually defined anywhere in the C++ standard.
#define M_PI 3.14159265358979323846
#endif

void BasicMultivariateNormal::base10ToBaseN(int value_base10, int base, std::vector<int> & value_base_n){
    /**
     * This function convert a number in base 10 to a new number in any base N
     */

     int index = 0 ;

     if (value_base10 == 0)
       value_base_n.push_back(0);
     else{
       while ( value_base10 != 0 ){
         int remainder = value_base10 % base ;  // assume K > 1
         value_base10  = value_base10 / base ;  // integer division
         value_base_n.push_back(remainder);
         index++ ;
      }
     }
}

//void BasicMultivariateNormal::basicMultivariateNormalInit(std::string data_filename, std::vector<double> mu){
void BasicMultivariateNormal::basicMultivariateNormalInit(unsigned int &rows, unsigned int &columns, std::vector<std::vector<double> > cov_matrix, std::vector<double> mu){
    /**
     * This is the base function that initializes the Multivariate normal distribution
     * Input Parameter
     * rows: first dimension of covariance matrix
     * columns: second dimension of covariance matrix
     * cov_matrix: covariance matrix stored in vector<vector<double> >
     * mu: mean value stored in vector<double>
     */

   _mu = mu;
   _cov_matrix = cov_matrix;

   std::vector<std::vector<double> > inverseCovMatrix (rows,std::vector< double >(columns));

   computeInverse(_cov_matrix, inverseCovMatrix);

   for (unsigned int i=0;i<rows;i++){
    std::vector<double> temp;
    for (unsigned int j=0;j<columns;j++)
     temp.push_back(inverseCovMatrix.at(i).at(j));
    _inverse_cov_matrix.push_back(temp);
   }

   unsigned int dimensions = _mu.size();
   //for(int i=0; i<dimensions; i++)
  //for(int j=0; j<dimensions; j++)
   //std::cerr<<_inverse_cov_matrix[i][j]<<std::endl;

   _determinant_cov_matrix = getDeterminant(_cov_matrix);

   _cholesky_C = choleskyDecomposition(_cov_matrix);

   if(rows != columns)
     throwError("MultivariateNormal error: covariance matrix in is not a square matrix.");

   // Creation BasicMultiDimensionalCartesianSpline(std::vector< std::vector<double> > & discretizations, std::vector<double> & values, std::vector<double> alpha, std::vector<double> beta, bool cdf_provided)

   int numberValues=1;
   std::vector< std::vector<double> > discretizations;
   std::vector<double> alpha (_mu.size());
   std::vector<double> beta (_mu.size());
   // for now we use this to be a bit more less problem dependent
   double floatDiscretizations = (1./std::pow(1.e-4,1./ (double)dimensions)) + 0.5;
   int numberOfDiscretizations = (int)floatDiscretizations;
   for(unsigned int i=0; i<dimensions; i++){
     alpha.at(i) = 0.0;
     beta.at(i)  = 0.0;
     numberValues = numberValues * numberOfDiscretizations;

     std::vector<double> discretization_temp;
     double sigma = sqrt(_cov_matrix[i][i]);
     double deltaSigma = 12.0*sigma/(double)numberOfDiscretizations;
     for(int n=0; n<numberOfDiscretizations; n++){
       double disc_value = mu.at(i) - 6.0 * sigma + deltaSigma * (double)n;
       discretization_temp.push_back(disc_value);
     }
     discretizations.push_back(discretization_temp);
     _lower_bounds.push_back(discretization_temp.at(0));
     _upper_bounds.push_back(discretization_temp.back());
   }
   std::vector< double > values (numberValues);
   for(int i=0; i<numberValues; i++){
     std::vector<int> intCoordinates;
     base10ToBaseN(i,numberOfDiscretizations,intCoordinates);
     std::vector<double> point_coordinates(dimensions);
     std::vector<int> intCoordinatesFormatted(dimensions);

     for(unsigned int j=0; j<dimensions; j++)
       intCoordinatesFormatted.at(j) = 0;
     for(unsigned int j=0; j<intCoordinates.size(); j++)
       intCoordinatesFormatted.at(j) = intCoordinates.at(j);

     for(unsigned int j=0; j<intCoordinates.size(); j++)
       point_coordinates.at(j) = discretizations.at(j).at(intCoordinatesFormatted.at(j));

     values.at(i) = getPdf(point_coordinates, _mu, _inverse_cov_matrix);
   }
   _cartesian_distribution = BasicMultiDimensionalCartesianSpline(discretizations,values,alpha,beta,false);

}

BasicMultivariateNormal::BasicMultivariateNormal(std::string data_filename, std::vector<double> mu){
    /**
     * This is the function that initializes the Multivariate normal distribution given:
     * - data_filename: it specifies the covariance matrix
     * - mu: the mean value vector
     */
  unsigned int rows,columns;
  std::vector<std::vector<double> > cov_matrix;
  readMatrix(data_filename, rows, columns, cov_matrix);
  basicMultivariateNormalInit(rows,columns,cov_matrix, mu);
}

BasicMultivariateNormal::BasicMultivariateNormal(const char * data_filename, std::vector<double> mu){
    /**
     * This is the function that initializes the Multivariate normal distribution given:
     * - data_filename: it specifies the covariance matrix
     * - mu: the mean value vector
     */
  unsigned int rows,columns;
  std::vector<std::vector<double> > cov_matrix;
  readMatrix(std::string(data_filename), rows, columns, cov_matrix);
  basicMultivariateNormalInit(rows,columns,cov_matrix, mu);
  //basicMultivariateNormalInit(std::string(data_filename) , mu);
}

BasicMultivariateNormal::BasicMultivariateNormal(std::vector<std::vector<double> > cov_matrix, std::vector<double> mu){
  /**
   * This is the function that initializes the Multivariate normal distribution given:
   * - cov_matrix: covariance matrix
   * - mu: the mean value vector
   */
  unsigned int rows, columns;
  rows = cov_matrix.size();
  columns = cov_matrix.at(0).size();

  basicMultivariateNormalInit(rows,columns,cov_matrix, mu);
  //_mu = mu;
  //_cov_matrix = cov_matrix;

  //computeInverse(_cov_matrix, _inverse_cov_matrix);

  //_determinant_cov_matrix = getDeterminant(_cov_matrix);
}

// Input Parameters: vectors of covariance and mu
BasicMultivariateNormal::BasicMultivariateNormal(std::vector<double> vec_cov_matrix, std::vector<double> mu){
  /**
   * This is the function that initializes the Multivariate normal distribution given:
   * Input Parameters
   * - vec_cov_matrix: covariance matrix stored in a vector<double>
   * - mu: the mean value vector
   */

  unsigned int rows, columns;
  std::vector<std::vector<double> > cov_matrix;
  // convert the vec_cov_matrix to cov_matrix, output the rows and columns of the covariance matrix
  vectorToMatrix(rows,columns,vec_cov_matrix,cov_matrix);

  basicMultivariateNormalInit(rows,columns,cov_matrix, mu);
}

BasicMultivariateNormal::BasicMultivariateNormal(std::vector<double> vec_cov_matrix, std::vector<double> mu, const char* type, int rank){
  /**
   * This is the function that initializes the Multivariate normal distribution given:
   * First, we will make sure the given covariance, i.e. vec_cov_matrix, is symmetric, function 'computeNearestSymmetricMatrix' will be called
   * Second, we will compute the svd of the computed symmetric matrix
   * Third, we will make sure the reconstructed covariance matrix will be symmetric positive semidefinite matrix, function resetSingularValues will be called
   * Reference for compute the nearest symmetric positive semidefinte matrix:
   * 1. Nicholas J. Higham, "Computing a Nearest Symmetric Positive Semidefinite Matrix," Linear Algebra and Its Applications, vol. 103, pp. 103-118 (1988)
   * 2. Risto Vanhanen, "Computing Positive Semidefinite Multigroup Nuclear Data Covariances," Nuclear Science and Engineering, vol. 179, pp. 411-422 (2015)
   * Input Parameters
   * - vec_cov_matrix: covariance matrix stored in a vector<double>
   * - mu: the mean value vector
   * - rank: the reduced dimension
   * - type: the type of given covariance matrix (vec_cov_matrix), it can be 'abs' or 'rel', which means absolute covariance matrix or relative convariance matrix respectively.
   */
  unsigned int rows, columns;
  std::vector<std::vector<double> > cov_matrix;
  std::vector<std::vector<double> > symmetricCovMatrix;
  // convert the vec_cov_matrix to cov_matrix, output the rows and columns of the covariance matrix
  vectorToMatrix(rows,columns,vec_cov_matrix,cov_matrix);
  // compute the nearest symmetric covariance matrix
  computeNearestSymmetricMatrix(cov_matrix,symmetricCovMatrix);
  _mu = mu;
  _cov_matrix = symmetricCovMatrix;
  _rank = (unsigned int) rank;
  _covariance_type = std::string(type);
  if(_rank > _mu.size()) {
    throwError("The  provided rank  is larger than the given problem's dimension, it should be less or equal!" );
  }
  if (_rank == _mu.size()) {
    std::vector<std::vector<double> > inverseCovMatrix (rows,std::vector< double >(columns));
    computeInverse(_cov_matrix, inverseCovMatrix);
    for (unsigned int i=0;i<rows;i++){
      std::vector<double> temp;
      for (unsigned int j=0;j<columns;j++)
      temp.push_back(inverseCovMatrix.at(i).at(j));
      _inverse_cov_matrix.push_back(temp);
    }
    _determinant_cov_matrix = getDeterminant(_cov_matrix);
  }

  //compute the svd
  computeSVD(_rank);
  //setup the nearest symmetric semi-positive definite covariance matrix
  resetSingularValues(_left_singular_vectors, _right_singular_vectors, _singular_values,_svd_transformed_matrix);

  unsigned int dimensions = _mu.size();
  // for now we use this to be a bit more less problem dependent
  double floatDiscretizations = (1./std::pow(1.e-4,1./ (double)dimensions)) + 0.5;
  int numberOfDiscretizations = (int)floatDiscretizations;

  for(unsigned int i=0; i<dimensions; i++){
    std::vector<double> discretization_temp;
    double sigma = sqrt(_cov_matrix[i][i]);
    double deltaSigma = 12.0*sigma/(double)numberOfDiscretizations;
    for(int n=0; n<numberOfDiscretizations; n++){
      double disc_value = mu.at(i) - 6.0 * sigma + deltaSigma * (double)n;
      discretization_temp.push_back(disc_value);
    }
    _lower_bounds.push_back(discretization_temp.at(0));
    _upper_bounds.push_back(discretization_temp.back());
  }

}

void BasicMultivariateNormal::computeSVD() {
  /**
   * This function will compute the svd for the covariance matrix stored in _cov_matrix
   * and store the left singular vectors in _left_singular_vectors, right singular vectors in _right_singular_vectors
   * singular values in _singular_values, and the transform matrix in _svd_transformed_matrix
   * The transform matrix is defined as: _left_singular_vectors*sqrt(diag(_singular_values))
   * @ In, None
   * @ Out, None
   */
  svdDecomposition(_cov_matrix,_left_singular_vectors,_right_singular_vectors,_singular_values,_svd_transformed_matrix);
}

void BasicMultivariateNormal::computeSVD(int rank) {
  /**
   * This function will compute the truncated svd for the covariance matrix stored in _cov_matrix
   * and store the left singular vectors in _left_singular_vectors, right singular vectors in _right_singular_vectors
   * singular values in _singular_values, and the transform matrix in _svd_transformed_matrix
   * The transform matrix is defined as: _left_singular_vectors*sqrt(diag(_singular_values))
   * @ In, rank, int, the number of singular values that will be kept for the truncated svd
   * @ Out, None
   */
  svdDecomposition(_cov_matrix,_left_singular_vectors,_right_singular_vectors,_singular_values,_svd_transformed_matrix, rank);
}

std::vector<double> BasicMultivariateNormal::getTransformationMatrix() {
  /**
   * this function returns the transformation matrix
   * @ In, None
   * @ Out, returnVectors,std::vector<double>, the vector stores the left singular vectors
   */
  std::vector<double> returnVectors;
  for(unsigned int i = 0; i < _svd_transformed_matrix.size(); ++i) {
    for(unsigned int j = 0; j < _svd_transformed_matrix.at(0).size(); ++j) {
      returnVectors.push_back(_svd_transformed_matrix.at(i).at(j));
    }
  }
  return returnVectors;
}

std::vector<double> BasicMultivariateNormal::getTransformationMatrix(std::vector<int> index) {
  /**
   * this function returns the transformation matrix
   * @ In, index, std::vector<int>, the index of transformation matrix
   * @ Out, returnVectors,std::vector<double>, the vector stores the left singular vectors associated with the provided index
   */
  std::vector<double> returnVectors;
  for(unsigned int i = 0; i < _svd_transformed_matrix.size(); ++i) {
    for(unsigned int j = 0; j < index.size(); ++j) {
      if (index.at(j) < 0) {
        throwError("Negative value is not allowed in the provided column index vector");
      }
      returnVectors.push_back(_svd_transformed_matrix.at(i).at(index.at(j)));
    }
  }
  return returnVectors;
}

std::vector<int> BasicMultivariateNormal::getTransformationMatrixDimensions() {
  /**
   * return the row and colum of the transformation matrix stored in returnVector.at(0) and returnVector.at(1) respectively
   * @ In, None
   * @ Out, returnVector, std::vector<int>, row stored in returnVector.at(0), and column stored in returnVector.at(1)
   */
  std::vector<int> returnVector;
  returnVector.push_back(_svd_transformed_matrix.size());
  returnVector.push_back(_svd_transformed_matrix.at(0).size());
  return returnVector;
}

std::vector<int> BasicMultivariateNormal::getTransformationMatrixDimensions(std::vector<int> index) {
  /**
   * return the row and colum of the transformation matrix
   * @ In, index, std::vector<int>, the index of transformation matrix
   * @ Out,returnVector, std::vector<int>, row stored in returnVector.at(0), and column stored in returnVector.at(1).
   */
  std::vector<int> returnVector;
  returnVector.push_back(_svd_transformed_matrix.size());
  returnVector.push_back(index.size());
  return returnVector;
}

std::vector<double> BasicMultivariateNormal::getInverseTransformationMatrix() {
  /**
   * this function returns the inverse transformation matrix
   * @ In, None
   * @ Out, returnVectors,std::vector<double>, the vector stores the inverse transformation matrix
   */
  std::vector<std::vector<double> > inverse_transformed_matrix;
  getInverseTransformedMatrix(_left_singular_vectors,_singular_values,inverse_transformed_matrix);
  std::vector<double> returnVectors;
  for(unsigned int i = 0; i < inverse_transformed_matrix.size(); ++i) {
    for(unsigned int j = 0; j < inverse_transformed_matrix.at(0).size(); ++j) {
      returnVectors.push_back(inverse_transformed_matrix.at(i).at(j));
    }
  }
  return returnVectors;
}

std::vector<double> BasicMultivariateNormal::getInverseTransformationMatrix(std::vector<int> index) {
  /**
   * this function returns the transformation matrix
   * @ In, index, std::vector<int>, the index of inverse transformation matrix
   * @ Out, returnVectors,std::vector<double>, the vector stores the inverse transformation matrix associated with the provided index
   */
  std::vector<std::vector<double> > inverse_transformed_matrix;
  getInverseTransformedMatrix(_left_singular_vectors,_singular_values,inverse_transformed_matrix);
  std::vector<double> returnVectors;
  for(unsigned int i = 0; i < inverse_transformed_matrix.size(); ++i) {
    for(unsigned int j = 0; j < index.size(); ++j) {
      if (index.at(j) < 0) {
        throwError("Negative value is not allowed in the provided column index vector");
      }
      returnVectors.push_back(inverse_transformed_matrix.at(i).at(index.at(j)));
    }
  }
  return returnVectors;
}

std::vector<int> BasicMultivariateNormal::getInverseTransformationMatrixDimensions() {
  /**
   * return the row and colum of the inverse transformation matrix stored in returnVector.at(0) and returnVector.at(1) respectively
   * @ In, None
   * @ Out, returnVector, std::vector<int>, row stored in returnVector.at(0), and column stored in returnVector.at(1)
   */
  std::vector<std::vector<double> > inverse_transformed_matrix;
  getInverseTransformedMatrix(_left_singular_vectors,_singular_values,inverse_transformed_matrix);
  std::vector<int> returnVector;
  returnVector.push_back(inverse_transformed_matrix.size());
  returnVector.push_back(inverse_transformed_matrix.at(0).size());
  return returnVector;
}

std::vector<int> BasicMultivariateNormal::getInverseTransformationMatrixDimensions(std::vector<int> index) {
  /**
   * return the row and colum of the transformation matrix
   * @ In, index, std::vector<int>, the index of inverse transformation matrix
   * @ Out,returnVector, std::vector<int>, row stored in returnVector.at(0), and column stored in returnVector.at(1).
   */
  std::vector<std::vector<double> > inverse_transformed_matrix;
  getInverseTransformedMatrix(_left_singular_vectors,_singular_values,inverse_transformed_matrix);
  std::vector<int> returnVector;
  returnVector.push_back(inverse_transformed_matrix.size());
  returnVector.push_back(index.size());
  return returnVector;
}

std::vector<double> BasicMultivariateNormal::getLeftSingularVectors() {
  /**
   * this function returns the left singular vectors
   * @ In, None
   * @ Out, returnVectors, std::vector<double>, the vector stores the left singular vectors
   */
  std::vector<double> returnVectors;
  for(unsigned int i = 0; i < _left_singular_vectors.size(); ++i) {
    for(unsigned int j = 0; j < _left_singular_vectors.at(0).size(); ++j) {
      returnVectors.push_back(_left_singular_vectors.at(i).at(j));
    }
  }
  return returnVectors;
}

std::vector<double> BasicMultivariateNormal::getLeftSingularVectors(std::vector<int> index) {
  /**
   * this function returns the left singular vectors associated with index
   * @ In, index, std::vector<int>, the index of left singular vectors
   * @ Out, returnVectors, std::vector<double> the vector stores the left singular vectors associated with index
   */
  std::vector<double> returnVectors;
  for(unsigned int i = 0; i < _left_singular_vectors.size(); ++i) {
    for(unsigned int j = 0; j < index.size(); ++j) {
      if (index.at(j) < 0) {
        throwError("Negative value is not allowed in the provided column index vector");
      }
      returnVectors.push_back(_left_singular_vectors.at(i).at(index.at(j)));
    }
  }
  return returnVectors;
}

std::vector<double> BasicMultivariateNormal::getRightSingularVectors() {
  /**
   * this function returns the right singular vectors
   * @ In, None
   * @ Out, returnVectors, std::vector<double>, the vector stores the right singular vectors
   */
  std::vector<double> returnVectors;
  for(unsigned int i = 0; i < _right_singular_vectors.size(); ++i) {
    for(unsigned int j = 0; j < _right_singular_vectors.at(0).size(); ++j) {
      returnVectors.push_back(_right_singular_vectors.at(i).at(j));
    }
  }
  return returnVectors;
}

std::vector<double> BasicMultivariateNormal::getRightSingularVectors(std::vector<int> index) {
  /**
   * this function returns the right singular vectors associated with the provided index
   * @ In, index, std::vector<int> the index of left singular vectors
   * @ Out, returnVectors, std::vector<double>, the vector stores the right singular vectors
   */
  std::vector<double> returnVectors;
  for(unsigned int i = 0; i < _right_singular_vectors.size(); ++i) {
    for(unsigned int j = 0; j < index.size(); ++j) {
      if (index.at(j)< 0) {
        throwError("Negative value is not allowed in the provided column index vector");
      }
      returnVectors.push_back(_right_singular_vectors.at(i).at(index.at(j)));
    }
  }
  return returnVectors;
}

std::vector<double> BasicMultivariateNormal::getSingularValues() {
  /**
   * this function returns the singular values
   * @ In, None
   * @ Out, _singular_values, std::vector<double> the vector stores the singular values
   */
  return _singular_values;
}

std::vector<double> BasicMultivariateNormal::getSingularValues(std::vector<int> index) {
  /**
   * this function returns the singular values associated with the provided index
   * @ In, index, std::vector<int>, the  index of left singular vectors
   * @ Out, returnVector, std::vector<double>, the vector stores the singular values associated with the provided inde
   */
  std::vector<double> returnVector;
  for(unsigned int i = 0; i < index.size(); ++i) {
    if (index.at(i) < 0) {
      throwError("Negative value is not allowed in the provided index vector");
    }
    returnVector.push_back(_singular_values.at(index.at(i)));
  }
  return returnVector;
}

std::vector<int> BasicMultivariateNormal::getLeftSingularVectorsDimensions() {
  /**
   * return the row and column of left singular vectors stored in returnVector.at(0) and returnVector.at(1) respectively
   * @ In, None
   * @ Out, returnVector, std::vector<int>, row stored in returnVector.at(0), and column stored in returnVector.at(1)
   */
  std::vector<int> returnVector;
  returnVector.push_back(_left_singular_vectors.size());
  returnVector.push_back(_left_singular_vectors.at(0).size());
  return returnVector;
}

std::vector<int> BasicMultivariateNormal::getLeftSingularVectorsDimensions(std::vector<int> index) {
  /**
   * return the row and column of left singular vectors stored in returnVector.at(0) and returnVector.at(1) respectively
   * @ In, index, std::vector<int>, the index of left singular vectors
   * @ Out, returnVector, std::vector<int>, return the row and column of left singular vectors with provided index  stored in returnVector.at(0) and returnVector.at(1) respectively
   */
  std::vector<int> returnVector;
  returnVector.push_back(_left_singular_vectors.size());
  returnVector.push_back(index.size());
  return returnVector;
}

std::vector<int> BasicMultivariateNormal::getRightSingularVectorsDimensions() {
  /**
   * return the row and column of right singular vectors stored in returnVector.at(0) and returnVector.at(1) respectively
   * @ In, None
   * @ Out, returnVector, std::vector<int>, row stored in returnVector.at(0), and column stored in returnVector.at(1)
   */
  std::vector<int> returnVector;
  returnVector.push_back(_right_singular_vectors.size());
  returnVector.push_back(_right_singular_vectors.at(0).size());
  return returnVector;
}

std::vector<int> BasicMultivariateNormal::getRightSingularVectorsDimensions(std::vector<int> index) {
  /**
   * return the row and column of right singular vectors stored in returnVector.at(0) and returnVector.at(1) respectively
   * @ In, index, std::vector<int>, the index of right singular vectors
   * @ Out, returnVector, std::vector<int>, return the row and column of right singular vectors with provided index  stored in returnVector.at(0) and returnVector.at(1) respectively
   */
  std::vector<int> returnVector;
  returnVector.push_back(_right_singular_vectors.size());
  returnVector.push_back(index.size());
  return returnVector;
}

int  BasicMultivariateNormal::getSingularValuesDimension() {
  /**
   * return the dimension of  singular value vector stored
   * @ In, None
   * @ Out, _singular_values.size(), int, the size of vector _singular_values
   */
  return _singular_values.size();
}

int  BasicMultivariateNormal::getSingularValuesDimension(std::vector<int> index) {
  /**
   * return the dimension of  singular value vector with provided index set.
   * @ In, index, std::vector<int>, the index of singular values
   * @ Out, index.size(), int, return the size of singular value vector with provided index set
   */
  return index.size();
}

std::vector<double> BasicMultivariateNormal::coordinateInTransformedSpace(int rank) {
  /**
   * This function will return the coordinate in the transformed space
   * This function will generate the coordinate for r (r=rank) random variables, each of them
   * are drew from single normal distribution. We need a random number between 0 and 1 to drew
   * the random variable. In addition, thi function will be used in the input dimensionality reduction
   * application. We will transform the correlated variables into uncorrelated variables, and using this
   * function to draw the samples for the uncorrelated variables, and later transform the samples to correlated
   * variables.
   * @ In, rank, int, the effective dimension of the transformed space
   * @ Out, coordinate, std::vector<double>, the coordinate in the transformed space
   */
  //std::cout << "BasicMultivariateNormal::coordinateInTransformedSpace" << std::endl;
  std::vector<double> coordinate;
  BasicNormalDistribution * normalDistribution = new BasicNormalDistribution(0,1);
  DistributionContainer *distributionInstance = & DistributionContainer::instance();
  double randValue = 0.0;
  for(int i = 0; i < rank; ++i) {
    randValue = distributionInstance->random();
    double coordinateValue = normalDistribution->inverseCdf(randValue);
    coordinate.push_back(coordinateValue);
  }
  delete normalDistribution;
  return coordinate;
}

std::vector<double> BasicMultivariateNormal::coordinateInverseTransformed(std::vector<double> & coordinate) {
  /**
   * This function will transform the coordinate back to the original space
   * and the transformation are computed using computeSVD.
   * @ In, coordinate, std::vector<double>, the coordinate in the transformed space
   * @ Out, originalCoordinate, std::vector<double>, the coordinate in the full space
   */
  //std::cout << "BasicMultivariateNormal::coordinateInverseTransformed" << std::endl;
  std::vector<double> originalCoordinate;
  for(unsigned int irow = 0; irow < _svd_transformed_matrix.size(); ++irow) {
    double tempSum = 0.0;
    for(unsigned int icol = 0; icol < _svd_transformed_matrix.at(0).size(); ++icol) {
      tempSum = tempSum + _svd_transformed_matrix.at(irow).at(icol) * coordinate.at(icol);
    }
    originalCoordinate.push_back(tempSum);
  }
  if(_covariance_type == "abs") {
    for(unsigned int idim = 0; idim < originalCoordinate.size(); ++idim) {
      originalCoordinate.at(idim) += _mu.at(idim);
    }
  } else if (_covariance_type == "rel") {
    for(unsigned int idim = 0; idim < originalCoordinate.size(); ++idim) {
      originalCoordinate.at(idim) = _mu.at(idim)*(1.0 + originalCoordinate.at(idim));
    }
  } else {
    throwError("MultivariateNormal Error: covariance type is not available");
  }
  return originalCoordinate;
}

std::vector<double> BasicMultivariateNormal::coordinateInverseTransformed(std::vector<double> & coordinate,std::vector<int> index) {
  /**
   * This function will transform the coordinate back to the original space
   * @ In, index, std::vector<int>, the index set associated with the provied coordinate
   * @ In, coordinate, std::vector<double>, the coordinate in the transformed space
   * @ Out, originalCoordinate, std::vector<double>, and the coordinate in the full space.
   */
  std::vector<double> originalCoordinate;
  for(unsigned int irow = 0; irow < _svd_transformed_matrix.size(); ++irow) {
    double tempSum = 0.0;
    for(unsigned int icol = 0; icol < index.size(); ++icol) {
      if (index[icol] < 0) {
        throwError("Negative value is not allowed for the index set.");
      }
      tempSum = tempSum + _svd_transformed_matrix.at(irow).at(index.at(icol)) * coordinate.at(icol);
    }
    originalCoordinate.push_back(tempSum);
  }
  if(_covariance_type == "abs") {
    for(unsigned int idim = 0; idim < originalCoordinate.size(); ++idim) {
      originalCoordinate.at(idim) += _mu.at(idim);
    }
  } else if (_covariance_type == "rel") {
    for(unsigned int idim = 0; idim < originalCoordinate.size(); ++idim) {
      originalCoordinate.at(idim) = _mu.at(idim)*(1.0 + originalCoordinate.at(idim));
    }
  } else {
    throwError("MultivariateNormal Error: covariance type is not available");
  }
  return originalCoordinate;
}

double BasicMultivariateNormal::cellProbabilityWeight(std::vector<double> center, std::vector<double> dx){
    /**
     * This function calculates the integral of the pdf in a cell region
     * In the 1D case a cell region is an interval [a,b], thus the integral of the pdf in such interval is
     * calculated as CDF(b)-CDF(a). This functions perform a similar evolution but for a generic ND cell
     * This function assumes all the input variables are uncorrelated, and follows univariate normal distribution N(0,1)
     * @ In, center, std::vector<double>, a vector to store the grid coordinate, for ND grid sampler, center represents the coordinate of given grid point
     * @ In, dx, std::vector<double>,  a vector to store the distance between given grid coordinate and its connected points, for ND grid sampler, dx represents the distance between grid_coordinate_plus_one - grid_coordinate_minus_one, where grid_coordinate_plus_one and grid_coordinate_minus_one are the shift of "center"
     * @ Out, value, double, the probability weight for the cell
     */

  double value = 1.0;
  double upperBound = 0.0;
  double lowerBound = 0.0;
  double cdfValue = 0.0;
  BasicNormalDistribution * normalDistribution = new BasicNormalDistribution(0,1);
  for (unsigned int i = 0; i < center.size(); ++i) {
    upperBound = center.at(i) + dx.at(i)/2.0;
    lowerBound = center.at(i) - dx.at(i)/2.0;
    cdfValue = normalDistribution->cdf(upperBound) - normalDistribution->cdf(lowerBound);
    value *= cdfValue;
  }
  return value;
}

double BasicMultivariateNormal::inverseMarginalForPCA(double f){
    /**
     * This function calculates the inverse marginal distribution at f of a MVN distribution when using pca decomposition
     * @ In, f, double, the value picked in the marginal cdf distribution
     * @ Out, normalDistribution->inverseCdf(f), double, the variable value corresponding to the marginal cdf distribution at f
     */
  BasicNormalDistribution * normalDistribution = new BasicNormalDistribution(0,1);
  return normalDistribution->inverseCdf(f);
}

double BasicMultivariateNormal::marginalCdfForPCA(double x){
    /**
     * This function calculates the marginal cdf at x of a MVN distribution when using pca decomposition
     * If PCA method is used, the marginal cdf is assumed to be standard normal distribution, i.e. mean = 0.0 , and sigma = 1.0
     * @ In, x, double, the variable value
     * @ Out, normalDistribution->cdf(x), double, the marginal cdf value at x
     */
  BasicNormalDistribution * normalDistribution = new BasicNormalDistribution(0,1);
  return normalDistribution->cdf(x);
}

double BasicMultivariateNormal::getPdf(std::vector<double> x, std::vector<double> mu, std::vector<std::vector<double> > inverse_cov_matrix){
  /**
   * This function calculates the pdf values at x of a MVN distribution
   */

  double value = 0;

   if(mu.size() == x.size()){
     int dimensions = mu.size();
     double expTerm=0;
     std::vector<double> tempVector (dimensions);
     for(int i=0; i<dimensions; i++){
       tempVector[i]=0;
       for(int j=0; j<dimensions; j++)
         tempVector[i] += inverse_cov_matrix[i][j]*(x[j]-mu[j]);
       expTerm += tempVector[i]*(x[i]-mu[i]);
     }
     value = 1/sqrt(_determinant_cov_matrix*pow(2*M_PI,dimensions))*exp(-0.5*expTerm);
   }else
     throwError("MultivariateNormal PDF error: evaluation point dimensionality is not correct");
   return value;
}

double BasicMultivariateNormal::pdfInTransformedSpace(std::vector<double> x){
  /**
   * This function calculates the pdf values at x in the PCA transformed space
   * @ In, x, double, the coordinate in the transformed space
   * @ Out, value, double, the pdf value in the transformed space
   */
  double value = 1.0;
  BasicNormalDistribution * normalDistribution = new BasicNormalDistribution(0,1);
  for (unsigned int i = 0; i < x.size(); ++i) {
    value *=  normalDistribution->pdf(x.at(i));
  }
  delete normalDistribution;
  return value;
}

double BasicMultivariateNormal::pdf(std::vector<double> x){
    /**
     * This function calculates the pdf values at x of a MVN distribution
     */
  return getPdf(x, _mu, _inverse_cov_matrix);
}

double BasicMultivariateNormal::cdf(std::vector<double> x){
    /**
     * This function calculates the cdf values at x of a MVN distribution
     */
  return _cartesian_distribution.cdf(x);
}

std::vector<double> BasicMultivariateNormal::inverseCdf(double f, double g){
    /**
     * This function calculates the inverse CDF values at f of a MVN distribution
     */
  return _cartesian_distribution.inverseCdf(f,g);
}

double BasicMultivariateNormal::inverseMarginal(double f, int dimension){
    /**
     * This function calculates the inverse marginal distribution at f for a specific dimension of a MVN distribution
     */
  return _cartesian_distribution.inverseMarginal(f,dimension);
}

int BasicMultivariateNormal::returnDimensionality(){
    /**
     * This function returns the dimensionality of a MVN distribution
     */
  return _mu.size();
}

void BasicMultivariateNormal::updateRNGparameter(double tolerance, double initial_divisions){
    /**
     * This function updates the random number generator parameters of a MVN distribution
     */
  return _cartesian_distribution.updateRNGparameter(tolerance,initial_divisions);
}

double BasicMultivariateNormal::marginal(double x, int dimension){
    /**
     * This function calculates the marginal distribution at x for a specific dimension of a MVN distribution
     */
  return _cartesian_distribution.marginal(x,dimension);
}

//double BasicMultivariateNormal::cdf_(std::vector<double> x){
//// if(_mu.size() == x.size()){
////  int dimensions = _mu.size();
////  //boost::math::chi_squared chiDistribution(dimensions);
////
////  double mahalanobis=0.0;
////  std::vector<double> tempVector (dimensions);
////  for(int i=0; i<dimensions; i++)
////   tempVector[i]=0.0;
////
////  for(int i=0; i<dimensions; i++){
////   tempVector[i]=0.0;
////   for(int j=0; j<dimensions; j++)
////    tempVector[i] += _inverseCovMatrix[i][j]*(x[j]-_mu[j]);
////   mahalanobis += tempVector[i]*(x[i]-_mu[i]);
////  }
////  value = boost::math::gamma_p<double,double>(dimensions/2,mahalanobis/2);
//// }else
////  throwError("MultivariateNormal CDF error: evaluation point dimensionality is not correct");
//
// double alpha = 2.5;
// int Nmax = 50;
// double epsilon= 0.01;
// double delta;
//
// int dimensions = _cov_matrix.size();
// double Intsum=0;
// double Varsum=0;
// int N = 0;
// double error=10*epsilon;
// std::vector<double> d (dimensions);
// std::vector<double> e (dimensions);
// std::vector<double> f (dimensions);
//
// d[0] = 0.0;
// e[0] = phi(x[0]/_cholesky_C[0][0]);
// f[0] = e[0] - d[0];
//
// boost::random::mt19937 rng;
// rng.seed(time(NULL));
// double range = rng.max() - rng.min();
//
// while (error>epsilon or N<Nmax){
//  std::vector<double> w (dimensions-1);
//
//  for (int i=0; i<(dimensions-1); i++){
//   w.at(i) = (rng()-rng.min())/range;
//   //std::cout<< "value: " << w.at(i) << std::endl;
//  }
//
//  std::vector<double> y (dimensions-1);
//
//  for (int i=1; i<dimensions; i++){
//   double tempY = d.at(i-1) + w.at(i-1) * (e.at(i-1)-d.at(i-1));
//
//   y.at(i-1) = phiInv(tempY);
//
//   double tempE = x.at(i);
//
//   for (int j=0; j<(i-1); j++)
//    tempE = tempE - _cholesky_C[i][j] * y.at(j) / _cholesky_C[i][i];
//
//   e.at(i)=phi(tempE);
//   d.at(i)=0.0;
//   f.at(i)=(e.at(i)-d.at(i))*f.at(i-1);
//  }
//
//  N++;
//  delta = (f.at(dimensions-1)-Intsum)/double(N);
//  Intsum = Intsum + delta;
//  Varsum = (double(N-2))*Varsum/double(N) + delta*delta;
//  error = alpha * sqrt(Varsum);
//
//  std::cout << "N " << N << " ; f: " << f.at(dimensions-1) << " ; delta: " << delta << " ; Intsum: " << Intsum << " ; Varsum: " << Varsum << "; error: " << error << std::endl;
// }
//
// return Intsum;
//}

BasicMultivariateNormal::~BasicMultivariateNormal(){

}

double BasicMultivariateNormal::phi(double x){
 double value = 0.5 * (1.0 + boost::math::erf<double>(x/sqrt(2.0)));
 //double value = 0.5 * (boost::math::erf<double>(x/sqrt(2)));
 return value;
}

double BasicMultivariateNormal::phiInv(double x){
 normal s;
 double value = quantile(s,x);
 return value;
}

//double BasicMultivariateNormal::rn(){
//    boost::random::mt19937 rng;
// rng.seed(time(NULL));
// double range = rng.max() - rng.min();
// double value = (rng()-rng.min())/range;
// std::cout<< "value: " << value << std::endl;
// return value;
//}

//double BasicMultivariateNormal::MVNDST(std::vector<double> a, std::vector<double> b, double alpha, double epsilon, int Nmax){
// int dimensions = _cov_matrix.size();
// double Intsum=0;
// double Varsum=0;
// int N = 0;
// double error;
// std::vector<double> d (dimensions);
// std::vector<double> e (dimensions);
// std::vector<double> f (dimensions);
//
// std::vector<std::vector<double> > cholesky_C = choleskyDecomposition(_cov_matrix);
//
// d[0] = phi(a[0]/cholesky_C[0][0]);
// e[0] = phi(b[0]/cholesky_C[0][0]);
// f[0] = e[0] - d[0];
//
//    boost::random::mt19937 rng;
// rng.seed(time(NULL));
// double range = rng.max() - rng.min();
//
// do{
//  std::vector<double> w (dimensions-1);
//  for (int i=0; i<(dimensions-1); i++){
//   w.at(i) = (rng()-rng.min())/range;
//   std::cout<< "value: " << rng() << std::endl;
//  }
//
//  std::vector<double> y (dimensions-1);
//  for (int i=1; i<dimensions; i++){
//   double tempY = d.at(i-1) + w.at(i-1)*(e.at(i-1)-d.at(i-1));
//   y.at(i-1) = phiInv(tempY);
//
//   double tempD = a.at(i);
//   double tempE = b.at(i);
//
//   for (int j=0; j<(i-1); j++){
//    tempD = tempD - cholesky_C[i][j] * y[j] / cholesky_C[i][i];
//    tempE = tempE - cholesky_C[i][j] * y[j] / cholesky_C[i][i];
//   }
//
//   d[i]=phi(tempD);
//   e[i]=phi(tempE);
//   f[i]=(e[i]-d[i])/f[i-1];
//  }
//
//  N++;
//  double delta = (f[dimensions-1]-Intsum)/N;
//  Intsum = Intsum + delta;
//  Varsum = (N-2)*Varsum/N + delta*delta;
//  error = alpha * sqrt(Varsum);
//
// } while (error<epsilon and N<Nmax);
//
// return Intsum;
//}

//http://rosettacode.org/wiki/Cholesky_decomposition#C
double *BasicMultivariateNormal::cholesky(double *A, int n) {
    double *L = (double*)calloc(n * n, sizeof(double));
    if (L == NULL)
  exit(EXIT_FAILURE);

    for (int i = 0; i < n; i++)
  for (int j = 0; j < (i+1); j++) {
      double s = 0;
      for (int k = 0; k < j; k++)
    s += L[i * n + k] * L[j * n + k];
      L[i * n + j] = (i == j) ?
         sqrt(A[i * n + i] - s) :
         (1.0 / L[j * n + j] * (A[i * n + j] - s));
  }

    return L;
}

std::vector<std::vector<double> > BasicMultivariateNormal::choleskyDecomposition(std::vector<std::vector<double> > matrix){
 std::vector<std::vector<double> > cholesky_C;

 int dimensions = matrix.size();
 double * m1 = new double[dimensions*dimensions];

 for (int r=0; r<dimensions; r++)
  for (int c=0; c<dimensions; c++)
   m1[r*dimensions+c] = matrix[r][c];

 double *c1 = cholesky(m1, dimensions);
 //std::cout << "choleskyDecomposition" << std::endl;
 //showMatrix(c1,dimensions);

 for (int r=0; r<dimensions; r++){
  std::vector<double> temp;
  for (int c=0; c<dimensions; c++)
   temp.push_back(c1[r*dimensions+c]);
  cholesky_C.push_back(temp);
 }
 delete m1;
 return cholesky_C;
}

void BasicMultivariateNormal::showMatrix(double *A, int n) {
    for (int i = 0; i < n; i++) {
  for (int j = 0; j < n; j++)
      printf("%2.5f ", A[i * n + j]);
  printf("\n");
    }
}

//template<class T>
//bool InvertMatrix(const matrix<T>& input, matrix<T>& inverse)
//{
// typedef permutation_matrix<std::size_t> pmatrix;
//
// // create a working copy of the input
// matrix<T> A(input);
//
// // create a permutation matrix for the LU-factorization
// pmatrix pm(A.size1());
//
// // perform LU-factorization
// int res = lu_factorize(A, pm);
// if (res != 0)
//  return false;
//
// // create identity matrix of "inverse"
// inverse.assign(identity_matrix<T> (A.size1()));
//
// // backsubstitute to get the inverse
// lu_substitute(A, pm, inverse);
//
// return true;
//}
//
//void getInverse(std::vector<std::vector<double> > matrix, std::vector<std::vector<double> > inverse_matrix){
// int dimension = matrix.size();
// double initialValues[dimension][dimension];
// matrix<double> A(dimension, dimension), Z(dimension, dimension);
//
// for(int i=0; i<dimension; i++)
//  for(int j=0; j<dimension; j++)
//   initialValues[i][j]=matrix[i][j];
// A = make_matrix_from_pointer(initialValues);
// InvertMatrix(A, Z);
//
// for(int i=0; i<dimension; i++){
//  std::vector<double> temp;
//  for(int j=0; j<dimension; j++)
//   temp.push_back(Z[i][j]);
//  inverse_matrix.push_back(temp);
// }
//}


#include <Eigen/Dense>
#include "cxxopts.hpp"
#include "simpleicp.h"

Eigen::MatrixXd ImportXYZFileToMatrix(const std::string &path_to_pc);

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

    cxxopts::Options options("simpleicp", "A simple version of the ICP algorithm.");

    // clang-format off
    options.add_options()
      ("f,fixed", "Path to fixed point cloud",
        cxxopts::value<std::string>())
      ("m,movable", "Path to movable point cloud",
        cxxopts::value<std::string>())
      ("c,correspondences", "Number of initially selected correspondences",
        cxxopts::value<int>()->default_value("1000"))
      ("n,neighbors", "Number of neighbors used for plane estimation",
        cxxopts::value<int>()->default_value("10"))
      ("p,min_planarity", "Minimal planarity value of planes used as correspondence",
        cxxopts::value<double>()->default_value("0.3"))
      ("o,max_overlap_distance", "Maximum initial overlap distance. Set to negative value if point "
      "clouds are fully overlapping.",
        cxxopts::value<double>()->default_value("-1"))
      ("i,min_change", "Minimal change of mean and standard deviation of distances (in percent) "
                       "needed to proceed to next iteration",
        cxxopts::value<double>()->default_value("1"))
      ("x,max_iterations", "Maximum number of iterations",
        cxxopts::value<int>()->default_value("100"))
      ("h,help", "Print usage")
      ;
    // clang-format on

    auto result = options.parse(argc, argv);

    if (result.count("help") || argc == 1)
    {
      std::cout << options.help() << std::endl;
      exit(0);
    }

    auto X_fix = ImportXYZFileToMatrix(std::string(result["fixed"].as<std::string>()));
    auto X_mov = ImportXYZFileToMatrix(std::string(result["movable"].as<std::string>()));

    Eigen::Matrix<double, 4, 4> H = SimpleICP(X_fix,
                                              X_mov,
                                              result["correspondences"].as<int>(),
                                              result["neighbors"].as<int>(),
                                              result["min_planarity"].as<double>(),
                                              result["max_overlap_distance"].as<double>(),
                                              result["min_change"].as<double>(),
                                              result["max_iterations"].as<int>());
  }
  catch (const std::exception &e)
  {
    std::cerr << "Caught exception: " << e.what() << std::endl;
    return 1;
  }
  catch (...)
  {
    std::cerr << "Caught unknown exception." << std::endl;
    return 1;
  }

  return 0;
}

Eigen::MatrixXd ImportXYZFileToMatrix(const std::string &path_to_pc)
{
  std::ifstream data(path_to_pc);
  if (data.is_open())
  {
    // Read data from file
    std::vector<std::vector<std::string>> parsedData;
    std::string line;
    while (getline(data, line))
    {
      std::stringstream lineStream(line);
      std::string cell; // single value
      std::vector<std::string> parsedRow;
      while (getline(lineStream, cell, ' '))
      {
        parsedRow.push_back(cell);
      }
      parsedData.push_back(parsedRow);
    }

    // Check if each line contains exactly 3 values
    for (int i = 0; i < parsedData.size(); i++)
    {
      if (parsedData[i].size() != 3)
      {
        std::cerr << "Line " << i + 1 << " does not contain exactly 3 values!" << std::endl;
        exit(-1);
      }
    }

    // Create eigen array
    Eigen::MatrixXd X(parsedData.size(), 3);
    for (int i = 0; i < parsedData.size(); i++)
    {
      for (int j = 0; j < parsedData[i].size(); j++)
      {
        try
        {
          X(i, j) = stod(parsedData[i][j]);
        }
        catch (std::exception &e)
        {
          std::cerr << "Conversion of " << parsedData[i][j] << " on row/column=" << i << "/" << j
                    << " is not possible!" << std::endl;
          exit(-1);
        }
      }
    }

    return X;
  }
  else
  {
    std::cerr << "Error opening file!" << std::endl;
    exit(-1);
  }
}