//
//  Copyright (c) 2016, Guillaume GODIN
//  All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//     * Neither the name of Institue of Cancer Research.
//       nor the names of its contributors may be used to endorse or promote
//       products derived from this software without specific prior written
//       permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// for build & set RDBASE! => export RDBASE=/Users/GVALMTGG/Github/rdkit_mine/

#include <GraphMol/RDKitBase.h>

#include "WHIM.h"
#include "MolData3Ddescriptors.h"

#include <math.h>
#include <Eigen/Dense>
#include <Eigen/SVD>

using namespace Eigen;

namespace RDKit {
namespace Descriptors {
namespace {

MolData3Ddescriptors moldata3D;

double roundn(double in, int factor) {
  return round(in * pow(10., factor)) / pow(10., factor);
}

MatrixXd GetCenterMatrix(MatrixXd &Mat) {
  VectorXd v = Mat.colwise().mean();
  MatrixXd X = Mat.rowwise() - v.transpose();
  return X;
}

MatrixXd GetCovMatrix(MatrixXd &X, MatrixXd &Weigth, double weigth) {
  return X.transpose() * Weigth * X / weigth;
}

JacobiSVD<MatrixXd> *getSVD(MatrixXd &Mat) {
  JacobiSVD<MatrixXd> *svd =
      new JacobiSVD<MatrixXd>(Mat, ComputeThinU | ComputeThinV);
  return svd;
}

std::vector<double> getWhimD(std::vector<double> weigthvector,
                             MatrixXd MatOrigin, int numAtoms, double th) {
  double *weigtharray = &weigthvector[0];

  Map<VectorXd> Weigth(weigtharray, numAtoms);

  MatrixXd WeigthMat = Weigth.asDiagonal();

  double weigth = WeigthMat.diagonal().sum();

  MatrixXd Xmean = GetCenterMatrix(MatOrigin);

  MatrixXd covmat = GetCovMatrix(Xmean, WeigthMat, weigth);

  JacobiSVD<MatrixXd> *svd = getSVD(covmat);

  std::vector<double> w(18);
  // prepare data for Whim parameter computation

  const double *SingVal = svd->singularValues().data();
  MatrixXd Scores = Xmean * svd->matrixV();  //  V is similar

  // compute parameters
  w[0] = SingVal[0];
  w[1] = SingVal[1];
  w[2] = SingVal[2];
  w[3] = SingVal[0] + SingVal[1] + SingVal[2];  // T
  w[4] = SingVal[0] * SingVal[1] + SingVal[0] * SingVal[2] +
         SingVal[1] * SingVal[2];                              // A
  w[5] = w[3] + w[4] + SingVal[0] * SingVal[1] * SingVal[2];   // V
  w[6] = SingVal[0] / (SingVal[0] + SingVal[1] + SingVal[2]);  // P1
  w[7] = SingVal[1] / (SingVal[0] + SingVal[1] + SingVal[2]);  // p2
  w[8] = SingVal[2] / (SingVal[0] + SingVal[1] + SingVal[2]);  // P3

  double res = 0.0;
  for (int i = 0; i < 3; i++) {
    res += std::abs(w[i] / w[3] - 1.0 / 3.0);
  }

  w[9] = 3.0 / 4.0 * res;  // K

  // center original matrix
  VectorXd v1 = Scores.col(0);
  VectorXd v2 = Scores.col(1);
  VectorXd v3 = Scores.col(2);

  //  inverse of the kurtosis
  if (v1.array().pow(4).sum() > 0) {
    w[10] = numAtoms * pow(w[0], 2) / v1.array().pow(4).sum();  // E1
  } else {
    w[10] = 0.0;
  }

  if (v2.array().pow(4).sum() > 0) {
    w[11] = numAtoms * pow(w[1], 2) / v2.array().pow(4).sum();  // E2
  } else {
    w[11] = 0.0;
  }

  if (v3.array().pow(4).sum() > 0) {
    w[12] = numAtoms * pow(w[2], 2) / v3.array().pow(4).sum();  // E3
  } else {
    w[12] = 0.0;
  }

  w[13] = (w[10] + w[11] + w[12]) / 3.0;  // mean total density of the atoms
                                          // called D is used on Dragon 6 not
                                          // just the sum!

  // check if the molecule is fully symmetrical "like CH4" using Canonical Rank
  // Index and/or Sphericity !

  double gamma[3];  // Gamma values
  double nAT = (double)numAtoms;

  // check if two atoms are symetric versus the new axis ie newx,newy,newz a
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < numAtoms; j++) {
      Scores(j, i) = roundn(Scores(j, i),
                            3);  // round the matrix! same as eigen tolerance !
    }
  }

// we should take into account atoms that are in the axis too!!! which is not trivial
  for (int i = 0; i < 3; i++) {
    std::vector<double> Symetric(2*numAtoms, 0.0);
    double ns = 0.0;
    double na = 0.0;
    for (int j = 0; j < numAtoms; j++) {
      bool amatch = false;
      for (int k = 0; k < numAtoms; k++) {
	      if (j==k) {
          continue;
        }
	      if (std::abs(Scores(j, i) + Scores(k, i)) <= th) {
		      // those that are close opposite & not close to the axis!
	        ns += 1;  // check only once the symetric none null we need to add +2!
		      // (reduce the loop duration)
		      amatch = true;
		      Symetric[j]=1.0;
		      Symetric[j+numAtoms]=2.0;
		      Symetric[k]=1.0;
		      Symetric[k+numAtoms]=2.0;
		      break;
        }
      }
      if (!amatch) {
        na +=1;
        Symetric[j]=0.0;
        Symetric[j+numAtoms]=std::abs(Scores(j, i));
      }
    }
    // take into account the atoms close to the axis
    for (int aj = 0; aj < numAtoms; aj++) {
	    if (Symetric[aj+numAtoms]<th && Symetric[aj]<1.0) {
		    ns +=1;
        na -=1;
      }
    }
    gamma[i] = 0.0;
    double gammainv=1.0;
    if (ns == 0) {
      gammainv = 1.0 - (na / nAT) * log(1.0 / nAT) / log(2.);
    }
    if (ns > 0) {
      gammainv = 1.0 - ((ns / nAT) * log(ns / nAT) / log(2.) +
                       (na / nAT) * log(1.0 / nAT) / log(2.));
    }
    gamma[i]=1.0/gammainv;
  }
  w[14] = gamma[0];  // G1
  w[15] = gamma[1];  // G2
  w[16] = gamma[2];  // G3
  w[17] = pow(gamma[0] * gamma[1] * gamma[2], 1.0 / 3.0);
  delete svd;

  return w;
}

void GetWHIMs(const Conformer &conf, std::vector<double> &result,
              double *Vpoints, double th) {
  std::vector<double> wu(18);
  std::vector<double> wm(18);
  std::vector<double> wv(18);
  std::vector<double> we(18);
  std::vector<double> wp(18);
  std::vector<double> wi(18);
  std::vector<double> ws(18);

  int numAtoms = conf.getNumAtoms();
  Map<MatrixXd> matorigin(Vpoints, 3, numAtoms);
  MatrixXd MatOrigin = matorigin.transpose();
  std::vector<double> weigthvector;

  // intermediate 18 values stored in this order per weighted vector :
  // "L1","L2","L3","T","A","V","P1","P2","P3","K","E1","E2","E3","D","G1","G2","G3","G"
  weigthvector = moldata3D.GetUn(numAtoms);
  wu = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativeMW(conf.getOwningMol());
  wm = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativeVdW(conf.getOwningMol());
  wv = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativeENeg(conf.getOwningMol());
  we = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativePol(conf.getOwningMol());
  wp = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativeIonPol(conf.getOwningMol());
  wi = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetIState(conf.getOwningMol());
  ws = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  result.clear();
  result.resize(126);

  for (int i = 0; i < 18; i++) {
    result[i + 18 * 0] = wu[i];
    result[i + 18 * 1] = wm[i];
    result[i + 18 * 2] = wv[i];
    result[i + 18 * 3] = we[i];
    result[i + 18 * 4] = wp[i];
    result[i + 18 * 5] = wi[i];
    result[i + 18 * 6] = ws[i];
  }
  wu.clear();
  wm.clear();
  wv.clear();
  we.clear();
  wp.clear();
  wi.clear();
  ws.clear();
}

void getWHIM(const ROMol &mol, std::vector<double> &res, int confId,
             double th) {
  int numAtoms = mol.getNumAtoms();
  const Conformer &conf = mol.getConformer(confId);
  double *Vpoints = new double[3 * numAtoms];

  for (int i = 0; i < numAtoms; ++i) {
    Vpoints[3 * i] = conf.getAtomPos(i).x;
    Vpoints[3 * i + 1] = conf.getAtomPos(i).y;
    Vpoints[3 * i + 2] = conf.getAtomPos(i).z;
  }

  std::vector<double> w(126);
  GetWHIMs(conf, w, Vpoints, th);
  delete [] Vpoints;

  // Dragon extract only this list in this order : L1 L2 L3 P1 P2 G1 G2 G3 E1 E2
  // E3
  int map1[11] = {0, 1, 2, 6, 7, 14, 15, 16, 10, 11, 12};

  for (int k = 0; k < 7; k++) {
    for (int i = 0; i < 11; i++) {
      res[i + 11 * k] = roundn(w[map1[i] + 18 * k], 3);
    }
  }

  for (int i = 0; i < 2; i++) {
    res[i + 13 * 7] = roundn(w[17 + 18 * i], 3);  // 92  93 for Gu  Gm
  }

  for (int i = 0; i < 7; i++) {
    res[i + 11 * 7] =
        roundn(w[3 + 18 * i],
               3);  // 78  79  80  81  82  83  84  for Tu  Tm  Tv  Te  Tp  Ti Ts
    res[i + 12 * 7] =
        roundn(w[4 + 18 * i],
               3);  // 85  86  87  88  89  90  91  for Tu  Am  Av  Ae  Ap  Ai As
    res[i + 13 * 7 + 2] =
        roundn(w[9 + 18 * i],
               3);  // 94  95  96  97  98  99  100 for Ku  Km  Kv  Ke  Kp  Ki Ks
    res[i + 14 * 7 + 2] =
        roundn(w[13 + 18 * i],
               3);  // 101 102 103 104 105 106 107 for Du  Dm  Dv  De  Dp  Di Ds
    res[i + 15 * 7 + 2] =
        roundn(w[5 + 18 * i],
               3);  // 108 109 110 111 112 113 114 for Vu  Vm  Vv  Ve  Vp  Vi Vs
  }
}

}  // end of anonymous namespace

void WHIM(const ROMol &mol, std::vector<double> &res, int confId, double th) {
  PRECONDITION(mol.getNumConformers() >= 1, "molecule has no conformers")
  // Dragon final list is: L1u L2u L3u P1u P2u G1u G2u G3u E1u E2u E3u L1m L2m
  // L3m P1m P2m G1m G2m G3m E1m E2m E3m L1v L2v L3v P1v P2v G1v G2v G3v E1v E2v
  // E3v L1e L2e L3e P1e P2e G1e G2e G3e E1e E2e E3e L1p L2p L3p P1p P2p G1p G2p
  // G3p E1p E2p E3p L1i L2i L3i P1i P2i G1i G2i G3i E1i E2i E3i L1s L2s L3s P1s
  // P2s G1s G2s G3s E1s E2s E3s Tu  Tm  Tv  Te  Tp  Ti  Ts  Au  Am  Av  Ae  Ap
  // Ai  As  Gu  Gm  Ku  Km  Kv  Ke  Kp  Ki  Ks  Du  Dm  Dv  De  Dp  Di  Ds  Vu
  // Vm  Vv  Ve  Vp  Vi  Vs

  res.clear();
  res.resize(114);
  getWHIM(mol, res, confId, th);
}
}  // end of Descriptors namespace
}  // end of RDKit namespace


//
// Copyright (c) 2016,2018 CNRS
//

#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/dynamics.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_FD )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment <4> (3).normalize();

  pinocchio::computeJointJacobians(model, data, q);

  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);

  const std::string RF = "rleg6_joint";
  const std::string LF = "lleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
  Data::Matrix6x J_LF (6, model.nv);
  J_LF.setZero();
  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);

  Eigen::MatrixXd J (12, model.nv);
  J.setZero();
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_LF;

  Eigen::VectorXd gamma (VectorXd::Ones(12));

  Eigen::MatrixXd H(J.transpose());

  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.,true);
  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  MatrixXd Minv (data.M.inverse());
  MatrixXd JMinvJt (J * Minv * J.transpose());

  Eigen::MatrixXd G_ref(J.transpose());
  cholesky::Uiv(model, data, G_ref);
  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
    Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
    BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));

  VectorXd lambda_ref = -JMinvJt.inverse() * (J*Minv*(tau - data.nle) + gamma);
  BOOST_CHECK(data.lambda_c.isApprox(lambda_ref, 1e-12));

  VectorXd a_ref = Minv*(tau - data.nle + J.transpose()*lambda_ref);

  Eigen::VectorXd dynamics_residual_ref (data.M * a_ref + data.nle - tau - J.transpose()*lambda_ref);
  BOOST_CHECK(dynamics_residual_ref.norm() <= 1e-11); // previously 1e-12, may be due to numerical approximations, i obtain 2.03e-12

  Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
  BOOST_CHECK(constraint_residual.norm() <= 1e-12);

  Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);

}

BOOST_AUTO_TEST_CASE ( test_FD_with_damping )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment <4> (3).normalize();

  pinocchio::computeJointJacobians(model, data, q);

  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);

  const std::string RF = "rleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);

  Eigen::MatrixXd J (12, model.nv);
  J.setZero();
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_RF;

  Eigen::VectorXd gamma (VectorXd::Ones(12));

  // Forward Dynamics with damping
  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12,true);

  // Matrix Definitions
  Eigen::MatrixXd H(J.transpose());
  data.M.triangularView<Eigen::StrictlyLower>() =
    data.M.transpose().triangularView<Eigen::StrictlyLower>();

  MatrixXd Minv (data.M.inverse());
  MatrixXd JMinvJt (J * Minv * J.transpose());

  // Check that JMinvJt is correctly formed
  Eigen::MatrixXd G_ref(J.transpose());
  cholesky::Uiv(model, data, G_ref);
  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
  BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));

  // Actual Residuals
  Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
  Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
  BOOST_CHECK(constraint_residual.norm() <= 1e-9);
  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
}

BOOST_AUTO_TEST_CASE ( test_ID )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment <4> (3).normalize();

  pinocchio::computeJointJacobians(model, data, q);

  VectorXd v_before = VectorXd::Ones(model.nv);

  const std::string RF = "rleg6_joint";
  const std::string LF = "lleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
  Data::Matrix6x J_LF (6, model.nv);
  J_LF.setZero();
  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);

  Eigen::MatrixXd J (12, model.nv);
  J.setZero();
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_LF;

  const double r_coeff = 1.;

  Eigen::MatrixXd H(J.transpose());

  pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, true);
  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  MatrixXd Minv (data.M.inverse());
  MatrixXd JMinvJt (J * Minv * J.transpose());

  Eigen::MatrixXd G_ref(J.transpose());
  cholesky::Uiv(model, data, G_ref);
  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
  BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));

  VectorXd lambda_ref = JMinvJt.inverse() * (-r_coeff * J * v_before - J * v_before);
  BOOST_CHECK(data.impulse_c.isApprox(lambda_ref, 1e-12));

  VectorXd v_after_ref = Minv*(data.M * v_before + J.transpose()*lambda_ref);

  Eigen::VectorXd constraint_residual (J * data.dq_after + r_coeff * J * v_before);
  BOOST_CHECK(constraint_residual.norm() <= 1e-12);

  Eigen::VectorXd dynamics_residual (data.M * data.dq_after - data.M * v_before - J.transpose()*data.impulse_c);
  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
}

BOOST_AUTO_TEST_CASE (timings_fd_llt)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
  const size_t NBT = 1000*1000;
#else
  const size_t NBT = 100;
#endif

#else
  const size_t NBT = 1;
  std::cout << "(the time score in debug mode is not relevant)  " ;
#endif // ifndef NDEBUG

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment <4> (3).normalize();

  pinocchio::computeJointJacobians(model, data, q);

  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);

  const std::string RF = "rleg6_joint";
  const std::string LF = "lleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
  Data::Matrix6x J_LF (6, model.nv);
  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);

  Eigen::MatrixXd J (12, model.nv);
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_LF;

  Eigen::VectorXd gamma (VectorXd::Ones(12));

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill( 1.);

  q = pinocchio::randomConfiguration(model);

  PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
  SMOOTH(NBT)
  {
    pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0., true);
  }
  timer.toc(std::cout,NBT);

}

BOOST_AUTO_TEST_SUITE_END ()


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

#pragma once

#include "algorithms/util/RTPGHI.hpp"
#include "algorithms/util/PeakDetection.hpp"
#include "algorithms/public/DataSetIdSequence.hpp"
#include "algorithms/util/DistanceFuncs.hpp"
#include "algorithms/public/STFT.hpp"
#include "algorithms/public/KDTree.hpp"
#include "algorithms/public/MelBands.hpp"
#include "algorithms/util/AlgorithmUtils.hpp"
#include "algorithms/util/FluidEigenMappings.hpp"
#include "algorithms/GraphPlayUtils.hpp"
#include "data/TensorTypes.hpp"
#include "data/FluidDataSet.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <fstream>

namespace fluid {
namespace algorithm {

class GraphLoop {

public:
  using  MatrixXd = Eigen::MatrixXd;
  using DataSet = FluidDataSet<std::string, double, 1>;

  void init(RealVectorView audio, index sampleRate,
            index windowSize, index fftSize, index hopSize, index numBands,
            index distance, double threshold, bool quantize, RealVectorView output) {
    using namespace Eigen;
    using namespace _impl;
    using namespace std;

    mWindowSize = windowSize;
    mFFTSize = fftSize;
    mHopSize = hopSize;
    mFrameSize = (mFFTSize / 2) + 1;
    mThreshold = threshold;
    mBeat = 0;

    STFT stft = STFT(mWindowSize, mFFTSize, mHopSize);
    mLength = std::floor((audio.size() + mHopSize) / mHopSize);
    mSpectrogram = ComplexMatrix(mLength, mFrameSize);
    stft.process(audio, mSpectrogram);
    mMagnitude = RealMatrix(mLength, mFrameSize);
    stft.magnitude(mSpectrogram, mMagnitude);
    mDM = mUtils.computeDM(mMagnitude, numBands, sampleRate, windowSize,
                           fftSize, distance);
    mDM.diagonal().setZero();

    MatrixXd sim = 1 - mDM.array();
    ArrayXd beatSpectrum = ArrayXd::Zero(mLength);
    for(index i = 0; i < mLength; i++){
      beatSpectrum(i) = sim.diagonal(i).sum() / (mLength - i);
    }
    PeakDetection pd;
    auto bsPeaks = pd.process(beatSpectrum.segment(1,lrint(beatSpectrum.size()/2)), 3, 0, false, true);
    mBeat = bsPeaks[0].first;
    if(bsPeaks.size() > 1 && bsPeaks[1].first < mBeat)mBeat = bsPeaks[1].first;
    if(bsPeaks.size() > 2 && bsPeaks[2].first < mBeat)mBeat = bsPeaks[2].first;
    mFilter.init(5);
    ArrayXd odf = mDM.diagonal(1).array();
    for(index i = 0; i < odf.size(); i++){
      odf(i) = odf(i) - mFilter.processSample(odf(i));
    }
    auto onsets = mPD.process(odf, 0, 0.1, false, false);
    mOnsets = Eigen::VectorXi::Zero(mLength);
    for(index i = 0; i < onsets.size(); i++){
      mOnsets(onsets[i].first) = 1;
    }
    mLoop = RealVector{0, static_cast<double>(mLength)};
    fit(threshold, quantize);
    output(0)  = mLoop(0);
    output(1)  = mLoop(1);
    output(2)  = mBeat;
    output(3)  = mNumLinks;
    mInitialized = true;
  }

  void fit(double threshold, bool quantize){
    index stride = quantize?mBeat:1;
    algorithm::DataSetIdSequence seq("", 0, 0);
    mDataSet = DataSet(2);
    for(index i = 0; i <mLength; i++){
      for(index j = i + stride; j < mDM.rows(); j+=stride){
        if(mDM(i,j) < threshold || (quantize && mOnsets(i) > 0)){
          RealVector tmp{
            static_cast<double>(i),
            static_cast<double>(j)
          };
          mDataSet.add(seq.next(),tmp);
        }
      }
    }
    mTree = KDTree(mDataSet);
    mNumLinks = mTree.size();
  }

  void findLoop(){
    RealVector tmpPoint(2);
    auto query = RealVector { static_cast<double>(mStartFrame), static_cast<double>(mEndFrame)};
    auto nearest = mTree.kNearest(query, 1);
    auto nearestIds = nearest.getIds();
    if(nearestIds.size() > 0){
      mDataSet.get(nearestIds(0), mLoop);
    }

  }

  void processFrame(ComplexVectorView out, double start, double end, RealVectorView output) {
    using namespace Eigen;
    using namespace _impl;
    index startFrame = lrint(start * mSpectrogram.rows());
    index endFrame = lrint(end * mSpectrogram.rows());
    if(startFrame != mStartFrame || endFrame != mEndFrame){
      mStartFrame = startFrame;
      mEndFrame = endFrame;
      findLoop();
    }
    out = mSpectrogram.row(mPos);
    mPos = (mPos + 1) % mSpectrogram.rows();
    if(mPos >= mLoop(1))mPos = mLoop(0);
    output(0)  = mLoop(0);
    output(1)  = mLoop(1);
    output(2)  = mBeat;
    output(3)  = mNumLinks;
  }

  bool initialized(){
    return mInitialized;
  }

  index mWindowSize;
  index mHopSize;
  index mFFTSize;

private:
  index mFrameSize;
  GraphPlayUtils mUtils;
  RealVector mLoop;
  ComplexMatrix mSpectrogram;
  RealMatrix mMagnitude;
  RealMatrix mMelSpectrogram;
  Eigen::VectorXi mOnsets;
  KDTree mTree;
  MatrixXd mDM;
  DataSet mDataSet;
  bool mInitialized{false};
  int mPos{0};
  index mLength;
  index mBeat;
  index mStartFrame;
  index mEndFrame;
  double mThreshold;
  index mNumLinks;
  MedianFilter mFilter;
  PeakDetection mPD;
};
} // namespace algorithm
} // namespace fluid


#define BOOST_TEST_NO_LIB
#include <boost/test/auto_unit_test.hpp>

#include "coconut/pulp/math/homogeneous.hpp"

using namespace coconut;
using namespace coconut::pulp;
using namespace coconut::pulp::math;

namespace /* anonymous */ {

BOOST_AUTO_TEST_SUITE(PulpTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathHomogeneousCoordsTestSuite);

BOOST_AUTO_TEST_CASE(HomogeneousCoordsConstructibleFromVec4) {
	const auto h = HomogeneousCoordinates({ 1.0f, 2.0f, 3.0f, 4.0f });
	BOOST_CHECK_EQUAL(h, Vec4(1.0f, 2.0f, 3.0f, 4.0f));
}

BOOST_AUTO_TEST_CASE(HomogeneousCoordsConstructibleFromVec3Point) {
	const auto h = HomogeneousPoint({ 1.0f, 2.0f, 3.0f });
	BOOST_CHECK_EQUAL(h, Vec4(1.0f, 2.0f, 3.0f, 1.0f));
}

BOOST_AUTO_TEST_CASE(HomogeneousCoordsConstructibleFromVec3Vector) {
	const auto h = HomogeneousVector({ 1.0f, 2.0f, 3.0f });
	BOOST_CHECK_EQUAL(h, Vec4(1.0f, 2.0f, 3.0f, 0.0f));
}

BOOST_AUTO_TEST_CASE(CanObtain3dImageOfHomogeneousCoordinates) {
	const auto p = HomogeneousPoint({ 1.0f, 2.0f, 3.0f });
	const auto v = HomogeneousVector({ 1.0f, 2.0f, 3.0f });
	const auto h = HomogeneousCoordinates({ 3.0f, 3.0f, 3.0f, 3.0f });

	BOOST_CHECK_EQUAL(p.to3dSpace(), Vec3(1.0f, 2.0f, 3.0f));
	BOOST_CHECK_EQUAL(v.to3dSpace(), Vec3(1.0f, 2.0f, 3.0f));
	BOOST_CHECK_EQUAL(h.to3dSpace(), Vec3(1.0f, 1.0f, 1.0f));
}

BOOST_AUTO_TEST_SUITE_END(/* PulpMathHomogeneousCoordsTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpMathTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpTestSuite */);

} // namespace anonymous


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <iterator>

#include <boost/type_traits.hpp>
#include <boost/static_assert.hpp>

#include <boost/compute/types.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/buffer_iterator.hpp>
#include <boost/compute/iterator/transform_iterator.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(value_type)
{
    using boost::compute::float4_;

    BOOST_STATIC_ASSERT((
        boost::is_same<
            boost::compute::transform_iterator<
                boost::compute::buffer_iterator<float>,
                boost::compute::sqrt<float>
            >::value_type,
            float
        >::value
    ));
    BOOST_STATIC_ASSERT((
        boost::is_same<
            boost::compute::transform_iterator<
                boost::compute::buffer_iterator<float4_>,
                boost::compute::length<float4_>
            >::value_type,
            float
        >::value
    ));
}

BOOST_AUTO_TEST_CASE(copy)
{
    int data[] = { 1, -2, 3, -4, 5 };
    boost::compute::vector<int> a(data, data + 5, queue);

    boost::compute::vector<int> b(5, context);
    boost::compute::copy(
        boost::compute::make_transform_iterator(
            a.begin(),
            boost::compute::abs<int>()
        ),
        boost::compute::make_transform_iterator(
            a.end(),
            boost::compute::abs<int>()
        ),
        b.begin(),
        queue
    );
    CHECK_RANGE_EQUAL(int, 5, b, (1, 2, 3, 4, 5));
}

BOOST_AUTO_TEST_CASE(copy_abs_doctest)
{
    int data[] = { -1, -2, -3, -4 };
    boost::compute::vector<int> input(data, data + 4, queue);
    boost::compute::vector<int> output(4, context);

//! [copy_abs]
// use abs() from boost.compute
using boost::compute::abs;

// copy the absolute value for each element in input to output
boost::compute::copy(
    boost::compute::make_transform_iterator(input.begin(), abs<int>()),
    boost::compute::make_transform_iterator(input.end(), abs<int>()),
    output.begin(),
    queue
);
//! [copy_abs]

    CHECK_RANGE_EQUAL(int, 4, output, (1, 2, 3, 4));
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <Eigen/Core>
#include <algorithm>
#include <memory>
#include "../mean_curvature_solver.h"
#include "../utilities.h"
#include "../uniform_lb_operator.h"
#include "../cotangent_lb_operator.h"

using SolverPtrT = std::unique_ptr<mcurv::MeanCurvatureSolver>;

char *getCmdOption(char **begin, char **end, const std::string &option) {
    char **itr = std::find(begin, end, option);
    if (itr != end && ++itr != end) {
        return *itr;
    }
    return 0;
}

bool cmdOptionExists(char **begin, char **end, const std::string &option) {
    return std::find(begin, end, option) != end;
}

void printHelpMessage() {
    std::cout << "The app usage: ./MeanCurvatureApp -i path1 -o path2 -c\n";
    std::cout << "-i  Path to .off file.\n";
    std::cout << "-o  Path to user's output file.\n";
    std::cout << "-c  (OPTIONAL) If specified cotangent Laplace-Beltrami operator will be used. Otherwise the uniform one.\n";
}

int main(int argc, char **argv) {
    // If requested - print help information
    if (cmdOptionExists(argv, argv + argc, "-h") ||
        cmdOptionExists(argv, argv + argc, "--help")) {
        printHelpMessage();
        return 0;
    }

    // If input and output flags are not present - error
    if (!cmdOptionExists(argv, argv + argc, "-i") ||
        !cmdOptionExists(argv, argv + argc, "-o")) {
        printHelpMessage();
        return 1;
    }

    std::string inputFileName(getCmdOption(argv, argv + argc, "-i"));
    std::string outputFileName (getCmdOption(argv, argv + argc, "-o"));

    // Load appropriate solver
    SolverPtrT solverPtr;
    if (cmdOptionExists(argv, argv + argc, "-c")) {
        solverPtr = SolverPtrT(
                new mcurv::MeanCurvatureSolver(mcurv::cotangentLBOperatorStrategy));
    } else {
        solverPtr = SolverPtrT(
                new mcurv::MeanCurvatureSolver(mcurv::uniformLBOperatorStrategy));
    }

    // Calculate the solution
    Eigen::MatrixXd solution;
    solverPtr->Execute(solution, inputFileName);

    // Save to specified file
    mcurv::dumpMatrixXdToFile(solution, outputFileName);

    return 0;
}


#include <gtest/gtest.h>

#include <Eigen/Eigen>
#include <boost/filesystem.hpp>

#include <CppADCodeGenEigenPy/ADModel.h>
#include <CppADCodeGenEigenPy/CompiledModel.h>
#include <CppADCodeGenEigenPy/Util.h>

#include "testing/models/ParameterizedTestModel.h"

namespace CppADCodeGenEigenPy {
namespace ParameterizedModelTest {

class ParameterizedTestModelFixture : public ::testing::Test {
   protected:
    using Vector = CompiledModel<Scalar>::Vector;
    using Matrix = CompiledModel<Scalar>::Matrix;

    static void SetUpTestSuite() {
        // Compile and load our model
        boost::filesystem::create_directories(DIRECTORY_PATH);
        ad_model_ptr_.reset(new ParameterizedTestModel<Scalar>());
        ad_model_ptr_->compile(MODEL_NAME, DIRECTORY_PATH,
                               DerivativeOrder::Second);
        compiled_model_ptr_.reset(
            new CompiledModel<Scalar>(MODEL_NAME, LIB_GENERIC_PATH));
    }

    static void TearDownTestSuite() {
        // Delete the compiled shared object.
        boost::filesystem::remove_all(DIRECTORY_PATH);
    }

    static std::unique_ptr<ADModel<Scalar>> ad_model_ptr_;
    static std::unique_ptr<CompiledModel<Scalar>> compiled_model_ptr_;
};

std::unique_ptr<ADModel<Scalar>>
    ParameterizedTestModelFixture::ad_model_ptr_ = nullptr;
std::unique_ptr<CompiledModel<Scalar>>
    ParameterizedTestModelFixture::compiled_model_ptr_ = nullptr;

TEST_F(ParameterizedTestModelFixture, Evaluation) {
    Vector input = 2 * Vector::Ones(NUM_INPUT);
    Vector parameters = Vector::Ones(NUM_INPUT);

    Vector output_expected = evaluate<Scalar>(input, parameters);
    Vector output_actual = compiled_model_ptr_->evaluate(input, parameters);
    EXPECT_TRUE(output_actual.isApprox(output_expected))
        << "Function evaluation is incorrect.";

    // if I forget to pass the parameters, I should get a runtime error
    EXPECT_THROW(compiled_model_ptr_->evaluate(input), std::runtime_error)
        << "Missing parameters did not throw error.";
}

TEST_F(ParameterizedTestModelFixture, Jacobian) {
    Vector input = 2 * Vector::Ones(NUM_INPUT);
    Vector parameters = Vector::Ones(NUM_INPUT);

    // note Jacobian of scalar function is a row vector, hence the transpose
    Matrix P = Matrix::Zero(NUM_INPUT, NUM_INPUT);
    P.diagonal() << parameters;
    Matrix J_expected = input.transpose() * P;
    Matrix J_actual = compiled_model_ptr_->jacobian(input, parameters);
    EXPECT_TRUE(J_actual.isApprox(J_expected)) << "Jacobian is incorrect.";

    EXPECT_THROW(compiled_model_ptr_->jacobian(input), std::runtime_error)
        << "Missing parameters did not throw error.";
}

TEST_F(ParameterizedTestModelFixture, Hessian) {
    Vector input = 2 * Vector::Ones(NUM_INPUT);
    Vector parameters = Vector::Ones(NUM_INPUT);

    Matrix H_expected = Matrix::Zero(NUM_INPUT, NUM_INPUT);
    H_expected.diagonal() << parameters;
    Matrix H_actual = compiled_model_ptr_->hessian(input, parameters, 0);
    EXPECT_TRUE(H_actual.isApprox(H_expected)) << "Hessian is incorrect.";
    EXPECT_THROW(compiled_model_ptr_->hessian(input, 0), std::runtime_error)
        << "Missing parameters did not throw error.";
}

}  // namespace ParameterizedModelTest
}  // namespace CppADCodeGenEigenPy


/**************************************************************************
 * @file:  UuidTests.cpp
 * @brief:
 *
 * Copyright (c) 2021 O-Net Technologies (Group) Limited.
 **************************************************************************/

#include <gtest/gtest.h>

#include <Eigen/Core>

TEST(EigenTests, MultiTypeVectors)
{
    Eigen::Vector4f v4f(12.1, 12.2, 12.3, 12.4);
    Eigen::Vector3i v3i(0, -1, 2);
    Eigen::Vector2d v2d(1.0, 1.5);
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the machine learning
    tools for sequence labeling in the dlib C++ Library.

    The general problem addressed by these tools is the following.
    Suppose you have a set of sequences of some kind and you want to
    learn to predict a label for each element of a sequence.  So for
    example, you might have a set of English sentences where each
    word is labeled with its part of speech and you want to learn a
    model which can predict the part of speech for each word in a new
    sentence.

    Central to these tools is the sequence_labeler object.  It is the
    object which represents the label prediction model. In particular,
    the model used by this object is the following.  Given an input
    sequence x, predict an output label sequence y such that:
        y == argmax_y dot(weight_vector, PSI(x,y))
    where PSI() is supplied by the user and defines the form of the
    model.  In this example program we will define it such that we
    obtain a simple Hidden Markov Model.  However, it's possible to
    define much more sophisticated models.  You should take a look
    at the following papers for a few examples:
        - Hidden Markov Support Vector Machines by
          Y. Altun, I. Tsochantaridis, T. Hofmann
        - Shallow Parsing with Conditional Random Fields by
          Fei Sha and Fernando Pereira

    In the remainder of this example program we will show how to
    define your own PSI(), as well as how to learn the "weight_vector"
    parameter.  Once you have these two items you will be able to
    use the sequence_labeler to predict the labels of new sequences.
*/

#include <iostream>
#include <dlib/svm_threaded.h>
#include <dlib/rand.h>

using namespace std;
using namespace dlib;

/*
    In this example we will be working with a Hidden Markov Model where
    the hidden nodes and observation nodes both take on 3 different states.
    The task will be to take a sequence of observations and predict the state
    of the corresponding hidden nodes.
*/

const unsigned long num_label_states = 3;
const unsigned long num_sample_states = 3;

// ----------------------------------------------------------------------------------------

class feature_extractor
{
    /*
        This object is where you define your PSI().  To ensure that the argmax_y
        remains a tractable problem, the PSI(x,y) vector is actually a sum of vectors,
        each derived from the entire input sequence x but only part of the label
        sequence y.  This allows the argmax_y to be efficiently solved using the
        well known Viterbi algorithm.
    */

public:
    // This defines the type used to represent the observed sequence.  You can use
    // any type here so long as it has a .size() which returns the number of things
    // in the sequence.
    typedef std::vector<unsigned long> sequence_type;

    unsigned long num_features() const
    /*!
        ensures
            - returns the dimensionality of the PSI() feature vector.
    !*/
    {
        // Recall that we are defining a HMM.  So in this case the PSI() vector
        // should have the same dimensionality as the number of parameters in the HMM.
        return num_label_states*num_label_states + num_label_states*num_sample_states;
    }

    unsigned long order() const
    /*!
        ensures
            - This object represents a Markov model on the output labels.
              This parameter defines the order of the model.  That is, this
              value controls how many previous label values get to be taken
              into consideration when performing feature extraction for a
              particular element of the input sequence.  Note that the runtime
              of the algorithm is exponential in the order.  So don't make order
              very large.
    !*/
    {
        // In this case we are using a HMM model that only looks at the
        // previous label.
        return 1;
    }

    unsigned long num_labels() const
    /*!
        ensures
            - returns the number of possible output labels.
    !*/
    {
        return num_label_states;
    }

    template <typename feature_setter, typename EXP>
    void get_features (
        feature_setter& set_feature,
        const sequence_type& x,
        const matrix_exp<EXP>& y,
        unsigned long position
    ) const
    /*!
        requires
            - EXP::type == unsigned long
              (i.e. y contains unsigned longs)
            - position < x.size()
            - y.size() == min(position, order) + 1
            - is_vector(y) == true
            - max(y) < num_labels()
            - set_feature is a function object which allows expressions of the form:
                - set_features((unsigned long)feature_index, (double)feature_value);
                - set_features((unsigned long)feature_index);
        ensures
            - for all valid i:
                - interprets y(i) as the label corresponding to x[position-i]
            - This function computes the part of PSI() corresponding to the x[position]
              element of the input sequence.  Moreover, this part of PSI() is returned as
              a sparse vector by invoking set_feature().  For example, to set the feature
              with an index of 55 to the value of 1 this method would call:
                set_feature(55);
              Or equivalently:
                set_feature(55,1);
              Therefore, the first argument to set_feature is the index of the feature
              to be set while the second argument is the value the feature should take.
              Additionally, note that calling set_feature() multiple times with the same
              feature index does NOT overwrite the old value, it adds to the previous
              value.  For example, if you call set_feature(55) 3 times then it will
              result in feature 55 having a value of 3.
            - This function only calls set_feature() with feature_index values < num_features()
    !*/
    {
        // Again, the features below only define a simple HMM.  But in general, you can
        // use a wide variety of sophisticated feature extraction methods here.

        // Pull out an indicator feature for the type of transition between the
        // previous label and the current label.
        if (y.size() > 1)
            set_feature(y(1)*num_label_states + y(0));

        // Pull out an indicator feature for the type of observed node given
        // the current label.
        set_feature(num_label_states*num_label_states +
                    y(0)*num_sample_states + x[position]);
    }
};

// We need to define serialize() and deserialize() for our feature extractor if we want
// to be able to serialize and deserialize our learned models.  In this case the
// implementation is empty since our feature_extractor doesn't have any state.  But you
// might define more complex feature extractors which have state that needs to be saved.
void serialize(const feature_extractor&, std::ostream&) {}
void deserialize(feature_extractor&, std::istream&) {}

// ----------------------------------------------------------------------------------------

void make_dataset (
    const matrix<double>& transition_probabilities,
    const matrix<double>& emission_probabilities,
    std::vector<std::vector<unsigned long> >& samples,
    std::vector<std::vector<unsigned long> >& labels,
    unsigned long dataset_size
);
/*!
    requires
        - transition_probabilities.nr() == transition_probabilities.nc()
        - transition_probabilities.nr() == emission_probabilities.nr()
        - The rows of transition_probabilities and emission_probabilities must sum to 1.
          (i.e. sum_cols(transition_probabilities) and sum_cols(emission_probabilities)
          must evaluate to vectors of all 1s.)
    ensures
        - This function randomly samples a bunch of sequences from the HMM defined by
          transition_probabilities and emission_probabilities.
        - The HMM is defined by:
            - The probability of transitioning from hidden state H1 to H2
              is given by transition_probabilities(H1,H2).
            - The probability of a hidden state H producing an observed state
              O is given by emission_probabilities(H,O).
        - #samples.size() == #labels.size() == dataset_size
        - for all valid i:
            - #labels[i] is a randomly sampled sequence of hidden states from the
              given HMM.  #samples[i] is its corresponding randomly sampled sequence
              of observed states.
!*/

// ----------------------------------------------------------------------------------------

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_sequence_labeler_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
{
    // We need a dataset to test the machine learning algorithms.  So we are going to
    // define a HMM based on the following two matrices and then randomly sample a
    // set of data from it.  Then we will see if the machine learning method can
    // recover the HMM model from the training data.

    matrix<double> transition_probabilities(num_label_states, num_label_states);
    transition_probabilities = 0.05, 0.90, 0.05,
                               0.05, 0.05, 0.90,
                               0.90, 0.05, 0.05;

    matrix<double> emission_probabilities(num_label_states,num_sample_states);
    emission_probabilities = 0.5, 0.5, 0.0,
                             0.0, 0.5, 0.5,
                             0.5, 0.0, 0.5;

    std::vector<std::vector<unsigned long> > samples;
    std::vector<std::vector<unsigned long> > labels;
    // sample 1000 labeled sequences from the HMM.
    make_dataset(transition_probabilities,emission_probabilities,
                 samples, labels, 1000);

    // print out some of the randomly sampled sequences
    for (int i = 0; i < 10; ++i)
    {
        cout << "hidden states:   " << trans(mat(labels[i]));
        cout << "observed states: " << trans(mat(samples[i]));
        cout << "******************************" << endl;
    }

    // Next we use the structural_sequence_labeling_trainer to learn our
    // prediction model based on just the samples and labels.
    structural_sequence_labeling_trainer<feature_extractor> trainer;
    // This is the common SVM C parameter.  Larger values encourage the
    // trainer to attempt to fit the data exactly but might overfit.
    // In general, you determine this parameter by cross-validation.
    trainer.set_c(4);
    // This trainer can use multiple CPU cores to speed up the training.
    // So set this to the number of available CPU cores.
    trainer.set_num_threads(4);

    // Learn to do sequence labeling from the dataset
    sequence_labeler<feature_extractor> labeler = trainer.train(samples, labels);

    // Test the learned labeler on one of the training samples.  In this
    // case it will give the correct sequence of labels.
    std::vector<unsigned long> predicted_labels = labeler(samples[0]);
    cout << "true hidden states:      "<< trans(mat(labels[0]));
    cout << "predicted hidden states: "<< trans(mat(predicted_labels));

    // We can also do cross-validation.  The confusion_matrix is defined as:
    //  - confusion_matrix(T,P) == the number of times a sequence element with label T
    //    was predicted to have a label of P.
    // So if all predictions are perfect then only diagonal elements of this matrix will
    // be non-zero.
    matrix<double> confusion_matrix;
    confusion_matrix = cross_validate_sequence_labeler(trainer, samples, labels, 4);
    cout << "\ncross-validation: " << endl;
    cout << confusion_matrix;
    cout << "label accuracy: "<< sum(diag(confusion_matrix))/sum(confusion_matrix) << endl;

    // In this case, the label accuracy is about 88%.  At this point, we want to know if
    // the machine learning method was able to recover the HMM model from the data.  So
    // to test this, we can load the true HMM model into another sequence_labeler and
    // test it out on the data and compare the results.

    matrix<double,0,1> true_hmm_model_weights = log(join_cols(reshape_to_column_vector(transition_probabilities),
                                                              reshape_to_column_vector(emission_probabilities)));
    // With this model, labeler_true will predict the most probable set of labels
    // given an input sequence.  That is, it will predict using the equation:
    //    y == argmax_y dot(true_hmm_model_weights, PSI(x,y))
    sequence_labeler<feature_extractor> labeler_true(true_hmm_model_weights);

    confusion_matrix = test_sequence_labeler(labeler_true, samples, labels);
    cout << "\nTrue HMM model: " << endl;
    cout << confusion_matrix;
    cout << "label accuracy: "<< sum(diag(confusion_matrix))/sum(confusion_matrix) << endl;

    // Happily, we observe that the true model also obtains a label accuracy of 88%.

    // Finally, the labeler can be serialized to disk just like most dlib objects.
    serialize("labeler.dat") << labeler;

    // recall from disk
    deserialize("labeler.dat") >> labeler;
}

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
//              Code for creating a bunch of random samples from our HMM.
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

void sample_hmm (
    dlib::rand& rnd,
    const matrix<double>& transition_probabilities,
    const matrix<double>& emission_probabilities,
    unsigned long previous_label,
    unsigned long& next_label,
    unsigned long& next_sample
)
/*!
    requires
        - previous_label < transition_probabilities.nr()
        - transition_probabilities.nr() == transition_probabilities.nc()
        - transition_probabilities.nr() == emission_probabilities.nr()
        - The rows of transition_probabilities and emission_probabilities must sum to 1.
          (i.e. sum_cols(transition_probabilities) and sum_cols(emission_probabilities)
          must evaluate to vectors of all 1s.)
    ensures
        - This function randomly samples the HMM defined by transition_probabilities
          and emission_probabilities assuming that the previous hidden state
          was previous_label.
        - The HMM is defined by:
            - P(next_label |previous_label) == transition_probabilities(previous_label, next_label)
            - P(next_sample|next_label)     == emission_probabilities  (next_label,     next_sample)
        - #next_label == the sampled value of the hidden state
        - #next_sample == the sampled value of the observed state
!*/
{
    // sample next_label
    double p = rnd.get_random_double();
    for (long c = 0; p >= 0 && c < transition_probabilities.nc(); ++c)
    {
        next_label = c;
        p -= transition_probabilities(previous_label, c);
    }

    // now sample next_sample
    p = rnd.get_random_double();
    for (long c = 0; p >= 0 && c < emission_probabilities.nc(); ++c)
    {
        next_sample = c;
        p -= emission_probabilities(next_label, c);
    }
}

// ----------------------------------------------------------------------------------------

void make_dataset (
    const matrix<double>& transition_probabilities,
    const matrix<double>& emission_probabilities,
    std::vector<std::vector<unsigned long> >& samples,
    std::vector<std::vector<unsigned long> >& labels,
    unsigned long dataset_size
)
{
    samples.clear();
    labels.clear();

    dlib::rand rnd;

    // now randomly sample some labeled sequences from our Hidden Markov Model
    for (unsigned long iter = 0; iter < dataset_size; ++iter)
    {
        const unsigned long sequence_size = rnd.get_random_32bit_number()%20+3;
        std::vector<unsigned long> sample(sequence_size);
        std::vector<unsigned long> label(sequence_size);

        unsigned long previous_label = rnd.get_random_32bit_number()%num_label_states;
        for (unsigned long i = 0; i < sample.size(); ++i)
        {
            unsigned long next_label = 0, next_sample = 0;
            sample_hmm(rnd, transition_probabilities, emission_probabilities,
                       previous_label, next_label, next_sample);

            label[i] = next_label;
            sample[i] = next_sample;

            previous_label = next_label;
        }

        samples.push_back(sample);
        labels.push_back(label);
    }
}

// ----------------------------------------------------------------------------------------


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>

#include <algorithm>
#include <vector>
#include <utility>
#include <iostream>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/graph_utility.hpp>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, bidirectionalS > Graph;

  Graph G(5);
  add_edge(0, 2, G);
  add_edge(1, 1, G);
  add_edge(1, 3, G);
  add_edge(1, 4, G);
  add_edge(2, 1, G);
  add_edge(2, 3, G);
  add_edge(2, 4, G);
  add_edge(3, 1, G);
  add_edge(3, 4, G);
  add_edge(4, 0, G);
  add_edge(4, 1, G);

  std::cout << "original graph:" << std::endl;
  print_graph(G, get(vertex_index, G));

  std::cout << std::endl << "reversed graph:" << std::endl;
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300  // avoid VC++ bug...
  reverse_graph<Graph> R(G);
  print_graph(R, get(vertex_index, G));
#else
  print_graph(make_reverse_graph(G), get(vertex_index, G));
#endif

  return EXIT_SUCCESS;
}


#include <stan/math/rev/mat.hpp>
#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <test/unit/math/rev/mat/functor/util_cvodes_adams.hpp>
#include <test/unit/math/prim/arr/functor/harmonic_oscillator.hpp>
#include <test/unit/math/prim/arr/functor/lorenz.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <string>

template <typename F, typename T_y0, typename T_theta>
void sho_value_test(F harm_osc, std::vector<double>& y0, double t0,
                    std::vector<double>& ts, std::vector<double>& theta,
                    std::vector<double>& x, std::vector<int>& x_int) {
  using stan::math::promote_scalar;
  using stan::math::var;

  std::vector<std::vector<var> > ode_res_vd = stan::math::integrate_ode_adams(
      harm_osc, promote_scalar<T_y0>(y0), t0, ts,
      promote_scalar<T_theta>(theta), x, x_int);

  EXPECT_NEAR(0.995029, ode_res_vd[0][0].val(), 1e-5);
  EXPECT_NEAR(-0.0990884, ode_res_vd[0][1].val(), 1e-5);

  EXPECT_NEAR(-0.421907, ode_res_vd[99][0].val(), 1e-5);
  EXPECT_NEAR(0.246407, ode_res_vd[99][1].val(), 1e-5);
}

void sho_finite_diff_test(double t0) {
  using stan::math::var;
  harm_osc_ode_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x;
  std::vector<int> x_int;

  test_ode_cvode(harm_osc, t0, ts, y0, theta, x, x_int, 1e-8, 1e-4);

  sho_value_test<harm_osc_ode_fun, double, var>(harm_osc, y0, t0, ts, theta, x,
                                                x_int);
  sho_value_test<harm_osc_ode_fun, var, double>(harm_osc, y0, t0, ts, theta, x,
                                                x_int);
  sho_value_test<harm_osc_ode_fun, var, var>(harm_osc, y0, t0, ts, theta, x,
                                             x_int);
}

void sho_data_finite_diff_test(double t0) {
  using stan::math::var;
  harm_osc_ode_data_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3, 1);
  std::vector<int> x_int(2, 0);

  test_ode_cvode(harm_osc, t0, ts, y0, theta, x, x_int, 1e-8, 1e-4);

  sho_value_test<harm_osc_ode_data_fun, double, var>(harm_osc, y0, t0, ts,
                                                     theta, x, x_int);
  sho_value_test<harm_osc_ode_data_fun, var, double>(harm_osc, y0, t0, ts,
                                                     theta, x, x_int);
  sho_value_test<harm_osc_ode_data_fun, var, var>(harm_osc, y0, t0, ts, theta,
                                                  x, x_int);
}

template <typename T_y0, typename T_theta, typename F>
void sho_error_test(F harm_osc, std::vector<double>& y0, double t0,
                    std::vector<double>& ts, std::vector<double>& theta,
                    std::vector<double>& x, std::vector<int>& x_int,
                    std::string error_msg) {
  using stan::math::promote_scalar;
  using stan::math::var;

  EXPECT_THROW_MSG(stan::math::integrate_ode_adams(
                       harm_osc, promote_scalar<T_y0>(y0), t0, ts,
                       promote_scalar<T_theta>(theta), x, x_int),
                   std::invalid_argument, error_msg);
}

// TODO(carpenter): g++6 failure
TEST(StanAgradRevOde_integrate_ode, harmonic_oscillator_finite_diff) {
  sho_finite_diff_test(0);
  sho_finite_diff_test(2.0);
  sho_finite_diff_test(-2.0);

  sho_data_finite_diff_test(0);
  sho_data_finite_diff_test(2.5);
  sho_data_finite_diff_test(-2.5);
}

TEST(StanAgradRevOde_integrate_ode, harmonic_oscillator_error) {
  using stan::math::var;
  harm_osc_ode_wrong_size_1_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  double t0 = 0;
  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3, 1);
  std::vector<int> x_int(2, 0);

  // aligned error handling with non-stiff case
  std::string error_msg
      = "cvodes_ode_data: dz_dt (3) and states (2) must match in size";

  sho_error_test<double, var>(harm_osc, y0, t0, ts, theta, x, x_int, error_msg);
  sho_error_test<var, double>(harm_osc, y0, t0, ts, theta, x, x_int, error_msg);
  sho_error_test<var, var>(harm_osc, y0, t0, ts, theta, x, x_int, error_msg);
}

// TODO(Yi Zhang): failure
// TEST(StanAgradRevOde_integrate_ode, lorenz_finite_diff) {
//   lorenz_ode_fun lorenz;

//   std::vector<double> y0;
//   std::vector<double> theta;
//   double t0;
//   std::vector<double> ts;

//   t0 = 0;

//   theta.push_back(10.0);
//   theta.push_back(28.0);
//   theta.push_back(8.0 / 3.0);
//   y0.push_back(10.0);
//   y0.push_back(1.0);
//   y0.push_back(1.0);

//   std::vector<double> x;
//   std::vector<int> x_int;

//   for (int i = 0; i < 100; i++)
//     ts.push_back(0.1 * (i + 1));

//   test_ode_cvode(lorenz, t0, ts, y0, theta, x, x_int, 1e-8, 1e-1);
// }


#include "hog_detector/interpolation.h"

#include <gtest/gtest.h>
#include <ros/ros.h>
#include <vector>
#include <boost/scoped_ptr.hpp>

using namespace std;
using namespace boost;

class LinearInterpTest : public ::testing::Test {
 protected:
  vector<double> x_;
  vector<double> y_;

  virtual void SetUp() {
    x_.clear();
    y_.clear();
  }
};

TEST_F(LinearInterpTest, EmptyInterpolator) {
  ASSERT_EQ(x_.size(), 0);
  ASSERT_EQ(y_.size(), 0);
  EXPECT_DEATH(LinearInterpolator(x_, y_),
               ".* insufficient number of points for interpolation type");
}

TEST_F(LinearInterpTest, SortedInterpolation) {
  double x[] = {0, 3, 4, 8, 9, 15, 17};
  double y[] = {1, 6, 9, 23, 15, 14, 18};
  x_.insert(x_.end(), x, x+7);
  y_.insert(y_.end(), y, y+7);
  LinearInterpolator interpolator(x_, y_);

  EXPECT_FLOAT_EQ(interpolator(2.0), 1 + 2.0*5/3);
  EXPECT_FLOAT_EQ(interpolator(3.5), 7.5);
  EXPECT_FLOAT_EQ(interpolator(10), 15 - 1.0/6);
  EXPECT_FLOAT_EQ(interpolator(14), 15 - 5.0/6);
  EXPECT_FLOAT_EQ(interpolator(16), 16.0);
}

TEST_F(LinearInterpTest, UnsortedInterpolation) {
  double x[] = {15, 9, 17, 0, 3, 4, 8};
  double y[] = {14, 15, 18, 1, 6, 9, 23};
  x_.insert(x_.end(), x, x+7);
  y_.insert(y_.end(), y, y+7);
  LinearInterpolator interpolator(x_, y_);

  EXPECT_FLOAT_EQ(interpolator(2.0), 1 + 2.0*5/3);
  EXPECT_FLOAT_EQ(interpolator(3.5), 7.5);
  EXPECT_FLOAT_EQ(interpolator(10), 15 - 1.0/6);
  EXPECT_FLOAT_EQ(interpolator(14), 15 - 5.0/6);
  EXPECT_FLOAT_EQ(interpolator(16), 16.0);
}

TEST_F(LinearInterpTest, IdenticalEntryInterpolation) {
  double x[] = {0, 3, 3, 4, 8, 9, 15, 17};
  double y[] = {1, 6, 6, 9, 23, 15, 14, 18};
  x_.insert(x_.end(), x, x+8);
  y_.insert(y_.end(), y, y+8);
  LinearInterpolator interpolator(x_, y_);

  EXPECT_FLOAT_EQ(interpolator(2.0), 1 + 2.0*5/3);
  EXPECT_FLOAT_EQ(interpolator(3.5), 7.5);
  EXPECT_FLOAT_EQ(interpolator(10), 15 - 1.0/6);
  EXPECT_FLOAT_EQ(interpolator(14), 15 - 5.0/6);
  EXPECT_FLOAT_EQ(interpolator(16), 16.0);
}

TEST_F(LinearInterpTest, OutOfBounds) {
  double x[] = {0, 3, 4, 8, 9, 15, 17};
  double y[] = {1, 6, 9, 23, 15, 14, 18};
  x_.insert(x_.end(), x, x+7);
  y_.insert(y_.end(), y, y+7);
  LinearInterpolator interpolator(x_, y_);

  EXPECT_THROW(interpolator(-3.0), Interpolator::out_of_bounds);
  EXPECT_THROW(interpolator(22.0), Interpolator::out_of_bounds);
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  ros::Time::init();
  return RUN_ALL_TESTS();
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <algorithm>
#include <utility>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/graph/incremental_components.hpp>

/*

  This example shows how to use the disjoint set data structure
  to compute the connected components of an undirected, changing
  graph.

  Sample output:

  An undirected graph:
  0 <--> 1 4
  1 <--> 0 4
  2 <--> 5
  3 <-->
  4 <--> 1 0
  5 <--> 2

  representative[0] = 1
  representative[1] = 1
  representative[2] = 5
  representative[3] = 3
  representative[4] = 1
  representative[5] = 5

  component 0 contains: 4 1 0
  component 1 contains: 3
  component 2 contains: 5 2

 */

using namespace std;

int main(int , char* [])
{
  using namespace boost;
  typedef adjacency_list <vecS, vecS, undirectedS> Graph;
  typedef graph_traits<Graph>::vertex_descriptor Vertex;
  typedef graph_traits<Graph>::vertices_size_type size_type;

  const int N = 6;
  Graph G(N);

  std::vector<size_type> rank(num_vertices(G));
  std::vector<Vertex> parent(num_vertices(G));
  typedef size_type* Rank;
  typedef Vertex* Parent;
  disjoint_sets<Rank, Parent>  ds(&rank[0], &parent[0]);

  initialize_incremental_components(G, ds);
  incremental_components(G, ds);

  graph_traits<Graph>::edge_descriptor e;
  bool flag;
  boost::tie(e,flag) = add_edge(0, 1, G);
  ds.union_set(0,1);

  boost::tie(e,flag) = add_edge(1, 4, G);
  ds.union_set(1,4);

  boost::tie(e,flag) = add_edge(4, 0, G);
  ds.union_set(4,0);

  boost::tie(e,flag) = add_edge(2, 5, G);
  ds.union_set(2,5);

  cout << "An undirected graph:" << endl;
  print_graph(G, get(vertex_index, G));
  cout << endl;

  graph_traits<Graph>::vertex_iterator i,end;
  for (boost::tie(i, end) = vertices(G); i != end; ++i)
    cout << "representative[" << *i << "] = " <<
      ds.find_set(*i) << endl;;
  cout << endl;

  typedef component_index<unsigned int> Components;
  Components components(&parent[0], &parent[0] + parent.size());

  for (Components::size_type c = 0; c < components.size(); ++c) {
    cout << "component " << c << " contains: ";
    Components::value_type::iterator
      j = components[c].begin(),
      jend = components[c].end();
    for ( ; j != jend; ++j)
      cout << *j << " ";
    cout << endl;
  }

  return 0;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/atan.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

STF_CASE_TPL (" atanreal",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::atan;

  using r_t = decltype(atan(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(atan(bs::Inf<T>()), bs::Pio_2<r_t>(), 0);
  STF_ULP_EQUAL(atan(bs::Minf<T>()), -bs::Pio_2<r_t>(), 0);
  STF_ULP_EQUAL(atan(bs::Nan<T>()), bs::Nan<r_t>(), 0);
#endif
  STF_ULP_EQUAL(atan(bs::Half<T>()), T(4.636476090008061e-01), 0.5);
  STF_ULP_EQUAL(atan(bs::Mhalf<T>()), T(-4.636476090008061e-01), 0.5);
  STF_ULP_EQUAL(atan(bs::Mone<T>()), -bs::Pio_4<r_t>(), 0.5);
  STF_ULP_EQUAL(atan(bs::One<T>()), bs::Pio_4<r_t>(), 0.5);
  STF_ULP_EQUAL(atan(bs::Zero<T>()), bs::Zero<r_t>(), 0.5);
}


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

#include <catch2/catch.hpp>

inline void make_first_positive(Eigen::VectorXi& v)
{
    for(uint32_t n = 0; n < v.size(); ++n)
    {
        if(v(n) == 0)
        {
            continue;
        }
        if(v(n) < 0)
        {
            v *= -1;
            return;
        }
        else
        {
            return;
        }
    }
}

struct hash_vector
{
    size_t operator()(const Eigen::VectorXi& v) const
    {
        std::size_t seed = v.size();
        for(uint32_t n = 0; n < v.size(); ++n)
        {
            seed ^= abs(v(n)) + 0x9e3779b9 + (seed << 6U) + (seed >> 2U);
        }
        return seed;
    }
};

inline int powi(int base, unsigned int exp)
{
    int res = 1;
    while(exp != 0U)
    {
        if((exp & 1U) != 0U)
        {
            res *= base;
        }
        exp >>= 1U;
        base *= base;
    }
    return res;
}

template<typename UINT, template<typename> class Basis>
Eigen::MatrixXd basisMatrix(const Basis<UINT>& basis)
{
    Eigen::MatrixXd res = Eigen::MatrixXd::Zero(1U << basis.getN(), basis.getDim());
    for(unsigned int n = 0; n < basis.getDim(); ++n)
    {
        auto bvec = basis.basisVec(n);
        for(const auto p : bvec)
        {
            res(p.first, n) = p.second;
        }
    }
    return res;
}

template<typename Basis> Eigen::VectorXd flip(Basis&& basis, const Eigen::VectorXd& r)
{
    Eigen::VectorXd res(r.size());
    for(int i = 0; i < r.size(); i++)
    {
        res(basis.flip(i)) = r(i);
    }
    return res;
}

inline void TestBasisMatrix(const Eigen::MatrixXd& r)
{
    using Catch::WithinAbs;
    Eigen::MatrixXd id = Eigen::MatrixXd::Identity(r.cols(), r.cols());
    REQUIRE_THAT((r.transpose() * r - id).cwiseAbs().maxCoeff(), WithinAbs(0.0, 1e-8));
}

Eigen::SparseMatrix<double> getSX();
Eigen::SparseMatrix<std::complex<double>> getSY();
Eigen::SparseMatrix<double> getSZ();
Eigen::SparseMatrix<double> getSXXYY();
Eigen::SparseMatrix<double> getSXX();
Eigen::SparseMatrix<double> getSYY();
Eigen::SparseMatrix<double> getSZZ();


////////////////////////////////////////////////////////////////////////////////////////////////
////                     Population Dynamics single cell simulations                       /////
////                          (For predtermined SNAIL levels)				               /////
////                Codes adopted from Tripathi et. al. 2020 PLOS Comp Bio                 /////
////                Modification done by Jain et al., Date: 19th Jan 2022                  /////
////////////////////////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <cmath>
#include <ctime>
#include <random>
#include <string>
#include <fstream>
#include <boost/array.hpp>
#include <boost/unordered_map.hpp>
#include <boost/numeric/odeint.hpp>

#define NUMNODES 4

typedef boost::array<double, NUMNODES> cell_state;
typedef boost::unordered_map<int, boost::array<double, NUMNODES>> population;

std::mt19937 generator; // for random number generation

double temp_var[13];  // temp_var hold the value for initialization parameters provided at start

//////////////////////// k combination out of n //////////////////////////////
double nchoosek(int n, int k)
{
	double result = 1.0;
	double result0 = 1.0;

	for(int i = 0; i < k; i++)
	{
		result *= (n - i);
	}
	for(int i = 1; i <= k; i++)
	{
		result0 *= i;
	}

	return(result / result0);
}
///////////////////////////////////////////////////////////////////////////////////

// function to break a string and separate numeral character based on the delimiter
void tokenize(std::string s, std::string del = " ")
{
	int start = 0;
	int end = s.find(del);
	int i = 0;
	while (end != -1) {
		//std::cout << s.substr(start, end - start) << std::endl;
		temp_var[i] = std::stod(s.substr(start, end - start));
		//std::cout << temp_var[i] << std::endl;
		start = end + del.size();
		end = s.find(del, start);
		i++;
	}
	//std::cout << s.substr(start, end - start);
	temp_var[i] = std::stod(s.substr(start, end - start));
	//std::cout << temp_var[i];
}
///////////////////////////////////////////////////////////////////////////////////

/////////////////// EMT core circuit ODEs (updates a cells' state) ////////////////
void EMT_system(const cell_state &x, cell_state &dxdt, double t)
{
	double ku200 = 0.05, kmz = 0.5, kz = 0.1;

	// Transcription rate:
	double gu200 = 2100, gmz = 11, gz = 100;

	// Hills function threshold :
	double z0u200 = 220000, z0mz = 25000, s0u200 = 180000, s0mz = 180000, u2000 = 10000;

	// Cooperativity:
	double nzu200 = 3, nsu200 = 2, nzmz = 2, nsmz = 2, nu200 = 6;

	// fold change
	double lamdazu200 = 0.1, lamdasu200 = 0.1, lamdazmz = 7.5, lamdasmz = 10;

	double Mu0=1/std::pow((1+x[0]/u2000),nu200);
	double Mu1=std::pow((x[0]/u2000),1)/std::pow((1+x[0]/u2000),nu200);
	double Mu2=std::pow((x[0]/u2000),2)/std::pow((1+x[0]/u2000),nu200);
	double Mu3=std::pow((x[0]/u2000),3)/std::pow((1+x[0]/u2000),nu200);
	double Mu4=std::pow((x[0]/u2000),4)/std::pow((1+x[0]/u2000),nu200);
	double Mu5=std::pow((x[0]/u2000),5)/std::pow((1+x[0]/u2000),nu200);
	double Mu6=std::pow((x[0]/u2000),6)/std::pow((1+x[0]/u2000),nu200);

	double Hillszu200=(1+lamdazu200*std::pow((x[2]/z0u200),nzu200))/(1+std::pow((x[2]/z0u200),nzu200));
	double Hillssu200=(1+lamdasu200*std::pow((x[3]/s0u200),nsu200))/(1+std::pow((x[3]/s0u200),nsu200));
	double Hillszmz=(1+lamdazmz*std::pow((x[2]/z0mz),nzmz))/(1+std::pow((x[2]/z0mz),nzmz));
	double Hillssmz=(1+lamdasmz*std::pow((x[3]/s0mz),nsmz))/(1+std::pow((x[3]/s0mz),nsmz));

	dxdt[0] = gu200*Hillszu200*Hillssu200-x[1]*(0.005*6*Mu1+2*0.05*15*Mu2+3*0.5*20*Mu3+4*0.5*15*Mu4+5*0.5*6*Mu5+6*0.5*Mu6)-ku200*x[0];
	dxdt[1] = gmz*Hillszmz*Hillssmz-x[1]*(0.04*6*Mu1+0.2*15*Mu2+20*Mu3+15*Mu4+6*Mu5+Mu6)-kmz*x[1];
	dxdt[2] = gz*x[1]*(Mu0+0.6*6*Mu1+0.3*15*Mu2+0.1*20*Mu3+0.05*15*Mu4+0.05*6*Mu5+0.05*Mu6)-kz*x[2];
	dxdt[3] = 0;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////// to assign phenotypes to cells /////////////////////////
void get_phenotypes(population &P, boost::unordered_map <int, int> &phenotype)
{
	double x1 = 193.2, x2 = 208.7, y1 = 243.09, y2 = 90.93;
	double a1 = y2 - y1, b1 = x1 - x2, c1 = x2*y1 - y2*x1;

	double u1 = 185.12, u2 = 224.67, v1 = 698.395, v2 = 495.802;
	double a2 = v2 - v1, b2 = u1 - u2, c2 = u2*v1 - v2*u1;

	double x, y, fac1, fac2;
	int state = -1;

	for(int i = 0; i < P.size(); i++)
	{
		x = P[i][3] / 1e3;
		y = P[i][1];
		state = -1;

		if(x < u1)
		{
			state = 0;
		}
		else if(x > u2)
		{
			state = 2;
		}

		else
		{
			fac1 = (a1*x + b1*y + c1) / b1;
			fac2 = (a2*x + b2*y + c2) / b2;
			if(fac2 >= 0)
			{
				state = 2;
			}
			else if(fac1 < 0)
			{
				state = 0;
			}
			else
				state = 1;

		}

		if(state == -1)
		{
			std::cout << "Error in phenotype assignment." << "\n";
		}
		else
		{
			phenotype[i] = state;
		}
	}
}

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

//// To initialize cellular states of the initial population
void initialize_Signal_lognormal(population& P, double* pop_fraction, const int pop_size, const double SNAIL)
{

	//std::normal_distribution <> dist{ 0.0, 1.0 };

	//std::vector<int> V[3];

	int flag = 0;
	//const unsigned int N = 100000;
	double I = SNAIL;

	/*double CV2 = 1.0;
	double SD = std::sqrt(std::log(CV2 * CV2 + 1.0));
	double M = 200e3;
	*/

	boost::unordered_map <int, int> phenotype;
	boost::array<int, 4> count = { 0,0,0,0 };

	population temp_P, temp_P_vector;
	std::ifstream steady_states, eigen_values;

	steady_states.open("steady_states.csv");
	eigen_values.open("eigen_values.csv");
	std::string line;

// load steady states matrix (save steady_states.csv in working directory)////

	int num_states = 0;

	while(getline(steady_states,line)){

		//std::cout<<line;
		tokenize(line,",");

		for(int i = 0; i < NUMNODES; i++){
			temp_P[num_states][i] = temp_var[i];
		}
			num_states++;
	}

// to load eigen values matrix (save steady_states.csv in working directory) ////

	num_states = 0;

	while(getline(eigen_values,line)){

		//std::cout<<line;
		tokenize(line,",");

		for(int i = 0; i < NUMNODES; i++){
			temp_P_vector[num_states][i] = temp_var[i];
		}
			num_states++;
	}
	///////////////////////////////////////////////////////////////

	steady_states.close();
	eigen_values.close();

	//////// to initialize cells based on random generated SNAIL levels and population fraction.///
	///// The logic is to search for nearest SNAIL value, compared to sampled I, in steady states and eigen values matrices
	/// and initialize cells with miR200, mZEB, and ZEB variable vector at that SNAIL value

	int first_time = 1, min_indx = 0, selected_P_indx = 0 ;
	double abs_diff = 0, diff = 0;

	while (count[3] < (pop_size))
	{

		if(I <= temp_P[temp_P.size()-1][3]){
			first_time = 1;
			min_indx = 0;
			selected_P_indx = 0 ;
			abs_diff = 0;
			diff = 0;

			population selected_P;

			for(int g = 0; g < temp_P.size(); g++){

				if(temp_P[g][3] - I >= 0){
					abs_diff = temp_P[g][3] - I;
				}
				else
				{
					abs_diff = -(temp_P[g][3] - I);
				}

				if(abs_diff<= 100 && temp_P_vector[g][0] < 0 && temp_P_vector[g][1] < 0 && temp_P_vector[g][2] < 0){

					if(first_time==1){
						diff = abs_diff;
						min_indx = g;
						first_time = 2;
					}
					else{
						if(abs_diff < diff)
							min_indx = g;
					}

				}
				else if(first_time == 2)
				{

				for(int i = 0; i < NUMNODES; i++){
					selected_P[selected_P_indx][i] = temp_P[g][i];

					/*std::cout<< selected_P[selected_P_indx][i] << " ";

					if(i ==  NUMNODES-1)
						std::cout<<"\n";*/

				}

				selected_P_indx++;
				first_time = 1;

				}
			}

			get_phenotypes(selected_P,phenotype);
			/*std::cout << selected_P.size() << "\n";

			for(int i = 0; i < phenotype.size(); i++){
				std::cout<<phenotype[i] << "\n";
			}*/

			for(int i = 0; i < selected_P.size();i++){

				if (phenotype[i] == 0 && count[0] < pop_size * pop_fraction[0]) {
					for(int j = 0; j < NUMNODES; j++){
						P[count[3]][j] = selected_P[i][j];

						/*std::cout<< selected_P[i][j] << " ";

						if(j ==  NUMNODES-1)
							std::cout<<"\n";*/
					}
					count[0]++;

				}
				else if (phenotype[i] == 1 && count[1] < pop_size * pop_fraction[1]){
					for(int j = 0; j < NUMNODES; j++){
						P[count[3]][j] = selected_P[i][j];

						/*std::cout<< selected_P[i][j] << " ";

						if(j ==  NUMNODES-1)
							std::cout<<"\n";*/

					}
					count[1]++;

				}
				else if (phenotype[i] == 2 && count[2] < pop_size * pop_fraction[2]){
					for(int j = 0; j < NUMNODES; j++){
						P[count[3]][j] = selected_P[i][j];
						/*std::cout<< selected_P[i][j] << " ";

						if(j ==  NUMNODES-1)
							std::cout<<"\n";*/
					}
					count[2]++;

				}

				count[3] = count[0] + count[1] + count[2];

			}

			selected_P.clear();
			phenotype.clear();
		}
	}
	/////////////////////////////////////////////////////////////////////////////////////////////////////

}

///// uniform sampling a cell from a subpopulation////////////////
int get_sample(boost::unordered_map <int, int> &phenotype, int pid)
{
	std::vector<int> V;
	int count = 0;
	for(int i = 0; i < phenotype.size(); i++)
	{
		if(phenotype[i] == pid)
		{
			V.push_back(i);
			count += 1;
		}
	}

	std::uniform_int_distribution<int> distribution(0, count - 1);

	return(V[distribution(generator)]);
}
//////////////////////////////////////////////////////////////////////

////////////////// adding duplication and partitioning noise/fluctuation on cell division //////////
void add_sym_noise_normal(population &P, int rep_id, int new_id, double * eta, int eta_id)
{
	std::normal_distribution <> distribution{0, 1};

	std::uniform_real_distribution <> distribution_unifrom{0.2, 0.4};

	double temp_snail[2];

	double rand_num[2];

	rand_num[0] =  distribution(generator);
	rand_num[1] =  distribution(generator);

	//do{
	if(eta_id == 0){
	        temp_snail[0] = P[rep_id][3] + distribution(generator)*distribution_unifrom(generator)*P[rep_id][3];
	        temp_snail[1] = P[rep_id][3] + distribution(generator)*distribution_unifrom(generator)*P[rep_id][3];
        }
        else{
        	temp_snail[0] = P[rep_id][3] + rand_num[0]*eta[0]*P[rep_id][3]/2 + rand_num[1]*eta[1]*(2*P[rep_id][3] + rand_num[0]*eta[0]*P[rep_id][3]) ;
	        temp_snail[1] = P[rep_id][3] + rand_num[0]*eta[0]*P[rep_id][3]/2 - rand_num[1]*eta[1]*(2*P[rep_id][3] + rand_num[0]*eta[0]*P[rep_id][3]) ;
        }

        //std::cout << temp_snail[0] << " " << temp_snail[1];

	//}while(temp_snail[0] <= 0.0 || temp_snail[1] <=0.0);

	P[rep_id][3] = temp_snail[0];
	P[new_id][3] = temp_snail[1];

        if(P[rep_id][3] < 0.0)
        {
                P[rep_id][3] = 0.0;
        }
        if(P[new_id][3] < 0.0)
        {
                P[new_id][3] = 0.0;
        }

}

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

////////// sampling a fraction of cells from a subpopulation when overall population size increase 80% of carrying capacity K
int FACS(population &P, int N, int type_start, std::vector <int> &cell_index)
{
	boost::unordered_map <int, int> phenotype;
	get_phenotypes(P, phenotype);
	std::vector<int> usefulIndex;
	int count = 0, flag = 1;

	for(int i = 0; i < phenotype.size(); i++)
	{
		if(phenotype[i] == type_start)
		{
			usefulIndex.push_back(i);
			count += 1;
		}
	}
	if(count < N)
	{
		std::cout << "Not enough cells of type" << std::to_string(type_start) << " present in the culture." << "\n";
		flag = 0;
		return(flag);
	}

	std::uniform_int_distribution<int> distribution(0, usefulIndex.size() - 1);

	int size = 0;
	int index = 0;
	while(size < N)
	{
		//index = usefulIndex[distribution(generator)];

		index = usefulIndex[size];
		cell_index.push_back(index);

		/*sorted_P[size+cell_count] = {0.0, 0.0, 0.0, 0.0};
		for(int i = 0; i < NUMNODES; i++)
		{
			sorted_P[size+cell_count][i] = P[index][i];
		}*/
		size += 1;
	}

	return(flag);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////

////////////////////////// population dynamics ////////////////////////////////////////////////////////////
int simulate_normal(population& P, double end_time, double * eta, int eta_id,int frac_id, int sim_type, boost::array <double, 3> GR, int GR_ratio_id, int pop_size, int sim_num, int SNAIL_id)
{
	boost::unordered_map <int, double> update_time;
	boost::unordered_map <int, int> phenotype;
	boost::array<double, 3> count;
	//boost::array<int, 3> cell_count;
	std::vector <int> cell_index;

	double temp_count[3];
	double day_indx = 1;

	for(int i = 0; i < P.size(); i++)
	{
		update_time[i] = 0.0;
	}

	int last_index = P.size();

	get_phenotypes(P, phenotype);

	//std::cout << GR[0] << " " << GR[1] << " " << GR[2] << "\n";

	boost::array <double, 3> growth;
	growth[0] = std::log(2) / GR[0];
	growth[1] = std::log(2) / GR[1];
	growth[2] = std::log(2) / GR[2];
	boost::array<double, 3> r0 = {growth[0], growth[1], growth[2]};
	boost::array<double, 3> d0 = {growth[0] / 10.0, growth[1] / 10.0, growth[2] / 10.0};
	boost::array<double, 6> rates = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

	int K = 5000;

	if(sim_type == 1)
	{
		K = 2000;
	}

	for(int i = 0; i < P.size(); i++)
	{
		if(phenotype[i] == 0)
		{
			if(P.size() < K)
			{
				rates[0] += r0[0]*(1 - float(P.size()) / K);
			}
			rates[3] += d0[0];
		}
		else if(phenotype[i] == 1)
		{
			if(P.size() < K)
			{
				rates[1] += r0[1]*(1 - float(P.size()) / K);
			}
			rates[4] += d0[1];
		}
		else
		{
			if(P.size() < K)
			{
				rates[2] += r0[2]*(1 - float(P.size()) / K);
			}
			rates[5] += d0[2];
		}
	}

	std::uniform_real_distribution<double> distribution(0.0, 1.0);

	double t = 0.0, a0, dt, p0, p1;
	double sum_prev, sum_new;
	double last_updated = 0.0;
	int event_id, rep_id, death_id;

	std::ofstream end_of_day, SNAIL_distribution, mZEB_distribution;

	if(sim_type == 1){

		count = {0.0, 0.0, 0.0};
		for(int i = 0; i < P.size(); i++)
		{
			count[phenotype[i]] += 1.0;
		}
		for(int i = 0; i < 3; i++)
		{
			count[i] = count[i] / P.size();
		}

		// Below, 'E_frac_spread_Data_20hrs_DT_4_weeks' is the folder name in which output data files will be stored.
		// Create it in current working directory before running the simulation.
		end_of_day.open("E_frac_spread_Data_20hrs_DT_4_weeks/end_of_day_counts_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" + std::to_string(GR_ratio_id) + "_" + std::to_string(SNAIL_id) + ".csv", std::ios::app);

		//SNAIL_distribution.open("E_frac_spread_SNAIL_dist_20hrs_DT_4_weeks/snail_dist_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" + std::to_string(GR_ratio_id) + "_" + std::to_string(SNAIL_id) + "_" + std::to_string(sim_num) + ".csv");

		//mZEB_distribution.open("E_frac_spread_ZEB_dist_20hrs_DT_4_weeks/mZEB_dist_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" + std::to_string(GR_ratio_id) + "_" + std::to_string(SNAIL_id) + "_" + std::to_string(sim_num) + ".csv");

		/*for(int i = 0; i < P.size(); i++){
			if(i < P.size() -1){
				SNAIL_distribution << P[i][3] << ",";
				mZEB_distribution << P[i][1] << ",";
			}
			else{
				SNAIL_distribution << P[i][3] << "\n";
				mZEB_distribution << P[i][1] << "\n";
			}
		}*/
		//phenotype_switch.open("phenotype_switch/end_of_day_counts_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" + std::to_string(GR_ratio_id) + "_" + std::to_string(sim_num) + ".csv", std::ios::app);
	}

	while(t <= end_time)
	{
		a0 = 0.0;

		for(int i = 0; i < 6; i++)
		{
			a0 += rates[i];
		}

		p0 = distribution(generator);
		dt = (1.0 / a0)*std::log(1 / p0);
		t += dt;

		p1 = distribution(generator);
		sum_prev = 0.0;
		sum_new = 0.0;
		event_id = -1;

		for(int i = 0; i < 6; i++)
		{
			sum_new = sum_prev + rates[i] / a0;
			if(p1 >= sum_prev && p1 < sum_new)
			{
				event_id = i;
				break;
			}
			sum_prev = sum_new;
		}

		if(event_id == 0)
		{
			rep_id = get_sample(phenotype, 0);
		}
		else if(event_id == 1)
		{
			rep_id = get_sample(phenotype, 1);
		}
		else if(event_id == 2)
		{
			rep_id = get_sample(phenotype, 2);
		}
		else if(event_id == 3)
		{
			death_id = get_sample(phenotype, 0);
		}
		else if(event_id == 4)
		{
			death_id = get_sample(phenotype, 1);
		}
		else if(event_id == 5)
		{
			death_id = get_sample(phenotype, 2);
		}

		if(event_id < 3)
		{

			boost::numeric::odeint::integrate(EMT_system, P[rep_id], 0.0, t - update_time[rep_id], 0.1);

			P[last_index] = {0.0, 0.0, 0.0, 0.0};  // last index holds the length of Population Array vector
			for(int j = 0; j < NUMNODES; j++)
			{
				P[last_index][j] = P[rep_id][j];
			}

			add_sym_noise_normal(P, rep_id, last_index, eta, eta_id);

			update_time[rep_id] = t;
			update_time[last_index] = t;
			last_index += 1;
		}
		else
		{
			for(int j = 0; j < NUMNODES; j++)
			{
				P[death_id][j] = P[last_index - 1][j];
			}
			update_time[death_id] = update_time[last_index - 1];
			phenotype[death_id] = phenotype[last_index - 1];
			P.erase(last_index - 1);
			update_time.erase(last_index - 1);
			phenotype.erase(last_index - 1);
			last_index -= 1;
		}

		if(P.size()==0)
		{
			if(sim_type == 1)
			{
				end_of_day.close();
				//SNAIL_distribution.close();
				//mZEB_distribution.close();
			}

			return(1);
		}

		if(true)
		{
			for(int i = 0; i < P.size(); i++)
			{
				if(t - update_time[i] > 0.0)
				{
					boost::numeric::odeint::integrate(EMT_system, P[i], 0.0, t - update_time[i], 0.1);
				}
				update_time[i] = t;
			}
		}

		get_phenotypes(P, phenotype);

		/// reducing population size by sampling different subpopulations as per their last distribution

		if(P.size() > 0.8 * K && sim_type == 1){

			count = {0.0, 0.0, 0.0};

			for(int i = 0; i < P.size(); i++)
			{
				count[phenotype[i]] += 1.0;
			}

			for(int i = 0; i < 3; i++)
			{
				count[i] = count[i] / P.size();

				double b = 0; // to truncate the double to two decimal places

				for (double k = 0; k<=100; k++)
				{
					if(k > 100*count[i])
					{
						b = 100*count[i] - (k-1);
						count[i] = count[i] - b/100;
						break;
					}
				}

				//std::cout << count[i] << "\n";

				int flag = FACS(P,pop_size*count[i],i,cell_index);
				if (flag == 0){
					std::cout <<" stopped simulation due to non-availability of cells\n";
					return(-1);
				}
			}

			std::sort(cell_index.begin(),cell_index.end());

			//for (int i = 0; i< cell_index.size(); i++) std::cout << cell_index[i] << "\n";
			//std::cout<< cell_index.size()<<"\n";

			for (int i = 0; i < cell_index.size(); i++){
					if(i != cell_index[i]){
						for(int j = 0; j < NUMNODES; j++){
							P[i][j] = P[cell_index[i]][j];
							//std::cout << P[i][j] << " ";
						}
						phenotype[i] = phenotype[cell_index[i]];
					}
					update_time[i] = t;

					//std::cout<<"\n";
			}
			for (int i = P.size()-1; i >= cell_index.size(); i--)
			{
				P.erase(i);
				update_time.erase(i);
				phenotype.erase(i);
			}
			last_index = P.size();

			/*std::cout << "\nsize of the sampled pop is " << last_index <<"\n";

			for (int i = 0; i < P.size(); i++)
			{
				std::cout << P[i][1] << " " << P[i][3] << "\n";
			}*/

			cell_index.clear();

		}
		///////////////////////////////////////////////////////////////////////////////////////

		count = {0.0, 0.0, 0.0};

		for(int i = 0; i < P.size(); i++)
		{
			count[phenotype[i]] += 1.0;
		}

		/*for(int i = 0; i < 3; i++)
		{
			count[i] = count[i] / P.size();
		}*/

		rates = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

		for(int i = 0; i < P.size(); i++)
		{
			if(phenotype[i] == 0)
			{
				if(P.size() < K)
				{
					rates[0] += r0[0]*(1 - float(P.size()) / K);
				}
				rates[3] += d0[0];
			}
			else if(phenotype[i] == 1)
			{
				if(P.size() < K)
				{
					rates[1] += r0[1]*(1 - float(P.size()) / K);
				}
				rates[4] += d0[1];
			}
			else
			{
				if(P.size() < K)
				{
					rates[2] += r0[2]*(1 - float(P.size()) / K);
				}
				rates[5] += d0[2];
			}
		}

		if(sim_type == 1)
		{

			if (t < 24* day_indx) {
				temp_count[0] = count[0];
				temp_count[1] = count[1];
				temp_count[2] = count[2];

			}
			else {

				//std::cout << day_indx << "," << temp_count[0] << "," << temp_count[1] << "," << temp_count[2] << "," << P.size() << "\n";

				//// the phenotypic distribution is stored each day in absolute cell counts ////

				end_of_day << day_indx << "," << temp_count[0] << "," << temp_count[1] << "," << temp_count[2] << "," << (P.size()-1) << "\n";

				/*if((int(day_indx)%14) == 0){ // save distribution data every four weeks
					for(int i = 0; i < P.size(); i++){
						if(i < P.size() -1){
							SNAIL_distribution << P[i][3] << ",";
							mZEB_distribution << P[i][1] << ",";
						}
						else{
							SNAIL_distribution << P[i][3] << "\n";
							mZEB_distribution << P[i][1] << "\n";
						}
					}
				}*/

				day_indx++;

				temp_count[0] = count[0];
				temp_count[1] = count[1];
				temp_count[2] = count[2];

			}
		}
	}

	if(sim_type == 1)
	{
		end_of_day.close();
	//	SNAIL_distribution.close();
	//	mZEB_distribution.close();
	}

	return(0);
}

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

	tokenize(argv[1], "_"); /// to separate parameters from input string

	boost::unordered_map <int, int> phenotype;
	boost::array <double, 3> GR; // doubling time of the three population
	std::vector <int> cell_index; // used with FACS
	int aborted;

	int count[3];

	// order the input parameter values delimited by '_' as follows:
// e.g. 1_0.2_0.1_0_1_0_0_0_20_20_20_0_50000
	int eta_id = temp_var[0];
	double eta[2] = { temp_var[1], temp_var[2] };
	int frac_id = temp_var[3];
	double pop_fraction[3] = { temp_var[4], temp_var[5], temp_var[6] };
	int GR_ratio_id = temp_var[7];
	GR[0] = temp_var[8];
	GR[1] = temp_var[9];
	GR[2] = temp_var[10];
	int SNAIL_id = temp_var[11];
	double SNAIL = temp_var[12];

	int sim_num = 1;
	int pop_size = 200;
	int total_inde_runs = 50;
	int time_in_days = 4 * 7 ; // 4 respresent 4 weeks
	int file_rows = 0;

	std::ifstream csv_file;

	std::string line;

	while(true){

		//// check how many runs of data is stored in output file
		csv_file.open("E_frac_spread_Data_20hrs_DT_32_weeks/end_of_day_counts_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" +
			std::to_string(GR_ratio_id) + "_"  + std::to_string(SNAIL_id) + ".csv");

		/*if(csv_file.is_open())
		{
			std::cout<<"file is correctly opened\n";
		}*/

		file_rows = 0;
		while(getline(csv_file,line)){
			file_rows++;
		}

		csv_file.close();
		//std::cout << file_rows << "\n";

		/////////// don't proceed if file has data of required number of runs or incomplete previous simulation data
		if(file_rows >= (total_inde_runs * time_in_days) || (file_rows%time_in_days) != 0){

			if(file_rows%time_in_days != 0)
				std::cout << "stopped bacause of already missing data \n";
			else if(file_rows > (total_inde_runs * time_in_days))
				std::cout << total_inde_runs * time_in_days << " enough data already present\n";
			break;
		}
		//////////////////////////////////////////////////////////////////////////////////////

		generator = std::mt19937(std::time(NULL) + file_rows);

		population P;

		initialize_Signal_lognormal(P, pop_fraction, 1, SNAIL); // initial pop size as 1 for single cell simualtion

		/*for(int i = 0; i < P.size(); i++){

			for(int j = 0; j < NUMNODES; j++){
				std::cout << P[i][j] << " ";
			}
			std::cout<<"\n";
		}*/

		std::cout << "initialization complete for eta id " << eta_id << " frac_id " << frac_id << " GR ratio id " << GR_ratio_id << " SNAIL_id " << SNAIL_id <<" and sim num is " << sim_num << std::endl;

		aborted = simulate_normal(P, 24 * time_in_days, eta, eta_id, frac_id, 1, GR, GR_ratio_id, pop_size, sim_num, SNAIL_id);

		if(aborted == 1)
		{
			std::cout<<"the simulation is aborted due to extinction of population\n";
		}else{

			std::cout << "simulation complete for eta id " << eta_id << " frac_id " << frac_id << " GR ratio id " << GR_ratio_id << " SNAIL_id " << SNAIL_id <<" and sim num is " << sim_num << std::endl;
			sim_num++;
		}

		P.clear();
	}

	return(0);

}


#define _USE_MATH_DEFINES
#include <cmath>
#include "Window.h"
#include "godec/ComponentGraph.h"
#include <boost/format.hpp>
#include <iomanip>

namespace Godec {

WindowComponent::~WindowComponent() {
}

LoopProcessor* WindowComponent::make(std::string id, ComponentGraphConfig* configPt) {
    return new WindowComponent(id, configPt);
}
std::string WindowComponent::describeThyself() {
    return "Chops incoming audio stream into windowed audio features";
}
WindowComponent::WindowComponent(std::string id, ComponentGraphConfig* configPt) :
    LoopProcessor(id,configPt) {
    addInputSlotAndUUID(SlotStreamedAudio, UUID_AudioDecoderMessage);

    std::list<std::string> requiredOutputSlots;
    requiredOutputSlots.push_back(SlotWindowedAudio);
    initOutputs(requiredOutputSlots);

    /* initialize sampling parameters for the current waveform */
    mLowLatency = configPt->get<bool>("low_latency", "Low-latency mode (assumes never-ending audio, can not be used in offline where utterances end!)");
    mSamplingRate = configPt->get<float>("sampling_frequency", "Source sampling rate");
    float local_fRate = configPt->get<float>("analysis_frame_step_size", "Analysis frame step size");
    float local_winDur = configPt->get<float>("analysis_frame_size", "Analysis frame size in milliseconds");
    std::string windowingFunction = configPt->get<std::string>("windowing_function", "Windowing function (hamming,blackman, rectangle)");
    windowSize = (int)round(0.001 * mSamplingRate * local_winDur);    /* points in window */
    stepSize = (int)round(0.001 * mSamplingRate * local_fRate);     /* points per shift */
    window = Vector(windowSize);
    if (windowingFunction == "hamming") {
        for(int idx = 0; idx < windowSize; idx++) {
            window(idx) = 0.54-0.46*cos(2.0*M_PI/(windowSize-1));
        }
    } else if (windowingFunction == "rectangle") {
        window.setConstant(1.0);
    } else {
        GODEC_ERR << "Not implemented yet!";
    }
    zeroMean = AccumCovariance::make(1, Diagonal, true, false);

    mUttReceivedAudio = 0;
    mUttStartStreamOffset = 0;
    mAccumAudioOffsetInUtt = 0;
    mProcessPointerInAccumAudio = 0;
}

void WindowComponent::ProcessMessage(const DecoderMessageBlock& msgBlock) {
    auto convStateMsg =msgBlock.get<ConversationStateDecoderMessage>(SlotConversationState);
    auto audioMsg =msgBlock.get<AudioDecoderMessage>(SlotStreamedAudio);
    if (mSamplingRate != audioMsg->mSampleRate) {
        GODEC_ERR << getLPId() << ": Expected sampling rate " << mSamplingRate << ", got " << audioMsg->mSampleRate << std::endl;
    }

    float ticksPerSample = audioMsg->mTicksPerSample;
    const Vector& audio = audioMsg->mAudio;
    mAccumAudio.conservativeResize(mAccumAudio.size()+audio.size());
    mAccumAudio.tail(audio.size()) = audio;
    mUttReceivedAudio += audio.size();

    int audioHoldoff = (mLowLatency || convStateMsg->mLastChunkInUtt) ? 0 : stepSize; // Problem is, we need to keep at least one window of audio around so that the last message can be tagged with the EOM boolean
    int nFrames = std::max(0.0, floor(((int)mAccumAudio.size() - mProcessPointerInAccumAudio - audioHoldoff) / (double)stepSize));
    if (nFrames == 0) {
        if (!convStateMsg->mLastChunkInUtt) return;
        else if (mLowLatency) GODEC_ERR << "You ran in low-latency mode but ended the utterance, this is not allowed";
        else GODEC_ERR << "We should never end up here";
    }

    Matrix outMat(windowSize, nFrames);
    std::vector<uint64_t> outTimestamps;
    int frameIdx = -1;

    Vector rawAudioSnippet(windowSize);
    while ((mProcessPointerInAccumAudio + stepSize) <= ((int)mAccumAudio.size() - audioHoldoff)) {
        frameIdx++;
        mProcessPointerInAccumAudio += stepSize;

        rawAudioSnippet.setZero();
        int64_t pickupStart = std::max((int64_t)0,mProcessPointerInAccumAudio-windowSize);
        int64_t pickupSize = mProcessPointerInAccumAudio-pickupStart;
        rawAudioSnippet.tail(mProcessPointerInAccumAudio-pickupStart) = mAccumAudio.segment(pickupStart, pickupSize);

        zeroMean->reset();
        zeroMean->addData(rawAudioSnippet);
        Vector normAudio = zeroMean->normalize(rawAudioSnippet);
        Vector filteredAudio = window.cwiseProduct(normAudio);

        outMat.col(frameIdx) = filteredAudio;
        uint64_t frameTimestamp = mUttStartStreamOffset + (int64_t)round(ticksPerSample*(mProcessPointerInAccumAudio+mAccumAudioOffsetInUtt));
        outTimestamps.push_back(frameTimestamp);
    }

    if (outTimestamps.size() != nFrames) {
        GODEC_ERR << "Number of frames was not estimated correctly. " << outTimestamps.size() << " vs " << nFrames << std::endl;
    }

    if (convStateMsg->mLastChunkInUtt && outTimestamps.size() > 0) {
        outTimestamps.back() = convStateMsg->getTime();
    }

    boost::format fmter("WINAUDIO[0:%1%]%%f");
    fmter % (windowSize - 1);

    DecoderMessage_ptr featMsg = FeaturesDecoderMessage::create(
                                     outTimestamps.back(), convStateMsg->mUtteranceId,
                                     outMat, fmter.str(), outTimestamps);
    if (audioMsg->getDescriptor("vtl_stretch") != "") {
        (boost::const_pointer_cast<DecoderMessage>(featMsg))->addDescriptor("vtl_stretch", audioMsg->getDescriptor("vtl_stretch"));
    }
    pushToOutputs(SlotWindowedAudio, featMsg);

    if (mProcessPointerInAccumAudio > 10*stepSize) {
        int64_t framesToRemove = mAccumAudio.size()-2*windowSize;
        mAccumAudio = (Vector)mAccumAudio.tail(mAccumAudio.size()-framesToRemove);
        mProcessPointerInAccumAudio -= framesToRemove;
        mAccumAudioOffsetInUtt += framesToRemove;
    }

    if (convStateMsg->mLastChunkInUtt) {
        mUttStartStreamOffset = featMsg->getTime()+1;
        mProcessPointerInAccumAudio = -1;
        mUttReceivedAudio = 0;
        mAccumAudioOffsetInUtt = 0;
        mAccumAudio = Vector();
    }
}

}