#include <iostream>
#include <cstdlib>
#include <vector>
#include <algorithm>
#include <random>
#include <complex>
#include <chrono>
#include <list>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iomanip>
#include <exception>
#include <iterator>
#include <stdexcept>
#include "../Matrix.hpp"

class Timer {
 private:
  std::chrono::time_point<std::chrono::high_resolution_clock>
    start_ = std::chrono::high_resolution_clock::now();
  std::chrono::time_point<std::chrono::high_resolution_clock>
    end_ = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double>
    duration_ = std::chrono::duration_cast
    <std::chrono::nanoseconds>(end_-start_);
 public:
  void start() {
    start_ = std::chrono::high_resolution_clock::now();
  }
  void stop(std::string statement) {
    end_ = std::chrono::high_resolution_clock::now();
    duration_ = std::chrono::duration_cast
      <std::chrono::nanoseconds>(end_-start_);
    std::cout << log2(duration_.count()) << " "+statement << std::endl;
    start_ = std::chrono::high_resolution_clock::now();
  }
  double get_time() {
    end_ = std::chrono::high_resolution_clock::now();
    duration_ = std::chrono::duration_cast
      <std::chrono::nanoseconds>(end_-start_);
    return log2(duration_.count());
  }
};

typedef Eigen::Triplet<std::complex<double>> Tri;
typedef std::vector<Tri> TriVec;

void printMEM() {
  auto sys = system("ps -eo cmd,%mem,%cpu --sort=-%mem | head -n 2 | tail -n 1");
}

void print_sparse(const std::string& s,
  const Eigen::SparseMatrix<std::complex<double>>& mat, Matrix& A) {
  std::cout << s << std::endl;
  for (int k=0; k<mat.outerSize(); ++k)
    for (Eigen::SparseMatrix<std::complex<double>>::InnerIterator it(mat,k);
      it; ++it) {
      std::cout << it.col() << " " << it.row() << " " << it.value() << std::endl;
    }
    std::cout << std::endl;
  std::cout << A.map_.to_string() << std::endl;
}

void
build_sparse(const uint64_t& dim, const uint64_t& n, TriVec& tri_vec,
  std::default_random_engine& rand_gen,
  std::uniform_real_distribution<double>& urd,
  Eigen::SparseMatrix<std::complex<double>>& m,
Matrix& M) {
  int sys;
  tri_vec.resize(0);
  try {
    tri_vec.resize(dim*dim);
  } catch(const std::exception& e) {
    std::cerr << "build_sparse->try->for->for->"
      << "tri_vec.resize(dim*dim);"
      << std::endl;
    std::cerr << e.what() << std::endl;
    throw;
  }
  std::complex<double> v;
  uint64_t i = 0;
  for (uint64_t row = 0; row < dim; row++) {
    for (uint64_t col = 0; col < dim; col++) {
      v = std::complex<double>(urd(rand_gen), urd(rand_gen));
      tri_vec[i]=(Tri(row,col,v));
      i++;
    }
  }
  auto it = tri_vec.begin();
  std::advance(it,dim*dim);
  std::shuffle(tri_vec.begin(), it, rand_gen);
  try {
  m.resize(dim,dim);
  } catch(std::exception& e) {
    std::cout << e.what() << std::endl;
    std::cout << "build_sparse->m.resize(dim,dim);" << std::endl;
    throw;
  }
  it = tri_vec.begin();
  std::advance(it,n);
  m.setFromTriplets(tri_vec.begin(), it);
  M.map_.clear();
  for(uint32_t i = 0; i < n; i++) {
    M.add(tri_vec[i].col(),tri_vec[i].row(),tri_vec[i].value());
  }
  //print_sparse("m=",m,M);
}

struct keep_complex_zero {
  bool operator() (const int& row, const int& col,
    const std::complex<double>& val) const {
    return false;
  }
};

void dealloc_sparse(Eigen::SparseMatrix<std::complex<double>>& m) {
  m.setZero();
  m.prune(keep_complex_zero());
  m.makeCompressed();
}

void eigen_oper_mult(const Eigen::SparseMatrix<std::complex<double>>& a,
               const Eigen::SparseMatrix<std::complex<double>>& b,
                     Eigen::SparseMatrix<std::complex<double>>& c) {
  for (int k_a=0; k_a<a.outerSize(); ++k_a)
    for (Eigen::SparseMatrix<std::complex<double>>::InnerIterator
      it_a(a,k_a); it_a; ++it_a) {
      for (int k_b=0; k_b<b.outerSize(); ++k_b)
        for (Eigen::SparseMatrix<std::complex<double>>::InnerIterator
          it_b(b,k_b); it_b; ++it_b) {
          c.coeffRef(it_a.row()^it_b.row(), it_a.col()^it_b.col())
            += it_a.value()*it_b.value();
        }
    }
  //std::cout << c.nonZeros() << std::endl;
}

double compare_objects(Eigen::SparseMatrix<std::complex<double>>& c, Matrix& C) {
  double v=0;
  for (int k_c=0; k_c<c.outerSize(); ++k_c)
    for (Eigen::SparseMatrix<std::complex<double>>::InnerIterator
      it_c(c,k_c); it_c; ++it_c) {
      v += std::abs(C.getCoeff(it_c.col(),it_c.row()) - it_c.value());
    }
  return v;
}

void test_trad_mult() {
  Timer stop_watch;
  std::random_device rd;
  std::default_random_engine rand_gen(rd());
  std::uniform_real_distribution<double> urd(-1, 1);
  Eigen::SparseMatrix<std::complex<double>> a;
  Eigen::SparseMatrix<std::complex<double>> b;
  Eigen::SparseMatrix<std::complex<double>> c;
  Matrix A;
  Matrix B;
  Matrix C;
  TriVec tri_vec;
  double exp = 2;
  uint64_t max_qubits = 10;
  uint64_t max_dim = 1<<max_qubits;
  uint64_t max_n = uint64_t(pow((max_dim),exp));
  try{
    a.reserve(max_n);
    b.reserve(max_n);
    c.reserve(max_dim*max_dim);
    tri_vec = TriVec(max_dim*max_dim);
    tri_vec.resize(tri_vec.capacity());
  } catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
    throw;
  }
  std::vector<double> times_mult;
  std::vector<double> times_mult_2;
  std::vector<double> times_alloc;
  std::vector<double> times_build;
  std::vector<double> a_size;
  std::vector<double> c_size;
  std::vector<double> times_sort;
  std::vector<double> times_allocM;
  std::vector<double> times_multM;
  std::vector<double> times_mult_2M;
  uint64_t dim;
  uint64_t n;
  for(uint64_t q = 1; q <= (max_qubits); q++ ) {
    dim = (1<<q);
    n = uint64_t(pow((dim),exp));
    std::cout << "q=" << q << " dim=" << dim << " n=" << n << " dim^2="
      << dim*dim << std::endl;
    stop_watch.start();
    //std::cout << "start build A" << std::endl;
    build_sparse(dim,n, tri_vec, rand_gen, urd, a, A);
    //std::cout << "start build B" << std::endl;
    build_sparse(dim,n, tri_vec, rand_gen, urd, b, B);
    times_build.push_back(stop_watch.get_time());
    //////////////////////////////////////////
    stop_watch.start();
    c.resize(dim,dim); //set to zero as well
    times_alloc.push_back(stop_watch.get_time());
    stop_watch.start();
    c.setZero();
    c += a*b;
    times_mult.push_back(stop_watch.get_time());
    c_size.push_back(log2(double(c.nonZeros())/double(dim*dim)));
    a_size.push_back(log2(double(a.nonZeros())/double(dim*dim)));
    stop_watch.start();
    c.setZero();
    c += a*b;
    times_mult_2.push_back(stop_watch.get_time());
    /////////////////////////////////////////////
    stop_watch.start();
    A.map_.sort_list(); B.map_.sort_list();
    times_sort.push_back(stop_watch.get_time());
    stop_watch.start();
    C.map_.clear();
    times_allocM.push_back(stop_watch.get_time());
    stop_watch.start();
    //std::cout << "start transpose" << std::endl;
    A.transpose_emplace();
    //std::cout << "start pesABt" << std::endl;
    //C.pesABt(1,B,A);
    times_multM.push_back(stop_watch.get_time());
    stop_watch.start();
    C.map_.clear();
    //std::cout << "start pesABt 2nd" << std::endl;
    //C.pesABt(1,B,A);
    times_mult_2M.push_back(stop_watch.get_time());
    ////////////////////////////////////////////////
    printMEM();
    std::cout << "compare_objects(c,C)=" << compare_objects(c,C) << std::endl;
  }

  std::cout << std::fixed << std::setprecision(3)
    << std::setw(6) << times_build[0] << " "
    << std::setw(6) << times_alloc[0] << " "
    << std::setw(6) << times_mult[0] << " "
    << std::setw(6) << times_mult_2[0] << " "
    << std::setw(6) << " "
    << std::setw(6) << " "
    << std::setw(6) << times_sort[0] << " "
    << std::setw(6) << times_allocM[0] << " "
    << std::setw(6) << times_multM[0] << " "
    << std::setw(6) << times_mult_2M[0] << " "
    << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  for (int i = 0; i < times_mult.size() - 1; i++) {
    std::cout << std::fixed << std::setprecision(3)
              << std::setw(6) << times_build[i+1] - times_build[i] << " "
              << std::setw(6) << times_alloc[i+1] - times_alloc[i] << " "
              << std::setw(6) << times_mult[i+1] - times_mult[i] << " "
              << std::setw(6) << times_mult_2[i+1] - times_mult_2[i] << " "
              << std::setw(6) << a_size[i] << " "
              << std::setw(6) << c_size[i] << " "
              << std::setw(6) << times_sort[i+1]-times_sort[i] << " "
              << std::setw(6) << times_allocM[i+1]-times_allocM[i] << " "
              << std::setw(6) << times_multM[i+1]-times_multM[i] << " "
              << std::setw(6) << times_mult_2M[i+1]-times_mult_2M[i] << " "
              << std::endl;
  }
  std::cout << "----------------------------------------------" << std::endl;
  auto end = times_alloc.size() - 1;
  std::cout << std::fixed << std::setprecision(3)
    << std::setw(6) << times_build[end] << " "
    << std::setw(6) << times_alloc[end] << " "
    << std::setw(6) << times_mult[end] << " "
    << std::setw(6) << times_mult_2[end] << " "
    << std::setw(6) << a_size[end] << " "
    << std::setw(6) << c_size[end] << " "
    << std::setw(6) << times_sort[end] << " "
    << std::setw(6) << times_allocM[end] << " "
    << std::setw(6) << times_multM[end] << " "
    << std::setw(6) << times_mult_2M[end] << " "
    << std::endl;
  //print_sparse("b=",b);
}

/*

void test_oper_mult() {
  Timer stop_watch;
  std::random_device rd;
  std::default_random_engine rand_gen(rd());
  std::uniform_real_distribution<double> urd(-1, 1);
  auto a = build_sparse(2, rand_gen, urd);
  auto b = build_sparse(2, rand_gen, urd);
  auto c = build_sparse(2, rand_gen, urd);
  std::vector<double> times_mult;
  std::vector<double> times_mult_2;
  std::vector<double> times_alloc;
  for(int q = 1; q < 3; q++ ) {
    // allocate
    stop_watch.start();
    a = build_sparse(q, rand_gen, urd);
    b = build_sparse(q, rand_gen, urd);
    times_alloc.push_back(stop_watch.get_time());
    // first mult
    dealloc_sparse(c);
    stop_watch.start();
    oper_mult(a,b,c);
    times_mult.push_back(stop_watch.get_time());
    // second mult
    dealloc_sparse(c);
    stop_watch.start();
    oper_mult(a,b,c);
    times_mult_2.push_back(stop_watch.get_time());
  }
  for (int i = 0; i < times_mult.size() - 1; i++) {
    std::cout << times_alloc[i+1] - times_alloc[i] << " "
              << times_mult[i+1] - times_mult[i] << " "
              << times_mult_2[i+1] - times_mult_2[i] << std::endl;
  }
}

*/
int main() {
  /*
  try {
    throw std::runtime_error("oops");
  } catch(const std::exception& e) {
    std::cerr << "build_sparse->try->for->for->tri_list.push_back(T(row,col,v))"
      << std::endl;
    std::cerr << e.what() << std::endl;
    std::cerr << "build_sparse->try->for->for->tri_list.push_back(T(row,col,v))"
      << std::endl;
    throw;
  }
  */
  test_trad_mult();
  return 0;
}


#pragma once
#include <vector>
#include <string>
#include <random>
#include <unordered_set>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>

#include "buffalo/algo.hpp"
#include "buffalo/concurrent_queue.hpp"

using namespace std;
using namespace Eigen;

static const int EXP_TABLE_SIZE = 1000;

namespace w2v {

struct job_t;
struct progress_t;

class CW2V : public Algorithm {
public:
    CW2V();
    ~CW2V();

    void release();
    bool init(string opt_path);
    bool parse_option(string out_path);

    void initialize_model(
            float* L0,
            int32_t L0_rows,
            int32_t* index,
            uint32_t* scale,
            int32_t* dist,
            int64_t total_word_count);

    void build_exp_table();

    void launch_workers();

    void add_jobs(
            int start_x,
            int next_x,
            int64_t* indptr,
            int32_t* sequences);

    void worker(int worker_id);

    double update_parameter(Array<float, 1, Dynamic, RowMajor>& work,
                            double alpha,
                            int input_word_idx,
                            vector<int>& negatives,
                            bool comtpue_loss);

    void progress_manager();

    double join();

private:
    Json opt_;
    Map<FactorTypeRowMajor> L0_;
    uint32_t *scale_;
    int32_t *index_, *dist_;
    FactorTypeRowMajor L1_;
    double alpha_;
    double total_processed_;
    vector<double> processed_;
    int D_;

    float exp_table_[EXP_TABLE_SIZE];

    vector<thread> workers_;
    thread* progress_manager_;
    Queue<job_t> job_queue_;
    Queue<progress_t> progress_queue_;
};

}


#ifndef PLAN_BY_CAPTURE_POINT
#define PLAN_BY_CAPTURE_POINT
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <deque>
#include <Eigen/Geometry>

#ifdef PLANNER_DEBUG
static constexpr bool planner_debug = true;
#else
static constexpr bool planner_debug = false;
#endif

template <class T>
static void debugPrint(T &&s, const std::string &label)
{
    if (planner_debug)
        std::cout << "Debug planner:" << label << "::" << std::forward<T>(s) << std::endl;
}

static constexpr double dt = 4.0 / 1000.0;   // sampling period (s)
static constexpr double zh = 0.2;            // height of CoM (m)
static constexpr double g = 9.8;             // gravity (m/s^2)
static constexpr double MAX_X_STRIDE = 0.30; //(m)
static constexpr double MAX_Y_STRIDE = 0.08; //(m)
static constexpr double MIN_Y_STRIDE = 0.03; //(m)

struct FootPrint
{
    double time_;
    double x_;                  //(m)ロボットのローカル座標での足配置位置
    double y_;                  //(m)ロボットのローカル座標での足配置位置
    bool support_foot_is_right; //支持脚がどちらか
};

/**
 * @brief capture pointによる歩行パターン生成方法
 *
 */
void footStepPlannerCapturePoint()
{
    double x = 0, y = 0, xinit = 0, yinit = 0;     //(m) CoMのワールド座標 {CoM = Center of Mass}
    double xd = 0, yd = 0, xdinit = 0, ydinit = 0; // CoMの速度 v(m/s) xdot(t)
    double px = 0.0, py = 0.0;                     //(m)　着地位置のワールド座標 これは実用的にはローカルの方が良いのでは？？
    constexpr double Tc = std::sqrt(zh / g);       //微分方程式の時定数
    constexpr double w = std::sqrt(g / zh);
    const double Tsup = 0.32;
    int_fast64_t step_n = 0;
    const int32_t steps_ = 7;
    const double stride_x = 0.3;
    const double stride_y = 0.1;
    double cp_x_target = 0.0, cp_y_target = 0.0;
    double cp_x_now = 0.0, cp_y_now = 0.0;
    double cp_x_old = 0.0, cp_y_old = 0.0;
    debugPrint(Tsup, "Tsup");
    std::deque<std::deque<double>> result;
    std::ofstream velofs("velo.dat");
    std::vector<FootPrint> footprint_list(100);
    for (double t = 0.0; t < (Tsup * static_cast<double>(steps_ * 2 + 1)); ++step_n)
    {
        //決められた次の一歩を着く地点までの遊脚の移動を行っている時のシミュレーション---------------
        for (double t_tmp = 0.0; t_tmp < Tsup; t_tmp += dt, t += dt)
        {
            // double C = std::cosh(t_tmp / Tc);
            // double S = std::sinh(t_tmp / Tc);
            double ewt = std::exp(w * t_tmp);
            double dT = Tsup - t_tmp; //次のCPまでの時間
            // x = (xinit - px) * C + Tc * xdinit * S + px; // x,xdともにn歩目開始時の状態
            // y = (yinit - py) * C + Tc * ydinit * S + py;
            // xd = (xinit - px) / Tc * S + xdinit * C;
            // yd = (yinit - py) / Tc * S + ydinit * C;
            // cp_x_now = ewt * cp_x_old + (1.0 - ewt)*px; //ここでのcp_targetは1ループ前の話。
            // cp_y_now = ewt * cp_y_old + (1.0 - ewt)*py;
            cp_x_now = ewt * cp_x_old + (1.0 - ewt) * px; // CPの軌道(x)
            cp_y_now = ewt * cp_y_old + (1.0 - ewt) * py; // CPの軌道(y)
            xdinit = xd;
            ydinit = yd;
            xd = -w * (x - cp_x_now);
            yd = -w * (y - cp_y_now);
            x += dt * xdinit;
            y += dt * ydinit;
            // result.push_back({x, y, cp_x_now, cp_y_now});
            result.push_back({x, y, cp_x_now, cp_y_now});
            velofs << t << " " << xd << " " << yd << std::endl;
        }
        xinit = x;
        yinit = y;
        xdinit = xd;
        ydinit = yd;
        //次の一歩の目標位置を計算------------------
        double b = std::exp(w * Tsup);
        std::cout << b << std::endl;
        double sx = 0.0, sy = 0.0;
        if (steps_ < step_n)
        {
            sx = 0.0;
            sy = 0.0;
        }
        else
        {
            sx = stride_x / 2;
            sy = ((step_n % 2) ? -1 : 1) * stride_y ;
        }
        cp_x_old = cp_x_now; //次の一歩の最初のCPの位置
        cp_y_old = cp_y_now;
        cp_x_target = cp_x_old + sx;
        cp_y_target = cp_y_old + sy;
        if (step_n != 0)
        {
            px = 1.0 / (1.0 - b) * cp_x_target - b / (1.0 - b) * cp_x_now;
            py = 1.0 / (1.0 - b) * cp_y_target - b / (1.0 - b) * cp_y_now;
        }
        result.back().push_back(px);
        result.back().push_back(py);
    }
    std::ofstream ofs("position.dat");
    for (auto &itr : result)
    {
        for (auto &item : itr)
        {
            ofs << item << " ";
        }
        ofs << std::endl;
    }
    // return footprint_list;
}

#endif // !PLAN_BY_CAPTURE_POINT


// Copyright (C) 2019 David Harmon and Artificial Necessity
// This code distributed under zlib, see LICENSE.txt for terms.

#pragma once

#include <vector>
#include <Eigen/Core>
#include <Eigen/Sparse>

#include "mesh.hpp"

// Use RowMajor so that matrix-vector products are multi-threaded by Eigen
using SparseMatrixd = Eigen::SparseMatrix<double,Eigen::RowMajor>;
//using SparseMatrixd = Eigen::SparseMatrix<double>;

using SparseTripletd = Eigen::Triplet<double>;

using VecXd = Eigen::VectorXd;
using Vec2d = Eigen::Vector2d;
using Vec3d = Eigen::Vector3d;
using Mat2d = Eigen::Matrix2d;
using Mat3d = Eigen::Matrix3d;
using MatXd = Eigen::MatrixXd;
using Mat2x3d = Eigen::Matrix<double, 2, 3>;
using Mat3x2d = Eigen::Matrix<double, 3, 2>;

// Abstract interface to define an energy term
class Energy {
public:
    void set_index(int i) { index_ = i; }
    int index() const { return index_; }

    const Eigen::VectorXd& weights() const { return weights_; };

    virtual int dim() const = 0;

    virtual Eigen::VectorXd reduce(const Eigen::VectorXd& x) const = 0;

    virtual void get_reduction(std::vector<Eigen::Triplet<double>> &triplets) const = 0;
    virtual void project(Eigen::VectorXd &zi) const = 0;

    virtual void update(int iter) { }

protected:
    Eigen::VectorXd weights_;

private:
    int index_;
};

class DynamicEnergy : public Energy {
public:
    virtual void multiply(const Eigen::VectorXd& x,
                          const Eigen::VectorXd& factor,
                          const Eigen::VectorXd& shift,
                          Eigen::VectorXd& out) const = 0;
};

class BaseEnergy
{
public:
    virtual ~BaseEnergy() { }

    virtual void precompute(const TriMesh& mesh) = 0;

    virtual void getForceAndHessian(const TriMesh& mesh,
                                    const Eigen::VectorXd& x,
				    Eigen::VectorXd& F,
				    SparseMatrixd& dFdx,
				    SparseMatrixd& dFdv) const = 0;

    virtual void getHessianPattern(const TriMesh& mesh,
                                   std::vector<SparseTripletd> &triplets) const = 0;

    // XPBD
    virtual size_t nbrEnergies(const TriMesh& mesh) const { return 0; }

    virtual void perVertexCount(const TriMesh& mesh, std::vector<int>& counts) const { }

    virtual void update(const TriMesh& mesh, const VecXd& x, double dt, VecXd& dx) { }

    void reset() { lambda_.setZero(); }

protected:
    VecXd lambda_; // For XPBD
};


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

#pragma once

#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <HISSTools_FFT/HISSTools_FFT.h>

namespace fluid {
namespace algorithm {

class FFT
{

public:
  using ArrayXcd = Eigen::ArrayXcd;
  using ArrayXcdRef = Eigen::Ref<ArrayXcd>;
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXdRef = Eigen::Ref<const ArrayXd>;

  FFT() = delete;

  FFT(index size)
      : mMaxSize(size), mSize(size), mFrameSize(size / 2 + 1),
        mLog2Size(static_cast<index>(std::log2(size))),
        mOutputBuffer(mFrameSize), mRealBuffer(mFrameSize),
        mImagBuffer(mFrameSize)
  {
    hisstools_create_setup(&mSetup, asUnsigned(mLog2Size));
    mSplit.realp = mRealBuffer.data();
    mSplit.imagp = mImagBuffer.data();
  }

  ~FFT() { hisstools_destroy_setup(mSetup); }

  FFT(const FFT& other) = delete;

  FFT(FFT&& other) { *this = std::move(other); }

  FFT& operator=(const FFT&) = delete;

  FFT& operator=(FFT&& other)
  {
    using std::swap;
    mMaxSize = other.mMaxSize;
    mSize = other.mSize;
    mFrameSize = other.mFrameSize;
    mLog2Size = other.mLog2Size;
    swap(mOutputBuffer, other.mOutputBuffer);
    swap(mRealBuffer, other.mRealBuffer);
    swap(mImagBuffer, other.mImagBuffer);
    swap(mSplit, other.mSplit);
    swap(mSetup, other.mSetup);
    other.mSetup = nullptr;
    return *this;
  }

  void resize(index newSize)
  {
    assert(newSize <= mMaxSize);
    mFrameSize = newSize / 2 + 1;
    mLog2Size = static_cast<index>(std::log2(newSize));
    mSize = newSize;
  }

  Eigen::Ref<ArrayXcd> process(const ArrayXdRef& input)
  {
    hisstools_rfft(mSetup, input.data(), &mSplit, asUnsigned(input.size()),
                   asUnsigned(mLog2Size));
    mSplit.realp[mFrameSize - 1] = mSplit.imagp[0];
    mSplit.imagp[mFrameSize - 1] = 0;
    mSplit.imagp[0] = 0;
    for (index i = 0; i < mFrameSize; i++)
    {
      mOutputBuffer(i) =
          0.5 * std::complex<double>(mSplit.realp[i], mSplit.imagp[i]);
    }
    return mOutputBuffer.segment(0, mFrameSize);
  }

protected:
  index mMaxSize{16384};
  index mSize{1024};
  index mFrameSize{513};
  index mLog2Size{10};

  FFT_SETUP_D         mSetup;
  FFT_SPLIT_COMPLEX_D mSplit;

private:
  ArrayXcd mOutputBuffer;
  ArrayXd  mRealBuffer;
  ArrayXd  mImagBuffer;
};

class IFFT : public FFT
{

public:
  IFFT(index size) : FFT(size), mOutputBuffer(size) {}

  using ArrayXcdRef = Eigen::Ref<const ArrayXcd>;
  using ArrayXdRef = Eigen::Ref<ArrayXd>;

  Eigen::Ref<ArrayXd> process(const Eigen::Ref<const ArrayXcd>& input)
  {
    for (index i = 0; i < input.size(); i++)
    {
      mSplit.realp[i] = input[i].real();
      mSplit.imagp[i] = input[i].imag();
    }
    mSplit.imagp[0] = mSplit.realp[mFrameSize - 1];
    hisstools_rifft(mSetup, &mSplit, mOutputBuffer.data(),
                    asUnsigned(mLog2Size));
    return mOutputBuffer.segment(0, mSize);
  }

private:
  ArrayXd mOutputBuffer;
};
} // namespace algorithm
} // namespace fluid


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_FULLCONNLAYER_HH
#define NETKET_FULLCONNLAYER_HH

#include <Eigen/Dense>
#include <complex>
#include <fstream>
#include <random>
#include <string>
#include <vector>
#include "Utils/all_utils.hpp"
#include "Utils/lookup.hpp"
#include "abstract_layer.hpp"

namespace netket {

class FullyConnected : public AbstractLayer {
  bool usebias_;

  int in_size_;        // input size
  int out_size_;       // output size
  int npar_;           // number of parameters in layer
  MatrixType weight_;  // Weight parameters, W(in_size x out_size)
  VectorType bias_;    // Bias parameters, b(out_size x 1)
                       // Note that input of this layer is also the output of
                       // previous layer

  std::string name_;

  std::size_t scalar_bytesize_;

 public:
  /// Constructor
  FullyConnected(const int input_size, const int output_size,
                 const bool use_bias = false)
      : usebias_(use_bias), in_size_(input_size), out_size_(output_size) {
    Init();
  }

  void Init() {
    scalar_bytesize_ = sizeof(Complex);

    weight_.resize(in_size_, out_size_);
    bias_.resize(out_size_);

    npar_ = in_size_ * out_size_;

    if (usebias_) {
      npar_ += out_size_;
    } else {
      bias_.setZero();
    }

    name_ = "Fully Connected Layer";
  }

  std::string Name() const override { return name_; }

  void to_json(json &pars) const override {
    json layerpar;
    layerpar["Name"] = "FullyConnected";
    layerpar["UseBias"] = usebias_;
    layerpar["Inputs"] = in_size_;
    layerpar["Outputs"] = out_size_;
    layerpar["Bias"] = bias_;
    layerpar["Weight"] = weight_;

    pars["Layers"].push_back(layerpar);
  }

  void from_json(const json &pars) override {
    if (FieldExists(pars, "Weight")) {
      weight_ = pars["Weight"];
    } else {
      weight_.setZero();
    }
    if (FieldExists(pars, "Bias")) {
      bias_ = pars["Bias"];
    } else {
      bias_.setZero();
    }
  }

  void InitRandomPars(int seed, double sigma) override {
    VectorType par(npar_);

    netket::RandomGaussian(par, seed, sigma);

    SetParameters(par);
  }

  int Npar() const override { return npar_; }

  int Ninput() const override { return in_size_; }

  int Noutput() const override { return out_size_; }

  void GetParameters(VectorRefType pars) const override {
    int k = 0;
    if (usebias_) {
      std::memcpy(pars.data(), bias_.data(), out_size_ * scalar_bytesize_);
      k += out_size_;
    }

    std::memcpy(pars.data() + k, weight_.data(),
                in_size_ * out_size_ * scalar_bytesize_);
  }

  void SetParameters(VectorConstRefType pars) override {
    int k = 0;

    if (usebias_) {
      std::memcpy(bias_.data(), pars.data() + k, out_size_ * scalar_bytesize_);

      k += out_size_;
    }

    std::memcpy(weight_.data(), pars.data() + k,
                in_size_ * out_size_ * scalar_bytesize_);
  }

  void UpdateLookup(const VectorType &input,
                    const std::vector<int> &input_changes,
                    const VectorType &new_input, const VectorType &output,
                    std::vector<int> &output_changes,
                    VectorType &new_output) override {
    const int num_of_changes = input_changes.size();
    if (num_of_changes == in_size_) {
      output_changes.resize(out_size_);
      new_output.resize(out_size_);
      Forward(new_input, new_output);
    } else if (num_of_changes > 0) {
      output_changes.resize(out_size_);
      new_output = output;
      UpdateOutput(input, input_changes, new_input, new_output);
    } else {
      output_changes.resize(0);
      new_output.resize(0);
    }
  }

  // Feedforward
  void Forward(const VectorType &input, VectorType &output) override {
    output = bias_;
    output.noalias() += weight_.transpose() * input;
  }

  // Updates theta given the input v, the change in the input (input_changes and
  // prev_input)
  inline void UpdateOutput(const VectorType &v,
                           const std::vector<int> &input_changes,
                           const VectorType &new_input,
                           VectorType &new_output) {
    const int num_of_changes = input_changes.size();
    for (int s = 0; s < num_of_changes; s++) {
      const int sf = input_changes[s];
      new_output += weight_.row(sf) * (new_input(s) - v(sf));
    }
  }

  // Computes derivative.
  void Backprop(const VectorType &prev_layer_output,
                const VectorType & /*this_layer_output*/,
                const VectorType &dout, VectorType &din,
                VectorRefType der) override {
    // dout = d(L) / d(z)
    // Derivative for bias, d(L) / d(b) = d(L) / d(z)
    int k = 0;

    if (usebias_) {
      Eigen::Map<VectorType> der_b{der.data() + k, out_size_};

      der_b.noalias() = dout;
      k += out_size_;
    }

    // Derivative for weights, d(L) / d(W) = [d(L) / d(z)] * in'
    Eigen::Map<MatrixType> der_w{der.data() + k, in_size_, out_size_};

    der_w.noalias() = prev_layer_output * dout.transpose();

    // Compute d(L) / d_in = W * [d(L) / d(z)]
    din.noalias() = weight_ * dout;
  }
};
}  // namespace netket

#endif


#include "encode_decode.h"
#include <queue>
#include <string>
#include <unordered_map>
#include <vector>
#include <boost/dynamic_bitset.hpp>
#include "char_node.h"
#include "encoding.pb.h"
#include "freq_node.h"
#include "inner_node.h"

namespace encode_decode {

	class Comparator {
	public:
	    bool operator()(const std::shared_ptr<FreqNode>& a,
	    				const std::shared_ptr<FreqNode>& b) {
	        return (a->get_freq() > b->get_freq());
	    }
	};

	std::unordered_map<char, int> count_freq(const std::string& s) {
		std::unordered_map<char, int> counter;
		for (auto it = s.begin(); it != s.end(); ++it) {
			std::unordered_map<char,int>::iterator res = counter.find(*it);
			if (res == counter.end()) {
				counter.emplace(*it, 1);
			} else {
				res->second = res->second + 1;
			}
		}
		return counter;
	}

	std::shared_ptr<FreqNode> create_optimal_hufftree(const std::string& s) {

		std::unordered_map<char, int>&& counter = count_freq(s);

		// We use shared_ptr because the priority queue only provides
		// access to the top element as a const reference.
		// Thus, it is impossible to move the ownership of the node or its
		// child nodes.
		// Alternatively, we could copy the node out of the queue,
		// but as we combine nodes into a tree throughout the algorithm,
		// this would entail copying the entire tree,
		// making the algorithm O(n^2) rather than O(n lg n).
		std::priority_queue<std::shared_ptr<FreqNode>,
							std::vector<std::shared_ptr<FreqNode>>,
							Comparator> minheap;
		for (auto it = counter.begin(); it != counter.end(); ++it) {
			// Allocate each character node on the heap
			minheap.push(std::make_shared<CharNode>(
				CharNode(it->first, it->second)));
		}

		int numchars = minheap.size();
		for (int i = 1; i < numchars; ++i) {
			std::shared_ptr<FreqNode> left(minheap.top());
			minheap.pop();
			std::shared_ptr<FreqNode> right(minheap.top());
			minheap.pop();
			minheap.push(std::shared_ptr<InnerNode>{
				new InnerNode(left, right)});
		}
		std::shared_ptr<FreqNode> root(minheap.top());
		minheap.pop();
		return root;
	}

	std::unordered_map<char, boost::dynamic_bitset<>> encode_tree(const FreqNode& root) {
		std::unordered_map<char, boost::dynamic_bitset<>> encodings;
		boost::dynamic_bitset<> bits;
		root.encode_node(bits, encodings);
		return encodings;
	}

	void concat_bitsets(boost::dynamic_bitset<>& bits1,
						const boost::dynamic_bitset<>& bits2) {
		size_t orig_size = bits1.size();
		bits1.resize(orig_size + bits2.size());
		for (size_t i = 0; i < bits2.size(); i++) {
			bits1[orig_size + i] = bits2[i];
		}
	}

	Status encode_string(const std::unordered_map<char, boost::dynamic_bitset<>>& encodings,
					   	 const std::string& s,
					   	 boost::dynamic_bitset<>& bits) {
		for (auto it = s.begin(); it != s.end(); ++it) {
			std::unordered_map<char,boost::dynamic_bitset<>>::const_iterator res = encodings.find(*it);
			if (res == encodings.end()) {
				return Status::kInvalid;
			}
			concat_bitsets(bits, res->second);
		}
		return Status::kOk;
	}

	Status from_proto(const FreqNodeProto& proto_node,
	 			      std::shared_ptr<FreqNode>& node) {
		if (proto_node.type() == FreqNodeProto_Type_CHAR) {
			// TODO: convert to char
			google::protobuf::int32 int_c = proto_node.c();
			char c = static_cast<char>(int_c);
			int freq = proto_node.freq();
			node = std::make_shared<CharNode>(CharNode(c, freq));
			return Status::kOk;
		} else if (proto_node.type() == FreqNodeProto_Type_INNER) {
			std::shared_ptr<FreqNode> lc;
			std::shared_ptr<FreqNode> rc;
			if (from_proto(proto_node.lc(), lc) == Status::kOk &&
				from_proto(proto_node.rc(), rc) == Status::kOk) {
				node = std::make_shared<InnerNode>(InnerNode(lc, rc));
				return Status::kOk;
			} else {
				return Status::kInvalid;
			}
		} else {
			return Status::kInvalid;
		}
	}

	Status decode_bits(const FreqNode& root,
					   const boost::dynamic_bitset<>& encoded_bits,
					   std::string& decoding) {
		size_t bit_index(0);
		while (bit_index < encoded_bits.size()) {
			char c;
			if (root.find_next_char(encoded_bits, bit_index, c) == Status::kOk) {
				decoding.push_back(c);
			} else {
				// An invalid status occurs if there are not enough bits
				// in the bitset to reach a char node.
				return Status::kInvalid;
			}
		}
		return Status::kOk;
	}
}


//
// Created by michel on 13-04-21.
//

#include "test-helper.h"
#include "org-simple/util/dsp/integration.h"
#include <boost/math/special_functions/relative_difference.hpp>

using namespace org::simple::util::dsp;

static bool same(double v1, double v2, double epsilon = 1e-12) {
  return boost::math::relative_difference(v1, v2) < epsilon;
}

BOOST_AUTO_TEST_SUITE(org_simple_dsp_integration_Tests)

BOOST_AUTO_TEST_CASE(testInitWithScaleOne) {
  Coefficients coeffs = Coefficients ::from_count(1.0, 1.0);

  BOOST_CHECK_EQUAL(1.0, coeffs.scale());

  BOOST_CHECK_EQUAL(exp(-1), coeffs.history_multiplier());

  BOOST_CHECK_EQUAL(1.0 - exp(-1), coeffs.input_multiplier());
  BOOST_CHECK_EQUAL(coeffs.input_multiplier(),
                    coeffs.input_multiplier_scaled());

  BOOST_CHECK_EQUAL(1.0, coeffs.integration_sample_count());
}

BOOST_AUTO_TEST_CASE(testInitWithScaleOneAndImplicitOne) {
  Coefficients coeffs1n = Coefficients ::from_count(1.0);
  Coefficients coeffs11 = Coefficients ::from_count(1.0, 1.0);

  BOOST_CHECK_EQUAL(coeffs11.scale(), coeffs1n.scale());

  BOOST_CHECK_EQUAL(coeffs11.history_multiplier(),
                    coeffs1n.history_multiplier());

  BOOST_CHECK_EQUAL(coeffs11.input_multiplier(), coeffs1n.input_multiplier());
  BOOST_CHECK_EQUAL(coeffs11.input_multiplier_scaled(),
                    coeffs1n.input_multiplier_scaled());

  BOOST_CHECK_EQUAL(coeffs11.integration_sample_count(),
                    coeffs1n.integration_sample_count());
}

BOOST_AUTO_TEST_CASE(testInitWithScaleTwo) {
  const double scale = 2.0;
  Coefficients coeffs1 = Coefficients ::from_count(1.0, 1.0);
  Coefficients coeffs2 = Coefficients ::from_count(1.0, scale);

  BOOST_CHECK_EQUAL(scale, coeffs2.scale());

  BOOST_CHECK_EQUAL(coeffs1.history_multiplier(), coeffs2.history_multiplier());

  BOOST_CHECK_EQUAL(coeffs1.input_multiplier(), coeffs2.input_multiplier());
  BOOST_CHECK_EQUAL(scale * coeffs1.input_multiplier(),
                    coeffs2.input_multiplier_scaled());

  BOOST_CHECK_EQUAL(1.0, coeffs2.integration_sample_count());
}

BOOST_AUTO_TEST_CASE(testCOuntOneScaleOneIntegrationConsistency) {
  Coefficients coeffs = Coefficients ::from_count(1.0, 1.0);
  double input = 1.0;
  double history1 = 0;
  double history2 = 0;
  double history3 = 0;
  double output1;
  double output2;

  output1 = coeffs.get_integrated(history1, input);
  output2 = coeffs.integrate_and_get(history2, input);
  coeffs.integrate(history3, input);

  BOOST_CHECK_EQUAL(output1, output2);
  BOOST_CHECK_EQUAL(output2, history2);
  BOOST_CHECK_EQUAL(output2, history3);

  BOOST_CHECK_EQUAL(input * coeffs.input_multiplier(), output1);

  history1 = history2;
  output1 = coeffs.get_integrated(history1, input);
  output2 = coeffs.integrate_and_get(history2, input);
  coeffs.integrate(history3, input);

  BOOST_CHECK_EQUAL(output1, output2);
  BOOST_CHECK_EQUAL(output2, history2);
  BOOST_CHECK_EQUAL(output2, history3);

  BOOST_CHECK_EQUAL(coeffs.input_multiplier() * (coeffs.history_multiplier() + input), output1);
}

BOOST_AUTO_TEST_CASE(testCOuntOneScaleTwoIntegrationConsistency) {
  double scale = 2;
  Coefficients coeffs = Coefficients ::from_count(1.0, scale);
  double input = 1.0;
  double history1 = 0;
  double history2 = 0;
  double history3 = 0;
  double output1;
  double output2;

  output1 = coeffs.get_integrated(history1, input);
  output2 = coeffs.integrate_and_get(history2, input);
  coeffs.integrate(history3, input);

  BOOST_CHECK_EQUAL(output1, output2);
  BOOST_CHECK_EQUAL(output2, history2);
  BOOST_CHECK_EQUAL(output2, history3);

  BOOST_CHECK_EQUAL(input * coeffs.input_multiplier_scaled(), output1);

  history1 = history2;
  output1 = coeffs.get_integrated(history1, input);
  output2 = coeffs.integrate_and_get(history2, input);
  coeffs.integrate(history3, input);

  BOOST_CHECK_EQUAL(output1, output2);
  BOOST_CHECK_EQUAL(output2, history2);
  BOOST_CHECK_EQUAL(output2, history3);

  BOOST_CHECK_EQUAL(scale * (coeffs.input_multiplier() * (coeffs.history_multiplier() + input)), output1);
}

BOOST_AUTO_TEST_CASE(testImpulseResponseSumIsScale) {
  Coefficients coeffs;
  for (double samples = 0.5; samples < 5; samples += 0.5) {
    for (double scale = 0.5; scale < 5; scale += 0.5) {
      coeffs = Coefficients::from_count_bound(samples, scale);
      double input = 1;
      double previous_output = 10 * scale * samples;
      double output = 0;
      double previous_sum = -1;
      double sum = 0;
      int i = 0;
      while (++i < 10 || (sum > previous_sum && output < previous_output && output > 1e-8)) {
        previous_output = output;
        previous_sum = sum;
        sum += coeffs.integrate_and_get(output, input);
        input = 0;
      }
      BOOST_CHECK(same(sum, scale, 1e-7));
      if (!same(sum, scale, 1e-7)) {
        std::cout << "Sum=" << sum << "; scale=" << scale << std::endl;
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(testStepResponseIsScale) {
  Coefficients coeffs;
  for (double samples = 0.5; samples < 5; samples += 0.5) {
    for (double scale = 0.5; scale < 5; scale += 0.5) {
      coeffs = Coefficients::from_count_bound(samples, scale);
      double input = 1;
      double output = 0;
      int i = 0;
      while (++i < 10000 || !same(output, scale)) {
        coeffs.integrate(output, input);
      }
      BOOST_CHECK(same(output, scale, 1e-7));
      if (!same(output, scale, 1e-7)) {
        std::cout << "Output=" << output << "; scale=" << scale << std::endl;
      }
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


#include <cmath>
#include <cstddef>
#include <vector>

#include <Eigen/Core>
#include <eigen-checks/gtest.h>
#include <gtest/gtest.h>
#include <SuiteSparseQR.hpp>

#include "truncated-svd-solver/linear-algebra-helpers.h"
#include "truncated-svd-solver/marginalization.h"

namespace truncated_svd_solver {

bool getR(cholmod_sparse* A, cholmod_sparse** R, cholmod_common* cholmod) {
  CHECK_NOTNULL(A);
  CHECK_NOTNULL(R);
  CHECK_NOTNULL(cholmod);

  cholmod_sparse* qr_A = cholmod_l_transpose(A, 1, cholmod);
  SuiteSparseQR<double>(SPQR_ORDERING_FIXED, SPQR_NO_TOL, qr_A->ncol, 0, qr_A,
                        nullptr, nullptr, nullptr, nullptr, R, nullptr, nullptr,
                        nullptr, nullptr, cholmod);
  cholmod_l_free_sparse(&qr_A, cholmod);
  return cholmod->status;
}

TEST(TruncatedSvdSolver, Marginalization) {
  cholmod_common cholmod;
  cholmod_l_start(&cholmod);

  // Create a random jacobian.
  Eigen::MatrixXd J = Eigen::MatrixXd::Random(5, 5);
  Eigen::MatrixXd J_cov_expected = (J.transpose() * J).inverse();

  // Convert to cholmod sparse format.
  cholmod_sparse* Jt_cholmod =
      eigenDenseToCholmodSparseCopy(J.transpose(), &cholmod, 1e-16);

  // Test the results.
  Eigen::MatrixXd NS, CS, Sigma, SigmaP, Omega;
  const double svLogSum = marginalize(Jt_cholmod, 0, NS, CS, Sigma,
    SigmaP, Omega);

  EXPECT_NEAR(std::fabs(svLogSum),
              std::fabs(std::log2(std::fabs(J_cov_expected.determinant()))),
              1e-8);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(Sigma, J_cov_expected, 1e-12));

  cholmod_l_finish(&cholmod);
}

}  // namespace truncated_svd_solver

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  google::InitGoogleLogging(argv[0]);
  google::InstallFailureSignalHandler();\
  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
  FLAGS_alsologtostderr = true;
  FLAGS_colorlogtostderr = true;
  return RUN_ALL_TESTS();
}


#include <igl/read_triangle_mesh.h>
#include <igl/get_seconds.h>
#include <igl/material_colors.h>
#include <igl/copyleft/marching_cubes.h>
#include <igl/copyleft/swept_volume.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Core>
#include <iostream>

#include "tutorial_shared_path.h"

int main(int argc, char * argv[])
{
  using namespace std;
  using namespace igl;
  Eigen::MatrixXi F,SF;
  Eigen::MatrixXd V,SV,VT;
  bool show_swept_volume = false;
  // Define a rigid motion
  const auto & transform = [](const double t)->Eigen::Affine3d
  {
    Eigen::Affine3d T = Eigen::Affine3d::Identity();
    T.rotate(Eigen::AngleAxisd(t*2.*M_PI,Eigen::Vector3d(0,1,0)));
    T.translate(Eigen::Vector3d(0,0.125*cos(2.*M_PI*t),0));
    return T;
  };
  // Read in inputs as double precision floating point meshes
  read_triangle_mesh(
      TUTORIAL_SHARED_PATH "/bunny.off",V,F);
  cout<<R"(Usage:
[space]  Toggle between transforming original mesh and swept volume
)";
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V,F);
  viewer.data().set_face_based(true);
  viewer.core.is_animating = !show_swept_volume;
  const int grid_size = 50;
  const int time_steps = 200;
  const double isolevel = 0.1;
  std::cerr<<"Computing swept volume...";
  igl::copyleft::swept_volume(
    V,F,transform,time_steps,grid_size,isolevel,SV,SF);
  std::cerr<<" finished."<<std::endl;

  viewer.callback_pre_draw =
    [&](igl::opengl::glfw::Viewer & viewer)->bool
    {
      if(!show_swept_volume)
      {
        Eigen::Affine3d T = transform(0.25*igl::get_seconds());
        VT = V*T.matrix().block(0,0,3,3).transpose();
        Eigen::RowVector3d trans = T.matrix().block(0,3,3,1).transpose();
        VT = ( VT.rowwise() + trans).eval();
        viewer.data().set_vertices(VT);
        viewer.data().compute_normals();
      }
      return false;
    };
  viewer.callback_key_down =
    [&](igl::opengl::glfw::Viewer & viewer, unsigned char key, int mod)->bool
    {
      switch(key)
      {
        default:
          return false;
        case ' ':
          show_swept_volume = !show_swept_volume;
          viewer.data().clear();
          if(show_swept_volume)
          {
            viewer.data().set_mesh(SV,SF);
            Eigen::Vector3d ambient = Eigen::Vector3d(SILVER_AMBIENT[0], SILVER_AMBIENT[1], SILVER_AMBIENT[2]);
            Eigen::Vector3d diffuse = Eigen::Vector3d(SILVER_DIFFUSE[0], SILVER_DIFFUSE[1], SILVER_DIFFUSE[2]);
            Eigen::Vector3d specular = Eigen::Vector3d(SILVER_SPECULAR[0], SILVER_SPECULAR[1], SILVER_SPECULAR[2]);
            viewer.data().uniform_colors(ambient,diffuse,specular);
          }
          else
          {
            viewer.data().set_mesh(V,F);
          }
          viewer.core.is_animating = !show_swept_volume;
          viewer.data().set_face_based(true);
          break;
      }
      return true;
    };
  viewer.launch();
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "Utils/Optimizer/GradientBased/GradientBasedCheck.h"
#include "Utils/Settings.h"
#include <Eigen/Core>

namespace Scine {
namespace Utils {

bool GradientBasedCheck::checkConvergence(const Eigen::VectorXd& parameter, double value, const Eigen::VectorXd& gradient) {
  if (_oldParams.size() != parameter.size()) {
    _oldParams = Eigen::VectorXd::Zero(parameter.size());
  }
  // Generate temporaries
  Eigen::VectorXd deltaParam = (parameter - _oldParams).eval();
  double deltaV = value - _oldValue;
  // Rotate stored old data
  _oldParams = parameter;
  _oldValue = value;
  // Check
  unsigned int converged = 0;
  if (gradient.cwiseAbs().maxCoeff() < gradMaxCoeff) {
    converged++;
  }
  if (deltaParam.cwiseAbs().maxCoeff() < stepMaxCoeff) {
    converged++;
  }
  if (sqrt(gradient.squaredNorm() / gradient.size()) < gradRMS) {
    converged++;
  }
  if (sqrt(deltaParam.squaredNorm() / deltaParam.size()) < stepRMS) {
    converged++;
  }
  return ((fabs(deltaV) < deltaValue) && (converged >= requirement));
}

bool GradientBasedCheck::checkMaxIterations(unsigned int currentIteration) const {
  return currentIteration >= maxIter;
}

void GradientBasedCheck::setParametersAndValue(const Eigen::VectorXd& parameter, double value) {
  _oldParams = parameter;
  _oldValue = value;
}

void GradientBasedCheck::addSettingsDescriptors(UniversalSettings::DescriptorCollection& collection) const {
  UniversalSettings::DoubleDescriptor step_max_coeff(
      "Convergence threshold for step vector's maximum absolute element.");
  step_max_coeff.setMinimum(0.0);
  step_max_coeff.setDefaultValue(stepMaxCoeff);
  collection.push_back(GradientBasedCheck::gconvStepMaxCoeffKey, step_max_coeff);

  UniversalSettings::DoubleDescriptor step_RMS("Convergence threshold for step vector's RMS.");
  step_RMS.setMinimum(0.0);
  step_RMS.setDefaultValue(stepRMS);
  collection.push_back(GradientBasedCheck::gconvStepRMSKey, step_RMS);

  UniversalSettings::DoubleDescriptor grad_max_coeff(
      "Convergence threshold for gradient vector's maximum absolute element.");
  grad_max_coeff.setMinimum(0.0);
  grad_max_coeff.setDefaultValue(gradMaxCoeff);
  collection.push_back(GradientBasedCheck::gconvGradMaxCoeffKey, grad_max_coeff);

  UniversalSettings::DoubleDescriptor grad_RMS("Convergence threshold for gradient vector's RMS.");
  grad_RMS.setMinimum(0.0);
  grad_RMS.setDefaultValue(gradRMS);
  collection.push_back(GradientBasedCheck::gconvGradRMSKey, grad_RMS);

  UniversalSettings::DoubleDescriptor delta_value(
      "Convergence threshold for the absolute difference in the value between the current and the last step.");
  delta_value.setMinimum(0.0);
  delta_value.setDefaultValue(deltaValue);
  collection.push_back(GradientBasedCheck::gconvDeltaValueKey, delta_value);

  UniversalSettings::IntDescriptor max_iter("The maximum number of iterations.");
  max_iter.setMinimum(0.0);
  max_iter.setDefaultValue(maxIter);
  collection.push_back(GradientBasedCheck::gconvMaxIterKey, max_iter);

  UniversalSettings::IntDescriptor requirements(
      "The number of threasholds besides the value one that need to converge for overall convergence.");
  requirements.setDefaultValue(requirement);
  requirements.setMaximum(4);
  requirements.setMinimum(0);
  collection.push_back(GradientBasedCheck::gconvRequirementKey, requirements);
}

void GradientBasedCheck::applySettings(const Settings& settings) {
  stepMaxCoeff = settings.getDouble(GradientBasedCheck::gconvStepMaxCoeffKey);
  stepRMS = settings.getDouble(GradientBasedCheck::gconvStepRMSKey);
  gradMaxCoeff = settings.getDouble(GradientBasedCheck::gconvGradMaxCoeffKey);
  gradRMS = settings.getDouble(GradientBasedCheck::gconvGradRMSKey);
  deltaValue = settings.getDouble(GradientBasedCheck::gconvDeltaValueKey);
  maxIter = settings.getInt(GradientBasedCheck::gconvMaxIterKey);
  requirement = settings.getInt(GradientBasedCheck::gconvRequirementKey);
}

} // namespace Utils
} // namespace Scine


// Kinematics plugin for MoveIt! implementing null-space optimization
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include "apc_kinematics.h"
#include "srdf_cache.h"

#include <urdf/model.h>

#include <ros/console.h>

#include <boost/make_shared.hpp>

#include <pluginlib/class_list_macros.h>

#include <rbdl/Kinematics.h>

#include <Eigen/Geometry>

#include <eigen_conversions/eigen_msg.h>

#include <angles/angles.h>

#include <math.h>

#include <exception>

#include <visualization_msgs/MarkerArray.h>

/** \brief Computes the jacobian
 *
 * \param model   	rigid body model
 * \param Q       	state vector of the joints
 * \param joint_mapping mapping from the joint ids to the rbdl joint ids
 * \param body_id 	the id of the body
 * \param base_id 	the id of the base
 * \param J       	a matrix where the result will be stored in
 *
 * \returns A 6 x \#dof_count matrix of the jacobian
 */
static void _CalcPointRotJacobian (RigidBodyDynamics::Model &model,
	const RigidBodyDynamics::Math::VectorNd &Q,
	const std::vector<int> &joint_mapping,
	unsigned int body_id,
	unsigned int base_id,
	RigidBodyDynamics::Math::MatrixNd &J)
{
	using namespace RigidBodyDynamics;
	using namespace Math;

	// Convert the joint vector into the rbdl convention
	Eigen::VectorXd rbdlJoints = Eigen::VectorXd::Zero(model.dof_count);
	for(unsigned int i = 0; i < joint_mapping.size(); ++i)
		rbdlJoints[joint_mapping[i]-1] = Q[i];

	Vector3d point_base_pos = CalcBodyToBaseCoordinates(model, rbdlJoints, body_id, Eigen::VectorXd::Zero(3), false);
	SpatialMatrix point_trans = Math::Xtrans_mat (point_base_pos);

	assert (J.rows() == 6 && J.cols() == (int)joint_mapping.size() );

	J.setZero();

	// we have to make sure that only the joints that contribute to the
	// bodies motion also get non-zero columns in the jacobian.
	// VectorNd e = VectorNd::Zero(Q.size() + 1);
	char *e = new char[model.dof_count + 1];
	if (e == NULL) {
		std::cerr << "Error: allocating memory." << std::endl;
		abort();
	}
	memset (&e[0], 0, model.dof_count + 1);

	unsigned int reference_body_id = body_id;

	if (model.IsFixedBodyId(body_id)) {
		unsigned int fbody_id = body_id - model.fixed_body_discriminator;
		reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
	}

	unsigned int j = reference_body_id;

	// e[j] is set to 1 if joint j contributes to the jacobian that we are
	// computing. For all other joints the column will be zero.

	while (j != 0) {
		e[j] = 1;
		j = model.lambda[j];
	}

	for (j = 1; j < model.mBodies.size(); j++) {
		if (e[j] == 1) {
			SpatialVector S_base;
			S_base = point_trans * spatial_inverse(model.X_base[j].toMatrix()) * model.S[j];

			// Map back from the rbdl indices
			int i = std::find(joint_mapping.begin(), joint_mapping.end(), j) - joint_mapping.begin();

			J.col(i) = S_base;
		}
	}

	delete[] e;
}

/**
 * Computes Selectively Damped Least Squares as described by Buss and Kim in:
 *
 * Buss, Samuel R., and Jin-Su Kim. "Selectively damped least squares for inverse kinematics." journal of graphics, gpu, and game tools 10.3 (2005): 37-49.
 *
 * \param jacobian   	the current jacobian
 * \param e       	the pose diff between the end effector and the desired target pose
 * \param modifiedJ 	a matrix where the modified jacobian will be stored in
 * \param invModifiedJ 	a matrix where the inverse of the modified jacobian will be stored in
 */
template<typename _Matrix_Type_>
void sdls(const _Matrix_Type_ &jacobian, Eigen::VectorXd &e, Eigen::MatrixXd &modifiedJ, Eigen::MatrixXd &invModifiedJ, double gamma_max = 0.78539816) //PI/4
{
	unsigned int cols = jacobian.cols();
	unsigned int rows = jacobian.rows();

	// Calculate the norms of columns of the jacobian
	Eigen::VectorXd p(cols);
	for(unsigned int j = 0; j<cols; j++)
	{
		p(j) = jacobian.col(j).norm();
	}

	Eigen::JacobiSVD< _Matrix_Type_ > svd(jacobian ,Eigen::ComputeThinU | Eigen::ComputeThinV);

	Eigen::VectorXd alpha(rows);
	for(unsigned int i = 0; i<rows; i++)
	{
		alpha(i) = svd.matrixU().col(i).transpose() * e;
	}

	// Calculate the norms of U
	Eigen::VectorXd N(rows); // each component of N is 1
	for(unsigned int i = 0; i<rows; i++)
	{
		N(i) = svd.matrixU().col(i).norm();
	}

	Eigen::VectorXd M(rows);
	for(unsigned int i = 0; i<rows; i++)
	{
		if(std::abs(svd.singularValues()(i)) < 1.0e-4)
			continue;

		double tmp = 0.0;
		for(unsigned int j = 0; j<cols; j++)
		{
			tmp += std::abs(svd.matrixV()(j, i)) * p(j);
		}

		M(i) = (1.0/svd.singularValues()(i)) * tmp;
	}

	Eigen::VectorXd gamma(rows);
	gamma.setConstant(gamma_max);
	for(unsigned int i = 0; i<rows; i++)
	{
		if(std::abs(svd.singularValues()(i)) < 1.0e-4)
			continue;

		if(N(i)<M(i))
		{
			gamma(i) *= N(i)/M(i);
		}
	}

	modifiedJ.setZero();
	invModifiedJ.setZero();

	for(unsigned int i=0; i<rows; i++)
	{
		if(std::abs(svd.singularValues()(i)) < 1.0e-4)
			continue;

		Eigen::VectorXd phi_tmp = (1.0/svd.singularValues()(i)) * alpha(i) * svd.matrixV().col(i);
		double max = phi_tmp.maxCoeff();

		Eigen::MatrixXd modifiedJtmp(rows, cols);
		modifiedJtmp = svd.singularValues()(i) * svd.matrixU().col(i) * svd.matrixV().col(i).transpose();

		Eigen::MatrixXd invModifiedJtmp(cols, rows);
		invModifiedJtmp = (1.0/svd.singularValues()(i)) * svd.matrixV().col(i) * svd.matrixU().col(i).transpose();

		if(gamma(i) < max)
		{
			invModifiedJtmp *= (gamma(i)/max);
			modifiedJtmp *= (max/gamma(i));
		}

		modifiedJ += modifiedJtmp;
		invModifiedJ += invModifiedJtmp;
	}
}

namespace apc_kinematics
{

APCKinematics::APCKinematics()
 : m_param_max_iterations("/ik/max_iterations", 1, 1, 100, 20)
 , m_param_max_posdiff("/ik/max_posdiff", 0.0, 0.01, 1.0, 0.1)
 , m_param_angle_weight_slope("/ik/angle_weight_slope", -10.0, 0.1, 0.0, -4.0)
 , m_param_nullspace_optimization("/ik/nullspace_optimization", true)
 , m_param_weight_convenient("/ik/cost/weight_convenient", 0.0, 0.1, 10.0, 1.0)
 , m_param_weight_seed("/ik/cost/weight_seed", 0.0, 0.1, 10.0, 1.0)
 , m_param_weight_limit("/ik/cost/weight_limit", 0.0, 0.1, 10.0, 1.0)
 , m_param_alpha("/ik/alpha", 0.0, 0.01, 1.0, 0.05)
 , m_param_alpha_decay("/ik/alpha_decay", 0.0, 0.01, 1.0, 0.9)
 , m_param_eef_enabled("/ik/eef/enabled", true)
 , m_param_eef_soft_limit_x("/ik/eef/soft_limit/x", 0.0, 0.001, 1.5, 0.005)
 , m_param_eef_soft_limit_z("/ik/eef/soft_limit/z", 0.0, 0.001, 1.5, 0.005)
 , m_param_eef_s("/ik/eef/s", 0.0, 0.001, 5000.0, 0.5)
 , m_param_eef_orient("/ik/eef/orient", 0.0, 0.001, 5000.0, 0.5)
 , m_ik_limit_link(-1)
{
}

APCKinematics::~APCKinematics()
{
}

bool APCKinematics::initialize(const std::string& robot_description,
	const std::string& group_name,
	const std::string& base_frame,
	const std::string& tip_frame,
	double search_discretization)
{
	ROS_INFO("IK base frame: %s", base_frame.c_str());
	setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);

	// Robot model (from URDF model on parameter server)
	m_urdf = boost::make_shared<urdf::Model>();
	if(!m_urdf->initParam(robot_description))
	{
		ROS_ERROR("Could not get URDF model");
		return false;
	}

	m_rbdl.initFrom(*m_urdf, base_frame);

	m_srdf = loadSRDF();

	// ... and search for the convenient state and the weighting
	bool convenient_state_found = false;
	bool weighting_found = false;
	std::vector<srdf::Model::GroupState> groupStates = m_srdf->getGroupStates();
	for(unsigned int i=0; i<groupStates.size(); ++i)
	{
		if(groupStates[i].group_ == group_name && groupStates[i].name_.substr(0, 10) == "convenient")
		{
			m_convenient_state = groupStates[i];
			convenient_state_found = true;
		}
		else if(groupStates[i].group_ == group_name && groupStates[i].name_.substr(0, 9) == "weighting")
		{
			m_weighting = groupStates[i];
			weighting_found = true;
		}
	}

	// we can only use the nullspace opimization if these states exist
	m_nullspaceOptimization = weighting_found && convenient_state_found;
	if(!m_nullspaceOptimization)
	{
		ROS_WARN_STREAM("Unable to find convenient state or weighting for group "<<group_name<<". Disabling nullspace optimization!");
	}

	unsigned int base_id = m_rbdl.GetBodyId(base_frame.c_str());
	if(base_id == (unsigned int)-1)
	{
		ROS_ERROR("Could not find base frame '%s'", base_frame.c_str());
		return false;
	}

	unsigned int tip = m_rbdl.GetBodyId(tip_frame.c_str());
	if(tip == (unsigned int)-1)
	{
		ROS_ERROR("Could not find tip_frame '%s'", tip_frame.c_str());
		return false;
	}

	if(tip >= m_rbdl.fixed_body_discriminator)
	{
		m_fixedTrans = m_rbdl.mFixedBodies[tip - m_rbdl.fixed_body_discriminator].mParentTransform;
		m_tip = m_rbdl.mFixedBodies[tip - m_rbdl.fixed_body_discriminator].mMovableParent;
	}
	else
	{
		m_fixedTrans.E.setIdentity();
		m_fixedTrans.r.setZero();
		m_tip = tip;
	}

	do
	{
		m_links.push_back(tip);
		m_linkNames.push_back(m_rbdl.GetBodyName(tip));

		if(tip < m_rbdl.fixed_body_discriminator)
		{
			m_joints.push_back(tip);
			m_jointNames.push_back(m_rbdl.jointName(tip));

			boost::shared_ptr<const urdf::Joint> urdfJoint = m_urdf->getJoint(m_jointNames.back());

			double epsilon;
			if(urdfJoint->type == urdf::Joint::PRISMATIC)
				epsilon = 0.001;
			else
				epsilon = 2.0 * (M_PI / 180.0);

			m_upperLimit.push_back(urdfJoint->limits->upper - epsilon);
			m_lowerLimit.push_back(urdfJoint->limits->lower + epsilon);

			ROS_DEBUG("Solver: Link '%s' with parent joint '%s' (limits: %f to %f)",
				m_linkNames.back().c_str(),
				m_jointNames.back().c_str(),
				m_lowerLimit.back(), m_upperLimit.back()
			);

			tip = m_rbdl.lambda[tip];
		}
		else
		{
			ROS_DEBUG("Fixed transform link '%s'", m_linkNames.back().c_str());
			tip = m_rbdl.mFixedBodies[tip - m_rbdl.fixed_body_discriminator].mMovableParent;
		}
	}
	while(tip != 0);

	m_rbdlMeasuredJointAngles = Eigen::VectorXd::Zero(m_rbdl.dof_count);
	m_sub_js = ros::NodeHandle().subscribe("/joint_states", 1,
		&APCKinematics::handleJointStates, this
	);

	// Disable eef nullspace for arm-only IK
	if(group_name.find("arm_with_eef") != std::string::npos)
	{
		m_worldLink = m_rbdl.GetBodyId("world");
		m_ik_limit_link = m_rbdl.GetBodyId("ik_limit_link");
		m_ik_wrist_link = m_rbdl.GetBodyId("wrist_2_link");
	}
	else
	{
		m_worldLink = m_ik_limit_link =  std::numeric_limits<unsigned int>::max();
		m_ik_wrist_link = std::numeric_limits<unsigned int>::max();
	}

	ROS_INFO("world link: %u, ik finger link: %u", m_worldLink, m_ik_limit_link);

	m_pub_elbowMarker = ros::NodeHandle().advertise<visualization_msgs::MarkerArray>(
		"/ik/" + group_name + "/elbow", 1
	);

	std::string ignoreRoll_param = "/robot_description_kinematics/" + group_name + "/apc_kinematics_ignore_roll";
	ros::NodeHandle nh;
	nh.param(ignoreRoll_param, m_ignoreRoll, false);

	ROS_INFO("Group '%s' is %s roll.", group_name.c_str(), m_ignoreRoll ? "ignoring" : "not ignoring");

	m_limitMarkers.markers.resize(LIMIT_COUNT);
	m_limitMarkers.markers[LIMIT_X].type = visualization_msgs::Marker::CUBE;
	m_limitMarkers.markers[LIMIT_X].header.frame_id = "world";
	m_limitMarkers.markers[LIMIT_X].id = LIMIT_X;
	m_limitMarkers.markers[LIMIT_X].scale.x = 0.01;
	m_limitMarkers.markers[LIMIT_X].scale.y = 5.0;
	m_limitMarkers.markers[LIMIT_X].scale.z = 5.0;
	m_limitMarkers.markers[LIMIT_X].color.r = 1.0;
	m_limitMarkers.markers[LIMIT_X].color.a = 0.3;
	m_limitMarkers.markers[LIMIT_X].pose.orientation.w = 1.0;

	m_limitMarkers.markers[LIMIT_Z].type = visualization_msgs::Marker::CUBE;
	m_limitMarkers.markers[LIMIT_Z].header.frame_id = "world";
	m_limitMarkers.markers[LIMIT_Z].id = LIMIT_Z;
	m_limitMarkers.markers[LIMIT_Z].scale.x = 5.0;
	m_limitMarkers.markers[LIMIT_Z].scale.y = 5.0;
	m_limitMarkers.markers[LIMIT_Z].scale.z = 0.01;
	m_limitMarkers.markers[LIMIT_Z].color.b = 1.0;
	m_limitMarkers.markers[LIMIT_Z].color.a = 0.3;
	m_limitMarkers.markers[LIMIT_Z].pose.orientation.w = 1.0;

	m_limitMarkers.markers[LIMIT_TABLE].type = visualization_msgs::Marker::CUBE;
	m_limitMarkers.markers[LIMIT_TABLE].header.frame_id = "base_link";
	m_limitMarkers.markers[LIMIT_TABLE].id = LIMIT_TABLE;
	m_limitMarkers.markers[LIMIT_TABLE].scale.x = 0.01;
	m_limitMarkers.markers[LIMIT_TABLE].scale.y = 5.0;
	m_limitMarkers.markers[LIMIT_TABLE].scale.z = 5.0;
	m_limitMarkers.markers[LIMIT_TABLE].color.b = 1.0;
	m_limitMarkers.markers[LIMIT_TABLE].color.a = 0.3;

	m_pub_limitMarkers = nh.advertise<visualization_msgs::MarkerArray>("limit_markers", 1);

	return true;
}

bool APCKinematics::getPositionIK(const geometry_msgs::Pose &ik_pose,
	const std::vector<double> &ik_seed_state,
	std::vector<double> &solution,
	moveit_msgs::MoveItErrorCodes &error_code,
	const kinematics::KinematicsQueryOptions &options) const
{
	ROS_INFO("1");
	return false;
}

bool APCKinematics::searchPositionIK(const geometry_msgs::Pose &ik_pose,
	const std::vector<double> &ik_seed_state,
	double timeout,
	std::vector<double> &solution,
	moveit_msgs::MoveItErrorCodes &error_code,
	const kinematics::KinematicsQueryOptions &options) const
{
	ROS_INFO("2");
	return false;
}

static Eigen::Vector3d getEuler(const Eigen::Matrix3d& R)
{
// 	Eigen::Vector3d e = mat.eulerAngles(2,1,0);
// 	return Eigen::Vector3d(e[2], e[1], e[0]);
// 	return mat.eulerAngles(0,1,2);

	Eigen::Vector3d p;

	// euler angles
	p( 1 ) = atan2( -R( 2, 0 ), sqrtf( R( 0, 0 )*R( 0, 0 ) + R( 1, 0 )*R( 1, 0 ) ) );
	if( fabs( p( 1 ) - 0.5*M_PI ) < 1e-5 ) {
		p( 2 ) = 0;
		p( 0 ) = atan2( R( 0, 1 ), R( 1, 1 ) );
	}
	else if( fabs( p( 1 ) + 0.5*M_PI ) < 1e-5 ) {
		p( 2 ) = 0;
		p( 0 ) = -atan2( R( 0, 1 ), R( 1, 1 ) );
	}
	else {
		p( 0 ) = atan2( R( 2, 1 ) / cos( p( 1 ) ), R( 2, 2 ) / cos( p( 1 ) ) );
		p( 2 ) = atan2( R( 1, 0 ) / cos( p( 1 ) ), R( 0, 0 ) / cos( p( 1 ) ) );
	}

	return p;
}

static Eigen::VectorXd calcPoseDiff(const Eigen::Affine3d& current, const Eigen::Affine3d& target, bool ignoreXRotation)
{
	Eigen::VectorXd ret(6);

	if(ignoreXRotation)
	{
		// Find minimum rotation to rotate target.x onto current.x
		Eigen::Vector3d axis = target.rotation().col(0).cross(current.rotation().col(0));
		double norm = axis.norm();
		if(norm < 1e-10)
			ret.head<3>().setZero();
		else
		{
			double angle = asin(norm);
			axis.normalize();

			Eigen::Matrix3d rot;
			rot = Eigen::AngleAxisd(angle, axis);

			ret.head<3>() = getEuler(rot);
		}
	}
	else
	{
		ret.head<3>() = getEuler(current.rotation() * target.rotation().transpose());
	}

	ret.tail<3>() = current.translation() - target.translation();

	return ret;
}

void APCKinematics::costFunction(int joint, double q, double seed_q, double* cost, double *costGradient) const
{
	double convenient = m_convenient_state.joint_values_.at(m_jointNames[joint])[0];
	double weight = m_weighting.joint_values_.at(m_jointNames[joint])[0];
	double angleDiff = 0.1 * (q - convenient);
	angleDiff *= pow(weight, 2.0);

	double diffFromSeed = q - seed_q;

	const double limitStartDiff = 0.1 * M_PI;
	const double limitSlope = 10.0;
	double maxJointAngleOffset = 0;
	double minJointAngleOffset = 0;
	double maxJointAngleOffsetGradient = 0;
	double minJointAngleOffsetGradient = 0;

	double maxJointAngleDiff = q - (m_upperLimit[joint] - limitStartDiff);
	if(maxJointAngleDiff > 0)
	{
		maxJointAngleOffset = limitSlope * maxJointAngleDiff * maxJointAngleDiff;
		maxJointAngleOffsetGradient = 2.0 * limitSlope * maxJointAngleDiff;

// 		if(q > m_upperLimit[joint])
// 			ROS_INFO("Joint limit violation %s (%f, upper limit %f) => cost %f", m_jointNames[joint].c_str(), q, m_upperLimit[joint], maxJointAngleOffset);
	}

	double minJointAngleDiff = q - (m_lowerLimit[joint] + limitStartDiff);
	if(minJointAngleDiff < 0)
	{
		minJointAngleOffset = limitSlope * minJointAngleDiff * minJointAngleDiff;
		minJointAngleOffsetGradient = 2.0 * limitSlope * minJointAngleDiff;

// 		if(q < m_lowerLimit[joint])
// 			ROS_INFO("Joint limit violation %s (%f, lower limit %f) => cost %f", m_jointNames[joint].c_str(), q, m_lowerLimit[joint], minJointAngleOffset);
	}

	*cost += m_param_weight_convenient()*angleDiff*angleDiff
		+ m_param_weight_seed()*diffFromSeed*diffFromSeed
		+ m_param_weight_limit()*(maxJointAngleOffset + minJointAngleOffset);

	*costGradient += m_param_weight_convenient() * 2.0 * angleDiff
			+ m_param_weight_seed()* 2.0 * diffFromSeed
			+ m_param_weight_limit()*(maxJointAngleOffsetGradient + minJointAngleOffsetGradient);
}

void APCKinematics::wristCostFunction(rbdl_parser::URDF_RBDL_Model& model, const Eigen::VectorXd& currentJointAngles, double* cost, Eigen::VectorXd* costGradient) const
{
	Eigen::MatrixXd Jwrist(6, m_joints.size());

	_CalcPointRotJacobian(model, currentJointAngles, m_joints, m_ik_limit_link, m_base, Jwrist);

	// We are only interested in the position (bottom three rows)
// 	Jwrist = Jwrist.bottomRows<3>();

	RigidBodyDynamics::Math::SpatialTransform eefTransform;

	if (m_ik_limit_link >= model.fixed_body_discriminator) {
		auto fBody = m_rbdl.mFixedBodies[m_ik_limit_link - m_rbdl.fixed_body_discriminator];
		auto fixedTransform = fBody.mParentTransform;
		auto parentPose = model.X_base[fBody.mMovableParent];

		eefTransform.E = parentPose.E.transpose() * fixedTransform.E.transpose();
		eefTransform.r = parentPose.r + parentPose.E.transpose() * fixedTransform.r;
	}
	else
	{
		eefTransform.E = model.X_base[m_ik_limit_link].E.transpose();
		eefTransform.r = model.X_base[m_ik_limit_link].r;
	}

	Eigen::Vector3d eef = eefTransform.r;

	RigidBodyDynamics::Math::SpatialTransform worldTransform;

	// TODO: Can we extract this piece of code? It is used everywhere.
	if((unsigned int)m_worldLink >= model.fixed_body_discriminator)
	{
		auto fBody = m_rbdl.mFixedBodies[m_worldLink - m_rbdl.fixed_body_discriminator];
		auto fixedTransform = fBody.mParentTransform;
		auto parentPose = model.X_base[fBody.mMovableParent];

		worldTransform.E = fixedTransform.E * parentPose.E;
		worldTransform.r = parentPose.r + parentPose.E.transpose() * fixedTransform.r;
	}
	else
		worldTransform = model.X_base[m_worldLink];

	// Transform elbow and Jelbow into base_link
	eef = RigidBodyDynamics::CalcBaseToBodyCoordinates(
		model, Eigen::VectorXd(), m_worldLink, eef, false
	);
	Jwrist.topRows<3>() = worldTransform.E * Jwrist.topRows<3>();
	Jwrist.bottomRows<3>() = worldTransform.E * Jwrist.bottomRows<3>();

	visualization_msgs::MarkerArray markers;
	{
		visualization_msgs::Marker marker;
		marker.header.frame_id = "base_link";
		marker.header.stamp = ros::Time::now();

		marker.action = marker.ADD;

		marker.id = 0;
		marker.type = marker.SPHERE;

		marker.scale.x = 0.1;
		marker.scale.y = 0.1;
		marker.scale.z = 0.1;

		marker.pose.position.x = eef.x();
		marker.pose.position.y = eef.y();
		marker.pose.position.z = eef.z();

		marker.pose.orientation.w = 1.0;

		marker.color.a = 1.0;
		marker.color.r = 1.0;

		markers.markers.push_back(marker);
	}

	m_pub_elbowMarker.publish(markers);

	double colCost = 0;
	Eigen::VectorXd cost_gradient(m_joints.size());
	cost_gradient.setZero();

	m_limitMarkers.markers[LIMIT_X].pose.position.x = m_param_eef_soft_limit_x();
	m_limitMarkers.markers[LIMIT_Z].pose.position.z = m_param_eef_soft_limit_z();

	if(eef.z() < m_param_eef_soft_limit_z())
	{
		double violation = eef.z() - m_param_eef_soft_limit_z();

		// translation
		colCost += m_param_eef_s() * pow(violation, 2);

		// fifth row of Jwrist: z component
		cost_gradient += (2.0 * m_param_eef_s() * violation) * Jwrist.row(5);

		// orientation
// 		double pitch = getEuler(eefTransform.E)[1];
// 		colCost += m_param_eef_orient() * pow(pitch, 2);
// 		cost_gradient += (2.0 * m_param_eef_orient() * pitch) * Jwrist.row(5);
	}

	if(eef.x() > m_param_eef_soft_limit_x())
	{
		double violation = eef.x() - m_param_eef_soft_limit_x();

		// translation
		colCost += m_param_eef_s() * pow(violation, 2);

		// third row of Jwrist: x component
		cost_gradient += (2.0 * m_param_eef_s() * violation) * Jwrist.row(3);

		// orientation
// 		double pitch = getEuler(eefTransform.E)[1];
// 		colCost += m_param_eef_orient() * pow(pitch, 2);
// 		cost_gradient += (2.0 * m_param_eef_orient() * pitch) * Jwrist.row(3);
	}

	*cost += colCost;
	*costGradient += cost_gradient;
}

void APCKinematics::tableCostFunction(rbdl_parser::URDF_RBDL_Model& model, const Eigen::VectorXd& currentJointAngles, double* cost, Eigen::VectorXd* costGradient) const
{
	Eigen::MatrixXd Jwrist(6, m_joints.size());

	_CalcPointRotJacobian(model, currentJointAngles, m_joints, m_ik_wrist_link, m_base, Jwrist);

	// We are only interested in the position (bottom three rows)
// 	Jwrist = Jwrist.bottomRows<3>();

	RigidBodyDynamics::Math::SpatialTransform eefTransform;

	if (m_ik_wrist_link >= model.fixed_body_discriminator) {
		auto fBody = m_rbdl.mFixedBodies[m_ik_wrist_link - m_rbdl.fixed_body_discriminator];
		auto fixedTransform = fBody.mParentTransform;
		auto parentPose = model.X_base[fBody.mMovableParent];

		eefTransform.E = parentPose.E.transpose() * fixedTransform.E.transpose();
		eefTransform.r = parentPose.r + parentPose.E.transpose() * fixedTransform.r;
	}
	else
	{
		eefTransform.E = model.X_base[m_ik_wrist_link].E.transpose();
		eefTransform.r = model.X_base[m_ik_wrist_link].r;
	}

	Eigen::Vector3d eef = eefTransform.r;

	constexpr double planeAngle = 40.0 * M_PI / 180.0;
	Eigen::Vector3d planeNormal(0.0, cos(planeAngle), sin(planeAngle));

	double planeDist = 0.45;

	{
		Eigen::Matrix3d rot;
		rot.col(0) = planeNormal;
		rot.col(1) = -Eigen::Vector3d::UnitX();
		rot.col(2) = rot.col(0).cross(rot.col(1));

		Eigen::Affine3d pose;
		pose = Eigen::Translation3d(planeNormal * planeDist) * rot;

		tf::poseEigenToMsg(pose, m_limitMarkers.markers[LIMIT_TABLE].pose);
	}

	ROS_INFO_STREAM("eef: " << eef.transpose());

	double violation = -planeNormal.dot(eef) + planeDist;

	double colCost = 0;
	Eigen::VectorXd cost_gradient(m_joints.size());
	cost_gradient.setZero();

	ROS_INFO("violation: %f", violation);

	if(violation > 0.0)
	{
		colCost += m_param_eef_s() * pow(violation, 2);
		cost_gradient += -(2.0 * m_param_eef_s() * violation) * (planeNormal.transpose() * Jwrist.bottomRows<3>());
	}

	*cost += colCost;
	*costGradient += cost_gradient;
}

bool APCKinematics::searchPositionIK(const geometry_msgs::Pose &ik_pose,
	const std::vector<double> &ik_seed_state,
	double timeout,
	const std::vector<double> &consistency_limits,
	std::vector<double> &solution,
	moveit_msgs::MoveItErrorCodes &error_code,
	const kinematics::KinematicsQueryOptions &options) const
{
	ROS_DEBUG("IK start");
	ros::Time startTime = ros::Time::now();

	rbdl_parser::URDF_RBDL_Model model = m_rbdl;

	const double convergence_threshold_posdiff = 1e-6;
	const double convergence_threshold_anglediff = 1e-6;
	const double convergence_threshold_costdiff = 1e-6;
	const double alpha = m_param_alpha();
	const int max_iterations = m_param_max_iterations();
	bool nullspace_optimization = m_nullspaceOptimization && m_param_nullspace_optimization();
	float max_posdiff = m_param_max_posdiff();
// 	float angle_weight_slope = m_param_angle_weight_slope();

	int numJoints = m_joints.size();

	Eigen::VectorXd startJointAngles = Eigen::VectorXd::Zero(numJoints);
	for(unsigned int i = 0; i < ik_seed_state.size(); ++i)
		startJointAngles[i] = ik_seed_state[i];

	Eigen::VectorXd currentJointAngles = startJointAngles;
	Eigen::Affine3d currentEndEffectorPose;

	Eigen::Affine3d targetEndEffectorPose;
	{
		Eigen::Affine3d pose;
		tf::poseMsgToEigen(ik_pose, targetEndEffectorPose);
	}

	Eigen::VectorXd poseDiff;
	double posdiff;
	double anglediff;
	double lastCost = 0;
	double cost = 0;
	Eigen::VectorXd costGradient = Eigen::VectorXd::Zero(numJoints);
	double costdiff;

	int iteration = 0;
	bool singular = false;

	bool haveEEF = m_param_eef_enabled()
			&& m_ik_limit_link < std::numeric_limits<unsigned int>::max()
			&& m_worldLink < std::numeric_limits<unsigned int>::max();

	while(iteration < max_iterations)
	{
		// Copy the current joint angles into rbdl convention and update the model
		// start at the current measured joint angles for the joints that we
		// do not control in this solver.
		Eigen::VectorXd tmpJoints = m_rbdlMeasuredJointAngles;
		for(unsigned int i = 0; i < m_joints.size(); ++i)
			tmpJoints[m_joints[i]-1] = currentJointAngles[i];
		RigidBodyDynamics::UpdateKinematicsCustom(model, &tmpJoints, 0, 0);

		// Calculate the current end effector pose...
		const auto& X = model.X_base[m_tip];
		currentEndEffectorPose.setIdentity();
		currentEndEffectorPose.translate(X.r + X.E.transpose() * m_fixedTrans.r);
		currentEndEffectorPose.rotate(X.E.transpose() * m_fixedTrans.E.transpose());

		poseDiff = calcPoseDiff(currentEndEffectorPose, targetEndEffectorPose, m_ignoreRoll);

		posdiff = poseDiff.tail<3>().norm();
		anglediff = poseDiff.head<3>().norm();

		// Weighting matrix for poseDiff
		Eigen::MatrixXd W = Eigen::MatrixXd::Identity(6, 6);

		if(posdiff > max_posdiff)
		{
			// Limit the maximal translational posediff
			poseDiff.tail<3>() *= (max_posdiff/posdiff);

// 			double angle_weight =  std::max(1 + angle_weight_slope * (posdiff-max_posdiff), 0.0);
//
// 			W(0,0) = angle_weight;
// 			W(1,1) = angle_weight;
// 			W(2,2) = angle_weight;
		}

		poseDiff = W * poseDiff;

		cost = 0;
		costGradient.setZero();
		bool nullSpaceActive = false;

		if(haveEEF)
		{
			wristCostFunction(model, currentJointAngles, &cost, &costGradient);
			tableCostFunction(model, currentJointAngles, &cost, &costGradient);

			nullSpaceActive = true;
		}

		if(nullspace_optimization)
		{
			// Calculate the costs for the current state
			for(unsigned int i = 0; i < m_joints.size(); ++i)
				costFunction(i, currentJointAngles[i], startJointAngles[i], &cost, &costGradient[i]);

			nullSpaceActive = true;
		}

		costdiff = std::abs(lastCost - cost);

		if( posdiff < convergence_threshold_posdiff
		 && anglediff < convergence_threshold_anglediff
		 && costdiff < convergence_threshold_costdiff)
		{
			break;
		}

		lastCost = cost;

		Eigen::MatrixXd J(6, numJoints);
		_CalcPointRotJacobian(model, currentJointAngles, m_joints, m_links.front(), m_base, J);

		if(m_ignoreRoll)
		{
			// remove the local roll from the jacobian to allow null space optimization to use it

			// convert rotation part into local frame
			J.topRows<3>() = targetEndEffectorPose.rotation().transpose() * J;

			// zero roll
			J.row(0).setZero();

			// convert back into global frame
			J.topRows<3>() = targetEndEffectorPose.rotation() * J;
		}

		J = W*J;

		Eigen::MatrixXd modifiedJ(6, numJoints);
		Eigen::MatrixXd invModifiedJ(numJoints, 6);

		sdls(J, poseDiff, modifiedJ, invModifiedJ);

		if(invModifiedJ.maxCoeff() > 1e4 || invModifiedJ.minCoeff() < -1e4)
		{
			ROS_WARN("invModifiedJ nearly singular");
			iteration = max_iterations;
			singular = true;
			break;
		}

		if(modifiedJ.maxCoeff() > 1e4 || modifiedJ.minCoeff() < -1e4)
		{
			ROS_WARN("modifiedJ nearly singular");
			iteration = max_iterations;
			singular = true;
			break;
		}

 		Eigen::VectorXd thetaDiff = invModifiedJ * poseDiff;

		if(nullSpaceActive)
		{
			Eigen::MatrixXd nullspace = Eigen::MatrixXd::Identity(numJoints, numJoints);

			// Since the inverse of the jacobian is computed with some damping, the correct nullspace needs also to be computed with a similarly modified jacobian
			nullspace = nullspace - invModifiedJ * modifiedJ;

			//decrease the step size in each iteration
			double stepsize = alpha * pow(m_param_alpha_decay(), iteration);

			thetaDiff += stepsize * nullspace * costGradient;
		}

		currentJointAngles -= thetaDiff;

		iteration++;

		pruneToLimits(currentJointAngles);
	}

	if(singular) // This should never happen
	{
		ROS_INFO("No solution found :-(");
		return false;
	}

	solution.resize(m_joints.size());

	// This should never happen
	for(unsigned int i = 0; i < m_joints.size(); ++i)
	{
		if(!std::isfinite(currentJointAngles[i]))
		{
			ROS_ERROR("NaN in kinematics on joint %d, reporting failure...", i);
			return false;
		}
		solution[i] = currentJointAngles[i];
	}

 	ROS_DEBUG_STREAM("IK end (" << iteration << " iter)."/* Solutions: " << currentJointAngles.transpose()*/);

	ROS_DEBUG_STREAM("IK end, posdiff: "<<posdiff<<" , anglediff: "<<anglediff);

	ros::Time endTime = ros::Time::now();

	ros::Duration ik_time = endTime - startTime;
	ROS_DEBUG_STREAM("IK end, ik_time in millis: "<<(ik_time.toSec()*1000.0));

	m_pub_limitMarkers.publish(m_limitMarkers);

	//TODO limit the maximal amount the joint angles are allowed to differ from the start angles

	return true;
}

void APCKinematics::pruneToLimits(Eigen::VectorXd &state) const
{
	for(unsigned int i = 0; i< m_joints.size(); ++i)
	{
		if(state[i] > m_upperLimit[i])
		{
			state[i] = m_upperLimit[i];
		}
		else if(state[i] < m_lowerLimit[i])
		{
			state[i] = m_lowerLimit[i];
		}
	}
}

bool APCKinematics::isStateValid(std::vector<double> &state) const
{
	bool result = true;
	for(unsigned int i = 0; i< m_joints.size(); ++i)
	{
		if(state[i] > m_upperLimit[i] || state[i] < m_lowerLimit[i])
		{
			result = false;
			break;
		}
	}

	return result;
}

bool APCKinematics::searchPositionIK(const geometry_msgs::Pose &ik_pose,
	const std::vector<double> &ik_seed_state,
	double timeout,
	std::vector<double> &solution,
	const IKCallbackFn &solution_callback,
	moveit_msgs::MoveItErrorCodes &error_code,
	const kinematics::KinematicsQueryOptions &options) const
{
	ROS_INFO("4");
	return false;
}

bool APCKinematics::searchPositionIK(const geometry_msgs::Pose &ik_pose,
	const std::vector<double> &ik_seed_state,
	double timeout,
	const std::vector<double> &consistency_limits,
	std::vector<double> &solution,
	const IKCallbackFn &solution_callback,
	moveit_msgs::MoveItErrorCodes &error_code,
	const kinematics::KinematicsQueryOptions &options) const
{
	ROS_INFO("5");
	return false;
}

bool APCKinematics::getPositionFK(const std::vector<std::string> &link_names,
	const std::vector<double> &joint_angles,
	std::vector<geometry_msgs::Pose> &poses) const
{
	rbdl_parser::URDF_RBDL_Model model = m_rbdl;

	if(joint_angles.size() != m_joints.size())
	{
		ROS_ERROR("Got invalid joint_angles");
		return false;
	}

	Eigen::VectorXd q(m_rbdl.dof_count);

	for(unsigned int i = 0; i < m_joints.size(); ++i)
	{
		q[m_joints[i]-1] = joint_angles[i];
	}

	RigidBodyDynamics::UpdateKinematicsCustom(model, &q, 0, 0);

	poses.resize(link_names.size());
	for(unsigned int i = 0; i < link_names.size(); ++i)
	{
		unsigned int id = model.GetBodyId(link_names[i].c_str());
		if(id == (unsigned int)-1)
		{
			ROS_ERROR("Invalid link '%s' requested for forward kinematics",
				link_names[i].c_str()
			);
			return false;
		}

		RigidBodyDynamics::Math::SpatialTransform X = model.X_base[id];

		Eigen::Affine3d pose;
		pose.translate(X.r);
		pose.rotate(X.E.transpose());

		tf::poseEigenToMsg(pose, poses[i]);
	}

	return true;
}

void APCKinematics::handleJointStates(const sensor_msgs::JointStateConstPtr& msg)
{
	if(msg->name.size() != msg->position.size())
		return;

	for(size_t i = 0; i < msg->name.size(); ++i)
	{
		int index = m_rbdl.findJointIndex(msg->name[i]);
		if(index < 0)
			continue;

		m_rbdlMeasuredJointAngles[index-1] = msg->position[i];
	}
}

}

PLUGINLIB_EXPORT_CLASS(apc_kinematics::APCKinematics, kinematics::KinematicsBase)


#ifndef PANACEA_PRIVATE_MATRIXEIGEN_H
#define PANACEA_PRIVATE_MATRIXEIGEN_H
#pragma once

// Local PANACEA includes
#include "matrix/matrix.hpp"

// Third party includes
#include <Eigen/Dense>

// Standard includes
#include <memory>

namespace panacea {

class MatrixEigen : public Matrix {
private:
  std::unique_ptr<Eigen::MatrixXd> matrix_;

public:
  MatrixEigen();
  virtual ~MatrixEigen() final{};
  virtual const MatrixType type() const final;
  virtual MatrixEigen &operator=(const MatrixEigen &mat) final;
  virtual MatrixEigen &operator=(const Matrix &mat) final;
  virtual double &operator()(const int row, const int col) final;
  virtual double operator()(const int row, const int col) const final;

  virtual double getDeterminant() const final;

  virtual bool isZero(const double threshold) const noexcept final;

  virtual void resize(const int rows, const int cols) final;

  virtual void makeIdentity() final;
  virtual void setZero() final;

  virtual int rows() const final;
  virtual int cols() const final;

  virtual void print() const final;

  // Local method should not be part of the interface
  Eigen::MatrixXd pseudoInverse() const;
};

void pseudoInverse(Matrix &return_mat, const MatrixEigen &mat);

} // namespace panacea

#endif // PANACEA_PRIVATE_MATRIXEIGEN_H


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

#ifndef CAMERA_PROJECTION_MATRICES_HPP
#define CAMERA_PROJECTION_MATRICES_HPP

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

// http://www.songho.ca/opengl/gl_projectionmatrix.html

namespace camera
{

template<typename T>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<T,4,4> getOrtographicProjection(T l, T r, T b, T t, T n, T f)
{
    Eigen::Matrix<T,4,4> m;

    m << T(2.0)/(r-l) , T(0.0)       , T(0.0)        , -(r+l)/(r-l),
         T(0.0)       , T(2.0)/(t-b) , T(0.0)        , -(t+b)/(t-b),
         T(0.0)       , T(0.0)       , T(-2.0)/(f-n) , -(f+n)/(f-n),
         T(0.0)       , T(0.0)       , T(0.0)        , T(1.0);

    return m;
}

// Camera Axis:
//   X - Right, Y - Up, Z - Back
// Image Origin:
//   Bottom Left
// Caution: Principal point defined with respect to image origin (0,0) at
//          top left of top-left pixel (not center, and in different frame
//          of reference to projection function image)
template<typename CameraModel>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<typename CameraModel::Scalar,4,4> getPerspectiveProjectionRUBBottomLeft(const CameraModel& cam, typename CameraModel::Scalar N, typename CameraModel::Scalar F)
{
    typedef typename CameraModel::Scalar Scalar;

    const Scalar L = +(cam.u0()) * N / -cam.fx();
    const Scalar T = +(cam.v0()) * N / cam.fy();
    const Scalar R = -(cam.width() - cam.u0()) * N / -cam.fx();
    const Scalar B = -(cam.height() - cam.v0()) * N / cam.fy();

    Eigen::Matrix<typename CameraModel::Scalar,4,4> ret;

    ret <<
    Scalar(2.0)*N/(R-L) , Scalar(0.0)         , (R+L)/(R-L) , Scalar(0.0),
    Scalar(0.0)         , Scalar(2.0)*N/(T-B) , (T+B)/(T-B) , Scalar(0.0),
    Scalar(0.0)         , Scalar(0.0)         ,-(F+N)/(F-N) ,-(Scalar(2.0)*F*N)/(F-N),
    Scalar(0.0)         , Scalar(0.0)         ,-Scalar(1.0) , Scalar(0.0);

    return ret;
}

// Camera Axis:
//   X - Right, Y - Down, Z - Forward
// Image Origin:
//   Top Left
// Pricipal point specified with image origin (0,0) at top left of top-left pixel (not center)
template<typename CameraModel>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<typename CameraModel::Scalar,4,4> getPerspectiveProjectionRDFTopLeft(const CameraModel& cam, typename CameraModel::Scalar N, typename CameraModel::Scalar F)
{
    typedef typename CameraModel::Scalar Scalar;

    const Scalar L = -(cam.u0()) * N / cam.fx();
    const Scalar T = -(cam.v0()) * N / cam.fy();
    const Scalar R = +(cam.width() - cam.u0()) * N / cam.fx();
    const Scalar B = +(cam.height() - cam.v0()) * N / cam.fy();

    Eigen::Matrix<typename CameraModel::Scalar,4,4> ret;

    ret <<
    Scalar(2.0)*N/(R-L) , Scalar(0.0)         , (R+L)/(L-R) , Scalar(0.0),
    Scalar(0.0)         , Scalar(2.0)*N/(T-B) , (T+B)/(B-T) , Scalar(0.0),
    Scalar(0.0)         , Scalar(0.0)         , (F+N)/(F-N) ,(Scalar(2.0)*F*N)/(N-F),
    Scalar(0.0)         , Scalar(0.0)         , Scalar(1.0) , Scalar(0.0);

    return ret;
}

// Camera Axis:
//   X - Right, Y - Down, Z - Forward
// Image Origin:
//   Bottom Left
// Pricipal point specified with image origin (0,0) at top left of top-left pixel (not center)
template<typename CameraModel>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<typename CameraModel::Scalar,4,4> getPerspectiveProjectionRDFBottomLeft(const CameraModel& cam, typename CameraModel::Scalar N, typename CameraModel::Scalar F)
{
    typedef typename CameraModel::Scalar Scalar;

    const Scalar L = -(cam.u0()) * N / cam.fx();
    const Scalar T = +(cam.height() - cam.v0()) * N / cam.fy();
    const Scalar R = +(cam.width() - cam.u0()) * N / cam.fx();
    const Scalar B = -(cam.v0()) * N / cam.fy();

    Eigen::Matrix<typename CameraModel::Scalar,4,4> ret;

    ret <<
    Scalar(2.0)*N/(R-L) , Scalar(0.0)         , (R+L)/(L-R) , Scalar(0.0),
    Scalar(0.0)         , Scalar(2.0)*N/(T-B) , (T+B)/(B-T) , Scalar(0.0),
    Scalar(0.0)         , Scalar(0.0)         , (F+N)/(F-N) ,(Scalar(2.0)*F*N)/(N-F),
    Scalar(0.0)         , Scalar(0.0)         , Scalar(1.0) , Scalar(0.0);

    return ret;
}

template<typename CameraModel>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<typename CameraModel::Scalar,4,4> getPerspectiveProjection(const CameraModel& cam, typename CameraModel::Scalar N, typename CameraModel::Scalar F)
{
    return getPerspectiveProjectionRDFTopLeft(cam,N,F);
}

// NOTE element (2,3) may be wrong

template<typename T>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<T,4,4> lookAtRUB(const Eigen::Matrix<T,3,1>& eye, const Eigen::Matrix<T,3,1>& center, const Eigen::Matrix<T,3,1>& up)
{
    Eigen::Matrix<T,4,4> view_matrix = Eigen::Matrix<T,4,4>::Zero();
    Eigen::Matrix<T,3,3> R;
    R.col(2) = (eye-center).normalized();
    R.col(0) = up.cross(R.col(2)).normalized();
    R.col(1) = R.col(2).cross(R.col(0));
    view_matrix.template topLeftCorner<3,3>() = R.transpose();
    view_matrix.template topRightCorner<3,1>() = -R.transpose() * center;
    view_matrix.row(3) << T(0.0), T(0.0), T(0.0), T(1.0);
    return view_matrix;
}

template<typename T>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<T,4,4> lookAtRDF(const Eigen::Matrix<T,3,1>& eye, const Eigen::Matrix<T,3,1>& center, const Eigen::Matrix<T,3,1>& up)
{
    Eigen::Matrix<T,4,4> view_matrix = Eigen::Matrix<T,4,4>::Zero();
    Eigen::Matrix<T,3,3> R;
    R.col(2) = (center-eye).normalized();
    R.col(0) = R.col(2).cross(up).normalized();
    R.col(1) = R.col(2).cross(R.col(0));
    view_matrix.template topLeftCorner<3,3>() = R.transpose();
    view_matrix.template topRightCorner<3,1>() = -R.transpose() * center;
    view_matrix.row(3) << T(0.0), T(0.0), T(0.0), T(1.0);
    return view_matrix;
}

template<typename T>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<T,4,4> lookAt(const Eigen::Matrix<T,3,1>& eye, const Eigen::Matrix<T,3,1>& center, const Eigen::Matrix<T,3,1>& up)
{
    return lookAtRDF(eye, center, up);
}

}

#endif // CAMERA_PROJECTION_MATRICES_HPP


/**
 * @file Mesh.hpp
 * @brief
 */
#ifndef MI4_MESH_HPP
#define MI4_MESH_HPP 1
#include <iostream>
#include <vector>
#include <utility>
#include <deque>
#include <map>
#include <cmath>
#include <Eigen/Dense>
namespace mi4
{
        class Mesh
        {
        private:
                std::string _name;
                std::vector<Eigen::Vector3d> _vertex; /// vertex position
                std::vector<size_t> _index; /// triangles
        public:
                void clone ( const Mesh& mesh )
                {
                        this->init();

                        for ( int i = 0 ; i < mesh.getNumVertices() ; ++i ) {
                                this->addPoint ( mesh.getPosition ( i ) );
                        }

                        for ( int i = 0 ; i < mesh.getNumFaces() ; ++i ) {

                                this->addFace ( mesh.getFaceIndices ( i ) );
                        }
                }

                size_t addPoint ( const Eigen::Vector3d& p ) const
                {
                        const_cast<Mesh*> ( this )->_vertex.push_back ( p );
                        return this->_vertex.size() - 1;
                }

                size_t addFace ( const std::vector<size_t>& fidx )
                {
                        if ( fidx.size() != 3 ) {
                                std::cerr << "only triangle is supported." << std::endl;
                                return 0;
                        }

                        this->_index.insert ( this->_index.end(), fidx.begin(), fidx.end() );
                        return this->_index.size() / 3 ; // ID
                }

                void addName ( const std::string name = std::string ( "mesh" ) )
                {
                        this->_name = name;
                        return;
                }

                inline bool isValidFaceId ( const size_t faceid ) const
                {
                        return ( faceid < this->getNumFaces() );
                }

                inline bool isValidVertexId ( const size_t vertexid ) const
                {
                        return ( vertexid < this->getNumVertices() );
                }

                inline std::vector<size_t> getFaceIndices ( const size_t faceid ) const
                {
                        std::vector<size_t> idx;

                        if ( this->isValidFaceId ( faceid ) ) {
                                for ( int i = 0 ; i < 3 ; i++ ) {
                                        idx.push_back ( this->_index.at ( faceid * 3 + i ) );
                                }
                        }

                        return idx;
                }

                inline Eigen::Vector3d getPosition ( const size_t vertexid ) const
                {
                        if ( this->isValidVertexId ( vertexid ) ) {
                                return this->_vertex.at ( vertexid );
                        }

                        return Eigen::Vector3d();
                }

                Eigen::Vector3d getNormal ( const size_t faceid, bool normalize = true ) const
                {
                        if ( ! this->isValidFaceId ( faceid ) ) {
                                return Eigen::Vector3d();
                        }

                        auto fidx = this->getFaceIndices ( faceid );
                        Eigen::Vector3d v0 = this->getPosition ( fidx[0] );
                        Eigen::Vector3d v1 = this->getPosition ( fidx[1] ) - v0;
                        Eigen::Vector3d v2 = this->getPosition ( fidx[2] ) - v0;
                        Eigen::Vector3d n = v1.cross ( v2 );

                        if ( normalize ) {
                                n.normalize();
                        }

                        return n;
                }

                inline std::string
                getName ( void ) const
                {
                        return this->_name;
                }

                void setPosition ( const size_t vertexid, const Eigen::Vector3d& pos )
                {
                        if ( this->isValidVertexId ( vertexid ) ) {
                                this->_vertex.at ( vertexid ) = pos;
                        }

                        return;
                };
                void init ( void )
                {
                        this->_vertex.clear();
                        this->_index.clear();
                        return;
                }

                inline size_t getNumVertices ( void ) const
                {
                        return this->_vertex.size();
                }

                inline size_t getNumFaces ( void ) const
                {
                        return this->_index.size() / 3;
                }

                void
                getBoundingBox ( Eigen::Vector3d& bmin, Eigen::Vector3d& bmax )
                {
                        Eigen::AlignedBox3d bbox;
                        const size_t numv = this->getNumVertices();

                        for ( size_t i = 1 ; i < numv ; ++i ) {
                                bbox.extend ( this->getPosition ( i ) );
                        }

                        bmin = bbox.min();
                        bmax = bbox.max();
                        return ;
                }

                void negateOrientation()
                {
                        for ( size_t i = 0 ; i < this->getNumFaces() ; ++i ) {
                                size_t f0 = this->_index [ i * 3 + 0 ];
                                this->_index[ i * 3 + 0 ] = this->_index [ i * 3 + 1];
                                this->_index[ i * 3 + 1 ] = f0;

                        }

                }

                double getArea ( const int faceId ) const
                {
                        std::vector<size_t> index = this->getFaceIndices ( faceId );
                        const Eigen::Vector3d p0 = this->getPosition ( index[0] );
                        const Eigen::Vector3d p1 = this->getPosition ( index[1] );
                        const Eigen::Vector3d p2 = this->getPosition ( index[2] );
                        const Eigen::Vector3d v0 = p1 - p0;
                        const Eigen::Vector3d v1 = p2 - p0;
                        return v0.cross ( v1 ).norm() * 0.5; //|v0^v1|/2
                }
        };
}//namespace mi4
#endif