#pragma once

#include <Eigen/Dense>
#include <random>

template<typename T>
using PCoord = Eigen::Matrix<T, 1, 3>;

int factorial(int n) {
    if(n==0) return 1;
    int ret = 1;
    for(int i=1; i<=n; i++) ret *= i;
    return ret;
}

template<typename T>
class WaveFn {
public:
    virtual ~WaveFn(){};
    // return the value of wave function
    virtual T value(const PCoord<T>&)=0;

    // return three components of grad wave function
    virtual PCoord<T> grad(const PCoord<T>&)=0;

    // return laplacian of the wave function
    virtual T laplace(const PCoord<T>&)=0;
};

// n define the quantum number n of wave function
// for lithium atom only (n=1, 2)
template<typename T, int N>
class SlaterWaveFn: public WaveFn<T> {
public:
    const int n = N;
    const int nterm = 7;
    SlaterWaveFn(const PCoord<T>& r0): r0(r0){
        for(int i=0; i<nterm; i++) {
            // recaluate zeta_val accorindg to orbital
            zeta_val[i] = zeta_val[i]/slap[n-1];
            norm_const[i] = std::pow(2*zeta_val[i], pnu_val[i]-0.5)/
                            std::sqrt(factorial(2*pnu_val[i]));
        }
    }
    ~SlaterWaveFn(){};

    T value(const PCoord<T>& r) {
        auto dist = (r - r0).norm();
        T ret = 0.0;
        for(int i=0; i<nterm; i++) {
            ret += norm_const[i]*phi_val[n-1][i]*
                   std::pow(dist, pnu_val[i]-1)*std::exp(-dist*zeta_val[i]);
        }
        return ret;
    }

    PCoord<T> grad(const PCoord<T>& r) {
        auto dist = (r - r0).norm();
        auto nvec = (r - r0).normalized();
        auto scalar_derv = 0;
        for(int i=0; i<nterm; i++) {
            scalar_derv += norm_const[i]*phi_val[n-1][i]*(
                                (pnu_val[i] - 1)*pow(dist, pnu_val[i] - 2) - zeta_val[i]*pow(dist, pnu_val[i] - 1))*
                                std::exp(-dist*zeta_val[i]);
        }
        return scalar_derv*nvec;
    }

    T laplace(const PCoord<T>& r) {
        auto dist = (r - r0).norm();
        auto scalar_derv = 0;
        auto scalar_dderv = 0;
        for(int i=0; i<nterm; i++) {
            // (ab)'' = a''b + 2a'b'+ ab''
            scalar_derv += norm_const[i]*phi_val[n-1][i]*(
                                (pnu_val[i] - 1)*pow(dist, pnu_val[i] - 2) - zeta_val[i]*pow(dist, pnu_val[i] - 1))*
                                std::exp(-dist*zeta_val[i]);
            scalar_dderv += norm_const[i]*phi_val[n-1][i]*(
                                (pnu_val[i] - 1)*(pnu_val[i] - 2)*pow(dist, pnu_val[i] - 3) -
                                2*zeta_val[i]*(pnu_val[i] - 1)*pow(dist, pnu_val[i] - 2) +
                                zeta_val[i]*zeta_val[i]*pow(dist, pnu_val[i] - 1))*
                                std::exp(-dist*zeta_val[i]);
        }
        scalar_dderv = scalar_dderv + 2*scalar_derv/dist; // why?
        return  scalar_derv;
    }

private:
    PCoord<T> r0;

    T phi_val[2][7] = {
        {-0.12220686, 1.11273225, 0.04125378, 0.09306499, -0.10260021, -0.00034191, 0.00021963},
        {0.47750469, 0.11140449,-1.25954273, -0.18475003, -0.02736293, -0.00025064, 0.00057962}
    };
    T zeta_val[7] = {
        0.72089388, 2.61691643, 0.69257443, 1.37137558, 3.97864549, 13.52900016, 19.30801440
    };
    T norm_const[7];
    int pnu_val[7] = {
        1, 1, 2, 2, 2, 2, 3 // 1s, 1s, 2s, 2s, 2s, 2s, 3s
    };
    T slap[2] = {1.00, 0.95};
};

// For Jastrow term of wave function
template<typename T>
class Jastrow: {
public:
    Jastrow(const PCoord<T> r1, const PCoord<T> r2): r1(r1), r2(r2) {

    }

private:
    PCoord<T> r1, r2;

};

// For lithium atom + Jastrow wave function
// Not a child class of wave function
template<typename T>
class SlaterDet: {
public:
    // Radom initialization, r0 is the position of nucleus
    SlaterDet(const PCoord<T> r0): r0(r0) {
        r1 = PCoord<T>::Random();
        r2 = PCoord<T>::Random();
        r3 = PCoord<T>::Random();
    }

    // Given initial coordinate
    SlaterDet(const PCoord<T> r0, const PCoord<T> r1, const PCoord<T> r2, const PCoord<T> r3)
        :r0(r0), r1(r1), r2(r2), r3(r3) {}

    ~SlaterDate() {
        delete s1, s2, s3;
    }

    T eval() {
        // For initialize evaluation and validation
        sdet << s1->value(r1), s2 -> value(r1), 0.0,
                s1->value(r2), s2 -> value(r2), 0.0,
                0.0, 0.0, s3 -> value(r3);
        inv_sdet = sdet.inverse();
        return sdet.determinant();
    }

    void update(PCoord<T> r, int i) {
        // update the slater det matrix and the inverse matrix
        std::uniform_real_distribution<T> rnum(0, 1);
        PCoord<T> svec;
        switch (i) {
            case 0:
                auto r = r1 + PCoord<T>::Random();
                svec << s1 -> value(r), s2 -> value(r), 0.0;
                break;
            case 1:
                auto r = r2 + PCoord<T>::Random();
                svec << s1 -> value(r), s2 -> value(r), 0.0;
                break;
            case 2;
                auto r = r3 + PCoord<T>::Random();
                svec << 0.0, 0.0, s3->value(r);
                break;
            default:
                break;
        }

        // Calculate Dnew/Dold
        auto ratio = (svec * inv_sdet).sum();
        // apply Sherman–Morrison formula
        // see https://en.wikipedia.org/wiki/Sherman%E2%80%93Morrison_formula
        if(ratio > 1 || ratio > rnum(gen)) {
            auto irow = inv_sdet.rows(i)/ratio;
            inv_sdet = inv_sdet - svec*inv_sdet*inv_sdet.transpose()/ratio;
            inv_sdet.rows(i) = irow;
            update = true;
        } else update = false;
    }

    // do we need this?
    // T value() {
    // }

    PCoord<T> grad(int i) {
        PCoord<T> svec;
        switch (i) {
            case 0:
                svec << s1 -> grad(r1), s2 -> grad(r1), 0.0;
                break;
            case 1:
                svec << s1 -> grad(r2), s2 -> grad(r2), 0.0;
                break;
            case 2;
                svec << 0.0, 0.0, s3->grad(r3);
                break;
            default:
                throw std::runtime_error("No such index" + std::to_string(i) + " exists");
        }
        return inv_sde * svec;
    }

    T energy() {
        // two different cases, whether/not sdet is updated
        if(update) {
            return 0;
        } else {
            return 0;
        }
    }

    T laplace(int i) {
        PCoord<T> svec;
        switch (i) {
            case 0:
                svec << s1 -> laplace(r1), s2 -> laplace(r1), 0.0;
                break;
            case 1:
                svec << s1 -> laplace(r2), s2 -> laplace(r2), 0.0;
                break;
            case 2;
                svec << 0.0, 0.0, s3->laplace(r3);
                break;
            default:
                throw std::runtime_error("No such index" + std::to_string(i) + " exists");
                break;
        }
        return inv_sde * svec;
    }

private:
    PCoord<T> r0;
    PCoord<T> r1, r2, r3;
    SlaterWaveFn<T, 1>* s1;
    SlaterWaveFn<T, 1>* s2; // 1s alpha, 1s beta
    SlaterWaveFn<T, 2>* s3; // 2s alpha
    SlaterDet<T, 3, 3> sdet, inv_sdet; // Slater determinant
    std::random_device rd;
    std::mt19937 rgen{rd()}
    bool update = false;
};


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_SDK_MEMORY_META_NEXT_POWER_OF_2_HPP_INCLUDED
#define NT2_SDK_MEMORY_META_NEXT_POWER_OF_2_HPP_INCLUDED

#include <cstddef>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/integral_c.hpp>

namespace nt2 { namespace details
{
  template<std::size_t N> struct next_power_of_2_impl
  {
    BOOST_STATIC_CONSTANT(std::size_t, x0    = N-1            );
    BOOST_STATIC_CONSTANT(std::size_t, x1    = x0 | (x0 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, x2    = x1 | (x1 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, x3    = x2 | (x2 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, x4    = x3 | (x3 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, x5    = x4 | (x4 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, value = x5 + 1         );
  };
} }

namespace nt2 { namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // Boolean meta-function computing the power of 2 greater or equal to any
  // integral constant.
  // Documentation: next_power_of_2_c.rst
  //////////////////////////////////////////////////////////////////////////////
  template<std::size_t N>
  struct  next_power_of_2_c
        : boost::mpl::size_t<details::next_power_of_2_impl<N>::value> {};

  //////////////////////////////////////////////////////////////////////////////
  // Boolean meta-function computing the power of 2 greater or equal to any
  // Integral Constant.
  // Documentation: next_power_of_2.rst
  //////////////////////////////////////////////////////////////////////////////
  template<class N>
  struct  next_power_of_2
        : boost::mpl::integral_c< typename N::value_type
                                , next_power_of_2_c<N::value>::value
                                > {};
} }

#endif


#include "tile/math/matrix.h"

#include <algorithm>
#include <set>
#include <string>
#include <utility>

#include <boost/format.hpp>

#include "base/util/compat.h"

namespace vertexai {
namespace tile {
namespace math {

struct DualMatrix {
  size_t size_;
  Matrix lhs_;
  Matrix rhs_;

  explicit DualMatrix(const Matrix& m) : size_(m.size1()), lhs_(IdentityMatrix(size_)), rhs_(m) {}

  void swapRows(size_t r1, size_t r2) {
    lhs_.swapRows(r1, r2);
    rhs_.swapRows(r1, r2);
  }
  void multRow(size_t r, Rational v) {
    lhs_.multRow(r, v);
    rhs_.multRow(r, v);
  }
  void addMultRow(size_t d, size_t s, Rational v) {
    lhs_.addRowMultToRow(d, s, v);
    rhs_.addRowMultToRow(d, s, v);
  }

  std::string toString() const {
    std::string r = "";
    for (size_t i = 0; i < size_; i++) {
      for (size_t j = 0; j < size_; j++) {
        r += str(boost::format("%4s, ") % to_string(lhs_(i, j)));
      }
      r += "      ";
      for (size_t j = 0; j < size_; j++) {
        r += str(boost::format("%4s, ") % to_string(rhs_(i, j)));
      }
      r += "\n";
    }
    return r;
  }

  // Do elimination, returns false if singular
  bool invert() {
    // First zero the lower triangle of the RHS
    for (size_t i = 0; i < size_; i++) {
      // Pivot first to non-zero entry in diagonal
      bool found_nonzero = false;
      for (size_t j = i; j < size_; j++) {
        if (rhs_(j, i) != 0) {
          found_nonzero = true;
          swapRows(i, j);
          break;
        }
      }
      if (!found_nonzero) {
        return false;
      }
      // Divide row to make (i, i) == 1
      multRow(i, 1 / rhs_(i, i));
      // Zero this column all the way down
      for (size_t j = i + 1; j < size_; j++) {
        addMultRow(j, i, -rhs_(j, i));
      }
    }
    // Now, zero the upper triangle of the RHS
    for (ssize_t i = size_ - 1; i >= ssize_t(0); i--) {
      // Zero this column all the way up
      for (ssize_t j = i - 1; j >= ssize_t(0); j--) {
        addMultRow(j, i, -rhs_(j, i));
      }
    }
    return true;
  }
};

void Matrix::swapRows(size_t r, size_t s) {
  for (size_t i = 0; i < size2(); i++) {
    std::swap((*this)(r, i), (*this)(s, i));
  }
}

void Matrix::multRow(size_t r, Rational multiplier) {
  for (size_t i = 0; i < size2(); i++) {
    (*this)(r, i) *= multiplier;
  }
}

void Matrix::addRowMultToRow(size_t dest_row, size_t src_row, const Rational& multiplier) {
  if (multiplier != 0) {
    for (size_t i = 0; i < size2(); i++) {
      (*this)(dest_row, i) += multiplier * (*this)(src_row, i);
    }
  }
}

void Matrix::makePivotAt(size_t row, size_t col) {
  if ((*this)(row, col) == 0) {
    throw std::runtime_error("Cannot pivot matrix at entry containing 0");
  }
  for (size_t r = 0; r < size1(); ++r) {
    if (r == row) {
      continue;
    }
    addRowMultToRow(r, row, -(*this)(r, col) / (*this)(row, col));
  }
  multRow(row, 1 / (*this)(row, col));
}

bool Matrix::invert() {
  if (size1() != size2()) {
    throw std::runtime_error("Trying to invert non-square matrix");
  }
  DualMatrix dm(*this);
  if (!dm.invert()) {
    return false;
  }
  *this = dm.lhs_;
  return true;
}

std::string Matrix::toString() const {
  std::string ret;
  ret += "\n";
  for (size_t i = 0; i < size1(); ++i) {
    ret += "[ ";
    for (size_t j = 0; j < size2(); ++j) {
      ret += ((*this)(i, j)).str() + "\t";
    }
    ret += "]\n";
  }
  return ret;
}

bool Matrix::operator==(const Matrix& m) {
  if (size1() != m.size1()) {
    return false;
  }
  if (size2() != m.size2()) {
    return false;
  }
  for (size_t i = 0; i < size1(); i++) {
    for (size_t j = 0; j < size2(); j++) {
      if ((*this)(i, j) != m(i, j)) {
        return false;
      }
    }
  }
  return true;
}

Vector VectorLit(const std::vector<Rational>& vec) {
  Vector r(vec.size());
  for (size_t i = 0; i < vec.size(); i++) {
    r(i) = vec[i];
  }
  return r;
}

Matrix MatrixLit(const std::vector<std::vector<Rational>>& vecs) {
  size_t rows = vecs.size();
  size_t columns = vecs[0].size();
  Matrix r(rows, columns);
  for (size_t i = 0; i < rows; i++) {
    if (vecs[i].size() != columns) {
      throw std::runtime_error("Non-rectangular matrix literal");
    }
    for (size_t j = 0; j < columns; j++) {
      r(i, j) = vecs[i][j];
    }
  }
  return r;
}

bool operator==(const Vector& a, const Vector& b) {
  if (a.size() != b.size()) {
    return false;
  }
  for (size_t i = 0; i < a.size(); i++) {
    if (a(i) != b(i)) {
      return false;
    }
  }
  return true;
}

std::tuple<Matrix, Vector> FromPolynomials(const std::vector<Polynomial<Rational>>& polys) {
  std::set<std::string> vars;
  for (size_t i = 0; i < polys.size(); i++) {
    for (const auto& kvp : polys[i].getMap()) {
      if (kvp.first != "") {
        vars.insert(kvp.first);
      }
    }
  }
  Matrix mat(polys.size(), vars.size());
  Vector vec(polys.size());
  for (size_t i = 0; i < polys.size(); i++) {
    vec(i) = polys[i].constant();
    size_t j = 0;
    for (const auto& v : vars) {
      mat(i, j) = polys[i][v];
      j++;
    }
  }
  return std::tie(mat, vec);
}

struct HermiteCompute {
  size_t rows_;
  size_t columns_;
  Matrix lhs_;

  void swap(size_t i, size_t j) { lhs_.swapRows(i, j); }

  void mult(size_t i, Integer m) {
    if (m != 1 && m != -1) {
      throw std::runtime_error("Cannot multiply row by nonunit constant in computing HNF.");
    }
    lhs_.multRow(i, m);
  }

  void addMult(size_t d, size_t s, Integer m) {
    IVLOG(6, "  Adding " << m << " * row " << s << " to row " << d);
    lhs_.addRowMultToRow(d, s, m);
  }

  void eliminate(size_t i, size_t j) {
    IVLOG(5, "    Eliminate " << i << ", " << j);
    if (lhs_(j, i) == 0) {
      IVLOG(5, "      Already 0, nothing to do");
      return;
    }
    Integer x, y;
    IVLOG(5, "      Computing XGCD of " << lhs_(i, i) << " and " << lhs_(j, i));
    Rational o = XGCD(lhs_(i, i), lhs_(j, i), x, y);
    IVLOG(5, "o = " << o << ", x = " << x << ", y = " << y);
    if (Abs(o) != lhs_(i, i)) {
      if (Abs(o) == Abs(lhs_(j, i))) {
        IVLOG(5, "      Swapping entry");
        swap(i, j);
        if (lhs_(i, i) < 0) {
          mult(i, -1);
        }
      } else {
        IVLOG(5, "      Updating entry");
        euclidean_reduce(i, j, i);
      }
    }
    Rational m = numerator(-lhs_(j, i) / o);
    IVLOG(5, "  m = " << m);
    addMult(j, i, numerator(-lhs_(j, i) / o));
  }

  void euclidean_reduce(size_t i, size_t j, size_t col) {
    Rational a = lhs_(i, col);
    Rational b = lhs_(j, col);
    if (a < 0) {
      a = -a;
      IVLOG(5, "    Negating row " << i)
      mult(i, -1);
      IVLOG(6, "  state\n" << toString());
    }
    if (b < 0) {
      b = -b;
      IVLOG(5, "    Negating row " << j)
      mult(j, -1);
      IVLOG(6, "  state\n" << toString());
    }
    if (a < b) {
      swap(i, j);
      IVLOG(6, "  state\n" << toString());
      a = lhs_(i, col);
      b = lhs_(j, col);
    }

    // Main Euclidean algorithm
    Rational r;
    Integer q = RatDiv(a, b, r);
    IVLOG(6, "Quotient " << q << ", Remainder " << r);
    while (true) {
      addMult(i, j, -q);
      swap(i, j);
      IVLOG(6, "  a = " << a << ", b = " << b << ", state\n" << toString());
      if (r == 0) {
        IVLOG(6, "Remainder 0, stopping");
        break;
      }
      a = b;
      b = r;
      q = RatDiv(a, b, r);
      IVLOG(6, "Quotient " << q << ", Remainder " << r);
    }
  }

  void normalize(size_t i, size_t j) {
    Integer m = -Floor(lhs_(j, i) / lhs_(i, i));
    addMult(j, i, m);
  }

  std::string toString() {
    std::stringstream ss;
    for (size_t i = 0; i < rows_; i++) {
      for (size_t j = 0; j < columns_; j++) {
        ss << lhs_(i, j).str() << " ";
      }
    }
    return ss.str();
  }

 public:
  explicit HermiteCompute(const Matrix& m) : rows_(m.size1()), columns_(m.size2()), lhs_(m) {}

  bool compute() {
    if (rows_ < columns_) {
      return false;
    }
    IVLOG(4, "Computing HNF, initial state\n" << toString());
    // TODO(T132): Technically, this ordering of the algorithm may be
    // exponential in the bit representation of the rational numbers
    // There is a polynomial time version, but the pivoting is tricker
    // so I'm skipping it for now.
    for (size_t i = 0; i < columns_; i++) {
      IVLOG(5, "Fixing column " << i);
      IVLOG(5, "  state\n" << toString());
      // First, we need to position (i, i) with a non-zero entry if possible
      for (size_t j = i; j < rows_; j++) {
        if (lhs_(j, i) != 0) {
          IVLOG(5, "  Swapping " << i << " and " << j);
          swap(i, j);
          IVLOG(6, "  state\n" << toString());
          break;
        }
      }
      // If they are all zeros, whatevs, on to the next column
      if (lhs_(i, i) == 0) {
        IVLOG(5, "  Skipping due to zeros");
        continue;
      }
      // Otherwise, fix sign and combine with all rows below
      if (lhs_(i, i) < 0) {
        IVLOG(6, " Multiplying " << i << " by -1");
        mult(i, -1);
        IVLOG(6, "  state\n" << toString());
      }
      for (size_t j = i + 1; j < rows_; j++) {
        eliminate(i, j);
        IVLOG(6, "  state\n" << toString());
      }
      // And normalize all rows above
      for (size_t j = 0; j < i; j++) {
        normalize(i, j);
        IVLOG(6, "  state\n" << toString());
      }
    }
    IVLOG(4, "Final state\n" << toString());
    return true;
  }
};

bool HermiteNormalForm(Matrix& m) {  // NOLINT(runtime/references)
  HermiteCompute hc(m);
  bool r = hc.compute();
  m = hc.lhs_;
  return r;
}

}  // namespace math
}  // namespace tile
}  // namespace vertexai


#include <iostream>
#include <Eigen/Dense>
#include <string>
#include <fstream>
#include <sstream>
#include <iostream>
#include "LR.hpp"
#include "Adadelta.hpp"
#include "Adagrad.hpp"
#include "Adam.hpp"

#define EIGEN_MPL2_ONLY
using namespace Eigen;
using namespace std;

void read(string filepath, MatrixXd& x,VectorXd& l){
  ifstream ifs(filepath);
  if(ifs.fail()){
    cerr << "File do not exist."<<endl;
    exit(0);
  }
  string s;
  int index = 0;
  while(getline(ifs,s)){
    stringstream ss(s);
    int t;
    ss>>t;
    if(t>0){
      l(index) = 1;
    }else{
      l(index) = 0;
    }
    string feature;
    while(ss>>feature){
      string::size_type idx = feature.find(":");
      int n = atoi(feature.substr(0,idx).c_str());
      double value = atof(feature.substr(idx+1).c_str());
      x(index,n-1) = value;
    }
    ++index;
  }
}

int main()
{
  MatrixXd x = MatrixXd::Zero(32561,123);
  VectorXd label(32561);
  MatrixXd x_test = MatrixXd::Zero(16281,123);
  VectorXd label_test(16281);
  int iteration = 250;
  read("a9a",x,label);
  read("a9a.t",x_test,label_test);
  // LR lr = LR(x.rows(),x.cols(),x,label,0.01,0.1,iteration);
  // lr.train();
  // vector<double> scores;
  // lr.predict(x_test,label_test,scores);
  // cout<<"=== LR ==="<<endl;
  // cout <<"Accuracy:"<< lr.Acc(scores,label_test) <<endl;

  // ofstream iofs("iters.txt");
  // for(double d : lr.iterscores){
  //   iofs<<d<<endl;
  // }

  // ofstream wofs("weight.txt");
  // for(int i = 0; i< lr.w.rows();i++){
  //   wofs<<lr.w(i)<<endl;
  // }

  Adam adam = Adam(x.rows(),x.cols(),x,label,0.01,0.002,0.1,0.001,0.000000001,0.00000001,iteration);
  adam.train();
  vector<double> adamscores;
  adam.predict(x_test,label_test,adamscores);
  cout<<"=== Adam ==="<<endl;
  cout <<"Accuracy:"<< adam.Acc(adamscores,label_test) <<endl;

  ofstream adamiofs("adamiters.txt");
  for(double d : adam.iterscores){
    adamiofs<<d<<endl;
  }

  ofstream adamwofs("adamweight.txt");
  for(int i = 0; i< adam.w.rows();i++){
    adamwofs<<adam.w(i)<<endl;
  }

  // Adadelta addlr = Adadelta(x.rows(),x.cols(),x,label,0.01,0.95,0.0000001,iteration);
  // addlr.train();
  // vector<double> addscores;
  // addlr.predict(x_test,label_test,addscores);
  // cout<<"=== Adadelta=="<<endl;
  // cout <<"Accuracy:"<< lr.Acc(addscores,label_test) <<endl;

  // ofstream addiofs("adadelta_iters.txt");
  // for(double d : addlr.iterscores){
  //   addiofs<<d<<endl;
  // }

  // ofstream addwofs("adadelta_weight.txt");
  // for(int i = 0; i< addlr.w.rows();i++){
  //   addwofs<<lr.w(i)<<endl;
  // }

  // Adagrad adglr = Adagrad(x.rows(),x.cols(),x,label,0.01,0.1,iteration);
  // adglr.train();
  // vector<double> adgscores;

  // adglr.predict(x_test,label_test,adgscores);
  // cout<<"=== Adagrad ==="<<endl;
  // cout <<"Accuracy:"<< adglr.Acc(adgscores,label_test) <<endl;

  // ofstream adgiofs("adagrad_iters.txt");
  // for(double d : adglr.iterscores){
  //   adgiofs<<d<<endl;
  // }

  // ofstream adgwofs("adagrad_weight.txt");
  // for(int i = 0; i< adglr.w.rows();i++){
  //   adgwofs<<adglr.w(i)<<endl;
  // }
}


#include <NTL/ZZ.h>
#include <cstdint>
#include <cmath>

#define NUM_BITS_REAL_MANTISSA 54

#include "binomials.hpp"
#include <cmath>

int main(int argc, char* argv[]){
  if(argc < 5){
     std::cout << "Complexity estimation of key enumeration" << std::endl << " Usage "
               << argv[0] << " <p> <d_v> <n_0> <m_0> <m_1> <m_2> <m_3>" << std::endl <<
               "All the non existing m_i should be passed as zeroes " << std::endl;
    return -1;
  }

  InitBinomials();
  NTL::RR::SetPrecision(NUM_BITS_REAL_MANTISSA);
  pi = NTL::ComputePi_RR();
  uint32_t p = atoi(argv[1]);
  uint32_t d_v = atoi(argv[2]);
  uint32_t n_0 = atoi(argv[3]);
  uint32_t m[4];
  m[0] = atoi(argv[4]);
  m[1] = atoi(argv[5]);
  m[2] = atoi(argv[6]);
  m[3] = atoi(argv[7]);

  NTL::RR Henum, Qenum;

  Henum = lnBinom(NTL::RR(p),NTL::RR(d_v));
  Henum = Henum*NTL::RR(n_0) / NTL::log(NTL::RR(2));

  Qenum = NTL::RR(n_0)* ( lnBinom(NTL::RR(p),NTL::RR(m[0])) +
                          lnBinom(NTL::RR(p),NTL::RR(m[1])) +
                          lnBinom(NTL::RR(p),NTL::RR(m[2])) +
                          lnBinom(NTL::RR(p),NTL::RR(m[3])) )/ NTL::log(NTL::RR(2));

  std::cout << "H enum classic/quantum cost :" << Henum << "  " << (Henum/NTL::RR(2)) << std::endl;
  std::cout << "Q enum classic/quantum cost :" << Qenum << "  " << (Qenum/NTL::RR(2)) << std::endl;
  return 0;
}


#include <opengv/optimization_tools/objective_function_tools/ObjectiveFunctionInfo.hpp>
#include <opengv/optimization_tools/solver_tools/SolverToolsNoncentralRelativePose.hpp>
#include <Eigen/Dense>
#include <opengv/types.hpp>
#include <iostream>

Eigen::Matrix3d SolverToolsNoncentralRelativePose::exp_R( Eigen::Matrix3d & X ){
  double phi = X.norm()/std::sqrt(2);
  Eigen::Matrix3d X1 = X/phi;
  Eigen::Matrix3d I = Eigen::Matrix< double, 3, 3 >::Identity();
  I = I + std::sin(phi)*X1 + ( 1 - std::cos(phi) )*X1*X1;
  return I;
}

opengv::rotation_t SolverToolsNoncentralRelativePose::rotation_solver(opengv::rotation_t & state_rotation, const opengv::translation_t & translation,
								      double &tol, ObjectiveFunctionInfo * info_function, int & k){
  double g = 1.0;
  double erro = 1.0;
  k = 0;
  opengv::rotation_t X = state_rotation;
  opengv::rotation_t previous_X = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d Z  = Eigen::Matrix3d::Zero(3,3);
  double zz = 0;
  Eigen::Matrix3d DX = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d Pt = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d P  = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d Q  = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d Qt = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d reference = Eigen::Matrix3d::Identity(3,3);
  //std::cout << "Inside rotation solver to check the values: " << std::endl;
  //std::cout << "Before process starts the rotation matrix is: " << X << std::endl;

  while( erro > tol && k < 1e3 )
    {

      //std::cout << "state at the beginning: " << std::endl << X << std::endl << std::endl;
      DX = info_function->rotation_gradient(X, translation);
      //std::cout << "Calculated gradient: " << std::endl << DX << std::endl << std::endl;
      Z   = DX*X.transpose() - X*DX.transpose();
      //std::cout << "Calculated riemaniann gradient: " << std::endl << Z << std::endl << std::endl;
      zz  = 0.5*( Z*Z.transpose() ).trace();
      //std::cout << "Coefficient zz : " << zz << std::endl << std::endl;
      Pt  = -g*Z;
      //std::cout << "Matrix Pt: " << std::endl << Pt << std::endl << std::endl;
      P   = exp_R( Pt );
      //std::cout << "The rotation matrix P: " << std::endl << P << std::endl << std::endl;
      Q   = P*P; // this seems strange
      //std::cout << "Matrix Q: " << std::endl << Q << std::endl << std::endl;
      Qt  = Q*X;
      //std::cout << "Matrix Qt: " << std::endl << Qt << std::endl << std::endl;
      //std::cout << "************************************************************" << std::endl << std::endl;
      //std::cout << "1st CYCLE" << std::endl << std::endl;

      //while( ( objective_function( M, X, translation ) - objective_function(M, Qt, translation ) ) >= g*zz  )
      while( ( info_function->objective_function_value(X, translation ) - info_function->objective_function_value(Qt, translation ) ) >= g*zz  )
        {
          g   = 2*g;
          //In order to prevent NAN's the following restriction is added
          if(g < 64){
            //std::cout << "g: " << g << std::endl;
            P   = Q;
            //std::cout << std::endl << "New P: " << std::endl << P << std::endl;
            Q   = P*P; // this seems strange
            //std::cout << std::endl << "New Q: " << std::endl << Q << std::endl;
            Qt  = Q*X;
            //std::cout << std::endl << "New Qt: " << std::endl << Qt << std::endl;
            //std::cout << "Current value for obj function: " << objective_function(M, X, translation) << std::endl;
            //std::cout << "Current value for new obj function: " << objective_function(M, Qt, translation ) << std::endl;
          }
          else{
            break;
          }
        }
      //std::cout << "End of 1st cycle" << std::endl;
      //std::cout << "******************************************************************" << std::endl;
      //std::cout << "**********************************************************************" << std::endl;
      //std::cout << "Enters 2nd cycle: " << std::endl;
      Qt = P * X;
      //while( ( objective_function( M, X, translation ) - objective_function(M, Qt, translation ) ) < 0.5*g*zz)
      while( ( info_function->objective_function_value( X, translation ) - info_function->objective_function_value( Qt, translation ) ) < 0.5*g*zz)
        {

          //   if ( f_obj( M, N, X, beta) - f_obj( M, N, Qt, beta ) < tol )
          //     break;

          g  = 0.5*g;
          //std::cout << "New g: " << std::endl << g << std::endl << std::endl;
          Pt = -g*Z;
          //std::cout << "New Pt: " << std::endl << Pt << std::endl << std::endl;
          P  = exp_R( Pt );
          //In order to prevent NAN's
          if( (P - reference).norm() < 1e-6){
            break;
          }
          //std::cout << "New P : " << std::endl << P << std::endl << std::endl;
          Qt = P*X;
          //std::cout << "New Qt: " << std::endl << std::endl << Qt << std::endl;
        }
      previous_X = X;
      X    = P*X;
      erro = ( X - previous_X ).norm();
      // std::cout << "Rotation: "    << std::endl << X << std::endl;
      /*std::cout << "inside rotation solver: " << std::endl;
      std::cout << "\nIteration: " << k << std::endl;
      std::cout << "Rotation: "    << std::endl << X << std::endl;
      std::cout << "Euclidean grad: " << std::endl << DX << std::endl;
      std::cout << "Function value: " << info_function->objective_function_value(X, translation) << std::endl;*/
      k++;
    }
  return X;
}

opengv::translation_t SolverToolsNoncentralRelativePose::translation_solver(const opengv::rotation_t & rotation, opengv::translation_t & translation, double &tol, ObjectiveFunctionInfo * info_function, double & step, int & k){

  double error = 1;
  k = 0;
  //std::cout << "Translation gradient: " << std::endl;
  opengv::translation_t state = translation;
  opengv::translation_t grad = info_function->translation_gradient(rotation, state);
  opengv::translation_t new_state = state - step * grad;
  //std::cout << "Beginning of the translation solver: " << std::endl;
  //std::cout << "The rotation used is: " << std::endl << rotation << std::endl;
  while (error > tol  && k < 1000){
    new_state = state - step * grad;
    double f_obj_current = info_function->objective_function_value(rotation, state);
    double f_obj_next = info_function->objective_function_value(rotation, new_state);

    /*std::cout << "current state: " << std::endl     << state      << std::endl;
    std::cout << "new state: "     << std::endl     << new_state  << std::endl;
    std::cout << "f(state): "      << f_obj_current << std::endl;
    std::cout << "f(new_state): "  << f_obj_next    << std::endl;
    std::cout << "gradient:     "  << std::endl     << grad       << std::endl;
    std::cout << std::endl         << std::endl     << std::endl;*/
    if(f_obj_next > f_obj_current){
      break;
    }
    error = std::abs(f_obj_current - f_obj_next);
    grad = info_function->translation_gradient(rotation, state);

    state = new_state;
    k++;
  }
  return state;
}


#include <algorithm>
#include <iostream>
#include <numeric>
#include <sstream>
#include <chrono>

#include <Eigen/SVD>

#include "ICP.h"
#include "ICPPoint.h"
#include "Misc.h"
#include "Point.h"
#include "PointOP.h"

ICP::ICP(std::array<PointCloudData, 2> &pcd) :
    m_point_cloud_data(pcd)
{
    m_LTS = 1.0;
    m_text = NULL;
    m_max_points = 0;
}

void ICP::SetLTS(float a) { m_LTS = a; }

void ICP::Run(Eigen::Matrix4d& Transform)
{
    // Some distance statistics
    std::vector<double> dist_sq;

    auto update_text = [&]() {
        while (m_app->Pending()) {
            m_app->Dispatch();
        }
    };

    if (m_text) {
        m_text->AppendText("Determining square distance to trim at ...\n");
        update_text();
    }

    for (size_t i = 0; i < m_points1.size(); i++) {
        if (i % 100000 == 0) {
            if (m_text) {
                std::stringstream ss;

                ss << i * 100 / (float)m_points1.size() << "% ";
                m_text->AppendText(ss.str());

                update_text();
            }
        }

        double dist;
        int index;

        m_ANN_points2.FindClosest(m_points1[i], dist, index);

        m_points1[i].nearest = m_points2[index];
        m_points1[i].dist_sq = dist;

        dist_sq.push_back(dist);
    }

    if (m_text) {
        m_text->AppendText("100%\n");
        update_text();
    }

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

    size_t pos = (int)(dist_sq.size() * m_LTS);

    if (pos >= dist_sq.size()) {
        pos = dist_sq.size() - 1;
    }

    double trimDist = dist_sq[pos];

    if (m_text) {
        std::stringstream ss;
        ss << "Trimming at distance: " << trimDist << "\n";
        m_text->AppendText(ss.str());
        update_text();
    }

    std::vector<ICPPoint> Points2p;

    // Find which points to use based on trimDist and also finds the centroid of
    // the two points
    Point centroid1, centroid2;

    centroid1.x = 0;
    centroid1.y = 0;
    centroid1.z = 0;

    centroid2.x = 0;
    centroid2.y = 0;
    centroid2.z = 0;

    double sum_dist = 0;
    int count = 0;

    for (size_t i = 0; i < m_points1.size(); i++) {
        if (m_points1[i].dist_sq > trimDist) {
            continue;
        }

        sum_dist += m_points1[i].dist_sq;

        centroid1.x += m_points1[i].x;
        centroid1.y += m_points1[i].y;
        centroid1.z += m_points1[i].z;

        centroid2.x += m_points1[i].nearest.x;
        centroid2.y += m_points1[i].nearest.y;
        centroid2.z += m_points1[i].nearest.z;

        count++;
    }

    m_MSE = sum_dist / count;

    centroid1.x /= count;
    centroid1.y /= count;
    centroid1.z /= count;

    centroid2.x /= count;
    centroid2.y /= count;
    centroid2.z /= count;

    /************************************
   * FIND OPTIMAL ROTATION USING SVD
   ************************************/

    Eigen::Matrix3d H;

    H.setZero();

    if (m_text) {
        m_text->AppendText("Calculating optimal transform ... \n");
        update_text();
    }
    std::chrono::system_clock::time_point start = std::chrono::system_clock::now();

    for (size_t i = 0; i < m_points1.size(); i++) {
        if (m_points1[i].dist_sq > trimDist) {
            continue;
        }

        Eigen::Vector3d A, B;

        A(0) = m_points1[i].x - centroid1.x;
        A(1) = m_points1[i].y - centroid1.y;
        A(2) = m_points1[i].z - centroid1.z;

        B(0) = m_points1[i].nearest.x - centroid2.x;
        B(1) = m_points1[i].nearest.y - centroid2.y;
        B(2) = m_points1[i].nearest.z - centroid2.z;

        H += A * B.transpose();
    }

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);

    // Optimal rotation
    Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose();

    // Final transformation matrix is T*R*Tc
    // Tc - translates point1 to the centre for finding the rotation
    // R - optimal rotation
    // T - translates point1 back to the centre of point2

    Eigen::Matrix4d Tc, T, RR;

    Tc.setIdentity();
    T.setIdentity();
    RR.setIdentity();

    Tc(0, 3) = -centroid1.x;
    Tc(1, 3) = -centroid1.y;
    Tc(2, 3) = -centroid1.z;

    T(0, 3) = centroid2.x;
    T(1, 3) = centroid2.y;
    T(2, 3) = centroid2.z;

    RR.block(0,0,3,3) = R;

    Transform = T * (RR * Tc);

    PointOP::ApplyTransform(m_points1, Transform);

    // Clean up memory
    {
        std::vector<ICPPoint> Empty;
        Points2p.swap(Empty);
    }
}

void ICP::SetMaxPoints(unsigned int max) { m_max_points = max; }

void ICP::SetPoints(std::vector<Point>& P1, std::vector<Point>& P2,
    float dist_threshold)
{
    const float sq_dist = dist_threshold * dist_threshold;

    if (m_max_points == 0) {
        throw std::runtime_error("ICP: Need to set m_max_points before running SetPoints");
    }

    ANN Point1DB, Point2DB;
    std::vector<Point> filtered1, filtered2;
    Point start, end;

    reverseable_shuffle_forward(P1, m_point_cloud_data[0].table);
    reverseable_shuffle_forward(P2, m_point_cloud_data[1].table);

    auto update_text = [&]() {
        while (m_app->Pending()) {
            m_app->Dispatch();
        }
    };

    // NOTE:
    // The aim of this is to save as much memory as possible.
    // We downsample the points before loading it into the ANN library. At the
    // same time we want to avoid over downsampling. We want the process result
    // in: point cloud2 -> overlapping region filtering -> down sample > initial outlier
    // distance -> max point filtering = number of points point cloud2 is m_max_points So
    // basically, we want all the filtering and downsampling stuff to leave enough
    // points to meet the user's requested MaxPoint parameter if possible

    // Downsample based on a factor of m_max_points
    size_t k = m_max_points * 2;

    if (P1.size() > k) {
        filtered1.resize(k);
    } else {
        filtered1.resize(P1.size());
    }

    for (size_t i = 0; i < filtered1.size(); i++) {
        filtered1[i] = P1[i];
    }

    if (P2.size() > k) {
        filtered2.resize(k);
    } else {
        filtered2.resize(P2.size());
    }

    for (size_t i = 0; i < filtered2.size(); i++) {
        filtered2[i] = P2[i];
    }

    // This consumes the most memory out of the entire program
    // Every 1 million point uses about 200MB of memory when loaded using ANN
    Point2DB.SetPoints(filtered2);

    // for displaying purposes
    if (m_text) {
        m_text->AppendText("Downsampling and filtering first point cloud for ICP ... \n");
    }

    for (size_t i = 0;
         i < filtered1.size() && m_points1.size() < m_max_points; i++) {
        if (i % 100000 == 0) {
            if (m_text) {
                std::stringstream ss;

                ss << i * 100 / (float)filtered1.size() << "% ";
                m_text->AppendText(ss.str());

                update_text();
            }
        }

        double dist;
        int index;

        Point2DB.FindClosest(filtered1[i], dist, index);

        if (dist < sq_dist) {
            m_points1.push_back(ICPPoint(filtered1[i]));
        }
    }

    if (m_text) {
        m_text->AppendText("100%\n");

        std::stringstream ss;
        ss << "Number of points after downsampling/filtering: " << m_points1.size() << "\n";
        m_text->AppendText(ss.str());

        update_text();
    }

    Point2DB.Free();

    {
        std::vector<Point> empty;
        filtered1.swap(empty);
    }

    Point1DB.SetPoints(m_points1);

    if (m_text) {
        m_text->AppendText("Downsampling and filtering second point cloud for ICP ... \n");
    }

    // Do the same again

    for (size_t i = 0;
         i < filtered2.size() && m_points2.size() < m_max_points; i++) {
        if (i % 100000 == 0) {
            if (m_text) {
                std::stringstream ss;
                ss << i * 100 / (float)filtered2.size() << "% ";
                m_text->AppendText(ss.str());

                update_text();
            }
        }

        double dist;
        int index;

        Point1DB.FindClosest(filtered2[i], dist, index);

        if (dist < sq_dist) {
            m_points2.push_back(filtered2[i]);
        }
    }

    if (m_text) {
        m_text->AppendText("100%\n");

        std::stringstream ss;
        ss << "Number of points after downsampling/filtering: " << m_points2.size() << "\n";
        m_text->AppendText(ss.str());

        update_text();
    }

    Point1DB.Free();

    m_ANN_points2.SetPoints(m_points2);

    reverseable_shuffle_backward(P1, m_point_cloud_data[0].table);
    reverseable_shuffle_backward(P2, m_point_cloud_data[1].table);
}

void ICP::Seteps(float e)
{
    m_eps = e;
    m_ANN_points2.Seteps(m_eps);
}

double ICP::GetMSE() { return m_MSE; }
void ICP::SetwxTextCtrl(wxTextCtrl* t) // Used for m_text feedback from ICP
{
    m_text = t;
}
void ICP::SetwxApp(wxApp* a) // Used for m_text feedback from ICP
{
    m_app = a;
}


/*! @file pitts_fixed_tensor3_split.hpp
* @brief contract two simple fixed-dimension rank-3 tensors (along third and first dimension)
* @author Melven Roehrig-Zoellner <Melven.Roehrig-Zoellner@DLR.de>
* @date 2019-12-29
* @copyright Deutsches Zentrum fuer Luft- und Raumfahrt e. V. (DLR), German Aerospace Center
*
**/

// include guard
#ifndef PITTS_FIXED_TENSOR3_SPLIT_HPP
#define PITTS_FIXED_TENSOR3_SPLIT_HPP

// includes
#include <array>
#pragma GCC push_options
#pragma GCC optimize("no-unsafe-math-optimizations")
#include <Eigen/Dense>
#pragma GCC pop_options
#include "pitts_fixed_tensor3.hpp"
#include "pitts_timer.hpp"

//! namespace for the library PITTS (parallel iterative tensor train solvers)
namespace PITTS
{
  //! split a fixed-size rank-3 tensor into 2 smaller tensors
  //!
  //! Split t3c into t3a and t3b such that
  //!   t3c_(i,k,j) = sum_l t3a_(i,k1,l) * t3b_(l,k2,j)   with k=k2*N+k1
  //!
  //! @tparam T  underlying data type (double, complex, ...)
  //! @tparam N  dimension
  //!
  //! @param[in]  t3c       rank-3 tensor
  //! @param[out] t3a       first part of splitted rank-3 tensor
  //! @param[out] t3b       second part of splitted rank-3 tensor
  //! @param[in]  leftOrtog make left part (t3a) orthogonal if true, otherwise t3b is made orthogonal
  //!
  template<typename T, int N>
  void split(const FixedTensor3<T,N*N>& t3c, FixedTensor3<T,N>& t3a, FixedTensor3<T,N>& t3b, bool leftOrthog = true)
  {
    const auto timer = PITTS::timing::createScopedTimer<FixedTensor3<T,N>>();

    using Matrix = Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>;
    using Stride = Eigen::OuterStride<Eigen::Dynamic>;
    using Map = Eigen::Map<Matrix, Eigen::Aligned128, Stride>;
    using ConstMap = Eigen::Map<const Matrix, Eigen::Aligned128, Stride>;

    const auto r1 = t3c.r1();
    const auto r2 = t3c.r2();
    if( r1*r2 == 0 )
      throw std::invalid_argument("Unsupported dimension of zero!");

    const auto t3cMap = ConstMap(&t3c(0,0,0), N*r1, N*r2, Stride(N*r1));

    // use a faster QR algorithm (at the risk of a badly estimated truncation error)
    if( leftOrthog )
    {
      Eigen::ColPivHouseholderQR<Matrix> qr(t3cMap);
      qr.setThreshold(1.e-10);
      const auto r = std::max(Eigen::Index(1), qr.rank());

      t3a.resize(r1,r);
      t3b.resize(r,r2);

      // A P = Q R
      // => A = Q (R P^(-1))
      const Matrix Q = qr.matrixQ();
      const Matrix R = qr.matrixR().topRows(r).template triangularView<Eigen::Upper>();
      const auto P = qr.colsPermutation();
      Map(&t3a(0,0,0), r1*N, r, Stride(r1*N)) = Q.leftCols(r);
      Map(&t3b(0,0,0), r, r2*N, Stride(r)) = R * P.inverse();
    }
    else // rightOrthog
    {
      Eigen::ColPivHouseholderQR<Matrix> qr(t3cMap.transpose());
      qr.setThreshold(1.e-10);
      const auto r = std::max(Eigen::Index(1), qr.rank());

      t3a.resize(r1,r);
      t3b.resize(r,r2);

      // A^T P = Q R
      // => P^T A = R^T Q^T
      // => A = (R P^(-1))^T Q^T
      const Matrix Q = qr.matrixQ();
      const Matrix R = qr.matrixR().topRows(r).template triangularView<Eigen::Upper>();
      const auto P = qr.colsPermutation();
      Map(&t3a(0,0,0), r1*N, r, Stride(r1*N)) = (R * P.inverse()).transpose();
      Map(&t3b(0,0,0), r, r2*N, Stride(r)) = Q.leftCols(r).transpose();
    }
    /*
    auto svd = Eigen::JacobiSVD<Matrix, Eigen::HouseholderQRPreconditioner>(t3cMap, Eigen::ComputeThinV | Eigen::ComputeThinU);
    svd.setThreshold(1.e-10);
    const auto r = svd.rank();

    t3a.resize(r1,r);
    t3b.resize(r,r2);
    if( leftOrthog )
    {
      Map(&t3a(0,0,0), r1*N,r) = svd.matrixU().leftCols(r);
      Map(&t3b(0,0,0), r,r2*N) = svd.singularValues().head(r).asDiagonal() * svd.matrixV().leftCols(r).adjoint();
    }
    else
    {
      Map(&t3a(0,0,0), r1*N,r) = svd.matrixU().leftCols(r) * svd.singularValues().head(r).asDiagonal();
      Map(&t3b(0,0,0), r,r2*N) = svd.matrixV().leftCols(r).adjoint();
    }
    //std::cout << "Singular value |.|: " << svd.singularValues().head(r).transpose().array().abs() << "\n";
    */
  }

}

#endif // PITTS_FIXED_TENSOR3_SPLIT_HPP


#ifndef SPLINE_FITTER_HPP
#define SPLINE_FITTER_HPP
#include <vector>
#include <Eigen/SparseLU>

/**
 * Quadratic B-spline curve fitter
 * This class computes the control points necessary to make a quadratic B-spline curve
 * interpolating a known set of points.
 *
 */
template<typename T>
class QuadraticSplineFitter
{
public:

  /**
   * Default constructor
   */
  QuadraticSplineFitter() : QuadraticSplineFitter({})
  {
  };

  /**
   * Constructor
   * @param points Points to interpolate
   */
  QuadraticSplineFitter(std::vector<T>& points){
    setPoints(points);
  }

  /**
   * Set points to interpolate
   * @param points points to interpolate
   */
  inline void
  setPoints(std::vector<T>& points)
  {
    m_points = points;
  }

  /**
   * Compute the control points necessary to interpolate the points set by setPoints
   * @return the control points
   */
  std::vector<T>
  compute_control_points() const
  {
    // Here:
    // Build a NxN matrix such that
    // matrix[row][row] =  matrix[row][row+1] = 1/2
    // rest = 0
    // build B such that B = [ep_start points ep_stop]
    // Solve Ax = B
    // x are the knots
    int n = m_points.size()+2;
    Eigen::SparseMatrix<T> A(n,n);

    Eigen::Matrix<T, Eigen::Dynamic, 1> b(n);
    std::vector<Eigen::Triplet<T> > triplets;
    triplets.reserve((n)*2);

    // Initialize coefficients for the points to reach

    for(int i = 1; i < n-1; i++)
    {
      triplets.push_back(Eigen::Triplet<T>(i,i-1,0.125));
      triplets.push_back(Eigen::Triplet<T>(i,i,0.75));
      triplets.push_back(Eigen::Triplet<T>(i,i+1,0.125));
      b(i) = m_points[i-1];
    }

    __insertStartpointCoeffs(triplets,b,0);
    __insertEndpointCoeffs(triplets,b,n-1);
    A.setFromTriplets(triplets.begin(), triplets.end());

    // Now solve the linear system using linear least squares
    Eigen::Matrix<T, Eigen::Dynamic, 1> x(n);
    Eigen::SparseLU<Eigen::SparseMatrix<T> > solver;

    solver.compute(A);
    x = solver.solve(b);

    //     printVector(x,n);
    // Copy back to std::vector
    std::vector<T> ret;

    ret.reserve(n);
    for(int i = 0; i < n; i++)
    {
      ret.push_back(x(i));
    }
    return ret;

  }

private:
  inline void
  __insertStartpointCoeffs( std::vector<Eigen::Triplet<T> >& triplets,
			    Eigen::Matrix<T, Eigen::Dynamic, 1>& b,
			    const int i) const
  {
    triplets.push_back(Eigen::Triplet<T>(i,i,-1));
    triplets.push_back(Eigen::Triplet<T>(i,i+1,1));
    b(i) = 0.0;
  }

  inline void
  __insertEndpointCoeffs( std::vector<Eigen::Triplet<T> >& triplets,
			  Eigen::Matrix<T, Eigen::Dynamic, 1>& b,
			  const int i) const
  {
    triplets.push_back(Eigen::Triplet<T>(i,i-1,-1));
    triplets.push_back(Eigen::Triplet<T>(i,i,1));
    b(i) = 0.0;
  }

  std::vector<T> m_points;

};

#endif


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

#ifndef SMOOTH__IMPL__SE2_HPP_
#define SMOOTH__IMPL__SE2_HPP_

#include <Eigen/Core>

#include "common.hpp"
#include "so2.hpp"

namespace smooth {

/**
 * @brief SE(2) Lie Group represented as C^1 ⋉ R^2
 *
 * Memory layout
 * -------------
 * Group:    x y qz qw
 * Tangent:  vx vy Ωz
 *
 * Lie group Matrix form
 * ---------------------
 * [ qw -qz x ]
 * [ qz  qw y ]
 * [  0   0 1 ]
 *
 * Lie algebra Matrix form
 * -----------------------
 * [ 0 -Ωz vx ]
 * [ Ωz  0 vy ]
 * [  0  0  0 ]
 *
 * Constraints
 * -----------
 * Group:   qz * qz + qw * qw = 1
 * Tangent: -pi < Ωz <= pi
 */
template<typename _Scalar>
class SE2Impl
{
public:
  using Scalar = _Scalar;

  static constexpr Eigen::Index RepSize = 4;
  static constexpr Eigen::Index Dim     = 3;
  static constexpr Eigen::Index Dof     = 3;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out) { g_out << Scalar(0), Scalar(0), Scalar(0), Scalar(1); }

  static void setRandom(GRefOut g_out)
  {
    g_out.template head<2>().setRandom();
    SO2Impl<Scalar>::setRandom(g_out.template tail<2>());
  }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    m_out.setIdentity();
    SO2Impl<Scalar>::matrix(g_in.template tail<2>(), m_out.template topLeftCorner<2, 2>());
    m_out.template topRightCorner<2, 1>() = g_in.template head<2>();
  }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    SO2Impl<Scalar>::composition(
      g_in1.template tail<2>(), g_in2.template tail<2>(), g_out.template tail<2>());
    Eigen::Matrix<Scalar, 2, 2> R1;
    SO2Impl<Scalar>::matrix(g_in1.template tail<2>(), R1);
    g_out.template head<2>() = R1 * g_in2.template head<2>() + g_in1.template head<2>();
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    Eigen::Matrix<Scalar, 2, 1> so2inv;
    SO2Impl<Scalar>::inverse(g_in.template tail<2>(), so2inv);

    Eigen::Matrix<Scalar, 2, 2> Rinv;
    SO2Impl<Scalar>::matrix(so2inv, Rinv);

    g_out.template head<2>() = -Rinv * g_in.template head<2>();
    g_out.template tail<2>() = so2inv;
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    using std::tan;

    Eigen::Matrix<Scalar, 1, 1> so2_log;
    SO2Impl<Scalar>::log(g_in.template tail<2>(), so2_log);
    const Scalar th  = so2_log(0);
    const Scalar th2 = th * th;

    const Scalar B = th / Scalar(2);
    Scalar A;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+x+%2F+tan+x+at+x%3D0
      A = Scalar(1) - th2 / Scalar(12);
    } else {
      A = B / tan(B);
    }

    Eigen::Matrix<Scalar, 2, 2> Sinv;
    Sinv(0, 0) = A;
    Sinv(1, 1) = A;
    Sinv(0, 1) = B;
    Sinv(1, 0) = -B;

    a_out.template head<2>() = Sinv * g_in.template head<2>();
    a_out(2)                 = th;
  }

  static void Ad(GRefIn g_in, TMapRefOut A_out)
  {
    SO2Impl<Scalar>::matrix(g_in.template tail<2>(), A_out.template topLeftCorner<2, 2>());
    A_out(0, 2) = g_in(1);
    A_out(1, 2) = -g_in(0);
    A_out(2, 0) = Scalar(0);
    A_out(2, 1) = Scalar(0);
    A_out(2, 2) = Scalar(1);
  }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    using std::cos, std::sin;

    const Scalar th  = a_in.z();
    const Scalar th2 = th * th;

    Scalar A, B;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+sin+x+%2F+x+at+x%3D0
      A = Scalar(1) - th2 / Scalar(6);
      // https://www.wolframalpha.com/input/?i=series+%28cos+x+-+1%29+%2F+x+at+x%3D0
      B = -th / Scalar(2) + th * th2 / Scalar(24);
    } else {
      A = sin(th) / th;
      B = (cos(th) - Scalar(1)) / th;
    }

    Eigen::Matrix<Scalar, 2, 2> S;
    S(0, 0) = A;
    S(1, 1) = A;
    S(0, 1) = B;
    S(1, 0) = -B;

    g_out.template head<2>() = S * a_in.template head<2>();
    SO2Impl<Scalar>::exp(a_in.template tail<1>(), g_out.template tail<2>());
  }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out.setZero();
    SO2Impl<Scalar>::hat(a_in.template tail<1>(), A_out.template topLeftCorner<2, 2>());
    A_out.template topRightCorner<2, 1>() = a_in.template head<2>();
  }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    SO2Impl<Scalar>::vee(A_in.template topLeftCorner<2, 2>(), a_out.template tail<1>());
    a_out.template head<2>() = A_in.template topRightCorner<2, 1>();
  }

  static void ad(TRefIn a_in, TMapRefOut A_out)
  {
    A_out.setZero();
    SO2Impl<Scalar>::hat(a_in.template tail<1>(), A_out.template topLeftCorner<2, 2>());
    A_out(0, 2) = a_in.y();
    A_out(1, 2) = -a_in.x();
  }

  static void dr_exp(TRefIn a_in, TMapRefOut A_out)
  {
    using TangentMap = Eigen::Matrix<Scalar, 3, 3>;
    using std::sin, std::cos;

    const Scalar th2 = a_in.z() * a_in.z();

    Scalar A, B;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+%281-cos+x%29+%2F+x%5E2+at+x%3D0
      A = Scalar(1) / Scalar(2) - th2 / Scalar(24);
      // https://www.wolframalpha.com/input/?i=series+%28x+-+sin%28x%29%29+%2F+x%5E3+at+x%3D0
      B = Scalar(1) / Scalar(6) - th2 / Scalar(120);
    } else {
      const Scalar th = a_in.z();
      A               = (Scalar(1) - cos(th)) / th2;
      B               = (th - sin(th)) / (th2 * th);
    }

    TangentMap ad_a;
    ad(a_in, ad_a);
    A_out = TangentMap::Identity() - A * ad_a + B * ad_a * ad_a;
  }

  static void dr_expinv(TRefIn a_in, TMapRefOut A_out)
  {
    using TangentMap = Eigen::Matrix<Scalar, 3, 3>;
    using std::sin, std::cos;

    const Scalar th  = a_in.z();
    const Scalar th2 = th * th;

    Scalar A;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+1%2Fx%5E2+-+%281+%2B+cos+x%29+%2F+%282+*+x+*+sin+x%29+at+x%3D0
      A = Scalar(1) / Scalar(12) + th2 / Scalar(720);
    } else {
      A = (Scalar(1) / th2) - (Scalar(1) + cos(th)) / (Scalar(2) * th * sin(th));
    }

    TangentMap ad_a;
    ad(a_in, ad_a);
    A_out = TangentMap::Identity() + ad_a / 2 + A * ad_a * ad_a;
  }
};

}  // namespace smooth

#endif  // SMOOTH__IMPL__SE2_HPP_


#include <boost/math/special_functions/bessel.hpp>

/**
 * Bessel J_0 function
 *
 * @param  n index
 * @param x argument
 * @return value
 */
extern "C"
double j0(double x) {
    return boost::math::cyl_bessel_j<int, double>(0, x);
}

/**
 * Bessel Y_0 function
 *
 * @param  n index
 * @param x argument
 * @return value
 */
extern "C"
double y0(double x) {
    return boost::math::cyl_neumann<int, double>(0, x);
}

/**
 * Bessel J_1 function
 *
 * @param  n index
 * @param x argument
 * @return value
 */
extern "C"
double j1(double x) {
    return boost::math::cyl_bessel_j<int, double>(1, x);
}

/**
 * Bessel Y_1 function
 *
 * @param  n index
 * @param x argument
 * @return value
 */
extern "C"
double y1(double x) {
    return boost::math::cyl_neumann<int, double>(1, x);
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CLTV_PROBLEM_HPP_
#define CLTV_PROBLEM_HPP_

#include <Eigen/Dense>
#include <cstdint>

struct CltvOcpDi
{
  // Must be defined
  constexpr static double T = 1.;
  constexpr static std::size_t nx = 2;
  constexpr static std::size_t nu = 1;

  // Custom types
  using state_t = Eigen::Matrix<double, nx, 1>;
  using input_t = Eigen::Matrix<double, nu, 1>;
  using A_t = Eigen::Matrix<double, nx, nx>;
  using B_t = Eigen::Matrix<double, nx, nu>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  // Constants
  const A_t A = (A_t() << 0., 1., 0., 0.).finished();
  const B_t B = (B_t() << 0., 1.).finished();
  const state_t E = (state_t() << 0., 0.).finished();
  const Q_t Q = (Q_t() << 1000., 0., 0., 10.).finished();
  const Q_t QT = (Q_t() << 100., 0., 0., 100.).finished();
  const R_t R = (R_t() << 0.0001).finished();

  void get_x0(Eigen::Ref<state_t> x0) const
  {
    x0 << 1., 0.;
  }

  void get_T(double & t) const
  {
    t = T;
  }

  void get_state_lb(double, Eigen::Ref<state_t> state_lb) const
  {
    state_lb <<
      -1000.,
      -1.;
  }

  void get_state_ub(double, Eigen::Ref<state_t> state_ub) const
  {
    state_ub <<
      1000.,
      1.;
  }

  void get_input_lb(double, Eigen::Ref<input_t> input_lb) const
  {
    input_lb << -1.;
  }

  void get_input_ub(double, Eigen::Ref<input_t> input_ub) const
  {
    input_ub << 1.;
  }

  const A_t & get_A(double) const
  {
    return A;
  }

  const B_t & get_B(double) const
  {
    return B;
  }

  const state_t & get_E(double) const
  {
    return E;
  }

  const Q_t & get_Q(double) const
  {
    return Q;
  }

  const R_t & get_R(double) const
  {
    return R;
  }

  const Q_t & get_QT() const
  {
    return QT;
  }

  state_t get_xldot(double) const {return state_t::Zero();}
};

#endif  // CLTV_PROBLEM_HPP_


#ifndef newton_raphson_hpp
#define newton_raphson_hpp 3141592

#include <iostream>
#include <Eigen/SparseCholesky>

template<typename U, typename V>
void newton_raphson(U& F,V& J,VectorXd& x0,double tol)
{
  int it = 0;
  int maxit = 1000;
  double res = tol+1;
  VectorXd x1,step;
  SparseLU<SpMat> solver;
  while(res > tol && it <maxit)
  {
    solver.analyzePattern(J(x0));
    solver.factorize(J(x0));
    x1 = x0- solver.solve(F(x0));
    res = (x1-x0).squaredNorm();
    x0 = x1;
    std::cout << "Residu: "<< res << std::endl;
    it += 1;
  }
}

template<typename U, typename V>
void quasi_newton_raphson(U& F,V& J,VectorXd& x0,double tol)
{
  int it = 0;
  int maxit = 1000;
  double res = tol+1;
  VectorXd x1,step;
  SparseLU<SpMat> solver;
  solver.analyzePattern(J(x0));
  solver.factorize(J(x0));
  while(res > tol && it <maxit)
  {
    x1 = x0- solver.solve(F(x0));
    res = (x1-x0).squaredNorm();
    x0 = x1;
    std::cout << "Residu: "<< res << std::endl;
    it += 1;
  }
}

#endif


#ifndef THREEDIMUTIL_MESH_HPP
#define THREEDIMUTIL_MESH_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace threedimutil
{
    template <typename Scalar, typename UInt>
    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> CalcVertexNormals(const Eigen::Matrix<Scalar, 3, Eigen::Dynamic>& vertex_data,
                                                               const Eigen::Matrix<UInt, 3, Eigen::Dynamic>&   index_data)
    {
        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> normal_data = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>::Zero(vertex_data.rows(), vertex_data.cols());

        for (unsigned face = 0; face < index_data.cols(); ++face)
        {
            const auto& x_0 = vertex_data.col(index_data(0, face));
            const auto& x_1 = vertex_data.col(index_data(1, face));
            const auto& x_2 = vertex_data.col(index_data(2, face));

            const auto area_scaled_face_normal = (x_1 - x_0).cross(x_2 - x_0);

            normal_data.col(index_data(0, face)) += area_scaled_face_normal;
            normal_data.col(index_data(1, face)) += area_scaled_face_normal;
            normal_data.col(index_data(2, face)) += area_scaled_face_normal;
        }

        for (unsigned vertex = 0; vertex < vertex_data.cols(); ++vertex)
        {
            normal_data.col(vertex).normalize();
        }

        return normal_data;
    }
}

#endif // THREEDIMUTIL_MESH_HPP


#ifndef FARHADIFARDIFFERENTIALFORCE_HPP_
#define FARHADIFARDIFFERENTIALFORCE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include "Exception.hpp"

#include "AbstractForce.hpp"
#include "VertexBasedCellPopulation.hpp"
#include "CellLabel.hpp"

#include <iostream>

/**
 * A force class for use in Vertex-based simulations. This force is based on the
 * Energy function proposed by Farhadifar et al in  Curr. Biol., 2007, 17, 2095-2104.
 */

template<unsigned DIM>
class FarhadifarDifferentialForce : public AbstractForce<DIM>
{
friend class TestForces;

private:

    friend class boost::serialization::access;
    /**
     * Boost Serialization method for archiving/checkpointing.
     * Archives the object and its member variables.
     *
     * @param archive  The boost archive.
     * @param version  The current version of this class.
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractForce<DIM> >(*this);
        archive & mCellAreaElasticityParameter;
        archive & mLabelledCellAreaElasticityParameter;
        archive & mCellPerimeterContractilityParameter;
        archive & mLabelledCellPerimeterContractilityParameter;
        archive & mCellCellLineTensionParameter;
        archive & mLabelledCellLabelledCellLineTensionParameter;
        archive & mLabelledCellCellLineTensionParameter;
        archive & mCellBoundaryLineTensionParameter;
        archive & mLabelledCellBoundaryLineTensionParameter;
    }

protected:

    /**
     * The strength of the area term in the model. Corresponds to K_alpha in
     * Farhadifar's paper.
     */
    double mCellAreaElasticityParameter;

    /**
     * The strength of the area term in the model for a labelled cell.
     * */
    double mLabelledCellAreaElasticityParameter;

    /**
     * The strength of the perimeter term in the model. Corresponds to
     * Gamma_alpha in Farhadifar's paper.
     */
    double mCellPerimeterContractilityParameter;

    /**
     * The strength of the perimeter term in the model for a labelled cell.
     */
    double mLabelledCellPerimeterContractilityParameter;

    /**
     * The strength of the cell-cell line tension between two cells.
     * Lambda_{i,j} in Farhadifar's paper.
     */
    double mCellCellLineTensionParameter;

    /**
     * The strength of the cell-cell line tension between two labelled cells.
     */
    double mLabelledCellLabelledCellLineTensionParameter;

    /**
     * The strength of the cell-cell line tension between a labelled and
     * non-labelled cell.
     */
    double mLabelledCellCellLineTensionParameter;

    /**
     * The strength of the line tension at the boundary. This term does
     * correspond to Lambda_{i,j} in Farhadifar's paper.
     */
    double mCellBoundaryLineTensionParameter;

    /**
     * The strength of the line tension at the boundary for labelled cells.
     */
    double mLabelledCellBoundaryLineTensionParameter;

public:

    /**
     * Constructor.
     */
    FarhadifarDifferentialForce();

    /**
     * Destructor.
     */
    virtual ~FarhadifarDifferentialForce() override;

    /**
     * Overridden AddForceContribution() method.
     *
     * Calculates the force on each node in the vertex-based cell population based on the energy function
     * Farhadifar's model.
     *
     * @param rCellPopulation reference to the cell population
     */
    virtual void AddForceContribution(AbstractCellPopulation<DIM>& rCellPopulation) override;

    /**
     * Get the line tension parameter for the edge between two given nodes.
     *
     * @param pNodeA one node
     * @param pNodeB the other node
     * @param rVertexCellPopulation reference to the cell population
     *
     * @return the line tension parameter for this edge.
     */
    double GetLineTensionParameter(Node<DIM>* pNodeA, Node<DIM>* pNodeB, VertexBasedCellPopulation<DIM>& rVertexCellPopulation);

    /**
     * @return mCellAreaElasticityParameter
     */
    double GetCellAreaElasticityParameter();

    /**
     * @return mLabelledCellAreaElasticityParameter
     */
    double GetLabelledCellAreaElasticityParameter();

    /**
     * @return mCellPerimeterContractilityParameter
     */
    double GetCellPerimeterContractilityParameter();

    /**
     * @return mLabelledCellPerimeterContractilityParameter
     */
    double GetLabelledCellPerimeterContractilityParameter();

    /**
     * @return mCellCellLineTensionParameter
     */
    double GetCellCellLineTensionParameter();

    /**
     * @return mLabelledCellLabelledCellLineTensionParameter
     */
    double GetLabelledCellLabelledCellLineTensionParameter();

    /**
     * @return mLabelledCellCellLineTensionParameter
     */
    double GetLabelledCellCellLineTensionParameter();

    /**
     * @return mCellBoundaryLineTensionParameter
     */
    double GetCellBoundaryLineTensionParameter();

    /**
     * @return mLabelledCellBoundaryLineTensionParameter
     */
    double GetLabelledCellBoundaryLineTensionParameter();

    /**
     * Set mCellAreaElasticityParameter.
     *
     * @param cellAreaElasticityParameter the new value of
     * mCellAreaElasticityParameter
     */
    void SetCellAreaElasticityParameter(double cellAreaElasticityParameter);

    /**
     * Set mLabelledCellAreaElasticityParameter.
     *
     * @param labelledCellAreaElasticityParameter the new value of
     * mLabelledCellAreaElasticityParameter
     */
    void SetLabelledCellAreaElasticityParameter(double labelledCellAreaElasticityParameter);

    /**
     * Set mCellPerimeterContractilityParameter.
     *
     * @param cellPerimeterContractilityParameter the new value of
     * cellPerimeterContractilityParameter
     */
    void SetCellPerimeterContractilityParameter(double cellPerimeterContractilityParameter);

    /**
     * Set mLabelledCellPerimeterContractilityParameter.
     *
     * @param labelledCellPerimeterContractilityParameter the new value of
     * labelledCellPerimeterContractilityParameter
     */
    void SetLabelledCellPerimeterContractilityParameter(
            double labelledCellPerimeterContractilityParameter);

    /**
     * Set mCellCellLineTensionParameter.
     *
     * @param cellLineTensionParameter the new value of mCellCellLineTensionParameter
     */
    void SetCellCellLineTensionParameter(double cellCellLineTensionParameter);

    /**
     * Set mLabelledCellLabelledCellLineTensionParameter.
     *
     * @param labelledCellLabelledCellLineTensionParameter the new value of
     * mLabelledCellLabelledCellLineTensionParameter
     */
    void SetLabelledCellLabelledCellLineTensionParameter(
            double labelledCellLabelledCellLineTensionParameter);

    /**
     * Set mLabelledCellCellLineTensionParameter.
     *
     * @param labelledCellCellLineTensionParameter the new value of
     * mLabelledCellCellLineTensionParameter
     */
    void SetLabelledCellCellLineTensionParameter(
            double labelledCellCellLineTensionParameter);

    /**
     * Set mCellBoundaryLineTensionParameter.
     *
     * @param cellBoundaryLineTensionParameter the new value of mCellBoundaryLineTensionParameter
     */
    void SetCellBoundaryLineTensionParameter(double cellBoundaryLineTensionParameter);

    /**
     * Set mLabelledCellBoundaryLineTensionParameter.
     *
     * @param labelledCellBoundaryLineTensionParameter the new value of
     * mLabelledCellBoundaryLineTensionParameter
     */
    void SetLabelledCellBoundaryLineTensionParameter(
            double labelledCellBoundaryLineTensionParameter);

    /**
     * Overridden OutputForceParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    void OutputForceParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(FarhadifarDifferentialForce)

#endif /*FARHADIFARDIFFERENTIALFORCE_HPP_*/


/*!
     \brief Complex a template implementation for complex arithmetics
        Copyright (C) 2012 Sebastian Schlenkrich
        Version 1.0
*/

#ifndef Cpx_Complex_hpp
#define Cpx_Complex_hpp

//#define _USE_MATH_DEFINES // for Visual Studio

#include <iostream>
#include <iomanip>

#include <map>
#include <vector>
#include <iterator>
//#include <cmath>
#include <complex>

//#define DEBUG              // low-level io debugging
//#define EXECUTABLE               // compile example and main function
#define MS_VISUAL_STUDIO     // math functions not in global scope

#ifdef MS_VISUAL_STUDIO
    #include <boost/math/special_functions/erf.hpp>
#endif

namespace Cpx {

#ifdef DEBUG
    #define MSG(msg_stream) std::cerr << msg_stream << std::endl ;
#else
    #define MSG(msg_stream)
#endif

#ifdef MS_VISUAL_STUDIO
    //! Some compilers don't find intrinsic double functions
    inline double exp(double x) { return std::exp(x); }
    inline double log(double x) { return std::log(x); }
    inline double sqrt(double x) { return std::sqrt(x); }
    inline double sin(double x) { return std::sin(x); }
    inline double cos(double x) { return std::cos(x); }
    inline double tan(double x) { return std::tan(x); }
    inline double atan2(double y, double x) { return std::atan2(y,x); }
    inline double erf(double x)     { return boost::math::erf(x); }
    inline double erf_inv(double x) { return boost::math::erf_inv(x); }
#endif

    //! Variable defines the class template for derivative evaluations
    template <class Type = double>
    class Complex {
    protected:
        Type real_;
        Type imag_;
    public:
        //! "standard" constructor
        Complex( const Type& real, const Type& imag) : real_(real), imag_(imag) {}
        //! Type conversion constructor
        Complex( const Type& real) : real_(real), imag_(0) {}
        //! no standard constructor and default copy constructor
        //! inspectors
        const Type& real()  const { return real_;  }
        const Type& imag() const { return imag_; }
        //! IO steaming
        //! Apply IO to the real and imaginary component
        inline friend std::ostream& operator << (std::ostream &output, const Complex<Type> &x) {
            return output << "(" << x.real_ << "," << x.imag_ << ")";
        }
        //! Intrinsic functions
        inline friend Complex<Type> exp(const Complex<Type> &x) {
            Type r = exp(x.real_);
            return Complex<Type>( r*cos(x.imag_), r*sin(x.imag_) );
        }
        inline friend Complex<Type> log(const Complex<Type> &x) {
            return Complex<Type>( 0.5*log(x.real_*x.real_ + x.imag_*x.imag_), atan2(x.imag_,x.real_) );
        }
        inline friend Complex<Type> sqrt(const Complex<Type> &x) {
            if (x.imag_==0) {
                if (x.real_<0) return Complex<Type>( 0, sqrt(-x.real_) );
                else           return Complex<Type>( sqrt(x.real_), 0 );
            }
            if (x.real_==0) {
                if (x.imag_<0) {
                    Type tmp  = sqrt(-x.imag_)*M_SQRT1_2;
                    return Complex<Type>( tmp, -tmp );
                } else {
                    Type tmp  = sqrt(x.imag_)*M_SQRT1_2;
                    return Complex<Type>( tmp, tmp );
                }
            }
            Type sqr_r  = sqrt(sqrt(x.real_*x.real_ + x.imag_*x.imag_));
            Type phi_2  = atan2(x.imag_,x.real_) / 2;
            return Complex<Type>( sqr_r*cos(phi_2), sqr_r*sin(phi_2) );
        }

        //! Unary operators
        inline friend Complex<Type> operator + (const Complex<Type> &x ) {
            return x;
        }
        inline friend Complex<Type> operator - (const Complex<Type> &x ) {
            return Complex<Type>( -x.real_, -x.imag_ );
        }
        //! Binary operators
        //! Complex x Complex
        inline friend Complex<Type> operator + (const Complex<Type> &x, const Complex<Type> &y ) {
            return Complex<Type>( x.real_ + y.real_, x.imag_ + y.imag_ );
        }
        inline friend Complex<Type> operator - (const Complex<Type> &x, const Complex<Type> &y ) {
            return Complex<Type>( x.real_ - y.real_, x.imag_ - y.imag_ );
        }
        inline friend Complex<Type> operator * (const Complex<Type> &x, const Complex<Type> &y ) {
            return Complex<Type>( x.real_*y.real_ - x.imag_*y.imag_, x.real_*y.imag_ + x.imag_*y.real_ );
        }
        inline friend Complex<Type> operator / (const Complex<Type> &x, const Complex<Type> &y ) {
            Type den = y.real_*y.real_ + y.imag_*y.imag_;
            return Complex<Type>( (x.real_*y.real_ + x.imag_*y.imag_)/den,
                          (x.imag_*y.real_ - x.real_*y.imag_)/den );
        }
        //! Complex x Type
        inline friend Complex<Type> operator + (const Complex<Type> &x, const Type &y ) {
            return Complex<Type>( x.real_ + y, x.imag_ );
        }
        inline friend Complex<Type> operator - (const Complex<Type> &x, const Type &y ) {
            return Complex<Type>( x.real_ - y, x.imag_ );
        }
        inline friend Complex<Type> operator * (const Complex<Type> &x, const Type &y ) {
            return Complex<Type>( x.real_*y, x.imag_*y );
        }
        inline friend Complex<Type> operator / (const Complex<Type> &x, const Type &y ) {
            return Complex<Type>( x.real_/y, x.imag_/y );
        }
        //! Type x Complex
        inline friend Complex<Type> operator + (const Type &x, const Complex<Type> &y ) {
            return Complex<Type>( x + y.real_, y.imag_ );
        }
        inline friend Complex<Type> operator - (const Type &x, const Complex<Type> &y ) {
            return Complex<Type>( x - y.real_, -y.imag_ );
        }
        inline friend Complex<Type> operator * (const Type &x, const Complex<Type> &y ) {
            return Complex<Type>( x*y.real_, x*y.imag_ );
        }
        inline friend Complex<Type> operator / (const Type &x, const Complex<Type> &y ) {
            Type den = y.real_*y.real_ + y.imag_*y.imag_;
            return Complex<Type>( x*y.real_/den, -x*y.imag_/den );
        }
    };

    //! Type x Complex
    template <class Type>
    inline Complex<Type> operator + (const double &x, const Complex<Type> &y ) {
        return Complex<Type>( x + y.real(), y.imag() );
    }
    template <class Type>
    inline Complex<Type> operator - (const double &x, const Complex<Type> &y ) {
        return Complex<Type>( x - y.real(), -y.imag() );
    }
    template <class Type>
    inline Complex<Type> operator * (const double &x, const Complex<Type> &y ) {
        return Complex<Type>( x*y.real(), x*y.imag() );
    }
    template <class Type>
    inline Complex<Type> operator / (const double &x, const Complex<Type> &y ) {
        Type den = y.real()*y.real() + y.imag()*y.imag();
        return Complex<Type>( x*y.real()/den, -x*y.imag()/den );
    }

}   // namespace Cpx

#endif   // Cpx_Complex_hpp

#ifdef EXECUTABLE

int main() {
    typedef Cpx::Complex<double> complex;
    //typedef std::complex<double> complex;

    std::cout << "Cpx Complex (C) Sebastian Schlenktich (2012)"<< std::endl;
    std::cout << std::endl;

    complex a(0,4), b(1,2);
    complex c(0); // = a/b;
    c = a + complex(5.0);
    std::cout << "c = " << (c = sqrt(a)) << std::endl;
    double z;
    std::cout << (z = atan2(-1,-1)) << std::endl;

    return 0;
}

#endif   // EXECUTABLE

/*  Example output

*/


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing sqrt capabilities

    Computes the square root of its parameter. For integers it is the
    truncation of the real square root.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = sqrt(x);
    @endcode

    @par Decorators

    - std_ calls std::sqrt

    - raw_ for floating entries can gain some speed with less accuracy on some architectures.

  **/
  Value sqrt(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/sqrt.hpp>
#include <boost/simd/function/simd/sqrt.hpp>

#endif


#pragma once
#ifndef __QTM_CORE_HPP__
#define __QTM_CORE_HPP__

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <vector>

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

namespace qtm {

/**
 * Clamping the given value in given ranges
 *
 * @tparam type Type of the clamping value
 * @param[in] num Clamping value
 * @param[in] min_value Lower clamping limit
 * @param[in] max_value Upper clamping limit
 */
template <typename type> type clamp(type num, type min_value, type max_value) {
  return std::max(std::min(num, max_value), min_value);
}

/**
 * Core queuing class
 *
 * ...
 *
 * Implements basic construction and solving final states;
 * also available getters/setters of internal characteristics
 */
class qtm final : private boost::noncopyable {
private:
  std::uint64_t channel_count_, queue_size_;
  double la_, mu_, nu_;
  std::int64_t n_;
  boost::numeric::ublas::vector<double> final_states_;

  bool is_fs_outdated_;

  /**
   * Perform auxiliary matrix initialization for future calculation
   */
  static boost::numeric::ublas::matrix<double>
  matrix_init(std::uint64_t channel_count, std::uint64_t queue_size, double la,
              double mu, double nu, std::int64_t n);

public:
  /**
   * Class construnctor
   * Permorm construnction of the intance of the queuing system
   *
   * @param[in] channel_count Channel count of the queuing system
   * @param[in] queue_size Queue size of the queuing system
   * @param[in] la Input flow rate of the queuing system
   * @param[in] mu Output flow rate  of the queuing system
   * @param[in] nu Impatience rate of the queuing system
   * @param[in] n Load sources of the queuing system
   */
  explicit qtm(std::uint64_t channel_count, std::uint64_t queue_size, double la,
               double mu, double nu = 0, std::int64_t n = -1);
  ~qtm(void) = default;

  /**
   * Get the current value of the channel count of the queuing system
   */
  std::uint64_t channel_count(void) const;
  /**
   * Get the current value of the queue size of the queuing system
   */
  std::uint64_t queue_size(void) const;
  /**
   * Get the current value of the load sources of the queuing system
   */
  std::int64_t n(void) const;
  /**
   * Get the current value of the input flow rate of the queuing system
   */
  double la(void) const;
  /**
   * Get the current value of the output flow rate of the queuing system
   */
  double mu(void) const;
  /**
   * Get the current value of the impatience rate of the queuing system
   */
  double nu(void) const;

  /**
   * Set the value of the channel count of the queuing system
   *
   * @param[in] channel_count_ New value of channel count
   */
  void channel_count(std::uint64_t channel_count_);
  /**
   * Set the value of the queue size of the queuing system
   *
   * @param[in] channel_count_ New value of queue size
   */
  void queue_size(std::uint64_t queue_size_);
  /**
   * Set the value of the load sources of the queuing system
   *
   * @param[in] channel_count_ New value of load sources
   */
  void n(std::int64_t n_);
  /**
   * Get the current value of the input flow rate of the queuing system
   *
   * @param[in] channel_count_ New value of input flow rate
   */
  void la(double la_);
  /**
   * Set the value of the output flow rate of the queuing system
   *
   * @param[in] channel_count_ New value of output flow rate
   */
  void mu(double mu_);
  /**
   * Set the value of the impatience rate of the queuing system
   *
   * @param[in] channel_count_ New value of impatience rate
   */
  void nu(double nu_);

  /**
   * Get the current value of the final states of the queuing system
   *
   * @throws std::runtime_error Thrown if final states calculation was not
   * performed
   */
  std::vector<double> const final_states(void) const;
  /**
   * Get information that the final states hasn't been updated after changes
   * to the internal characteristics of the queuing system.
   */
  bool is_fs_outdated(void) const;
  /**
   * Perform calculation he final states of the queuing system.
   */
  std::vector<double> calc_final_states(void);
};

}; // namespace qtm

#endif // !__QTM_CORE_HPP__


#include <dimensional/quantity.hpp>
#include <dimensional/arithmetic.hpp>
#include <dimensional/systems/si/all.hpp>
#include <dimensional/systems/nonsi/hour.hpp>
#include <dimensional/systems/si/derived_units/area.hpp>
#include <dimensional/systems/si/derived_units/angle.hpp>
#include <dimensional/io.hpp>

#include <dimensional/systems/si/prefix.hpp>
#include <dimensional/math/all.hpp>
#include <dimensional/systems/currency/jpy.hpp>
#include <dimensional/systems/nonsi/degree_celsius.hpp>
#include <dimensional/expr.hpp>
#include <dimensional/delta.hpp>
#include <boost/type_index.hpp>
#include <iostream>

#define REPL(...) \
    do { std::cout << "$ " << #__VA_ARGS__ << "\n=> " << (__VA_ARGS__) << std::endl; } while(false)
int main(){
    using namespace mitama;
    namespace si = mitama::systems::si;
    namespace nonsi = mitama::systems::nonsi;
    { // Homogeneous dimension examples
        std::cout << "--[Homogeneous dimension examples]--\n";

        // quantity<Dimension, Type = double>:
        // quantity is the type T that is meaningful by Dimension;
        // Dimension is a phantom template parameter.
        constexpr quantity<si::meter_t, int> a = 2;
        constexpr quantity<si::millimeter_t, int> b = 2;

        // `a + b` is valid operation if and only if `a` and `b` has same base dimensions.
        auto r1 = a + b;
        REPL(r1);

        // `a - b` is valid operation if and only if `a` and `b` has same base dimensions.
        auto r2 = a - b;
        REPL(r2);

        // `a * b` is always valid and:
        // - Result quantity has heterogeneous dimension inducted from `a` and `b`, and
        // - If both a and b values are of the same dimension, these values are automatically scaled to high precision units.
        auto r3 = a * b;
        REPL(r3);

        // a / b := a * b^{-1}
        auto r4 = a / b;
        REPL(r4);
        REPL(1 + r4);
        REPL(boost::typeindex::type_id<decltype(r4)>().pretty_name());

        quantity<si::millimeter_t, int> d(a);
        REPL(d);

        constexpr auto e = 3 | si::meter<2> ;
        constexpr auto f = 3 | si::millimeter<2> ;
        REPL(e);
        REPL(f);
        REPL(e + f);

        auto v = (1|si::meters) * (1|si::millimeters);
        auto u = (1|si::centimeters);
        REPL(v);
        REPL(u);
        REPL(v/u);
        quantity<si::millimeter_t, int> milli = u;
        REPL(milli);
        std::cout << "------------------------\n";
    }

    { // Heterogeneous dimesnsion examples
        std::cout << "--[Heterogeneous dimension examples]--\n";
        // speed := km/h
        using speed_t = decltype(si::kilometers/nonsi::hour_t{});

        constexpr quantity<si::meter_t> L(1.2);
        constexpr quantity<si::second_t> T(0.3);
        // unit is automatically convert from m/s to km/h
        quantity<speed_t> V = L/T;
        std::cout << boost::typeindex::type_id<decltype(L*T)>().pretty_name() << std::endl;

        std::cout << V.value() << "[ km/h ]" << std::endl;
        {
            auto w = 36 | si::kilogram<> * si::meter<2> * si::second<-2> * si::ampere<-1>;
            std::cout << boost::typeindex::type_id<decltype(w)>().pretty_name() << std::endl;
        }
        using newton_t = decltype(si::kilogram<> * si::meter<> * si::second<-2>);
        quantity<newton_t> N = (1.0|si::kilograms) * V / (2|si::seconds);
        REPL(N);
        std::cout << "------------------------\n";
    }

    { // compare examples
        std::cout << "--[dimension type comparisons examples]--\n";
        std::cout << std::boolalpha;
        REPL((1|si::meters) == (1|si::millimeters));
        REPL((1|si::meters) == (1000|si::millimeters));
        REPL((1|si::meters) != (1|si::millimeters));
        REPL((1|si::meters) != (1000|si::millimeters));
        REPL((1|si::meters) < (1|si::millimeters));
        REPL((1|si::meters) > (1|si::millimeters));
        REPL((1|si::meters) < (1000|si::millimeters));
        REPL((1|si::meters) > (1000|si::millimeters));
        REPL((1|si::meters) <= (1000|si::millimeters));
        REPL((1|si::meters) >= (1000|si::millimeters));
        std::cout << "------------------------\n";
    }

    { // math function examples
        std::cout << "--[dimension type math functions examples]--\n";
        quantity_t<std::decay_t<decltype(si::millimeter<2>)>, double> v = (1|si::meters) * (1|si::millimeters);
        REPL(sqrt(v));
        REPL(cbrt(v));

        REPL(mitama::min((1|si::meters),(1|si::millimeters),(1|si::centimeters)));
        REPL(mitama::max((1|si::meters),(1|si::millimeters),(1|si::centimeters)));
        std::cout << "------------------------\n";

        REPL(pow<5>(2|si::meters));
        REPL(square(2|si::meters));
        REPL(cubic(2|si::meters));

        REPL(hypot(2.|si::meters, 2.|si::meters));

        REPL(ceil(2.2|si::meters));
        REPL(floor(2.2|si::meters));
        REPL(trunc(2.2|si::meters));
        REPL(round(2.2|si::meters));
        REPL(lround(2.2|si::meters));
        REPL(llround(2.2|si::meters));
        REPL(nearbyint(2.2|si::meters));
        REPL(rint(2.2|si::meters));
        REPL(lrint(2.2|si::meters));
        REPL(llrint(2.2|si::milli*si::meters));
    }

    { // User defined dimension examples
        // currency units
        REPL(100|yen);
    }

    { // User defined dimension examples
        // currency units
        REPL(100|nonsi::degree_celsius);
        quantity<si::kelvin_t> s( 100.|nonsi::degree_celsius );
        REPL(s);
    }

    {
        quantity<si::kelvin_t> hoge = as_expr(1|si::kelvins) + (2|nonsi::degree_celsius);
        REPL(hoge);
    }

    {
        quantity_t a1 = accepts<si::area_r> |= (2|si::meters) * (7|si::meters);
        REPL(a1);

        quantity_t a2 = accepts<si::area_r> |= (2|si::millimeters) * (7|si::millimeters);
        REPL(a2);

        // error!
        // quantity_t a3 = accepts<area_r> |= (2|si::millimeters);

        quantity_t a3 = partial_accepts_for<sym::M<>> |= (2|si::meters) * (2|si::meters) * (2|si::kilograms) / (2|si::second<2>);
        REPL(a3);
    }

    {
        delta d = (2|si::kelvins) - (1|si::kelvins);
        REPL((1|nonsi::degree_celsius) + d);
    }

    {
        quantity_t m = 1.0 | si::meters * si::radian;
        quantity_for<double, si::meter_<>> x = m.into();
        REPL(x);
    }
}


#ifndef FDM_HH
#define FDM_HH

#include <vector>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>

#include "PDE.hh"

typedef std::vector<Eigen::VectorXd> matrix;

class FDMBase {
 protected:
  std::shared_ptr<BlackScholesPDE> pde;

  //Space domain [Nminus*dx,Nplus*dx]
  int long Nminus;
  int long Nplus;
  int long N;
  double dx;
  std::vector<double> x_values;

  //Time domain [0, 1/2*simga^2*T]
  unsigned long M;
  double dt;
  std::vector<double> tau_values;

  //coefficients
  double alpha;

  //initial vector;
  Eigen::VectorXd u0;

  //matrices
  Eigen::SparseMatrix<double, Eigen::RowMajor> A, I;

  // Constructor
  FDMBase(int _N , double dx, unsigned long _M, std::shared_ptr<BlackScholesPDE> _pde);

  void calculate_step_sizes();
  void set_initial_conditions();
  void calculate_boundary_conditions(Eigen::VectorXd&, double);

 public:

  double get_dt()const{ return dt; }
  double get_dx()const{ return dx; }
  double get_alpha()const{ return alpha; }
  std::vector<double> get_x() const{ return x_values; }
  std::vector<double> get_tau() const{ return tau_values; }

  void BuildMatrix();
  void change_var(matrix&);
  virtual matrix solve() = 0;
};

class FDMEulerExplicit : public FDMBase {
 public:
  FDMEulerExplicit(int _N, double dx, unsigned long _M, std::shared_ptr<BlackScholesPDE> _pde):
    FDMBase(_N, dx, _M, _pde){}

  matrix solve();
};

class FDMEulerImplicit : public FDMBase{
  public:
    FDMEulerImplicit(int _N, double dx, unsigned long _M, std::shared_ptr<BlackScholesPDE> _pde):
      FDMBase(_N, dx, _M, _pde){}

    matrix solve();
};

class FDMCranckNicholson : public FDMBase{
public:
  FDMCranckNicholson(int _N, double dx, unsigned long _M, std::shared_ptr<BlackScholesPDE> _pde):
    FDMBase(_N, dx, _M, _pde){}

  matrix solve();
};

#endif


#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <Eigen/Core>
#include <vector>
#include <string>
#include <boost/format.hpp>
#include <iostream>

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
// 内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// 基线
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png"); // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// TODO implement this function
/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21,
    bool show = false);

// TODO implement this function
/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21,
    bool show = false);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y)
{
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]);
}

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 1000;
    int boarder = 40;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++)
    {
        int x = rng.uniform(boarder, left_img.cols - boarder); // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder); // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++)
    { // 1~10
        cout<<"Image "<<i<<"--------------------------------------"<<endl;
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref, true); // first you need to test single layer
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref, true);
    }
}

void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21,
    bool show)
{

    // parameters
    int half_patch_size = 4;
    int iterations = 100;

    double cost = 0, lastCost = 0;
    int nGood = 0; // good projections
    VecVector2d goodProjection;
    VecVector2d goodOriginal;

    int iter_cnt = 0;
    for (int iter = 0; iter < iterations; iter++)
    {
        iter_cnt = iter;
        nGood = 0;
        goodProjection.clear();
        goodOriginal.clear();

        // Define Hessian and bias
        Matrix6d H = Matrix6d::Zero(); // 6x6 Hessian
        Vector6d b = Vector6d::Zero(); // 6x1 bias

        for (size_t i = 0; i < px_ref.size(); i++)
        {

            // compute the projection in the second image
            // TODO START YOUR CODE HERE
            Eigen::Vector3d P((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
            P *= depth_ref[i];
            Eigen::Vector3d q = T21 * P;
            float u = q[0] / q[2] * fx + cx, v = q[1] / q[2] * fy + cy;

            if (u <= half_patch_size || u >= img2.cols - half_patch_size || v <= half_patch_size || v >= img2.rows - half_patch_size)
            {
                continue;
            }
            nGood++;
            goodProjection.push_back(Eigen::Vector2d(u, v));
            goodOriginal.push_back(px_ref[i]);

            // and compute error and jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)
                for (int y = -half_patch_size; y < half_patch_size; y++)
                {

                    double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) - GetPixelValue(img2, u + x, v + y);

                    Matrix26d J_pixel_xi; // pixel to \xi in Lie algebra
                    J_pixel_xi(0, 0) = fx / q[2];
                    J_pixel_xi(0, 1) = 0;
                    J_pixel_xi(0, 2) = -fx * q[0] / (q[2] * q[2]);
                    J_pixel_xi(0, 3) = -fx * q[0] * q[1] / (q[2] * q[2]);
                    J_pixel_xi(0, 4) = fx + fx * q[0] * q[0] / (q[2] * q[2]);
                    J_pixel_xi(0, 5) = -fx * q[1] / q[2];
                    J_pixel_xi(1, 0) = 0;
                    J_pixel_xi(1, 1) = fy / q[2];
                    J_pixel_xi(1, 2) = -fy * q[1] / (q[2] * q[2]);
                    J_pixel_xi(1, 3) = -fy - fy * q[1] * q[1] / (q[2] * q[2]);
                    J_pixel_xi(1, 4) = fy * q[0] * q[1] / (q[2] * q[2]);
                    J_pixel_xi(1, 5) = fy * q[0] / q[2];
                    Eigen::Vector2d J_img_pixel; // image gradients
                    J_img_pixel[0] = 0.5 * (GetPixelValue(img2, u + x + 1, v + y) - GetPixelValue(img2, u + x - 1, v + y));
                    J_img_pixel[1] = 0.5 * (GetPixelValue(img2, u + x, v + y + 1) - GetPixelValue(img2, u + x, v + y - 1));

                    // total jacobian
                    Vector6d J = -J_pixel_xi.transpose() * J_img_pixel;

                    H += J * J.transpose();
                    b += -error * J;
                    cost += error * error;
                }
            // END YOUR CODE HERE
        }

        // solve update and put it into estimation
        // TODO START YOUR CODE HERE
        Vector6d update;
        update = H.ldlt().solve(b);
        T21 = Sophus::SE3d::exp(update) * T21;
        // END YOUR CODE HERE

        cost /= nGood;

        if (isnan(update[0]))
        {
            // sometimes occurred when we have a black or white patch and H is irreversible
            std::cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost)
        {
            // cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        lastCost = cost;
        if(show)
            std::cout << "iter = "<< iter <<", cost = " << cost << ", good = " << nGood <<"/"<< px_ref.size() << endl;
    }
    if (show)
    {
        std::cout << "end_iter = " << iter_cnt - 1 << ", lastcost = " << lastCost << ", good = " << nGood << "/" << px_ref.size() << endl;
        // std::cout << "good projection: " << nGood << endl;
        std::cout << "T21 = \n"
                  << T21.matrix() << endl;

        // in order to help you debug, we plot the projected pixels here
        cv::Mat img1_show, img2_show;
        cv::cvtColor(img1, img1_show, CV_GRAY2BGR);
        cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
        for (auto &px : px_ref)
        {
            cv::rectangle(img1_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
                          cv::Scalar(0, 250, 0));
        }
        // for (auto &px: goodProjection) {
        //     cv::rectangle(img2_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
        //                   cv::Scalar(0, 250, 0));
        // }
        for (int i = 0; i < goodProjection.size(); i++)
        {
            cv::circle(img2_show, cv::Point2f(goodProjection[i][0], goodProjection[i][1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(goodProjection[i][0], goodProjection[i][1]), cv::Point2f(goodOriginal[i][0], goodOriginal[i][1]), cv::Scalar(0, 250, 0));
        }
        cv::imshow("reference", img1_show);
        cv::imshow("current", img2_show);
        cv::waitKey(0);
    }
}

void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21,
    bool show)
{

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<cv::Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE
    for (int i = 0; i < pyramids; i++)
    {
        if (i == 0)
        {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
            continue;
        }
        cv::Mat dst1, dst2;
        cv::resize(img1, dst1, cv::Size(), scales[i], scales[i]);
        cv::resize(img2, dst2, cv::Size(), scales[i], scales[i]);
        pyr1.push_back(dst1);
        pyr2.push_back(dst2);
    }

    // END YOUR CODE HERE

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy; // backup the old values
    for (int level = pyramids - 1; level >= 0; level--)
    {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px : px_ref)
        {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // TODO START YOUR CODE HERE
        // scale fx, fy, cx, cy in different pyramid levels
        fx = fxG*scales[level];
        cx = cxG*scales[level];
        fy = fyG*scales[level];
        cy = cyG*scales[level];
        // END YOUR CODE HERE
        if (level==0)
            DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21, show);
        else{
            DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
        }
    }
}