/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  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 the Willow Garage 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.
 *********************************************************************/

//
// Sparse Pose Adjustment classes and functions
//

//
// Ok, we're going with 2-pose constraints, and added scale variables.
// Format for files:
// Format for files:
//   #nodes #scales #p2p_constraints #scale_constraints
//   x y z rx ry rz   ; first node, vector part of unit quaternion
//   ...
//   p1 p2 Lambda_12  ; first p2p constraint
//   ...
//   p1 p2 s1 K_12 w  ; first scale constraint,
//                    ; (p1-p2)^2 = s1*K_12 with weight w

#include <stdio.h>
#include "sparse_bundle_adjustment/sba.h"
#include <Eigen/Cholesky>
#include <chrono>

using namespace Eigen;
using namespace std;

#include <iostream>
#include <iomanip>
#include <fstream>
#include <utility>

// elapsed time in microseconds
static long long utime()
{
  auto duration = std::chrono::system_clock::now().time_since_epoch();
  return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
}

namespace sba
{
  // reads in a file of pose constraints
  bool readP2File(char *fname, SysSPA spa)
  {
    // Format for files:
    //   #nodes #scales #p2p_constraints #scale_constraints
    //   x y z rx ry rz   ; first node, vector part of unit quaternion
    //   ...
    //   p1 p2 Lambda_12  ; first p2p constraint
    //   ...
    //   p1 p2 s1 K_12 w  ; first scale constraint,
    //                    ; (p1-p2)^2 = s1*K_12 with weight w

    ifstream ifs(fname);
    if (!ifs)
      {
        cout << "Can't open file " << fname << endl;
        return false;
      }
    ifs.precision(10);          // what is this for???

    // read header
    string line;
    if (!getline(ifs,line) || line != "# P2 Constraint File")
    {
      cout << "Bad header" << endl;
      return false;
    }
    cout << "Found P2 constraint file" << endl;

    // read number of cameras, scalers, and p2p constraints
    int ncams, nss, nscs, np2s;
    if (!(ifs >> ncams >> nss >> np2s >> nscs))
    {
      cout << "Bad entity count" << endl;
      return false;
    }
    cout << "Number of cameras: " << ncams
         << "  Number of scalers: " << nss
         << "  Number of p2p constraints: " << np2s
         << "  Number of scale constraints: " << nscs << endl;

    cout << "Reading in camera data..." << flush;
    std::vector<Node,Eigen::aligned_allocator<Node> > &nodes = spa.nodes;
    Node nd;

    for (int i=0; i<ncams; i++)
      {
        double v1,v2,v3;
        if (!(ifs >> v1 >> v2 >> v3))
          {
            cout << "Bad node translation params at number " << i << endl;
            return false;
          }
        nd.trans = Vector4d(v1,v2,v3,1.0);

        if (!(ifs >> v1 >> v2 >> v3))
          {
            cout << "Bad node rotation quaternion at number " << i << endl;
            return false;
          }
        Matrix3d m;
        nd.qrot = Quaternion<double>(v1,v2,v2,0.0);
        nd.normRot();           // normalize to unit vector and compute w

        nodes.push_back(nd);
      }
    cout << "done" << endl;

    // read in p2p constraints
    cout << "Reading in constraint data..." << flush;
    std::vector<ConP2,Eigen::aligned_allocator<ConP2> > &cons = spa.p2cons;
    ConP2 con;

    for (int i=0; i<np2s; i++)
      {
        int p1, p2;
        double vz[6];           // mean
        double vv[36];          // prec

        if (!(ifs >> p1 >> p2))
          {
            cout << "Bad node indices at constraint " << i << endl;
            return false;
          }

        for (int i=0; i<6; i++)
          {
            if (!(ifs >> vz[i]))
              {
                cout << "Bad constraint mean at constraint " << i << endl;
                return false;
              }
          }

        for (int i=0; i<36; i++)
          {
            if (!(ifs >> vv[i]))
              {
                cout << "Bad constraint precision at constraint " << i << endl;
                return false;
              }
          }

        con.ndr = p1;
        con.nd1 = p2;

        //        con.setMean(Matrix<double,6,1>(vz));
        con.prec = Matrix<double,6,6>(vv);

        cons.push_back(con);
      }

    // read in scale constraints
    cout << "Reading in scale constraint data..." << flush;
    std::vector<ConScale,Eigen::aligned_allocator<ConScale> > &scons = spa.scons;
    ConScale scon;

    for (int i=0; i<nscs; i++)
      {
        int p1, p2, sv;
        double ks, w;

        if (!(ifs >> p1 >> p2 >> sv >> ks >> w))
          {
            cout << "Bad scale constraint at constraint " << i << endl;
            return false;
          }

        scon.nd0 = p1;
        scon.nd1 = p2;
        scon.sv  = sv;
        scon.ks  = ks;
        scon.w   = w;

        scons.push_back(scon);
      }

    return true;
  }

// Do we want to normalize quaternion vectors based on the sign of w?
#define NORMALIZE_Q

  // set up Jacobians
  // see Konolige RSS 2010 submission for details

  void ConP2::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
  {
    // node references
    Node &nr = nodes[ndr];
    Matrix<double,4,1> &tr = nr.trans;
    Quaternion<double> &qr = nr.qrot;
    Node &n1 = nodes[nd1];
    Matrix<double,4,1> &t1 = n1.trans;
    Quaternion<double> &q1 = n1.qrot;

    // first get the second frame in first frame coords
    Eigen::Matrix<double,3,1> pc = nr.w2n * t1;

    // Jacobians wrt first frame parameters

    // translational part of 0p1 wrt translational vars of p0
    // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
    J0.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p0
    // dR'/dq * [pw - t]
    Eigen::Matrix<double,3,1> pwt;
    pwt = (t1-tr).head(3);   // transform translations

    // dx
    Eigen::Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,3) = dp;
    // dy
    dp = nr.dRdy * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,4) = dp;
    // dz
    dp = nr.dRdz * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,5) = dp;

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J0.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = qpmean * s0' * q0' * q1

    // dqdx
    Eigen::Quaternion<double> qr0, qr1, qrn, qrd;
    qr1.coeffs() = q1.coeffs();
    qrn.coeffs() = Vector4d(-qpmean.w(),-qpmean.z(),qpmean.y(),qpmean.x());  // qpmean * ds0'/dx
    qr0.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
    qr0 = qr0*qr1;              // rotate to zero mean
    qrd = qpmean*qr0;           // for normalization check
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,3) = qrn.vec();

    // dqdy
    qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y());  // qpmean * ds0'/dy
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,4) = qrn.vec();

    // dqdz
    qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z());  // qpmean * ds0'/dz
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,5) = qrn.vec();

    // transpose
    J0t = J0.transpose();

    //  cout << endl << "J0 " << ndr << endl << J0 << endl;

    // Jacobians wrt second frame parameters
    // translational part of 0p1 wrt translational vars of p1
    // this is just R0'  [from 0t1 = R0'(t1 - t0)]
    J1.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p1: zero
    J1.block<3,3>(0,3).setZero();

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J1.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = q0'*s1*q1

    Eigen::Quaternion<double> qrc;
    qrc.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
    qrc = qpmean*qrc*qr1;       // mean' * qr0' * qr1
    qrc.normalize();

    //    cout << endl << "QRC  : " << qrc.coeffs().transpose() << endl;

    double dq = 1.0e-8;
    double wdq = 1.0 - dq*dq;
    qr1.coeffs() = Vector4d(dq,0,0,wdq);
    //    cout << "QRC+x: " << (qrc*qr1).coeffs().transpose() << endl;
    //    cout << "QRdx:  " << ((qrc*qr1).coeffs().transpose() - qrc.coeffs().transpose())/dq << endl;

    // dqdx
    qrn.coeffs() = Vector4d(1,0,0,0);
    qrn = qrc*qrn;

    //    cout << "J1dx:  " << qrn.coeffs().transpose() << endl;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,3) = qrn.vec();

    // dqdy
    qrn.coeffs() = Vector4d(0,1,0,0);
    qrn = qrc*qrn;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,4) = qrn.vec();

    // dqdz
    qrn.coeffs() = Vector4d(0,0,1,0);
    qrn = qrc*qrn;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,5) = qrn.vec();

    // transpose
    J1t = J1.transpose();

    //  cout << endl << "J1 " << nd1 << endl << J1 << endl;

  };

  // set up Jacobians
  // see Konolige RSS 2010 submission for details

  void ConScale::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
  {
    // node references
    Node &n0 = nodes[nd0];
    Matrix<double,4,1> &t0 = n0.trans;
    Node &n1 = nodes[nd1];
    Matrix<double,4,1> &t1 = n1.trans;

    Eigen::Matrix<double,3,1> td = (t1-t0).head(3);

    // Jacobians wrt first frame parameters
    //  (ti - tj)^2 - a*kij

    // scale error wrt translational vars t0
    J0 = -2.0 * td;

    // scale error wrt translational vars t1
    J1 =  2.0 * td;

    // scale error wrt scale variable is just -kij
  };

  //
  // This hasn't been tested, and is probably wrong.
  //

  void ConP3P::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > nodes)
  {
    // node references
    Node nr = nodes[ndr];
    Matrix<double,4,1> &tr = nr.trans;
    Quaternion<double> &qr = nr.qrot;
    Node n1 = nodes[nd1];
    Matrix<double,4,1> &t1 = n1.trans;
    Quaternion<double> &q1 = n1.qrot;
    Node n2 = nodes[nd2];
    Matrix<double,4,1> &t2 = n2.trans;
    Quaternion<double> &q2 = n2.qrot;

    // calculate J10 -> d(0p1)/d(p0)
    // rows are indexed by 0p1 parameters
    // cols are indexed by p0 parameters

    // translational part of 0p1 wrt translational vars of p0
    // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
    J10.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p0
    // dR'/dq * [pw - t]
    Matrix<double,3,1> pwt = (t1-tr).head(3);
    Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dqx * [pw - t]
    J10.block<3,1>(0,3) = dp;
    dp = nr.dRdy * pwt; // dR'/dqy * [pw - t]
    J10.block<3,1>(0,4) = dp;
    dp = nr.dRdz * pwt; // dR'/dqz * [pw - t]
    J10.block<3,1>(0,5) = dp;

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J10.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = q0'*q1

    // dqdx
    double wn = -1.0/qr.w();  // need to switch params for small qr(3)
    dp[0] = wn*t1[0]*qr.x() - t1[3];
    dp[1] = wn*t1[1]*qr.x() + t1[2];
    dp[2] = wn*t1[2]*qr.x() - t1[1];
    J10.block<3,1>(3,3) = dp;

    // dqdy
    dp[0] = wn*t1[0]*qr.y() - t1[2];
    dp[1] = wn*t1[1]*qr.y() - t1[3];
    dp[2] = wn*t1[2]*qr.y() + t1[0];
    J10.block<3,1>(3,4) = dp;

    // dqdz
    dp[0] = wn*t1[0]*qr.z() + t1[1];
    dp[1] = wn*t1[1]*qr.z() - t1[0];
    dp[2] = wn*t1[2]*qr.z() - t1[3];
    J10.block<3,1>(3,5) = dp;

    // whew, J10 done...

    // J20 is similar, just using p2
    // translational part of 0p2 wrt translational vars of p0
    // this is just -R0'  [from 0t2 = R0'(t2 - t0)]
    J20.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);

    // translational part of 0p2 wrt rotational vars of p0
    // dR'/dq * [pw - t]
    pwt = (t2-tr).head(3);
    dp = nr.dRdx * pwt; // dR'/dqx * [pw - t]
    J20.block<3,1>(0,3) = dp;
    dp = nr.dRdy * pwt; // dR'/dqy * [pw - t]
    J20.block<3,1>(0,4) = dp;
    dp = nr.dRdz * pwt; // dR'/dqz * [pw - t]
    J20.block<3,1>(0,5) = dp;

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J20.block<3,3>(3,0).setZero();

    // rotational part of 0p2 wrt rotational vars of p0
    // from 0q1 = q0'*q2

    // dqdx
    wn = -1.0/qr.w();  // need to switch params for small qr(3)
    dp[0] = wn*q2.x()*qr.x() - q2.w();
    dp[1] = wn*q2.y()*qr.x() + q2.z();
    dp[2] = wn*q2.z()*qr.x() - q2.y();
    J20.block<3,1>(3,3) = dp;

    // dqdy
    dp[0] = wn*q2.x()*qr.y() - q2.z();
    dp[1] = wn*q2.y()*qr.y() - q2.w();
    dp[2] = wn*q2.z()*qr.y() + q2.x();
    J20.block<3,1>(3,4) = dp;

    // dqdz
    dp[0] = wn*q2.x()*qr.z() + q2.y();
    dp[1] = wn*q2.y()*qr.z() - q2.x();
    dp[2] = wn*q2.z()*qr.z() - q2.w();
    J20.block<3,1>(3,5) = dp;

    // calculate J11 -> d(0p1)/d(p1)
    // rows are indexed by 0p1 parameters
    // cols are indexed by p1 parameters

    // translational part of 0p1 wrt translational vars of p1
    // this is just R0'  [from 0t1 = R0'(t1 - t0)]
    J11.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p1 => zero
    J11.block<3,3>(0,3).setZero();

    // rotational part of 0p1 wrt translation vars of p1 => zero
    J11.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p1
    // from 0q1 = q0'*q1

    // dqdx1
    wn = 1.0/q1.w();  // need to switch params for small q1(3)
    dp[0] = wn*qr.x()*q1.x() + qr.w();
    dp[1] = wn*qr.y()*q1.x() - qr.z();
    dp[2] = wn*qr.z()*q1.x() + qr.y();
    J11.block<3,1>(3,3) = dp;

    // dqdy1
    dp[0] = wn*qr.x()*q1.y() + qr.z();
    dp[1] = wn*qr.y()*q1.y() + qr.w();
    dp[2] = wn*qr.z()*q1.y() - qr.x();
    J11.block<3,1>(3,4) = dp;

    // dqdz1
    dp[0] = wn*qr.x()*q1.z() - qr.y();
    dp[1] = wn*qr.y()*q1.z() + qr.x();
    dp[2] = wn*qr.z()*q1.z() + qr.w();
    J11.block<3,1>(3,5) = dp;

    // whew, J11 done...

    // J22 is similar to J11
    // rows are indexed by 0p2 parameters
    // cols are indexed by p2 parameters

    // translational part of 0p2 wrt translational vars of p2
    // this is just R0'  [from 0t2 = R0'(t2 - t0)]
    J22.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);

    // translational part of 0p2 wrt rotational vars of p2 => zero
    J22.block<3,3>(0,3).setZero();

    // rotational part of 0p2 wrt translation vars of p2 => zero
    J22.block<3,3>(3,0).setZero();

    // rotational part of 0p2 wrt rotational vars of p2
    // from 0q2 = q0'*q2

    // dqdx2
    wn = 1.0/q2.w();  // need to switch params for small q2.w()
    dp[0] = wn*qr.x()*q2.x() + qr.w();
    dp[1] = wn*qr.y()*q2.x() - qr.z();
    dp[2] = wn*qr.z()*q2.x() + qr.y();
    J22.block<3,1>(3,3) = dp;

    // dqdy1
    dp[0] = wn*qr.x()*q2.y() + qr.z();
    dp[1] = wn*qr.y()*q2.y() + qr.w();
    dp[2] = wn*qr.z()*q2.y() - qr.x();
    J22.block<3,1>(3,4) = dp;

    // dqdz1
    dp[0] = wn*qr.x()*q2.z() - qr.y();
    dp[1] = wn*qr.y()*q2.z() + qr.x();
    dp[2] = wn*qr.z()*q2.z() + qr.w();
    J22.block<3,1>(3,5) = dp;

    // whew, J22 done...

  };

  // error function
  inline double ConP2::calcErr(const Node &nd0, const Node &nd1)
    {
      Quaternion<double> q0p,q1;
      q0p.vec()   = -nd0.qrot.coeffs().head(3); // invert quaternion
      q0p.w()     =  nd0.qrot.w();
      q1          =  nd1.qrot;
      err.block<3,1>(0,0) = nd0.w2n * nd1.trans - tmean;

      //      cout << endl;
      //      cout << q0p.coeffs().transpose() << endl;
      //      cout << q1.coeffs().transpose() << endl;
      //      cout << qpmean.coeffs().transpose() << endl;

      q1 = qpmean * q0p * q1;

      //      cout << q1.coeffs().transpose() << endl << endl;

// this seems to mess up convergence...
#ifdef NORMALIZE_Q
      if (q1.w() < 0.0)
        err.block<3,1>(3,0) = -q1.vec(); // normalized
      else
#endif
        err.block<3,1>(3,0) = q1.vec(); // do we have to normalize w???
      //      cout << endl << "Error: " << err.transpose() << endl << endl;
      return err.dot(prec * err);
    }

  // error function for distance cost
  double ConP2::calcErrDist(const Node &nd0, const Node &nd1)
    {
      Vector3d derr;
      Quaternion<double> q0p,q1;
      q0p.vec()   = -nd0.qrot.vec(); // invert quaternion
      q0p.w()     =  nd0.qrot.w();
      q1          =  nd1.qrot;
      derr = nd0.w2n * nd1.trans - tmean;
      return derr.dot(derr);
    }

  // error function
  inline double ConScale::calcErr(const Node &nd0, const Node &nd1, double alpha)
    {
      err = (nd1.trans - nd0.trans).squaredNorm();
      err -= ks*alpha;
      //      cout << "Scale err: " << err << endl;
      return w*err*err;
    }

  // Adds a node to the system.
  // \return the index of the node added.
  int SysSPA::addNode(Eigen::Matrix<double,4,1> &trans,
                      Eigen::Quaternion<double> &qrot,
                      bool isFixed)
  {
    Node nd;
    nd.trans = trans;
    nd.qrot = qrot;
    nd.isFixed = isFixed;
    nd.setTransform(); // set up world2node transform
    nd.setDr(true); // set rotational derivatives
    // Should this be local or global?
    nd.normRot();//Local();
    nodes.push_back(nd);
    return nodes.size()-1;
  }

  // add a constraint
  // <nd0>, <nd1> are node id's
  // <mean> is x,y,th, with th in radians
  // <prec> is a 3x3 precision matrix (inverse covariance
  // returns true if nodes are found
  bool SysSPA::addConstraint(int nd0, int nd1,
                             Eigen::Vector3d &tmean,
                             Eigen::Quaterniond &qpmean,
                             Eigen::Matrix<double,6,6> &prec)
  {
    if (nd0 >= (int)nodes.size() || nd1 >= (int)nodes.size())
      return false;

    ConP2 con;
    con.ndr = nd0;
    con.nd1 = nd1;

    con.tmean = tmean;
    Quaternion<double> qr;
    qr = qpmean;
    qr.normalize();
    con.qpmean = qr.inverse(); // inverse of the rotation measurement
    con.prec = prec;

    p2cons.push_back(con);
    return true;
  }

  // error measure, squared
  // assumes node transforms have already been calculated
  // <tcost> is true if we just want the distance offsets
  double SysSPA::calcCost(bool tcost)
  {
    double cost = 0.0;

    // do distance offset
    if (tcost)
      {
        for(size_t i=0; i<p2cons.size(); i++)
          {
            ConP2 &con = p2cons[i];
            double err = con.calcErrDist(nodes[con.ndr],nodes[con.nd1]);
            cost += err;
          }
      }

    // full cost
    else
      {
        for(size_t i=0; i<p2cons.size(); i++)
          {
            ConP2 &con = p2cons[i];
            double err = con.calcErr(nodes[con.ndr],nodes[con.nd1]);
            cost += err;
          }
        if (scons.size() > 0)       // can have zero scale constraints
          for(size_t i=0; i<scons.size(); i++)
            {
              ConScale &con = scons[i];
              double err = con.calcErr(nodes[con.nd0],nodes[con.nd1],scales[con.sv]);
              cost += err;
            }
      }

    return cost;
  }

  // Set up linear system, from RSS submission (konolige 2010)
  // This is a relatively compact version of the algorithm!
  // but not Jacobians

  void SysSPA::setupSys(double sLambda)
  {
    // set matrix sizes and clear
    // assumes scales vars are all free
    int nFree = nodes.size() - nFixed;
    int nscales = scales.size();
    A.setZero(6*nFree+nscales,6*nFree+nscales);
    B.setZero(6*nFree+nscales);
    VectorXi dcnt(nFree);
    dcnt.setZero(nFree);

    // lambda augmentation
    double lam = 1.0 + sLambda;

    // loop over P2 constraints
    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        ConP2 &con = p2cons[pi];
        con.setJacobians(nodes);

        // add in 4 blocks of A; actually just need upper triangular
        // i0 < i1
        int i0 = 6*(con.ndr-nFixed); // will be negative if fixed
        int i1 = 6*(con.nd1-nFixed); // will be negative if fixed

        if (i0>=0)
          {
            A.block<6,6>(i0,i0) += con.J0t * con.prec * con.J0;
            dcnt(con.ndr - nFixed)++;
          }
        if (i1>=0)
          {
            dcnt(con.nd1 - nFixed)++;
            Matrix<double,6,6> tp = con.prec * con.J1;
            A.block<6,6>(i1,i1) += con.J1t * tp;
            if (i0>=0)
              {
                A.block<6,6>(i0,i1) += con.J0t * con.prec * con.J1;
                A.block<6,6>(i1,i0) += con.J1t * con.prec * con.J0;
              }
          }

        // add in 2 blocks of B
        if (i0>=0)
          B.block<6,1>(i0,0) -= con.J0t * con.prec * con.err;
        if (i1>=0)
          B.block<6,1>(i1,0) -= con.J1t * con.prec * con.err;
      } // finish P2 constraints

    // loop over Scale constraints
    if (scons.size() > 0)       // could be zero
      for(size_t pi=0; pi<scons.size(); pi++)
        {
          ConScale &con = scons[pi];
          con.setJacobians(nodes);
        // add in 4 blocks of A for t0, t1; actually just need upper triangular
        // i0 < i1
        int i0 = 6*(con.nd0-nFixed); // will be negative if fixed
        int i1 = 6*(con.nd1-nFixed); // will be negative if fixed

        if (i0>=0)
          {
            A.block<3,3>(i0,i0) += con.w * con.J0 * con.J0.transpose();
          }
        if (i1>=0)
          {
            A.block<3,3>(i1,i1) += con.w * con.J1 * con.J1.transpose();
            if (i0>=0)
              {
                A.block<3,3>(i0,i1) += con.w * con.J0 * con.J1.transpose();
                A.block<3,3>(i1,i0) = A.block<3,3>(i0,i1).transpose();
              }
          }

        // add in 2 blocks of B
        if (i0>=0)
          B.block<3,1>(i0,0) -= con.w * con.J0 * con.err;
        if (i1>=0)
          B.block<3,1>(i1,0) -= con.w * con.J1 * con.err;

        // scale variable, add in 5 blocks; actually just need upper triangular
        int is = 6*nFree+con.sv;
        A(is,is) += con.w * con.ks * con.ks;
        if (i0>=0)
          {
            A.block<3,1>(i0,is) += con.w * con.J0 * -con.ks;
            A.block<1,3>(is,i0) = A.block<3,1>(i0,is).transpose();
          }
        if (i1>=0)
          {
            A.block<3,1>(i1,is) += con.w * con.J1 * -con.ks;
            A.block<1,3>(is,i1) = A.block<3,1>(i1,is).transpose();
          }

        // add in scale block of B
        B(is) -= con.w * (-con.ks) * con.err;

      } // finish Scale constraints

    // augment diagonal
    A.diagonal() *= lam;

    // check the matrix and vector
    for (int i=0; i<6*nFree; i++)
      for (int j=0; j<6*nFree; j++)
        if (std::isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }

    for (int j=0; j<6*nFree; j++)
      if (std::isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }

    int ndc = 0;
    for (int i=0; i<nFree; i++)
      if (dcnt(i) == 0) ndc++;

    if (ndc > 0)
      cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
  }

  // Set up sparse linear system; see setupSys for algorithm.
  // Currently doesn't work with scale variables
  void SysSPA::setupSparseSys(double sLambda, int iter, int sparseType)
  {
    // set matrix sizes and clear
    // assumes scales vars are all free
    int nFree = nodes.size() - nFixed;

    //    long long t0, t1, t2, t3;
    //    t0 = utime();

    if (iter == 0)
      csp.setupBlockStructure(nFree); // initialize CSparse structures
    else
      csp.setupBlockStructure(0); // zero out CSparse structures

    //    t1 = utime();

    VectorXi dcnt(nFree);
    dcnt.setZero(nFree);

    // lambda augmentation
    double lam = 1.0 + sLambda;

    // loop over P2 constraints
    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        ConP2 &con = p2cons[pi];
        con.setJacobians(nodes);

        // add in 4 blocks of A; actually just need upper triangular
        int i0 = con.ndr-nFixed; // will be negative if fixed
        int i1 = con.nd1-nFixed; // will be negative if fixed

        if (i0>=0)
          {
           Matrix<double,6,6> m = con.J0t*con.prec*con.J0;
            csp.addDiagBlock(m,i0);
            dcnt(con.ndr - nFixed)++;
          }
        if (i1>=0)
          {
            dcnt(con.nd1 - nFixed)++;
            Matrix<double,6,6> tp = con.prec * con.J1;
            Matrix<double,6,6> m = con.J1t * tp;
            csp.addDiagBlock(m,i1);
            if (i0>=0)
              {
                Matrix<double,6,6> m2 = con.J0t * tp;
                if (i1 < i0)
                  {
                    m = m2.transpose();
                    csp.addOffdiagBlock(m,i1,i0);
                  }
                else
                  csp.addOffdiagBlock(m2,i0,i1);
              }
          }

        // add in 2 blocks of B
        if (i0>=0)
          csp.B.block<6,1>(i0*6,0) -= con.J0t * con.prec * con.err;
        if (i1>=0)
          csp.B.block<6,1>(i1*6,0) -= con.J1t * con.prec * con.err;
      } // finish P2 constraints

    //    t2 = utime();

    // set up sparse matrix structure from blocks
    if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
      csp.incDiagBlocks(lam);   // increment diagonal block
    else
      csp.setupCSstructure(lam,iter==0);

    //    t3 = utime();

    //    printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
    //           (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);

    int ndc = 0;
    for (int i=0; i<nFree; i++)
      if (dcnt(i) == 0) ndc++;

    if (ndc > 0)
      cout << "[SetupSparseSys] " << ndc << " disconnected nodes" << endl;
  }

  /// Run the LM algorithm that computes a nonlinear SPA estimate.
  /// <niter> is the max number of iterations to perform; returns the
  /// number actually performed.
  /// <lambda> is the diagonal augmentation for LM.
  /// <useCSparse> is true for sparse Cholesky.
  ///                2 for gradient system, 3 for block jacobian PCG
  /// <initTol> is the initial tolerance for CG
  /// <maxCGiters> is max # of iterations in BPCG

  int SysSPA::doSPA(int niter, double sLambda, int useCSparse, double initTol,
                      int maxCGiters)
  {
    Node::initDr();
    int nFree = nodes.size() - nFixed; // number of free nodes

    // number of nodes
    int ncams = nodes.size();
    // number of scale variables
    int nscales = scales.size();

    // save old scales
    vector<double> oldscales;
    oldscales.resize(nscales);

    // initialize vars
    if (sLambda > 0.0)          // do we initialize lambda?
      lambda = sLambda;

    // set number of constraints
    int ncons = p2cons.size();

    // check for fixed frames
    for (int i=0; i<ncams; i++)
      {
        Node &nd = nodes[i];
        if (i >= nFixed)
          nd.isFixed = false;
        else
          nd.isFixed = true;
        nd.setTransform();      // set up world-to-node transform for cost calculation
        nd.setDr(true);         // always use local angles
      }

    // initialize vars
    double laminc = 2.0;        // how much to increment lambda if we fail
    double lamdec = 0.5;        // how much to decrement lambda if we succeed
    int iter = 0;               // iterations
    sqMinDelta = 1e-8 * 1e-8;
    double cost = calcCost();
    if (verbose)
      cout << iter << " Initial squared cost: " << cost << " which is "
           << sqrt(cost/ncons) << " rms error" << endl;

    int good_iter = 0;
    for (; iter<niter; iter++)  // loop at most <niter> times
    {
        // set up and solve linear system
        // NOTE: shouldn't need to redo all calcs in setupSys if we
        //   got here from a bad update

        long long t0, t1, t2, t3;
        t0 = utime();
        if (useCSparse)
          setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
        else
          setupSys(lambda);     // set up linear system

        // use appropriate linear solver
        if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
          {
            if (csp.B.rows() != 0)
              {
                int iters = csp.doBPCG(maxCGiters,initTol,iter);
                if (verbose)
                  cout << "[Block PCG] " << iters << " iterations" << endl;
              }
          }
        else if (useCSparse > 0)
        {
            bool ok = csp.doChol();
            if (!ok)
              cout << "[DoSPA] Sparse Cholesky failed!" << endl;
        }
        else
          A.ldlt().solveInPlace(B); // Cholesky decomposition and solution

        // get correct result vector
        VectorXd &BB = useCSparse ? csp.B : B;

        // check for convergence
        // this is a pretty crummy convergence measure...
        double sqDiff = BB.squaredNorm();
        if (sqDiff < sqMinDelta) // converged, done...
        {
          break;
        }

        // update the frames
        int ci = 0;
        for(int i=0; i < ncams; i++)
        {
            Node &nd = nodes[i];
            if (nd.isFixed) continue; // not to be updated
            nd.oldtrans = nd.trans; // save in case we don't improve the cost
            nd.oldqrot = nd.qrot;
            nd.trans.head<3>() += BB.segment<3>(ci);

            Quaternion<double> qr;
            qr.vec() = BB.segment<3>(ci+3);
            qr.w() = sqrt(1.0 - qr.vec().squaredNorm());

            Quaternion<double> qrn,qrx;
            qrn = nd.qrot;
            qr = qrn*qr;        // post-multiply
            qr.normalize();
            if (qr.w() < 0.0)
              nd.qrot.coeffs() = -qr.coeffs();
            else
              nd.qrot.coeffs() = qr.coeffs();

            nd.setTransform();  // set up projection matrix for cost calculation
            nd.setDr(true);     // set rotational derivatives
            ci += 6;            // advance B index
        }

        // update the scales
        ci = 6*nFree;       // head of scale vars
        if (nscales > 0)        // could be empty
          for(int i=0; i < nscales; i++)
          {
              oldscales[i] = scales[i];
              scales[i] += B(ci);
              ci++;
          }

        // new cost
        double newcost = calcCost();
        if (verbose)
          cout << iter << " Updated squared cost: " << newcost << " which is "
           << sqrt(newcost/ncons) << " rms error" << endl;

        // check if we did good
        if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
        {
            cost = newcost;
            lambda *= lamdec;   // decrease lambda
            //      laminc = 2.0;       // reset bad lambda factor; not sure if this is a good idea...
            good_iter++;
        }
        else
        {
            lambda *= laminc;   // increase lambda
            laminc *= 2.0;      // increase the increment

            // reset nodes
            for(int i=0; i<ncams; i++)
            {
                Node &nd = nodes[i];
                if (nd.isFixed) continue; // not to be updated
                nd.trans = nd.oldtrans;
                nd.qrot = nd.oldqrot;
                nd.setTransform(); // set up projection matrix for cost calculation
                nd.setDr(true);
            }

            // reset scales
            if (nscales > 0)    // could be empty
              for(int i=0; i < nscales; i++)
                scales[i] = oldscales[i];

            cost = calcCost();  // need to reset errors
            if (verbose)
              cout << iter << " Downdated cost: " << cost << endl;
            // NOTE: shouldn't need to redo all calcs in setupSys
        }
      }

    // return number of iterations performed
    return good_iter;

  }

  // write out the precision matrix for CSparse
  void SysSPA::writeSparseA(char *fname, bool useCSparse)
  {
    ofstream ofs(fname);
    if (!ofs)
      {
        cout << "Can't open file " << fname << endl;
        return;
      }

    // cameras
    if (useCSparse)
      {
        setupSparseSys(0.0,0,useCSparse);

        int *Ai = csp.A->i;
        int *Ap = csp.A->p;
        double *Ax = csp.A->x;

        for (int i=0; i<csp.csize; i++)
          for (int j=Ap[i]; j<Ap[i+1]; j++)
            if (Ai[j] <= i)
              ofs << Ai[j] << " " << i << setprecision(16) << " " << Ax[j] << endl;
      }
    else
      {
        Eigen::IOFormat pfmt(16);

        int nrows = A.rows();
        int ncols = A.cols();

        for (int i=0; i<nrows; i++)
          for (int j=i; j<ncols; j++)
            {
              double a = A(i,j);
              if (A(i,j) != 0.0)
                ofs << i << " " << j << setprecision(16) << " " << a << endl;
            }
      }

    ofs.close();
  }

  // Set up spanning tree initialization
  void SysSPA::spanningTree(int node)
  {
    int nnodes = nodes.size();

    // set up an index from nodes to their constraints
    vector<vector<int> > cind;
    cind.resize(nnodes);

    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        ConP2 &con = p2cons[pi];
        int i0 = con.ndr;
        int i1 = con.nd1;
        cind[i0].push_back(i1);
        cind[i1].push_back(i0);
      }

    // set up breadth-first algorithm
    VectorXd dist(nnodes);
    dist.setConstant(1e100);
    if (node >= nnodes)
      node = 0;
    dist[node] = 0.0;
    std::multimap<double,int> open;  // open list, priority queue - can have duplicates
    open.emplace(0.0,node);

    // do breadth-first computation
    while (!open.empty())
      {
        // get top node, remove it
        int ni = open.begin()->second;
        double di = open.begin()->first;
        open.erase(open.begin());
        if (di > dist[ni]) continue; // already dealt with

        // update neighbors
        Node &nd = nodes[ni];
        Matrix<double,3,4> n2w;
        transformF2W(n2w,nd.trans,nd.qrot); // from node to world coords

        vector<int> &nns = cind[ni];
        for (int i=0; i<(int)nns.size(); i++)
          {
            ConP2 &con = p2cons[nns[i]];
            double dd = con.tmean.norm(); // incremental distance
            // neighbor node index
            int nn = con.nd1;
            if (nn == ni)
              nn = con.ndr;
            Node &nd2 = nodes[nn];
            Vector3d tmean = con.tmean;
            Quaterniond qpmean = con.qpmean;
            if (nn == con.ndr)       // wrong way, reverse
              {
                qpmean = qpmean.inverse();
                tmean = nd.qrot.toRotationMatrix().transpose()*nd2.qrot.toRotationMatrix()*tmean;
              }

            if (dist[nn] > di + dd) // is neighbor now closer?
              {
                // set priority queue
                dist[nn] = di+dd;
                open.emplace(di+dd,nn);
                // update initial pose
                Vector4d trans;
                trans.head(3) = tmean;
                trans(3) = 1.0;
                nd2.trans.head(3) = n2w*trans;
                nd2.qrot = qpmean*nd.qrot;
                nd2.normRot();
                nd2.setTransform();
                nd2.setDr(true);
              }
          }
      }

  }

}  // namespace sba


#pragma once
#include <Eigen/Dense>

enum class MaterialType
{
    Diffuse,

    // light
    LightArea
};

class MaterialBase
{
public:
    MaterialType get_type() const { return type; };

    virtual bool has_emission() const = 0;

    virtual Eigen::Vector3f calc_brdf(Eigen::Vector3f normal, Eigen::Vector3f in, Eigen::Vector3f out) const = 0;

    virtual Eigen::Vector3f sample(Eigen::Vector3f normal, Eigen::Vector3f in) const = 0;

    virtual Eigen::Vector3f at(double u, double v) const = 0;

protected:
    MaterialBase(MaterialType type) : type(type) {};

    MaterialType type;
};


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

#include <cstdio>
#include <cmath>

#include <string>
#include <fstream>
#include <streambuf>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include "medial.h"

namespace bg = boost::geometry;

typedef bg::model::d2::point_xy<int64_t> polygon_integral_point;
typedef bg::model::referring_segment<polygon_integral_point> polygon_segment;
typedef bg::model::polygon<polygon_integral_point> polygon;

int main(int argc, char **argv)
{
    // https://upload.wikimedia.org/wikipedia/commons/5/55/SFA_Polygon_with_hole.svg
    const int f = 1000;
    polygon bg_polygon;
    std::vector<int64_t> segments;

    std::ifstream t(argv[1]);
    std::string wkt_polygon = std::string(std::istreambuf_iterator<char>(t),
                                          std::istreambuf_iterator<char>());

    int n = -1;
    double *return_data;

    bg::read_wkt(wkt_polygon.data(), bg_polygon);
    bg::for_each_segment(bg_polygon, [&segments](polygon_segment s) {
        segments.push_back(s.first.x() * f);
        segments.push_back(s.first.y() * f);
        segments.push_back(s.second.x() * f);
        segments.push_back(s.second.y() * f);
    });

    // Compute the internal axis
    n = get_skeleton(segments.size(), segments.data(), &return_data);
    fprintf(stderr, "n = %d\n", n);

    // Display the internal axis
    for (int i = 0; i < n; i += 6)
    {
        double x1, y1, d1, x2, y2, d2, length;

        x1 = return_data[i + 0] / f;
        y1 = return_data[i + 1] / f;
        x2 = return_data[i + 2] / f;
        y2 = return_data[i + 3] / f;
        d1 = return_data[i + 4] / f;
        d2 = return_data[i + 5] / f;
        length = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));

        fprintf(stderr, "INTERNAL EDGE: (%lf %lf) (%lf %lf) with distances %lf %lf and length %lf\n", x1, y1, x2, y2, d1, d2, length);
    }

    // Cleanup
    free(return_data);
    return_data = nullptr;

    return 0;
}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2014-2015 Samuel Debionne, Grenoble, France.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_POINT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_POINT_HPP

#include <cstddef>
#include <algorithm>

#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

#include <boost/geometry/strategies/compare.hpp>
#include <boost/geometry/policies/compare.hpp>

#include <boost/geometry/algorithms/detail/normalize.hpp>
#include <boost/geometry/algorithms/detail/envelope/transform_units.hpp>

#include <boost/geometry/algorithms/dispatch/expand.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace expand
{

template
<
    typename StrategyLess, typename StrategyGreater,
    std::size_t Dimension, std::size_t DimensionCount
>
struct point_loop
{
    template <typename Box, typename Point>
    static inline void apply(Box& box, Point const& source)
    {
        typedef typename strategy::compare::detail::select_strategy
            <
                StrategyLess, 1, Point, Dimension
            >::type less_type;

        typedef typename strategy::compare::detail::select_strategy
            <
                StrategyGreater, -1, Point, Dimension
            >::type greater_type;

        typedef typename select_coordinate_type
            <
                Point, Box
            >::type coordinate_type;

        less_type less;
        greater_type greater;

        coordinate_type const coord = get<Dimension>(source);

        if (less(coord, get<min_corner, Dimension>(box)))
        {
            set<min_corner, Dimension>(box, coord);
        }

        if (greater(coord, get<max_corner, Dimension>(box)))
        {
            set<max_corner, Dimension>(box, coord);
        }

        point_loop
            <
                StrategyLess, StrategyGreater, Dimension + 1, DimensionCount
            >::apply(box, source);
    }
};

template
<
    typename StrategyLess, typename StrategyGreater, std::size_t DimensionCount
>
struct point_loop
    <
        StrategyLess, StrategyGreater, DimensionCount, DimensionCount
    >
{
    template <typename Box, typename Point>
    static inline void apply(Box&, Point const&) {}
};

// implementation for the spherical equatorial and geographic coordinate systems
template
<
    typename StrategyLess,
    typename StrategyGreater,
    std::size_t DimensionCount
>
struct point_loop_on_spheroid
{
    template <typename Box, typename Point>
    static inline void apply(Box& box, Point const& point)
    {
        typedef typename point_type<Box>::type box_point_type;
        typedef typename coordinate_type<Box>::type box_coordinate_type;

        typedef math::detail::constants_on_spheroid
            <
                box_coordinate_type,
                typename coordinate_system<Box>::type::units
            > constants;

        // normalize input point and input box
        Point p_normalized = detail::return_normalized<Point>(point);
        detail::normalize(box, box);

        // transform input point to be of the same type as the box point
        box_point_type box_point;
        detail::envelope::transform_units(p_normalized, box_point);

        box_coordinate_type p_lon = geometry::get<0>(box_point);
        box_coordinate_type p_lat = geometry::get<1>(box_point);

        typename coordinate_type<Box>::type
            b_lon_min = geometry::get<min_corner, 0>(box),
            b_lat_min = geometry::get<min_corner, 1>(box),
            b_lon_max = geometry::get<max_corner, 0>(box),
            b_lat_max = geometry::get<max_corner, 1>(box);

        if (math::equals(math::abs(p_lat), constants::max_latitude()))
        {
            // the point of expansion is the either the north or the
            // south pole; the only important coordinate here is the
            // pole's latitude, as the longitude can be anything;
            // we, thus, take into account the point's latitude only and return
            geometry::set<min_corner, 1>(box, (std::min)(p_lat, b_lat_min));
            geometry::set<max_corner, 1>(box, (std::max)(p_lat, b_lat_max));
            return;
        }

        if (math::equals(b_lat_min, b_lat_max)
            && math::equals(math::abs(b_lat_min), constants::max_latitude()))
        {
            // the box degenerates to either the north or the south pole;
            // the only important coordinate here is the pole's latitude,
            // as the longitude can be anything;
            // we thus take into account the box's latitude only and return
            geometry::set<min_corner, 0>(box, p_lon);
            geometry::set<min_corner, 1>(box, (std::min)(p_lat, b_lat_min));
            geometry::set<max_corner, 0>(box, p_lon);
            geometry::set<max_corner, 1>(box, (std::max)(p_lat, b_lat_max));
            return;
        }

        // update latitudes
        b_lat_min = (std::min)(b_lat_min, p_lat);
        b_lat_max = (std::max)(b_lat_max, p_lat);

        // update longitudes
        if (math::smaller(p_lon, b_lon_min))
        {
            box_coordinate_type p_lon_shifted = p_lon + constants::period();

            if (math::larger(p_lon_shifted, b_lon_max))
            {
                // here we could check using: ! math::larger(.., ..)
                if (math::smaller(b_lon_min - p_lon, p_lon_shifted - b_lon_max))
                {
                    b_lon_min = p_lon;
                }
                else
                {
                    b_lon_max = p_lon_shifted;
                }
            }
        }
        else if (math::larger(p_lon, b_lon_max))
        {
            // in this case, and since p_lon is normalized in the range
            // (-180, 180], we must have that b_lon_max <= 180
            if (b_lon_min < 0
                && math::larger(p_lon - b_lon_max,
                                constants::period() - p_lon + b_lon_min))
            {
                b_lon_min = p_lon;
                b_lon_max += constants::period();
            }
            else
            {
                b_lon_max = p_lon;
            }
        }

        geometry::set<min_corner, 0>(box, b_lon_min);
        geometry::set<min_corner, 1>(box, b_lat_min);
        geometry::set<max_corner, 0>(box, b_lon_max);
        geometry::set<max_corner, 1>(box, b_lat_max);

        point_loop
            <
                StrategyLess, StrategyGreater, 2, DimensionCount
            >::apply(box, point);
    }
};

}} // namespace detail::expand
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// Box + point -> new box containing also point
template
<
    typename BoxOut, typename Point,
    typename StrategyLess, typename StrategyGreater,
    typename CSTagOut, typename CSTag
>
struct expand
    <
        BoxOut, Point,
        StrategyLess, StrategyGreater,
        box_tag, point_tag,
        CSTagOut, CSTag
    > : detail::expand::point_loop
        <
            StrategyLess, StrategyGreater, 0, dimension<Point>::value
        >
{
    BOOST_MPL_ASSERT_MSG((geofeatures_boost::is_same<CSTagOut, CSTag>::value),
                         COORDINATE_SYSTEMS_MUST_BE_THE_SAME,
                         (types<CSTagOut, CSTag>()));
};

template
<
    typename BoxOut, typename Point,
    typename StrategyLess, typename StrategyGreater
>
struct expand
    <
        BoxOut, Point,
        StrategyLess, StrategyGreater,
        box_tag, point_tag,
        spherical_equatorial_tag, spherical_equatorial_tag
    > : detail::expand::point_loop_on_spheroid
        <
            StrategyLess, StrategyGreater, dimension<Point>::value
        >
{};

template
<
    typename BoxOut, typename Point,
    typename StrategyLess, typename StrategyGreater
>
struct expand
    <
        BoxOut, Point,
        StrategyLess, StrategyGreater,
        box_tag, point_tag,
        geographic_tag, geographic_tag
    > : detail::expand::point_loop_on_spheroid
        <
            StrategyLess, StrategyGreater, dimension<Point>::value
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_POINT_HPP


#ifndef GIFS_SH_QM_INTERFACE_HPP
#define GIFS_SH_QM_INTERFACE_HPP

#include "properties.hpp"
#include <armadillo>

/*
  Abstract base class for all implementations of QMInterface
*/

class QMInterface{
  friend class PrintBomd;
public:
  QMInterface(const arma::uvec& in_qmids,
	      arma::mat& in_qm_crd,
	      arma::mat& in_mm_crd,
	      arma::vec& in_mm_chg,
	      const int charge,
	      const int mult,
	      const int excited_states,
              const int min_state);
  virtual void update(); // if overriding, be sure to call the parent too.
  inline size_t nqm() const noexcept { return NQM; };
  inline int call_idx() const noexcept { return qm_call_idx; };

  virtual void get_properties(PropMap &props) =0;
  virtual ~QMInterface(){};

protected:
  //Properties
  const size_t NQM;             // const, actually NQM+NLink
  const int qm_charge;
  int qm_multiplicity;          // non-const since a child might want to override
  size_t excited_states;        // non-const since a child might want to override
  size_t min_state;             // non-const since a child might want to override

  //  fixed size
  const arma::uvec& atomids;    // NQM
  arma::mat& crd_qm;      // NQM*3
  // flexible
  size_t NMM;
  arma::mat& crd_mm;      // NMM*3
  arma::vec& chg_mm;      // NMM

private:
  int qm_call_idx = 0; // tracks each call to update;
};

#endif


// Copyright (c) 2015-2019 Daniel Cooke and Gerton Lunter
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef simd_pair_hmm_wrapper_hpp
#define simd_pair_hmm_wrapper_hpp

#include <tuple>

#include <boost/variant.hpp>

#include "exceptions/user_error.hpp"
#include "simd_pair_hmm_factory.hpp"

namespace octopus { namespace hmm { namespace simd {

namespace detail {

constexpr unsigned ipow(unsigned base, unsigned exponent) noexcept
{
    return exponent == 0 ? 1 : base * ipow(base, exponent - 1);
}

template <typename T, std::size_t... I>
constexpr auto make_phmm_tuple(std::index_sequence<I...>) { return std::make_tuple(SimdPairHMM<ipow(2, I + 3), T>{}...); }

template <typename Tuple>
struct to_variant;

template <typename... Ts>
struct to_variant<std::tuple<Ts...>>
{
    using type = boost::variant<Ts...>;
};

template <typename Tuple>
using to_variant_t = typename to_variant<Tuple>::type;

} // namespace detail

class PairHMMWrapper
{
public:
    enum class ScorePrecision { int16, int32 };

    class TooLargeBandSizeError : public std::runtime_error
    {
    public:
        TooLargeBandSizeError() = delete;

        TooLargeBandSizeError(int requested, int max)
        : std::runtime_error {"requested band size is too large"}
        , requested_ {requested}
        , max_ {max}
        {}

        int requested() const noexcept { return requested_; };
        int max() const noexcept { return max_; }

    private:
        int requested_, max_;
    };

    PairHMMWrapper(int min_band_size = 8, ScorePrecision score_precision = ScorePrecision::int16)
    {
        reset(min_band_size, score_precision);
    }

    PairHMMWrapper(const PairHMMWrapper&)            = default;
    PairHMMWrapper& operator=(const PairHMMWrapper&) = default;
    PairHMMWrapper(PairHMMWrapper&&)                 = default;
    PairHMMWrapper& operator=(PairHMMWrapper&&)      = default;

    ~PairHMMWrapper() = default;

    int band_size() const noexcept
    {
        return boost::apply_visitor([] (const auto& hmm) noexcept { return hmm.band_size(); }, hmm_);
    }

    const char* name() const noexcept
    {
        return boost::apply_visitor([] (const auto& hmm) noexcept { return hmm.name(); }, hmm_);
    }

    void reset(int min_band_size, ScorePrecision score_precision = ScorePrecision::int16)
    {
        hmm_ = make_simd_pair_hmm(min_band_size, score_precision);
    }

    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    align(const char* truth,
          const char* target,
          const std::int8_t* qualities,
          const int truth_len,
          const int target_len,
          const OpenPenaltyArrayOrConstant gap_open,
          const ExtendPenaltyArrayOrConstant gap_extend,
          short nuc_prior) const noexcept
    {
        return boost::apply_visitor([&] (const auto& hmm) noexcept {
            return hmm.align(truth, target, qualities, truth_len, target_len, gap_open, gap_extend, nuc_prior);
        }, hmm_);
    }
    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    align(const char* truth,
          const char* target,
          const std::int8_t* qualities,
          const int truth_len,
          const int target_len,
          const char* snv_mask,
          const std::int8_t* snv_prior,
          const OpenPenaltyArrayOrConstant gap_open,
          const ExtendPenaltyArrayOrConstant gap_extend,
          short nuc_prior) const noexcept
    {
        return boost::apply_visitor([&] (const auto& hmm) noexcept {
            return hmm.align(truth, target, qualities, truth_len, target_len, snv_mask, snv_prior, gap_open, gap_extend, nuc_prior);
        }, hmm_);
    }
    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    align(const char* truth,
          const char* target,
          const std::int8_t* qualities,
          const int truth_len,
          const int target_len,
          const OpenPenaltyArrayOrConstant gap_open,
          const ExtendPenaltyArrayOrConstant gap_extend,
          short nuc_prior,
          int& first_pos,
          char* align1,
          char* align2) const noexcept
    {
        return boost::apply_visitor([&] (const auto& hmm) noexcept {
            return hmm.align(truth, target, qualities, truth_len, target_len, gap_open, gap_extend, nuc_prior, first_pos, align1, align2);
        }, hmm_);
    }
    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    align(const char* truth,
          const char* target,
          const std::int8_t* qualities,
          const int truth_len,
          const int target_len,
          const char* snv_mask,
          const std::int8_t* snv_prior,
          const OpenPenaltyArrayOrConstant gap_open,
          const ExtendPenaltyArrayOrConstant gap_extend,
          short nuc_prior,
          int& first_pos,
          char* align1,
          char* align2) const noexcept
    {
        return boost::apply_visitor([&] (const auto& hmm) noexcept {
            return hmm.align(truth, target, qualities, truth_len, target_len, snv_mask, snv_prior, gap_open, gap_extend, nuc_prior, first_pos, align1, align2);
        }, hmm_);
    }
    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    calculate_flank_score(const int truth_len,
                          const int lhs_flank_len,
                          const int rhs_flank_len,
                          const std::int8_t* quals,
                          const OpenPenaltyArrayOrConstant gap_open,
                          const ExtendPenaltyArrayOrConstant gap_extend,
                          const short nuc_prior,
                          const int first_pos,
                          const char* aln1,
                          const char* aln2,
                          int& target_mask_size) const noexcept
    {
        return boost::apply_visitor([&] (const auto& hmm) noexcept {
            return hmm.calculate_flank_score(truth_len, lhs_flank_len, rhs_flank_len, quals, gap_open, gap_extend, nuc_prior, first_pos, aln1, aln2, target_mask_size);
        }, hmm_);
    }
    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    calculate_flank_score(int truth_len,
                          int lhs_flank_len,
                          int rhs_flank_len,
                          const char* target,
                          const std::int8_t* quals,
                          const char* snv_mask,
                          const std::int8_t* snv_prior,
                          const OpenPenaltyArrayOrConstant gap_open,
                          const ExtendPenaltyArrayOrConstant gap_extend,
                          short nuc_prior,
                          int first_pos,
                          const char* aln1,
                          const char* aln2,
                          int& target_mask_size) const noexcept
    {
        return boost::apply_visitor([&] (const auto& hmm) noexcept {
            return hmm.calculate_flank_score(truth_len, lhs_flank_len, rhs_flank_len, target, quals, snv_mask, snv_prior, gap_open, gap_extend, nuc_prior, first_pos, aln1, aln2, target_mask_size);
        }, hmm_);
    }

private:
    using ShortPairHMMs = decltype(detail::make_phmm_tuple<short>(std::make_index_sequence<6>()));
    using IntPairHMMs   = decltype(detail::make_phmm_tuple<int>(std::make_index_sequence<6>()));
    using PairHMMs      = decltype(std::tuple_cat(ShortPairHMMs {}, IntPairHMMs {}));

    using PairHmmVariant = detail::to_variant_t<PairHMMs>;

    PairHmmVariant hmm_;

    template <typename Hmms>
    constexpr static int max_band_size() { return std::tuple_element_t<std::tuple_size<Hmms>::value - 1, Hmms>::band_size(); }

    template <typename Hmms, std::size_t... Is>
    PairHmmVariant make_simd_pair_hmm_helper(const int min_band_size, std::index_sequence<Is...>) const
    {
        if (min_band_size <= max_band_size<Hmms>()) {
            PairHmmVariant result {};
            int unused[] = {((Is == 0 || std::tuple_element_t<(Is > 0 ? Is - 1 : 0), Hmms>::band_size() < min_band_size)
                            && min_band_size <= std::tuple_element_t<Is, Hmms>::band_size()
                            ? (result = std::tuple_element_t<Is, Hmms> {}, 0) : 0)...};
            (void) unused;
            return result;
        } else {
            throw TooLargeBandSizeError {min_band_size, max_band_size<Hmms>()};
        }
    }
    PairHmmVariant make_simd_pair_hmm(const int min_band_size, const ScorePrecision score_precision) const
    {
        if (score_precision == ScorePrecision::int16) {
            using Hmms = ShortPairHMMs;
            return make_simd_pair_hmm_helper<Hmms>(min_band_size, std::make_index_sequence<std::tuple_size<Hmms>::value>());
        } else {
            using Hmms = IntPairHMMs;
            return make_simd_pair_hmm_helper<Hmms>(min_band_size, std::make_index_sequence<std::tuple_size<Hmms>::value>());
        }
    }

public:
    static int max_band_size(ScorePrecision score_precision) noexcept
    {
        if (score_precision == ScorePrecision::int16) {
            return max_band_size<ShortPairHMMs>();
        } else {
            return max_band_size<IntPairHMMs>();
        }
    }
};

} // namespace simd
} // namespace hmm
} // namespace octopus

#endif


/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */

#include <stdio.h>
#include <string.h>
#include "fix_smd_integrate_ulsph.h"
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "atom.h"
#include "comm.h"
#include "force.h"
#include "neighbor.h"
#include "neigh_list.h"
#include "neigh_request.h"
#include "update.h"
#include "integrate.h"
#include "respa.h"
#include "memory.h"
#include "error.h"
#include "pair.h"
#include "domain.h"
#include <Eigen/Eigen>

using namespace Eigen;
using namespace LAMMPS_NS;
using namespace FixConst;

/* ---------------------------------------------------------------------- */

FixSMDIntegrateUlsph::FixSMDIntegrateUlsph(LAMMPS *lmp, int narg, char **arg) :
		Fix(lmp, narg, arg) {

	if ((atom->e_flag != 1) || (atom->vfrac_flag != 1))
		error->all(FLERR, "fix smd/integrate_ulsph command requires atom_style with both energy and volume");

	if (narg < 3)
		error->all(FLERR, "Illegal number of arguments for fix smd/integrate_ulsph command");

	adjust_radius_flag = false;
	xsphFlag = false;
	vlimit = -1.0;
	int iarg = 3;

	if (comm->me == 0) {
		printf("\n>>========>>========>>========>>========>>========>>========>>========>>========\n");
		printf("fix smd/integrate_ulsph is active for group: %s \n", arg[1]);
	}
	while (true) {

		if (iarg >= narg) {
			break;
		}

		if (strcmp(arg[iarg], "xsph") == 0) {
			xsphFlag = true;
			if (comm->me == 0) {
				error->one(FLERR, "XSPH is currently not available");
				printf("... will use XSPH time integration\n");
			}
		} else if (strcmp(arg[iarg], "adjust_radius") == 0) {
			adjust_radius_flag = true;

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected three numbers following adjust_radius: factor, min, max");
			}

			adjust_radius_factor = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected three numbers following adjust_radius: factor, min, max");
			}

			min_nn = force->inumeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected three numbers following adjust_radius: factor, min, max");
			}

			max_nn = force->inumeric(FLERR, arg[iarg]);

			if (comm->me == 0) {
				printf("... will adjust smoothing length dynamically with factor %g to achieve %d to %d neighbors per particle.\n ",
						adjust_radius_factor, min_nn, max_nn);
			}

		} else if (strcmp(arg[iarg], "limit_velocity") == 0) {
			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected number following limit_velocity");
			}
			vlimit = force->numeric(FLERR, arg[iarg]);

			if (comm->me == 0) {
				printf("... will limit velocities to <= %g\n", vlimit);
			}
		} else {
			char msg[128];
			sprintf(msg, "Illegal keyword for smd/integrate_ulsph: %s\n", arg[iarg]);
			error->all(FLERR, msg);
		}

		iarg++;

	}

	if (comm->me == 0) {
		printf(">>========>>========>>========>>========>>========>>========>>========>>========\n\n");
	}

	// set comm sizes needed by this fix
	atom->add_callback(0);

	time_integrate = 1;
}

/* ---------------------------------------------------------------------- */

int FixSMDIntegrateUlsph::setmask() {
	int mask = 0;
	mask |= INITIAL_INTEGRATE;
	mask |= FINAL_INTEGRATE;
	return mask;
}

/* ---------------------------------------------------------------------- */

void FixSMDIntegrateUlsph::init() {
	dtv = update->dt;
	dtf = 0.5 * update->dt * force->ftm2v;
	vlimitsq = vlimit * vlimit;
}

/* ----------------------------------------------------------------------
 allow for both per-type and per-atom mass
 ------------------------------------------------------------------------- */

void FixSMDIntegrateUlsph::initial_integrate(int vflag) {
	double **x = atom->x;
	double **v = atom->v;
	double **f = atom->f;
	double **vest = atom->vest;
	double *rmass = atom->rmass;

	int *mask = atom->mask;
	int nlocal = atom->nlocal;
	int i, itmp;
	double dtfm, vsq, scale;
	double vxsph_x, vxsph_y, vxsph_z;

	//printf("initial_integrate at timestep %d\n", update->ntimestep);

	/*
	 * get smoothed velocities from ULSPH pair style
	 */

	Vector3d *smoothVel = (Vector3d *) force->pair->extract("smd/ulsph/smoothVel_ptr", itmp);

	if (xsphFlag) {
		if (smoothVel == NULL) {
			error->one(FLERR, "fix smd/integrate_ulsph failed to access smoothVel array");
		}
	}

	if (igroup == atom->firstgroup)
		nlocal = atom->nfirst;

	for (i = 0; i < nlocal; i++) {
		if (mask[i] & groupbit) {
			dtfm = dtf / rmass[i];

			v[i][0] += dtfm * f[i][0];
			v[i][1] += dtfm * f[i][1];
			v[i][2] += dtfm * f[i][2];

			if (vlimit > 0.0) {
				vsq = v[i][0] * v[i][0] + v[i][1] * v[i][1] + v[i][2] * v[i][2];
				if (vsq > vlimitsq) {
					scale = sqrt(vlimitsq / vsq);
					v[i][0] *= scale;
					v[i][1] *= scale;
					v[i][2] *= scale;

					vest[i][0] = v[i][0];
					vest[i][1] = v[i][1];
					vest[i][2] = v[i][2];
				}
			}

			if (xsphFlag) {

				// construct XSPH velocity
				vxsph_x = v[i][0] - 0.5 * smoothVel[i](0);
				vxsph_y = v[i][1] - 0.5 * smoothVel[i](1);
				vxsph_z = v[i][2] - 0.5 * smoothVel[i](2);

				vest[i][0] = vxsph_x + dtfm * f[i][0];
				vest[i][1] = vxsph_y + dtfm * f[i][1];
				vest[i][2] = vxsph_z + dtfm * f[i][2];

				x[i][0] += dtv * vxsph_x;
				x[i][1] += dtv * vxsph_y;
				x[i][2] += dtv * vxsph_z;

			} else {

				// extrapolate velocity from half- to full-step
				vest[i][0] = v[i][0] + dtfm * f[i][0];
				vest[i][1] = v[i][1] + dtfm * f[i][1];
				vest[i][2] = v[i][2] + dtfm * f[i][2];

				x[i][0] += dtv * v[i][0];
				x[i][1] += dtv * v[i][1];
				x[i][2] += dtv * v[i][2];
			}
		}
	}
}

/* ---------------------------------------------------------------------- */

void FixSMDIntegrateUlsph::final_integrate() {
	double **v = atom->v;
	double **f = atom->f;
	double *e = atom->e;
	double *de = atom->de;
	double *vfrac = atom->vfrac;
	double *radius = atom->radius;
	double *contact_radius = atom->contact_radius;
	int *mask = atom->mask;
	int nlocal = atom->nlocal;
	if (igroup == atom->firstgroup)
		nlocal = atom->nfirst;
	double dtfm, vsq, scale;
	double *rmass = atom->rmass;
	double vol_increment;
	Matrix3d D;

	/*
	 * get current number of SPH neighbors from ULSPH pair style
	 */

	int itmp;
	int *nn = (int *) force->pair->extract("smd/ulsph/numNeighs_ptr", itmp);
	if (nn == NULL) {
		error->one(FLERR, "fix smd/integrate_ulsph failed to accesss num_neighs array");
	}

	Matrix3d *L = (Matrix3d *) force->pair->extract("smd/ulsph/velocityGradient_ptr", itmp);
	if (L == NULL) {
		error->one(FLERR, "fix smd/integrate_ulsph failed to accesss velocityGradient array");
	}

	for (int i = 0; i < nlocal; i++) {
		if (mask[i] & groupbit) {

			dtfm = dtf / rmass[i];
			v[i][0] += dtfm * f[i][0];
			v[i][1] += dtfm * f[i][1];
			v[i][2] += dtfm * f[i][2];

			if (vlimit > 0.0) {
				vsq = v[i][0] * v[i][0] + v[i][1] * v[i][1] + v[i][2] * v[i][2];
				if (vsq > vlimitsq) {
					scale = sqrt(vlimitsq / vsq);
					v[i][0] *= scale;
					v[i][1] *= scale;
					v[i][2] *= scale;
				}
			}

			e[i] += dtf * de[i];

			if (adjust_radius_flag) {
				if (nn[i] < min_nn) {
					radius[i] *= adjust_radius_factor;
				} else if (nn[i] > max_nn) {
					radius[i] /= adjust_radius_factor;
				}
				radius[i] = MAX(radius[i], 1.25 * contact_radius[i]);
				radius[i] = MIN(radius[i], 4.0 * contact_radius[i]);

			}

			D = 0.5 * (L[i] + L[i].transpose());
			vol_increment = vfrac[i] * update->dt * D.trace(); // Jacobian of deformation
			vfrac[i] += vol_increment;
		}
	}
}

/* ---------------------------------------------------------------------- */

void FixSMDIntegrateUlsph::reset_dt() {
	dtv = update->dt;
	dtf = 0.5 * update->dt * force->ftm2v;
}


#include <iostream>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <cstring>

#include <sstream>
#include <vector>
#include <string>
#include <set>
#include <iostream>
#include <fstream>
#include <time.h>
#include <limits.h>
#include <map>
#include <chrono>

#include <stdint.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <functional>

#include <getopt.h>

#include <mutex>
#include <unistd.h>
#include <unordered_map>
#include <unordered_set>
#include <thread>

#include "../common/Common.h"
#include "../common/abundance.h"

#include "../function/distance.h"
#include "../function/funcAB.h"
#include "../function/cluster.h"

#include "../io/ioMatrix.h"
#include "../io/ioHT.h"
#include "../io/ioFastQ.h"

#include "../utils/fastq.h"

#include "../hash/HashTables.h"
#include "../hash/hash.h"
#include "../hash/lshash.h"

#include "../kmer/kmc_api/kmc_file.h"
#include "../kmer/kmc_reader.h"
#include "../kmer/Kmer.h"

//#include "../utils/threadpool.hpp"
//#include <boost/bind.hpp>

#include "../utils/alglib-3.17.0/src/statistics.h"
#include "../utils/alglib-3.17.0/src/ap.h"
//#include <boost/thread.hpp>

using namespace std;
//using namespace boost;
//using namespace boost::threadpool;
using namespace std::chrono;
using namespace Core;
using namespace Utility;

struct HyperParams
{
  size_t k;
  float min_similarity, max_similarity;
  float scale;
  int cluster_iteration;
  int max_memory;
  int count_min;
  int size_thresh;
  uint64_t batch_size, extracted_size;
  float pval_thresh;
  float kmer_vote;
  bool verbose;
  bool kmc, bin, clustering, extracting;
  unsigned int threads_to_use;
  string tmp_dir;
  string input1, input2;
  string output1, output2;
  string clust_file_name;
};

void PrintHyperParams(const HyperParams& params) {
  cout << "************ kmers Cluster for Subtractiva Assembly Params Setting ****************" << endl;
  cout << "cluster iteration: " << params.cluster_iteration << endl;
  cout << "group1 result prefix: " << params.output1 << endl;
  cout << "group2 result prefix: " << params.output2 << endl;
  cout << "cluster result file: " << params.clust_file_name << endl;
  cout << "group1 input file: " << params.input1 << endl;
  cout << "group2 input file: " << params.input2 << endl;
  cout << "min similarity: " << params.min_similarity << endl;
  cout << "max similarity: " << params.max_similarity << endl;
  cout << "threds to use: " << params.threads_to_use << endl;
  cout << "cluster size threshold: " << params.size_thresh << endl;
  cout << "percentage threshold of differential k-mers in distinctive reads" << params.kmer_vote << endl;
  cout << "********************************************************" << endl;
}

void kLSH_PrintUsage() {
  cerr << "kmerLSHSA "<< KLSH_VERSION << endl << endl;
  cerr << "Clustering of k-mers for Subtractive Assembly " << endl << endl;
  cerr << "Usage: ./kmerLSHSA -a input1 -b input2 -o output1_prefix -p output2_prefix [options]";
  cerr << endl << endl <<
	"-a, --input1=STRING             Input filename for metagenome group A" << endl <<
  "-b, --input2=STRING             Input filename for metagenome group B" << endl <<
	"-o, --output1=STRING            Prefix for output of metagenome A" << endl <<
  "-p, --output2=STRING            Prefix for output of metagenome B" << endl <<
  "-I, --cluster_iteration=INT           number of iteration for LSH <default 100>" << endl <<
  "-N, --min_similarity=FLOAT           minimum threshold of similarity <default 0.85>" << endl <<
  "-X, --min_similarity=FLOAT           maximum threshold of similarity <default 0.95>" << endl <<
  "-K, --kmer_size=INT             Size of k-mers, at most " << (int) (Kmer::MAX_K-1)<< endl <<
	"-T, --threads_to_use=INT        Number of threads for running KMC etc. <default 8>" << endl <<
	"-R, --max-memory=INT            Max memory for running KMC <default 12>" << endl <<
	"-C, --count-min=INT            Min threshold of k-mer count for running KMC <default 2>" << endl <<
  "-S, --size_thresh=INT       Threshold of the size of clustering for T-Test <default 500000>" << endl <<
  "-V, --kmer_vote=FLOAT           Percentage threshold of differential k-mers in distinctive reads <default 0.5>" << endl <<
  "-F, --clust_file_name=STRING           intermediate clustering result file name <default 'clustering_result.txt'>" << endl <<
	"-M, --mode=STRING                Optional K : run kmc, B : make bin file, C : clustering, E : extract differential reads " << endl <<
	"    --verbose                   Print messages during run" << endl << endl <<
	"    --only                   Run only the setting mode " << endl << endl
	;
}

void SetHyperParams(HyperParams* params) {
  (*params).cluster_iteration = 100;  //TODO: Tune this param.
  (*params).max_similarity = 0.95;
  (*params).min_similarity = 0.85;  //TODO: Tune this param.
  (*params).scale = 1;
  (*params).threads_to_use = 12;
  (*params).kmc = true;
  (*params).bin = true;
  (*params).extracting = true;
  (*params).clustering = true;
  (*params).max_memory = 12;
  (*params).count_min = 2;
  (*params).k = 23;
  (*params).size_thresh = 500000;
  (*params).kmer_vote = 0.5;
  (*params).tmp_dir = "tmp/";
  (*params).clust_file_name = "clustering_result.txt";
}

void ParsingCommands(int argc, char*argv[], HyperParams* params) {
  int verbose_flag = 0;
  int only_flag = 0;
  string mode = "";
  const char* opt_string = "o:p:a:b:H:I:N:X:C:T:K:S:P:V:F:M:";
  static struct option long_options[] =
  {
    {"verbose", no_argument,  &verbose_flag, 1},
    {"only", no_argument,  &only_flag, 1},
    {"output1", required_argument, 0, 'o'},
    {"output2", required_argument, 0, 'p'},
  	{"input1", required_argument, 0, 'a'},
    {"input2", required_argument, 0, 'b'},
    {"cluster_iteration", optional_argument, 0, 'I'},
    {"min_similarity", optional_argument, 0, 'N'},
    {"max_similarity", optional_argument, 0, 'X'},
  	{"max-memory", optional_argument, 0, 'R'},
  	{"count-min", optional_argument, 0, 'C'},
  	{"threads_to_use", optional_argument, 0, 'T'},
    {"kmer_size", optional_argument, 0, 'K'},
    {"size_thresh", optional_argument, 0, 'S'},
    {"kmer_vote", optional_argument, 0, 'V'},
    {"tmp_dir", optional_argument, 0, 'D'},
    {"clust_file_name", optional_argument, 0, 'F'},
    {"mode", optional_argument, 0, 'M'},
  	{0,0,0,0}
  };

    int option_index = 0;
    int c;
    stringstream ss;
    while (true) {
      c = getopt_long(argc,argv,opt_string, long_options, &option_index);

      if (c == -1) {
        break;
      }
      switch (c) {
        case 'o':
          (*params).output1 = optarg;
          break;
        case 'p':
          (*params).output2 = optarg;
          break;
        case 'a':
          (*params).input1 = optarg;
  	      break;
        case 'b':
          (*params).input2 = optarg;
  	      break;
        case 'I':
          (*params).cluster_iteration = atoi(optarg);
          break;
      	case 'N':
          (*params).min_similarity = atof(optarg);
          break;
        case 'X':
          (*params).max_similarity = atof(optarg);
          break;
        case 'R':
          (*params).max_memory = atoi(optarg);
          break;
        case 'C':
          (*params).count_min = atoi(optarg);
          break;
        case 'T':
          (*params).threads_to_use = atoi(optarg);
          break;
        case 'K':
          (*params).k = atoi(optarg);
          break;
        case 'S':
          (*params).size_thresh = atoi(optarg);
          break;
        case 'V':
          (*params).kmer_vote = atof(optarg);
          break;
        case 'D':
          (*params).tmp_dir = optarg;
          break;
        case 'F':
          (*params).clust_file_name = optarg;
          break;
        case 'M':
          mode = optarg;
          break;
        default:
          break;
      }
    }
    if (verbose_flag) {
      (*params).verbose = true;
    }

    if (only_flag){
      if (mode == "K"){
        (*params).bin = false;
        (*params).clustering = false;
        (*params).extracting = false;
      }
      else if (mode == "B"){
        (*params).kmc = false;
        (*params).clustering = false;
        (*params).extracting = false;
      }
      else if (mode == "C"){
        (*params).bin = false;
        (*params).kmc = false;
        (*params).extracting = false;
      }
      else if (mode == "E"){
        (*params).bin = false;
        (*params).kmc = false;
        (*params).clustering = false;
      }
    } else {
      if (mode == "B"){
        (*params).kmc = false;
      }
      else if (mode == "C"){
        (*params).bin = false;
        (*params).kmc = false;
      }
      else if (mode == "E"){
        (*params).bin = false;
        (*params).kmc = false;
        (*params).clustering = false;
      }
    }
  }

void init_clustering(vector<Abundance*>* unknown_abundance_ptr, int dim, float min_similarity, float max_similarity, int cluster_iteration, unsigned int threads_to_use, int tot_sample, size_t kmap_size, vector<float_t> v_kmers, int bucket_size_threshold, string tmp_dir, const uint64_t batch_thresh, bool verbose){
  auto start_time = chrono::high_resolution_clock::now();
  ifstream inStream("kmer_count.bin", ios::binary);
  vector<Abundance*> out_abundance, local_abundance, *out_abundance_ptr, *local_abundance_ptr;
  local_abundance_ptr = & local_abundance;
  out_abundance_ptr = & out_abundance;
  float similarity = min_similarity;
  //const uint64_t batch_thresh = 100000000; //if changed, remember to change 2D array parameter vec_count in readHT and batchTest
  uint64_t batch_size;
  string read_tmp_file = "", write_tmp_file = "", read_tmp_file_clust = "", write_tmp_file_clust = "";
  deque<int> tmp_files;
  size_t total_size = 0;

  //initialize 2D array
  uint16_t** ary_count = new uint16_t*[tot_sample];
  for (int i=0; i < tot_sample; i++) {
  	ary_count[i] = new uint16_t[batch_thresh];
  }

  if (verbose) {
    cout << "\n...Loading and testing in batches..." << endl;
	  cout << v_kmers.size() << endl;
  }

  //load and test kmer count info in batches
  uint64_t kcnt_rem = kmap_size;
  streamoff batch_offset = 0;
  int iter = kmap_size / batch_thresh;
  int tmp = 0;
  if(verbose){
    cout << "iteration : " << iter << " kmap_size : " << kmap_size << endl;
  }
  //First, read the binary file of k-mers hash table.
  //Clustering the block of hash table
  //Saving to the other binary file.
  for(int i = 0; i< iter+1 ; i++){
    if(i == iter){
      //batch_offset = batch_thresh*i;
      batch_size = kcnt_rem - batch_offset;
    }
    else{
      batch_size = batch_thresh;
      //batch_offset = batch_thresh * i;
    }
    //cout << "i: " << i << " batch_size : " << batch_size << " batch_offset : " << batch_offset << endl;
    ReadHT(inStream, tot_sample, kmap_size, ary_count, batch_size, batch_offset);
    IOMat::convertHTMat(ary_count, v_kmers,  tot_sample, verbose, batch_size, batch_offset, local_abundance_ptr);
    Cluster(local_abundance_ptr, similarity, max_similarity,  cluster_iteration,  threads_to_use, dim, bucket_size_threshold, verbose);
    total_size += local_abundance.size();
    //////write result on tmp bin
    if(i == 0){
      write_tmp_file = tmp_dir+to_string(tmp)+".bin";
      write_tmp_file_clust = write_tmp_file+".clust";
      IOMat::SaveResult(local_abundance_ptr,  write_tmp_file_clust, true, 0, verbose);
      IOMat::SaveBinary(local_abundance_ptr, write_tmp_file, true, 0, verbose);
      tmp++;
    }
    else{
      IOMat::SaveResult(local_abundance_ptr,  write_tmp_file_clust, false, 0, verbose);
      IOMat::SaveBinary(local_abundance_ptr, write_tmp_file, false,  0, verbose);
    }
	  for(size_t j = 0; j< local_abundance.size(); j++){
      delete local_abundance[j];
    }
    vector<Abundance*>().swap(local_abundance);
	  batch_offset += batch_size;
    if (verbose) {
  	  cout << "# loaded kmers: " << batch_offset << endl;
  	}
  }

  for (int i = 0; i < tot_sample; ++i) {
  	delete [] ary_count[i];
  }
  delete [] ary_count;
  inStream.close();

  /////read temporary directory files/////////
  while (total_size > batch_thresh*5){
    similarity -=0.001;
    batch_offset = 0;
    read_tmp_file = write_tmp_file;
    read_tmp_file_clust = write_tmp_file_clust;
    write_tmp_file = tmp_dir+to_string(tmp)+".bin";
    write_tmp_file_clust = write_tmp_file+".clust";
    tmp++;
    iter = total_size / batch_thresh;
    kcnt_rem = total_size;
    total_size = 0;
    for(int i = 0; i< iter+1; i++){
      if (i == iter ){
        //batch_offset = batch_thresh*i;
        batch_size = kcnt_rem - batch_offset;
      }
      else{
        //batch_offset = batch_thresh*i;
        batch_size = batch_thresh;
      }
      cout << "i: " << i << " batch_size : " << batch_size << " batch_offset : " << batch_offset << endl;
      IOMat::ReadCluster(local_abundance_ptr, dim, read_tmp_file, batch_offset, batch_size, verbose);
      //IOMat::ReadCluster(local_abundance_ptr, read_tmp_file+".clust", batch_offset, batch_size, verbose);
      Cluster(local_abundance_ptr, similarity, max_similarity, cluster_iteration+4,  threads_to_use, dim, bucket_size_threshold, verbose);
      total_size += local_abundance.size();

      if(i == 0){
        IOMat::SaveResult(local_abundance_ptr,  write_tmp_file_clust, true, 0, verbose);
        IOMat::SaveBinary(local_abundance_ptr, write_tmp_file, true, 0, verbose);
      }
      else{
        IOMat::SaveResult(local_abundance_ptr,  write_tmp_file_clust, false, 0, verbose);
        IOMat::SaveBinary(local_abundance_ptr, write_tmp_file, false, 0, verbose);
      }

      for(size_t j = 0; j< local_abundance.size(); j++){
        delete local_abundance[j];
      }
      vector<Abundance*>().swap(local_abundance);
	    batch_offset += batch_size;
	    if (verbose) {
      	cout << "# loaded kmers: " << batch_offset << endl;
      }
    }
    ///////////delete read temporary file ///////////////
    if(remove(read_tmp_file.c_str()) != 0){
      perror("The temporary file deletion failed");
    }
    else{
      cout << read_tmp_file << "file are removed" << endl;
    }
    if(remove(read_tmp_file_clust.c_str()) != 0){
      perror("The temporary file deletion failed");
    }
    else{
      cout << read_tmp_file_clust << "file are removed" << endl;
    }
  }

  read_tmp_file = write_tmp_file;
  read_tmp_file_clust = write_tmp_file_clust;
  IOMat::ReadClusterAll(local_abundance_ptr, dim, read_tmp_file, verbose);
  //IOMat::ReadClusterAll(local_abundance_ptr, read_tmp_file+".clust");

  unknown_abundance_ptr->swap(local_abundance);
  vector<Abundance*>().swap(local_abundance);

  auto end_time = chrono::high_resolution_clock::now();
  auto elapsed_read = chrono::duration_cast<std::chrono::duration<float>>(end_time - start_time).count();

  if(verbose){
    cout << "Finish conversion matrix" << endl;
	  cout << "reading Matrix takes secs:\t" << elapsed_read << endl;
  }

}

void kmerCluster(HyperParams& params){
  vector<string> samples1, samples2, kmc_names1, kmc_names2 ;
  vector<string> samples, kmc_names;
  vector<float_t> v_kmers;
  int tot_sample, num_sample1, num_sample2;
  Kmer::set_k(params.k);
  size_t kmap_size;
  float_t kmer_coverage;
  int bucket_size_threshold = BUCKET_SIZE_THRESH;
  const uint64_t batch_thresh = BATCH_THRESH;
  uint64_t extracted_size_thresh = EXTRACTED_SIZE_THRESH;

  auto start_time_total = chrono::high_resolution_clock::now();

  GetInput(params.input1, samples1, kmc_names1);
  GetInput(params.input2, samples2, kmc_names2);

  samples = samples1;
	samples.insert(samples.end(), samples2.begin(), samples2.end());
	kmc_names = kmc_names1;
	kmc_names.insert(kmc_names.end(), kmc_names2.begin(), kmc_names2.end());

	num_sample1 = samples1.size();
	num_sample2 = samples2.size();
	tot_sample = samples.size();
	if (params.verbose) {
		cout << endl << "# samples in group 1: " << num_sample1 << endl << "# samples in group 2: " << num_sample2 << endl;
	}

  if (params.kmc || params.bin){
     buildKHtable( &v_kmers, &kmap_size,   params.kmc, params.verbose, params.k, params.count_min, params.threads_to_use, params.max_memory, samples, kmc_names);
  }

  if (params.clustering){
    //store v_kmer without buildKHtable
    if(!params.bin){
      v_kmers.reserve(tot_sample);
      ifstream logStream("kmer_count.log");
      string line;
      getline(logStream, line);
      istringstream ss(line);
      ss >> kmap_size;
      for (int i = 0; i < tot_sample; i++) {
        ss >> kmer_coverage;
        v_kmers.push_back(kmer_coverage/kmap_size);
      }
	  }

    // Creating a directory
    if (mkdir((params.tmp_dir).c_str(), 0777) == -1)
        cerr << "Error :  " << strerror(errno) << endl;

    else
        cout << "Temporary Directory created";

    vector<Abundance*> unknown_abundance, *unknown_abundance_ptr;
    unknown_abundance_ptr = & unknown_abundance;

    init_clustering(unknown_abundance_ptr, tot_sample, params.max_similarity, 0.97, 1, params.threads_to_use, tot_sample, kmap_size, v_kmers, bucket_size_threshold, params.tmp_dir, batch_thresh, params.verbose);

    Cluster(unknown_abundance_ptr, params.min_similarity, params.max_similarity, params.cluster_iteration, params.threads_to_use,tot_sample, bucket_size_threshold, params.verbose);

    // Save clusters.
    auto start_time_save_result = chrono::high_resolution_clock::now();
    if(params.verbose){
      cout << "Saving cluster results starts: " << endl;
    }
    IOMat::SaveResult(unknown_abundance_ptr,  params.clust_file_name+".clust", true, 5, params.verbose);
    IOMat::SaveBinary(unknown_abundance_ptr, params.clust_file_name, true, 5, params.verbose);
    auto end_time = chrono::high_resolution_clock::now();
    auto elapsed_read = chrono::duration_cast<std::chrono::duration<float>>(end_time - start_time_save_result).count();
    if(params.verbose){
      cout << "Save cluster results takes secs: " << elapsed_read << endl;
    }
    // Release allocated memory for spectra w/ charge; save pointers to spectra
    // w/o charge to variable 'spectra_of_no_charge'.
    auto start_time = chrono::high_resolution_clock::now();
    if(params.verbose){
      cout << "Releasing memory starts." << endl;
    }
    for(size_t i = 0; i< unknown_abundance.size(); i++){
        delete unknown_abundance[i];
    }

    end_time = chrono::high_resolution_clock::now();
    elapsed_read = chrono::duration_cast<std::chrono::duration<float>>(end_time - start_time).count();
    if(params.verbose){
      cout << "Releasing memory takes: " << elapsed_read << endl;
    }

  }
  if (params.extracting){
    if(params.verbose){
      cout << "Start to extract the differential reads from raw data" << endl;
    }
    auto start_time_extracting = chrono::high_resolution_clock::now();
    vector<Abundance*> clusteredab, *clusteredab_ptr;
    clusteredab_ptr = &clusteredab;

    uset_t g_kmer1, g_kmer2, *g_kmer1_ptr, *g_kmer2_ptr;  //differential kmers for two groups in comparison
	  g_kmer1_ptr = &g_kmer1;
	  g_kmer2_ptr = &g_kmer2;

    unordered_set<uint64_t> g_kmer_id1, g_kmer_id2, *g_kmer_id1_ptr, *g_kmer_id2_ptr;
    g_kmer_id1_ptr = &g_kmer_id1;
    g_kmer_id2_ptr = &g_kmer_id2;

    string head = "";
    int dim = 0;
    //read clustering result and do statistical testing(WRS)
    IOMat::ReadClusterAll(clusteredab_ptr, tot_sample, params.clust_file_name, params.verbose);
    AB::dynamicST(g_kmer_id1_ptr, g_kmer_id2_ptr, clusteredab_ptr, num_sample1, num_sample2, params.pval_thresh, params.size_thresh, extracted_size_thresh, params.verbose);
    for(size_t i; i< clusteredab.size(); i++){
      delete clusteredab[i];
    }

    if(params.verbose){
      cout << "# of differential kmers in group A : " << g_kmer_id1.size() << endl;
      cout << "# of differential kmers in group B : " << g_kmer_id2.size() << endl;
    }
    //read kmers in kvec to restore the original kmer order in kmap

    ifstream logStream("kmer_count.log");
    string line;
    getline(logStream, line);
    istringstream ss(line);
    ss >> kmap_size;
    logStream.close();

    ifstream kmer_file("kmer_set.hex");
	  //kvec.resize(kmap_size);
	  uint8_t bytes[(Kmer::MAX_K)/4];
	  size_t idx_k = 0;

	  for (size_t i = 0; i < kmap_size; i++) {
	    kmer_file.read(reinterpret_cast<char*> (&bytes[0]), sizeof(bytes[0])*(Kmer::MAX_K)/4);
	    Kmer km(bytes);

      if(g_kmer_id1.find(i) != g_kmer_id1.end() ){
        g_kmer1.insert(km);
      }
      else if(g_kmer_id2.find(i) != g_kmer_id2.end()){
        g_kmer2.insert(km);
      }
	    idx_k ++;
    }

    //clear the id sets and close file
    unordered_set<uint64_t>().swap(g_kmer_id1);
    unordered_set<uint64_t>().swap(g_kmer_id2);
    kmer_file.close();

    IOFQ::Extracting(samples1, g_kmer1_ptr, params.output1, params.threads_to_use, params.kmer_vote, params.verbose);
    IOFQ::Extracting(samples2, g_kmer2_ptr, params.output2, params.threads_to_use, params.kmer_vote, params.verbose);

    // Report time for all the procedures done above.
    auto end_time_extracting = chrono::high_resolution_clock::now();
    auto elapsed_read = chrono::duration_cast<std::chrono::duration<float>>(end_time_extracting - start_time_extracting).count();
    if(params.verbose){
      cout << "extracting reads takes (secs): " << elapsed_read << endl;
    }
  }

  // Report time for all the procedures done above.
  auto end_time = chrono::high_resolution_clock::now();
  auto elapsed_read = chrono::duration_cast<std::chrono::duration<float>>(end_time - start_time_total).count();
  cout << "kmerLSHSA algorithm in total takes (secs): " << elapsed_read << endl;

}

int main(int argc, char **argv) {
	if (argc < 2) {
		kLSH_PrintUsage();
	} else {
    	HyperParams params;
    	SetHyperParams(&params);
    	ParsingCommands(argc, argv, &params);
    	PrintHyperParams(params);
    	kmerCluster(params);
	}

}


#include <ripple/beast/clock/manual_clock.h>
#include <ripple/beast/unit_test.h>
#include <test/csf.h>
#include <utility>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <algorithm>
#include <sstream>
#include <fstream>
#include <string>

namespace ripple {
namespace test {

class DistributedValidators_test : public beast::unit_test::suite
{

    void
    completeTrustCompleteConnectFixedDelay(
            std::size_t numPeers,
            std::chrono::milliseconds delay = std::chrono::milliseconds(200),
            bool printHeaders = false)
    {
        using namespace csf;
        using namespace std::chrono;

        std::string const prefix =
                "DistributedValidators_"
                "completeTrustCompleteConnectFixedDelay";
        std::fstream
                txLog(prefix + "_tx.csv", std::ofstream::app),
                ledgerLog(prefix + "_ledger.csv", std::ofstream::app);

        log << prefix << "(" << numPeers << "," << delay.count() << ")"
            << std::endl;

        BEAST_EXPECT(numPeers >= 1);

        Sim sim;
        PeerGroup peers = sim.createGroup(numPeers);

        peers.trust(peers);

        peers.connect(peers, delay);

        TxCollector txCollector;
        LedgerCollector ledgerCollector;
        auto colls = makeCollectors(txCollector, ledgerCollector);
        sim.collectors.add(colls);

        sim.run(1);

        std::chrono::nanoseconds const simDuration = 10min;
        std::chrono::nanoseconds const quiet = 10s;
        Rate const rate{100, 1000ms};

        HeartbeatTimer heart(sim.scheduler);

        auto peerSelector = makeSelector(peers.begin(),
                                     peers.end(),
                                     std::vector<double>(numPeers, 1.),
                                     sim.rng);
        auto txSubmitter = makeSubmitter(ConstantDistribution{rate.inv()},
                                     sim.scheduler.now() + quiet,
                                     sim.scheduler.now() + simDuration - quiet,
                                     peerSelector,
                                     sim.scheduler,
                                     sim.rng);

        heart.start();
        sim.run(simDuration);

        log << std::right;
        log << "| Peers: "<< std::setw(2) << peers.size();
        log << " | Duration: " << std::setw(6)
            << duration_cast<milliseconds>(simDuration).count() << " ms";
        log << " | Branches: " << std::setw(1) << sim.branches();
        log << " | Synchronized: " << std::setw(1)
            << (sim.synchronized() ? "Y" : "N");
        log << " |" << std::endl;

        txCollector.report(simDuration, log, true);
        ledgerCollector.report(simDuration, log, false);

        std::string const tag = std::to_string(numPeers);
        txCollector.csv(simDuration, txLog, tag, printHeaders);
        ledgerCollector.csv(simDuration, ledgerLog, tag, printHeaders);

        log << std::endl;
    }

    void
    completeTrustScaleFreeConnectFixedDelay(
            std::size_t numPeers,
            std::chrono::milliseconds delay = std::chrono::milliseconds(200),
            bool printHeaders = false)
    {
        using namespace csf;
        using namespace std::chrono;

        std::string const prefix =
                "DistributedValidators__"
                "completeTrustScaleFreeConnectFixedDelay";
        std::fstream
                txLog(prefix + "_tx.csv", std::ofstream::app),
                ledgerLog(prefix + "_ledger.csv", std::ofstream::app);

        log << prefix << "(" << numPeers << "," << delay.count() << ")"
            << std::endl;

        int const numCNLs    = std::max(int(1.00 * numPeers), 1);
        int const minCNLSize = std::max(int(0.25 * numCNLs),  1);
        int const maxCNLSize = std::max(int(0.50 * numCNLs),  1);
        BEAST_EXPECT(numPeers >= 1);
        BEAST_EXPECT(numCNLs >= 1);
        BEAST_EXPECT(1 <= minCNLSize
                && minCNLSize <= maxCNLSize
                && maxCNLSize <= numPeers);

        Sim sim;
        PeerGroup peers = sim.createGroup(numPeers);

        peers.trust(peers);

        std::vector<double> const ranks =
                sample(peers.size(), PowerLawDistribution{1, 3}, sim.rng);
        randomRankedConnect(peers, ranks, numCNLs,
                std::uniform_int_distribution<>{minCNLSize, maxCNLSize},
                sim.rng, delay);

        TxCollector txCollector;
        LedgerCollector ledgerCollector;
        auto colls = makeCollectors(txCollector, ledgerCollector);
        sim.collectors.add(colls);

        sim.run(1);

        std::chrono::nanoseconds simDuration = 10min;
        std::chrono::nanoseconds quiet = 10s;
        Rate rate{100, 1000ms};

        HeartbeatTimer heart(sim.scheduler);

        auto peerSelector = makeSelector(peers.begin(),
                                     peers.end(),
                                     std::vector<double>(numPeers, 1.),
                                     sim.rng);
        auto txSubmitter = makeSubmitter(ConstantDistribution{rate.inv()},
                                     sim.scheduler.now() + quiet,
                                     sim.scheduler.now() + simDuration - quiet,
                                     peerSelector,
                                     sim.scheduler,
                                     sim.rng);

        heart.start();
        sim.run(simDuration);

        log << std::right;
        log << "| Peers: "<< std::setw(2) << peers.size();
        log << " | Duration: " << std::setw(6)
            << duration_cast<milliseconds>(simDuration).count() << " ms";
        log << " | Branches: " << std::setw(1) << sim.branches();
        log << " | Synchronized: " << std::setw(1)
            << (sim.synchronized() ? "Y" : "N");
        log << " |" << std::endl;

        txCollector.report(simDuration, log, true);
        ledgerCollector.report(simDuration, log, false);

        std::string const tag = std::to_string(numPeers);
        txCollector.csv(simDuration, txLog, tag, printHeaders);
        ledgerCollector.csv(simDuration, ledgerLog, tag, printHeaders);

        log << std::endl;
    }

    void
    run() override
    {
        std::string const defaultArgs = "5 200";
        std::string const args = arg().empty() ? defaultArgs : arg();
        std::stringstream argStream(args);

        int maxNumValidators = 0;
        int delayCount(200);
        argStream >> maxNumValidators;
        argStream >> delayCount;

        std::chrono::milliseconds const delay(delayCount);

        log << "DistributedValidators: 1 to " << maxNumValidators << " Peers"
            << std::endl;

        completeTrustCompleteConnectFixedDelay(1, delay, true);
        for(int i = 2; i <= maxNumValidators; i++)
        {
            completeTrustCompleteConnectFixedDelay(i, delay);
        }

        completeTrustScaleFreeConnectFixedDelay(1, delay, true);
        for(int i = 2; i <= maxNumValidators; i++)
        {
            completeTrustScaleFreeConnectFixedDelay(i, delay);
        }
    }
};

BEAST_DEFINE_TESTSUITE_MANUAL_PRIO(DistributedValidators, consensus, ripple, 2);

}
}