// ========================================================================== //
//                                                                            //
// Copyright (c) 2014-2020 The University of Texas at Austin.                 //
// 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.           //
// A copy of the License is included with this software in the file LICENSE.  //
// If your copy does not contain the License, you may obtain a copy of the    //
// License at:                                                                //
//                                                                            //
//     https://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.                                             //
//                                                                            //
// ========================================================================== //

#include <iostream>

#include <unistd.h>
#include <string.h>
#include <sys/types.h>
// #include <cstdlib>
#include <math.h>

#include <dtypes.h>

#include "Datasets.h"
#include "MHSampleClientServer.h"

#include <time.h>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

using namespace boost;
using namespace gxy;
using namespace std;

namespace gxy
{

// Two types of functions map volume samples to a probability distribution: a
// linear transformation that maps linearly with 0 at some data value and 1 at some
// other data value, with 0 everywhere else, and a gaussian transformation with a
// given mean and standard deviation.   Default is linear from the data value min
// to the data value max

#define TF_NONE     0
#define TF_LINEAR   1
#define TF_GAUSSIAN 2

MHSampleClientServer::MHSampleClientServer(SocketHandler *sh) : MultiServerHandler(sh)
{
  args.radius       = 0.02;
  args.tf_type      = TF_NONE;
  args.sigma        = 1.0;
  args.n_iterations = 20000;
  args.n_startup    = 1000;
  args.n_skip       = 10;
  args.n_miss       = 10;
  args.r = args.g = args.b = 0.8; args.a = 1.0;

  volume = NULL;
  particles = NULL;
};

static variate_generator<mt19937, normal_distribution<> > *generator;

static void normally_distributed_vector(double* v)
{
  for (size_t i = 0; i < 3; ++i)
    v[i]=(*generator)();
}

static float gaussian(float x, float m, float s)
{
  return ( 1 / ( s * sqrt(2*M_PI) ) ) * exp( -0.5 * pow( (x-m)/s, 2.0 ) );
}

#define RNDM ((float)rand() / RAND_MAX)
static vec3f get_starting_point(VolumeP v)
{
  Box *box = v->get_local_box();
  return vec3f(box->xyz_min.x + RNDM*(box->xyz_max.x - box->xyz_min.x),
               box->xyz_min.y + RNDM*(box->xyz_max.y - box->xyz_min.y),
               box->xyz_min.z + RNDM*(box->xyz_max.z - box->xyz_min.z));
}

static float Q(VolumeP v, float s, MHSampleClientServer::Args *a)
{
  float q;

  if (a->tf_type == TF_LINEAR || a->tf_type == TF_NONE)
  {
    if (a->tf0 < a->tf1)
    {
      if (s < a->tf0) q = 0.0;
      else if (s > a->tf1) q = 1.0;
      else q = (s - a->tf0) / (a->tf1 - a->tf0);
    }
    else
    {
      if (s < a->tf1) q = 1.0;
      else if (s > a->tf0) q = 0.0;
      else q = (s - a->tf0) / (a->tf1 - a->tf0);
    }
  }
  else
  {
    q = gaussian(s, a->tf0, a->tf1);
  }

  return q;
}

static float sample(MHSampleClientServer::Args *args, VolumeP v, float x, float y, float z)
{
  float dx, dy, dz;
  v->get_deltas(dx, dy, dz);

  float ox, oy, oz;
  v->get_ghosted_local_origin(ox, oy, oz);

  x = (x - ox) / dx;
  y = (y - oy) / dy;
  z = (z - oz) / dz;

  int ix = (int)x;
  int iy = (int)y;
  int iz = (int)z;

  dx = x - ix;
  dy = y - iy;
  dz = z - iz;

  int v000 = (ix + 0) * args->istep + (iy + 0) * args->jstep + (iz + 0) * args->kstep;
  int v001 = (ix + 0) * args->istep + (iy + 0) * args->jstep + (iz + 1) * args->kstep;
  int v010 = (ix + 0) * args->istep + (iy + 1) * args->jstep + (iz + 0) * args->kstep;
  int v011 = (ix + 0) * args->istep + (iy + 1) * args->jstep + (iz + 1) * args->kstep;
  int v100 = (ix + 1) * args->istep + (iy + 0) * args->jstep + (iz + 0) * args->kstep;
  int v101 = (ix + 1) * args->istep + (iy + 0) * args->jstep + (iz + 1) * args->kstep;
  int v110 = (ix + 1) * args->istep + (iy + 1) * args->jstep + (iz + 0) * args->kstep;
  int v111 = (ix + 1) * args->istep + (iy + 1) * args->jstep + (iz + 1) * args->kstep;

  float b000 = (1.0 - dx) * (1.0 - dy) * (1.0 - dz);
  float b001 = (1.0 - dx) * (1.0 - dy) * (dz);
  float b010 = (1.0 - dx) * (dy) * (1.0 - dz);
  float b011 = (1.0 - dx) * (dy) * (dz);
  float b100 = (dx) * (1.0 - dy) * (1.0 - dz);
  float b101 = (dx) * (1.0 - dy) * (dz);
  float b110 = (dx) * (dy) * (1.0 - dz);
  float b111 = (dx) * (dy) * (dz);

  if (v->get_type() == Volume::FLOAT)
  {
    float *p = (float *)v->get_samples();
    return p[v000]*b000 + p[v001]*b001 + p[v010]*b010 + p[v011]*b011 +
           p[v100]*b100 + p[v101]*b101 + p[v110]*b110 + p[v111]*b111;
  }
  else
  {
    unsigned char *p = (unsigned char *)v->get_samples();
    return p[v000]*b000 + p[v001]*b001 + p[v010]*b010 + p[v011]*b011 +
           p[v100]*b100 + p[v101]*b101 + p[v110]*b110 + p[v111]*b111;
  }
}

float sample(MHSampleClientServer::Args *args, VolumeP v, Particle& p) { return sample(args, v, p.xyz.x, p.xyz.y, p.xyz.z); }
float sample(MHSampleClientServer::Args *args, VolumeP v, vec3f xyz) { return sample(args, v, xyz.x, xyz.y, xyz.z); }

static void
Metropolis_Hastings(MHSampleClientServer::Args *a)
{
  generator = new variate_generator<mt19937, normal_distribution<> >(mt19937(time(0)), normal_distribution<>(0.0, a->sigma));

  VolumeP v = Volume::Cast(KeyedDataObject::GetByKey(a->vk));
  ParticlesP p = Particles::Cast(KeyedDataObject::GetByKey(a->pk));

  p->clear();

  p->CopyPartitioning(v);

  p->SetDefaultColor(a->r, a->g, a->b, a->a);

  float deltaX, deltaY, deltaZ;
  v->get_deltas(deltaX, deltaY, deltaZ);

  float ox, oy, oz;
  v->get_local_origin(ox, oy, oz);

  int lioff, ljoff, lkoff;
  v->get_ghosted_local_offsets(lioff, ljoff, lkoff);

  int nli, nlj, nlk;
  v->get_local_counts(nli, nlj, nlk);

  int gnli, gnlj, gnlk;
  v->get_ghosted_local_counts(gnli, gnlj, gnlk);

  int local_count = nli*nlj*nlk;

  float max_x = ox + (nli-1) * deltaX,
        max_y = oy + (nlj-1) * deltaY,
        max_z = oz + (nlk-1) * deltaZ;

  int ngx, ngy, ngz;
  v->get_local_counts(ngx, ngy, ngz);
  int global_count = ngx*ngy*ngz;

  a->istep = 1;
  a->jstep = gnli;
  a->kstep = gnli * gnlj;

  Particle tp;
  tp.xyz = get_starting_point(v);
  tp.u.value = sample(a, v, tp.xyz);

  float tq = Q(v, tp.u.value, a);

  Box *partition_box = v->get_local_box();

  int miss_count = 0;
  for (int iteration = 0; iteration < (a->n_startup + a->n_iterations); )
  {
    double rv[3];
    normally_distributed_vector(rv);

    Particle cp(tp.xyz.x + rv[0], tp.xyz.y + rv[1], tp.xyz.z + rv[2], float(0.0));
    if (! partition_box->isIn(cp.xyz)) continue;

    iteration ++;

    cp.u.value = sample(a, v, cp);
    float cq = Q(v, cp.u.value, a);

    if ((cq > tq) || (RNDM < (cq/tq)))
    {
      tp = cp;
      tq = cq;
      if ((iteration > a->n_startup) && ((iteration % a->n_skip) == 0))
        p->push_back(tp);
      miss_count = 0;
    }
    else if (++miss_count > a->n_miss)
    {
      tp.xyz = get_starting_point(v);
      tp.u.value = sample(a, v, tp.xyz);
      miss_count = 0;
    }
  }

  std::cerr << "created " << p->GetNumberOfVertices() << " samples\n";
}

class MHSampleMsg : public Work
{
public:
  MHSampleMsg(MHSampleClientServer* a) : MHSampleMsg(sizeof(MHSampleClientServer::Args))
  {
    a->args.vk = a->volume->getkey();
    a->args.pk = a->particles->getkey();
    *(MHSampleClientServer::Args *)contents->get() = a->args;
  }

  WORK_CLASS(MHSampleMsg, true)

  bool Action(int s)
  {
    Metropolis_Hastings((MHSampleClientServer::Args *)get());
    return false;
  }
};

WORK_CLASS_TYPE(MHSampleMsg)

extern "C" void
init()
{
  srand(GetTheApplication()->GetRank());
  MHSampleMsg::Register();
}

extern "C" MultiServerHandler *
new_handler(SocketHandler *sh)
{
  return new MHSampleClientServer(sh);
}

bool
MHSampleClientServer::handle(std::string line, std::string& reply)
{
  DatasetsP theDatasets = Datasets::Cast(MultiServer::Get()->GetGlobal("global datasets"));
  if (! theDatasets)
  {
    theDatasets = Datasets::NewP();
    MultiServer::Get()->SetGlobal("global datasets", theDatasets);
  }

  stringstream ss(line);

  string cmd;
  ss >> cmd;

  if (cmd == "sigma")
  {
    ss >> args.sigma;
    if (ss.fail())
    {
      reply = "error MHSampler sigma command requires a float argument";
      return true;
    }

    reply = "ok";
    return true;
  }
  else if (cmd == "radius")
  {
    ss >> args.radius;
    if (ss.fail())
    {
      reply = "error MHSampler radius command requires a float argument";
      return true;
    }
    reply = "ok";
    return true;
  }
  else if (cmd == "volume")
  {
    string  name;
    ss >> name;
    if (ss.fail())
    {
      reply = "error MHSampler volume command requires a string argument";
      return true;
    }

    volume = Volume::Cast(theDatasets->Find(name));
    if (! volume)
    {
      reply = "error MHSampler volume command must name a pre-existing volume";
      return true;
    }

    reply = "ok";
    return true;
  }
  else if (cmd == "particles")
  {
    string name;
    ss >> name;
    if (ss.fail())
    {
      reply = "error MHSampler particles command requires a string argument";
      return true;
    }

    particles = Particles::NewP();
    theDatasets->Insert(name, particles);

    reply = "ok";
    return true;
  }
  else if (cmd == "iterations")
  {
    ss >> args.n_iterations;
    if (ss.fail())
    {
      reply = "error MHSampler iterations command requires an integer argument";
      return true;
    }

    reply = "ok";
    return true;
  }
  else if (cmd == "startup")
  {
    ss >> args.n_startup;
    if (ss.fail())
    {
      reply = "error MHSampler startup command requires an integer argument";
      return true;
    }

    reply = "ok";
    return true;
  }
  else if (cmd == "skip")
  {
    ss >> args.n_skip;
    if (ss.fail())
    {
      reply = "error MHSampler skip command requires an integer argument";
      return true;
    }

    reply = "ok";
    return true;
  }
  else if (cmd == "miss")
  {
    ss >> args.n_miss;
    if (ss.fail())
    {
      reply = "error MHSampler miss command requires an integer argument";
      return true;
    }

    reply = "ok";
    return true;
  }
  else if (cmd == "color")
  {
    ss >> args.r >> args.g >> args.b >> args.a;
    if (ss.fail())
    {
      reply = "error MHSampler color command requires four float arguments";
      return true;
    }

    reply = "ok";
    return true;
  }
  else if (cmd == "tf-linear")
  {
    args.tf_type = TF_LINEAR;
    ss >> args.tf0 >> args.tf1;
    if (ss.fail())
    {
      reply = "error MHSampler tf-linear command requires two float arguments";
      return true;
    }

    reply = "ok";
    return true;
  }
  else if (cmd == "tf-gaussian")
  {
    args.tf_type = TF_GAUSSIAN;
    ss >> args.tf0 >> args.tf1;
    if (ss.fail())
    {
      reply = "error MHSampler tf-gaussian command requires two float arguments";
      return true;
    }

    reply = "ok";
    return true;
  }
  else if (cmd == "tf-none")
  {
    args.tf_type = TF_NONE;

    reply = "ok";
    return true;
  }
  else if (cmd == "sample" || cmd == "commit")
  {
    if (!volume)
    {
      reply = "error need a volume first";
      return true;
    }
    else if (!particles)
    {
      reply = "error need to set a particles dataset name first";
      return true;
    }
    else
    {
      args.vk = volume->getkey();
      args.pk = particles->getkey();

      if (args.tf_type == TF_NONE)
        volume->get_global_minmax(args.tf0, args.tf1);

      MHSampleMsg msg(this);
      msg.Broadcast(false, false);

      if (! particles->Commit())
      {
        reply = "error committing particles";
        return true;
      }
    }

    reply = "ok";
    return true;
  }
  else
    return false;
}

}


/** Copyright 2017-2020 CNRS-AIST JRL and CNRS-UM LIRMM */

#include <tvm/defs.h>

#include <tvm/robot/internal/FrictionCone.h>

#include <Eigen/Geometry>

namespace tvm
{

namespace robot
{

namespace internal
{

FrictionCone::FrictionCone(const Eigen::Matrix3d & frame, unsigned int nrGen, double mu, double dir) : generators(nrGen)
{
  Eigen::Vector3d normal = frame.row(2);
  Eigen::Vector3d tan = dir * frame.row(0);
  double angle = std::atan(mu);

  Eigen::Vector3d gen = Eigen::AngleAxisd(angle, tan) * normal;
  double step = tvm::constant::pi * 2. / nrGen;

  for(unsigned int i = 0; i < nrGen; ++i)
  {
    generators[i] = Eigen::AngleAxisd(dir * step * i, normal) * gen;
  }
}

} // namespace internal

} // namespace robot

} // namespace tvm


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

int main(int, char**)
{
    typedef mtl::dense2D<double> Matrix;

    Matrix A(3,4);
    A[0][0]=1;A[0][1]=2;A[0][2]=3;A[0][3]=4;
    A[1][0]=5;A[1][1]=6;A[1][2]=7;A[1][3]=8;
    A[2][0]=9;A[2][1]=10;A[2][2]=11;A[2][3]=12;

    std::cout << "First Matrix A is: \n" << A << "\n";

    Matrix B = mtl::clone(sub_matrix(A,0,1,0,1));
    std::cout << "Matrix B is: \n" << B << "\n";

    Matrix C = mtl::clone(sub_matrix(A,0,1,0,2));
    std::cout << "Matrix C is: \n" << C << "\n";

    Matrix D = mtl::clone(sub_matrix(A,0,1,0,3));
    std::cout << "Matrix D is: \n" << D << "\n";

    Matrix E = mtl::clone(sub_matrix(A,0,1,0,4));
    std::cout << "Matrix E is: \n" << E << "\n";

    Matrix f = mtl::clone(sub_matrix(A,0,2,0,1));
    std::cout << "Matrix f is: \n" << f << "\n";

    MTL_THROW_IF(f[1][0] != 5.0, mtl::runtime_error("Clown has wrong value."));
    f[1][0]= 6.0;
    std::cout << "Matrix f after f[1][0]= 6.0 is: \n" << f << "\n";
    std::cout << "Matrix A after f[1][0]= 6.0 is: \n" << A << "\n";

    MTL_THROW_IF(f[1][0] != 6.0, mtl::runtime_error("Clown has wrong value after change."));
    MTL_THROW_IF(A[1][0] != 5.0, mtl::runtime_error("Original matrix was changed."));

    Matrix g = mtl::clone(sub_matrix(A,0,3,0,1));
    std::cout << "Matrix g is: \n" << g << "\n";

    Matrix h = mtl::clone(sub_matrix(A,0,3,0,1));
    std::cout << "Matrix h is: \n" << h << "\n";

    Matrix i = mtl::clone(sub_matrix(A,0,2,0,4));
    std::cout << "Matrix i is: \n" << i << "\n";

    Matrix k = mtl::clone(sub_matrix(A,0,2,0,2));
    std::cout << "Matrix k is: \n" << k << "\n";

    Matrix l = mtl::clone(sub_matrix(A,1,3,0,2));
    std::cout << "Matrix l is: \n" << l << "\n";

    return 0;
}


//
// HOGpp - Fast histogram of oriented gradients computation using integral
// histograms
//
// Copyright 2022 Sergiu Deitsch <sergiu.deitsch@gmail.com>
//
// 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.
//

#define BOOST_TEST_MODULE hogpp

#include <hogpp/l1hys.hpp>
#include <hogpp/l1norm.hpp>
#include <hogpp/l1sqrt.hpp>
#include <hogpp/l2hys.hpp>
#include <hogpp/l2norm.hpp>

#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/list.hpp>
#include <boost/test/included/unit_test.hpp>

using Scalars = boost::mp11::mp_list<float, double, long double>;

// clang-format off
using Norms = boost::mp11::mp_list
<
      boost::mp11::mp_quote<hogpp::L1Hys>
    , boost::mp11::mp_quote<hogpp::L1Norm>
    , boost::mp11::mp_quote<hogpp::L1Sqrt>
    , boost::mp11::mp_quote<hogpp::L2Hys>
    , boost::mp11::mp_quote<hogpp::L2Norm>
>;
// clang-format on

using PrecisionNorms =
    boost::mp11::mp_product<boost::mp11::mp_invoke_q, Norms, Scalars>;

BOOST_TEST_DECORATOR(* boost::unit_test::label("zero"))
BOOST_AUTO_TEST_CASE_TEMPLATE(zero, Norm, PrecisionNorms)
{
    using Scalar = typename Norm::Scalar;

    Eigen::TensorFixedSize<Scalar, Eigen::Sizes<8, 8> > block;
    block.setZero();

    Norm{}(block);

    const Eigen::Tensor<Scalar, 0> s = block.sum();
    BOOST_TEST(s(0) == 0);
}

BOOST_TEST_DECORATOR(* boost::unit_test::label("negative_near_zero"))
BOOST_AUTO_TEST_CASE_TEMPLATE(negative_near_zero, Norm, PrecisionNorms)
{
    using Scalar = typename Norm::Scalar;

    Eigen::TensorFixedSize<Scalar, Eigen::Sizes<8, 8> > block;
    block.setConstant(-Eigen::NumTraits<Scalar>::dummy_precision());

    Norm{}(block);

    const Eigen::Tensor<Scalar, 0> s = block.abs().sum();
    BOOST_TEST(s(0) >= 0);
}

BOOST_TEST_DECORATOR(* boost::unit_test::label("positive_near_zero"))
BOOST_AUTO_TEST_CASE_TEMPLATE(positive_near_zero, Norm, PrecisionNorms)
{
    using Scalar = typename Norm::Scalar;

    Eigen::TensorFixedSize<Scalar, Eigen::Sizes<8, 8> > block;
    block.setConstant(+Eigen::NumTraits<Scalar>::dummy_precision());

    Norm{}(block);

    const Eigen::Tensor<Scalar, 0> s = block.abs().sum();
    BOOST_TEST(s(0) >= 0);
}


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

#include <hpx/hpx_init.hpp>
#include <hpx/include/actions.hpp>
#include <hpx/include/util.hpp>
#include <hpx/include/lcos.hpp>
#include <hpx/include/iostreams.hpp>

#include <vector>

#include <boost/format.hpp>

#include "EasyBMP/EasyBMP.h"
#include "EasyBMP/EasyBMP.cpp"

int const sizeY = 256;
int const sizeX = sizeY;

int fractals(float x0, float y0, int max_iteration);

// This is to generate the required boilerplate we need for the remote
// invocation to work.
HPX_PLAIN_ACTION(fractals, fractals_action);

///////////////////////////////////////////////////////////////////////////////
int fractals(float x0, float y0, int max_iteration)
{
    float x = 0, y = 0;
    int iteration = 0;
    while (x*x + y*y < 2*2 && iteration < max_iteration)
    {
        float xtemp = x*x - y*y + x0;
        y = 2*x*y + y0;
        x = xtemp;

        ++iteration;
    }
    return iteration;
}

///////////////////////////////////////////////////////////////////////////////
int hpx_main()
{
    BMP SetImage;
    SetImage.SetBitDepth(24);
    SetImage.SetSize(sizeX * 2,sizeY);
    hpx::util::high_resolution_timer t;

    {
        using namespace std;

        using hpx::future;
        using hpx::async;
        using hpx::wait_all;

        int const max_iteration = 255;

        vector<future<int> > iteration;
        iteration.reserve(sizeX*sizeY);

        hpx::cout << "Initial setup completed in "
                  << t.elapsed()
                  << "s. Initializing and running futures...\n";
        t.restart();

        hpx::id_type const here = hpx::find_here();
        fractals_action fractal_pixel;

        for (int i = 0; i < sizeX; i++)
        {
            for (int j = 0; j < sizeY; j++)
            {
                float x0 = (float)i * 3.5f / (float)sizeX - 2.5f;
                float y0 = (float)j * 2.0f / (float)sizeY - 1.0f;

                iteration.push_back(async(fractal_pixel, here, x0, y0, max_iteration));
            }
        }
        wait_all(iteration);

        hpx::cout << sizeX*sizeY << " calculations run in "
                  << t.elapsed()
                  << "s. Transferring from futures to general memory...\n";
        t.restart();

        for (int i = 0; i < sizeX; ++i)
        {
            for (int j = 0; j < sizeY; ++j)
            {
                int it = iteration[i*sizeX + j].get();
                for (int k = 0; k < 2; ++k)
                {
                    int p = (it * 255) / max_iteration;
                    SetImage.SetPixel(i * 2 + k, j, RGBApixel(p, p, p));
                }
            }
        }
    }

    hpx::cout << "Transfer process completed in "
              << t.elapsed() << "s. Writing to hard disk...\n";
    t.restart();

    SetImage.WriteToFile("out.bmp");

    hpx::cout << "Fractal image written to file \"out.bmp\" from memory in "
              << t.elapsed() << "s.\nInitializing shutdown process.\n";

    return hpx::finalize(); // Handles HPX shutdown
}

int main(int argc, char* argv[])
{
    return hpx::init(argc, argv);
}


//
// Created by yifu wang on 2021/8/27.
//

#include <fstream>

#include <Eigen/Eigen>
#include <opencv2/core/eigen.hpp>

#include <opengv2/frame/Bodyframe.hpp>
#include <opengv2/system/SystemBase.hpp>
#include <opengv2/viewer/PointLandmarkViewer.hpp>
#include <cv_calib.hpp>
#include <time_measurement.hpp>
#include <chrono>

using namespace opengv2;
using namespace std;

void writeTofile(const string &file, double time, Eigen::Quaterniond &unitQ, Eigen::Vector3d &t);

void calibrateHandEye(const std::vector<std::map<double, Eigen::Matrix<double, 7, 1>>> &multi_Tws,
                      const std::vector<std::map<double, Eigen::Matrix<double, 7, 1>>> &multi_Tw1b,
                      Eigen::Quaterniond &unitQsb, Eigen::Vector3d &tsb,
                      std::vector<Eigen::Quaterniond> &multi_unitQww1, std::vector<Eigen::Vector3d> &multi_tww1,
                      char **argv);

void HandEyeErrors(const std::vector<std::map<double, Eigen::Matrix<double, 7, 1>>> &multi_Tws,
                   const std::vector<std::map<double, Eigen::Matrix<double, 7, 1>>> &multi_Tw1b,
                   Eigen::Quaterniond &unitQsb, Eigen::Vector3d &tsb,
                   std::vector<Eigen::Quaterniond> &multi_unitQww1, std::vector<Eigen::Vector3d> &multi_tww1,
                   char **argv, double &R_error, double &t_error);

double generateRandomNum(double factor)
{
    double randomize = 1 + ((((double)rand()) / ((double)RAND_MAX)) - 0.5) * factor * 2;
    return randomize;
}

void writeTofile(const string &file, Eigen::Quaterniond &unitQ, Eigen::Vector3d &t, bool AddToEnd = false)
{
    if (AddToEnd)
    {
        std::ofstream f(file, std::ofstream::app);
        f << std::fixed << std::setprecision(10);
        //timestamp tx ty tz qx qy qz qw
        f << t[0] << " " << t[1] << " " << t[2] << " "
          << unitQ.x() << " " << unitQ.y() << " " << unitQ.z() << " " << unitQ.w() << std::endl;
        f.close();
    }
    else
    {
        std::ofstream f(file, std::ofstream::trunc);
        f << std::fixed << std::setprecision(10);
        //timestamp tx ty tz qx qy qz qw
        f << t[0] << " " << t[1] << " " << t[2] << " "
          << unitQ.x() << " " << unitQ.y() << " " << unitQ.z() << " " << unitQ.w() << std::endl;
        f.close();
    }
}

int main(int argc, char **argv)
{
    if (argc < 4)
    {
        cerr << endl
             << "Usage: ./handEyeCalib settingFilePath GTFilePath TrajectoryFile"
             << endl;
        return 1;
    }

    //Check settings file
    cv::FileStorage fsSettings(string(argv[1]), cv::FileStorage::READ);
    if (!fsSettings.isOpened())
    {
        cerr << "Failed to open settings file at: " << string(argv[1]) << endl;
        exit(-1);
    }

    // map part
    MapBase::Ptr map = make_shared<MapBase>();
    // viewer part
    PointLandmarkViewer::Ptr viewer = make_shared<PointLandmarkViewer>(string(argv[1]), map, nullptr);
    // Extrinsic parameters, to be calibrated
    std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond>> Qsb;
    std::vector<Eigen::Vector3d> tsb;
    Qsb.emplace_back(1, 0, 0, 0);
    tsb.emplace_back(0, 0, 0);

    SystemBase SLAM(nullptr, map, viewer, nullptr, Qsb, tsb, nullptr, nullptr, string(argv[1]));

    /* // add landmarks
    bool isAsymmetric = true;
    fsSettings["Is_Pattern_Asymmetric"] >> isAsymmetric;
    int rows = fsSettings["BoardSize_Rows"];
    int cols = fsSettings["BoardSize_Cols"];
    double squareSize = fsSettings["Square_Size"];
    if (isAsymmetric)
    {
        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++)
            {
                auto lm = std::make_shared<LandmarkBase>(j + i * cols, Eigen::Vector3d((2 * j + i % 2) * squareSize, i * squareSize, 0));
                SLAM.map->addLandmark(lm);
            }
    }
    else
    {
        for (int i = 0; i < rows; ++i)
            for (int j = 0; j < cols; ++j)
            {
                auto lm = std::make_shared<LandmarkBase>(j + i * cols, Eigen::Vector3d(j * squareSize, i * squareSize, 0));
                SLAM.map->addLandmark(lm);
            }
    } */

    //Get base DIR
    string base_dir = fsSettings["Input"];

    // load GT, cvTrajectory
    std::vector<std::map<double, Eigen::Matrix<double, 7, 1>>> Twbs, Tcps;
    for (size_t i = 2; i < argc; i = i + 2)
    {
        std::map<double, Eigen::Matrix<double, 7, 1>> Twb, Tcp;
        opengv2::SystemBase::loadTUM(base_dir + string(argv[i]), Twb);
        opengv2::SystemBase::loadTUM(base_dir + string(argv[i + 1]), Tcp);
        Twbs.push_back(Twb);
        Tcps.push_back(Tcp);
        std::cout << "GT " << i << " has " << Twb.size() << " frames." << std::endl;
        std::cout << "Camera " << i << " has " << Tcp.size() << " frames." << std::endl;
    }

    Eigen::Quaterniond unitQpb;
    Eigen::Vector3d tpb;
    std::vector<Eigen::Quaterniond> unitQcws;
    std::vector<Eigen::Vector3d> tcws;
    unitQcws.resize(Twbs.size());
    tcws.resize(Twbs.size());

    //Do Camera Calibration
    double R_error = 0;
    double t_error = 0;
    calibrateHandEye(Tcps, Twbs, unitQpb, tpb, unitQcws, tcws, argv);
    HandEyeErrors(Tcps, Twbs, unitQpb, tpb, unitQcws, tcws, argv, R_error, t_error);

    std::cout.precision(6);
    std::ofstream f(base_dir + "result/result.txt", std::ofstream::app);
    f << std::fixed << std::setprecision(10);
    //timestamp tx ty tz qx qy qz qw
    f << R_error << " " << t_error << std::endl;
    f.close();

    for (size_t i = 0; i < Tcps.size(); i++)
    {
        //size_t i = 0;
        Eigen::Quaterniond unitQwc = unitQcws[i].conjugate();
        Eigen::Vector3d twc = -(unitQwc * tcws[i]);
        //std::map<double, Eigen::Matrix<double, 7, 1>> Twp;
        for (const auto &it : Tcps[i])
        {
            Eigen::Quaterniond unitQcp(it.second.data());
            Eigen::Vector3d tcp(it.second.data() + 4);
            unitQcp.normalize();

            Eigen::Quaterniond unitQwp = unitQwc * unitQcp;
            Eigen::Vector3d twp = unitQwc * tcp + twc;

            vector<FrameBase::Ptr> frames(1);
            auto bf = std::make_shared<Bodyframe>(frames, it.first + i * 1000, twp, unitQwp);
            SLAM.map->addFrame(bf);
        }
    }

    //SLAM.loadGT(0, Twbs[0]);
    SLAM.viewer->updateViewer();

    std::cout << "press any key to exit..." << std::endl;
    std::cin.ignore();
    SLAM.shutdown();
    return 0;
}

void calibrateHandEye(const std::vector<std::map<double, Eigen::Matrix<double, 7, 1>>> &multi_Tws,
                      const std::vector<std::map<double, Eigen::Matrix<double, 7, 1>>> &multi_Tw1b,
                      Eigen::Quaterniond &unitQsb, Eigen::Vector3d &tsb,
                      std::vector<Eigen::Quaterniond> &multi_unitQww1, std::vector<Eigen::Vector3d> &multi_tww1,
                      char **argv)
{
    cv::FileStorage fsSettings(string(argv[1]), cv::FileStorage::READ);
    std::vector<std::vector<cv::Mat>> multi_Rbw1Set, multi_tbw1Set, multi_RswSet, multi_tswSet;
    int useMultiCam = fsSettings["UseMultiCam"];
    int useTsb = fsSettings["UseTsb"];
    unitQsb.w() = 0;

    for (size_t i = 0; i < multi_Tws.size(); i++)
    {
        auto &Tws = multi_Tws[i];
        auto &Tw1b = multi_Tw1b[i];
        auto &unitQww1 = multi_unitQww1[i];
        auto &tww1 = multi_tww1[i];

        std::vector<cv::Mat> Rbw1Set, tbw1Set, RswSet, tswSet;
        for (const auto &itr : Tws)
        {
            // Tbw1Set
            auto n1 = Tw1b.lower_bound(itr.first);
            if (n1 == Tw1b.end())
            {
                n1--;
            }
            else if (n1 != Tw1b.begin())
            {
                auto n2 = n1--;
                n1 = std::abs(itr.first - n1->first) > std::abs(itr.first - n2->first) ? n2 : n1;
            }

            auto itr1 = Tws.lower_bound(n1->first);
            if (itr1 == Tws.end())
            {
                itr1--;
            }
            else if (itr1 != Tws.begin())
            {
                auto itr2 = itr1--;
                itr1 = std::abs(n1->first - itr1->first) > std::abs(n1->first - itr2->first) ? itr2 : itr1;
            }

            if (itr1->first != itr.first || std::abs(n1->first - itr.first) > 3e-2)
                continue;

            Eigen::Quaterniond unitQw1b(n1->second.data());
            Eigen::Vector3d tw1b(n1->second.data() + 4);

            Eigen::Quaterniond unitQbw1 = unitQw1b.conjugate();
            Eigen::Vector3d tbw1 = -(unitQbw1 * tw1b);
            cv::Mat cv_Rbw1, cv_tbw1;
            cv::eigen2cv(unitQbw1.toRotationMatrix(), cv_Rbw1);
            cv::eigen2cv(tbw1, cv_tbw1);
            Rbw1Set.push_back(cv_Rbw1);
            tbw1Set.push_back(cv_tbw1);

            // TswSet
            Eigen::Quaterniond unitQws(itr.second.data());
            Eigen::Vector3d tws(itr.second.data() + 4);

            cv::Mat cv_Rsw, cv_tsw;
            cv::eigen2cv(unitQws.conjugate().toRotationMatrix(), cv_Rsw);
            cv::eigen2cv(Eigen::Vector3d(unitQws.conjugate() * (-tws)), cv_tsw);
            RswSet.push_back(cv_Rsw);
            tswSet.push_back(cv_tsw);

            //std::cout << "timestamps " << n1->first << " and " << itr.first << " are pairs." << std::endl;
            int NumofMeas = fsSettings["NumOfMeasures"];
            if (RswSet.size() > NumofMeas) //MUST BE REMOVED AFTER EXPERIMENT!!!!!!!!!!!!
            {
                cout << "Num of Measurements:" << RswSet.size() << std::endl;
                break;
            }
        }
        //std::cout << "Camera " << i << " has " << Rbw1Set.size() << " pairs." << std::endl;
        std::cout << "Calibration for " << string(argv[i * 2 + 2]) << " has " << Rbw1Set.size() << " pairs." << std::endl;

        multi_Rbw1Set.push_back(Rbw1Set);
        multi_tbw1Set.push_back(tbw1Set);
        multi_RswSet.push_back(RswSet);
        multi_tswSet.push_back(tswSet);

        //Use standard Handeye Calibration
        if (!useMultiCam)
        {
            cv::Mat cv_Rww1, cv_tww1;
            cv::Mat cv_Rsb, cv_tsb;

            switch (fsSettings["HandeyeSolver"])
            {
            case 0:
                calibrateRobotWorldHandEye(RswSet, tswSet, Rbw1Set, tbw1Set, cv_Rww1, cv_tww1, cv_Rsb, cv_tsb,
                                           cv::CALIB_ROBOT_WORLD_HAND_EYE_SHAH);
                break;

            case 1:
                calibrateRobotWorldHandEye(RswSet, tswSet, Rbw1Set, tbw1Set, cv_Rww1, cv_tww1, cv_Rsb, cv_tsb,
                                           cv::CALIB_ROBOT_WORLD_HAND_EYE_LI);
                break;

            default:
                break;
            }
            Eigen::Matrix3d Rsb_mono;
            Eigen::Vector3d tsb_mono;
            Eigen::Quaterniond unitQsb_mono;

            cv::cv2eigen(cv_Rsb, Rsb_mono);
            cv::cv2eigen(cv_tsb, tsb_mono);
            unitQsb_mono = Rsb_mono;
            tsb = tsb + tsb_mono;

            if (unitQsb_mono.w() > 0)
            {
                unitQsb.w() += unitQsb_mono.w();
                unitQsb.x() += unitQsb_mono.x();
                unitQsb.y() += unitQsb_mono.y();
                unitQsb.z() += unitQsb_mono.z();
            }
            else
            {
                unitQsb.w() -= unitQsb_mono.w();
                unitQsb.x() -= unitQsb_mono.x();
                unitQsb.y() -= unitQsb_mono.y();
                unitQsb.z() -= unitQsb_mono.z();
            }

            //unitQsb.normalize();
            std::cout << "Target extrinsic parameter for camera " << string(argv[i * 2 + 2]) << ": " << std::endl
                      << unitQsb_mono.coeffs().transpose() << std::endl
                      << tsb_mono.transpose() << std::endl;

            Eigen::Matrix3d Rww1;
            cv::cv2eigen(cv_Rww1, Rww1);
            cv::cv2eigen(cv_tww1, tww1);

            multi_unitQww1[i] = Rww1;
            multi_unitQww1[i].normalize();
            multi_tww1[i] = tww1;

            //Get Tw1w, which is the pose of camera inside the world cordinate
            Eigen::Vector3d tw1w;
            Eigen::Quaterniond unitQw1w;
            unitQw1w = multi_unitQww1[i].conjugate();
            tw1w = -(unitQw1w * tww1);

            /*         std::cout.precision(6);
                 std::cout << "Estimated extrinsic parameter: " << std::endl
                  << unitQsb.coeffs().transpose() << std::endl
                  << tsb.transpose() << std::endl; */

            std::cout.precision(6);
            std::cout << "Camera extrinsic parameter for camera " << string(argv[i * 2 + 2]) << ": " << std::endl
                      << (multi_unitQww1[0].conjugate() * unitQw1w.conjugate()).coeffs().transpose() << std::endl
                      << (multi_unitQww1[0] * (tw1w + (multi_unitQww1[0].conjugate() * multi_tww1[0]))).transpose() << std::endl;
        }
    }
    // Normalize Tsb
    if (!useMultiCam)
    {
        tsb = tsb / multi_Tws.size();
        unitQsb.normalize();
        std::cout << "Target extrinsic parameter for all camera " << std::endl
                  << unitQsb.coeffs().transpose() << std::endl
                  << tsb.transpose() << std::endl;
    }

    // Use Known Tsb
    if (!useTsb){
        //ToDo
    }

    //Use New Handeye Calibration
    if (useMultiCam)
    {
        std::cout << "------------New Approach------------" << std::endl;
        std::vector<cv::Mat> cv_multi_Rww1, cv_multi_tww1;
        cv::Mat cv_Rsb, cv_tsb;
        cv_multi_Rww1.resize(multi_RswSet.size());
        cv_multi_tww1.resize(multi_RswSet.size());
        calibrateRobotWorldHandEyeMulti(multi_RswSet, multi_tswSet, multi_Rbw1Set, multi_tbw1Set, cv_multi_Rww1, cv_multi_tww1, cv_Rsb, cv_tsb);

        Eigen::Matrix3d Rsb;
        cv::cv2eigen(cv_Rsb, Rsb);
        cv::cv2eigen(cv_tsb, tsb);
        unitQsb = Rsb;
        unitQsb.normalize();

        std::cout.precision(10);
        std::cout << "Estimated extrinsic parameter: " << std::endl
                  << unitQsb.coeffs().transpose() << std::endl
                  << tsb.transpose() << std::endl;

        for (size_t i = 0; i < multi_RswSet.size(); i++)
        {
            Eigen::Matrix3d Rww1;
            cv::cv2eigen(cv_multi_Rww1[i], Rww1);
            cv::cv2eigen(cv_multi_tww1[i], multi_tww1[i]);
            multi_unitQww1[i] = Rww1;
            multi_unitQww1[i].normalize();

            //Get Tw1w, which is the pose of camera inside the world cordinate
            Eigen::Vector3d tw1w;
            Eigen::Quaterniond unitQw1w;
            unitQw1w = multi_unitQww1[i].conjugate();
            tw1w = -(unitQw1w * multi_tww1[i]);

            std::cout.precision(6);
            std::cout << "Camera extrinsic parameter for camera " << string(argv[i * 2 + 2]) << ": " << std::endl
                      << (multi_unitQww1[0].conjugate() * unitQw1w.conjugate()).coeffs().transpose() << std::endl
                      << (multi_unitQww1[0] * (tw1w + (multi_unitQww1[0].conjugate() * multi_tww1[0]))).transpose() << std::endl;
/*                      std::cout.precision(6);
                       std::cout << "Camera extrinsic parameter for camera " << i << ": " << std::endl
                                 << unitQw1w.coeffs().transpose() << std::endl
                                 << tw1w.transpose() << std::endl;  */
        }
    }
}

void HandEyeErrors(const std::vector<std::map<double, Eigen::Matrix<double, 7, 1>>> &multi_Tws,
                   const std::vector<std::map<double, Eigen::Matrix<double, 7, 1>>> &multi_Tw1b,
                   Eigen::Quaterniond &unitQsb, Eigen::Vector3d &tsb,
                   std::vector<Eigen::Quaterniond> &multi_unitQww1, std::vector<Eigen::Vector3d> &multi_tww1,
                   char **argv, double &R_error, double &t_error)
{
    cv::FileStorage fsSettings(string(argv[1]), cv::FileStorage::READ);

    double total_terr = 0;
    double total_Rerr = 0;
    double count = 0;

    for (size_t i = 0; i < multi_Tws.size(); i++)
    {
        auto &Tws = multi_Tws[i];
        auto &Tw1b = multi_Tw1b[i];
        auto &unitQww1 = multi_unitQww1[i];
        auto &tww1 = multi_tww1[i];

        std::vector<cv::Mat> Rbw1Set, tbw1Set, RswSet, tswSet;
        for (const auto &itr : Tws)
        {
            // Tbw1Set
            auto n1 = Tw1b.lower_bound(itr.first);
            if (n1 == Tw1b.end())
            {
                n1--;
            }
            else if (n1 != Tw1b.begin())
            {
                auto n2 = n1--;
                n1 = std::abs(itr.first - n1->first) > std::abs(itr.first - n2->first) ? n2 : n1;
            }

            auto itr1 = Tws.lower_bound(n1->first);
            if (itr1 == Tws.end())
            {
                itr1--;
            }
            else if (itr1 != Tws.begin())
            {
                auto itr2 = itr1--;
                itr1 = std::abs(n1->first - itr1->first) > std::abs(n1->first - itr2->first) ? itr2 : itr1;
            }

            if (itr1->first != itr.first || std::abs(n1->first - itr.first) > 3e-2)
                continue;

            // Tw1bSet
            Eigen::Quaterniond unitQw1b(n1->second.data());
            Eigen::Vector3d tw1b(n1->second.data() + 4);

            Eigen::Quaterniond unitQbw1 = unitQw1b.conjugate();
            Eigen::Vector3d tbw1 = -(unitQbw1 * tw1b);

            // TswSet
            Eigen::Quaterniond unitQws(itr.second.data());
            Eigen::Vector3d tws(itr.second.data() + 4);

            Eigen::Quaterniond unitQsw = unitQws.conjugate();
            Eigen::Vector3d tsw = -(unitQsw * tws);

            // Tw1w
            Eigen::Quaterniond unitQw1w = unitQww1.conjugate();
            Eigen::Vector3d tw1w = -(unitQw1w * tww1);

            // Tbs
            Eigen::Quaterniond unitQbs = unitQsb.conjugate();
            Eigen::Vector3d tbs = -(unitQbs * tsb);

            //Error Term
            //(RY*RB)^T * (RA*RX) => (Qbs*Qsw)^T * (Qbw1 * Qw1w)
            Eigen::Quaterniond unitQerr = unitQws * unitQsb * unitQbw1 * unitQw1w;
            //(RA*tX + tA) - (RY*tB + tY) => (Qbw1*tw1w + tbw1) - (Qbs*tsw + tbs)
            Eigen::Vector3d terr = (unitQbw1 * tw1w + tbw1) - (unitQbs * tsw + tbs);

            Eigen::AngleAxisd axisangle(unitQerr);
            total_Rerr = total_Rerr + axisangle.angle();
            total_terr = total_terr + terr.norm();
            count = count + 1;
        }
    }
    t_error = total_terr / count;
    R_error = total_Rerr / count * 180 / M_PI;
    /*     std::cout.precision(6);
    std::cout << "Extrinsic error for camera " << std::endl
              << (total_Rerr / count * 180 / M_PI) << std::endl
              << (total_terr / count) << std::endl; */
}


/** Function to do optical flow based on hough transform tracking

Expected inputs:

Outputs:

*/

#include <iostream>
#include <vector>
#include <map>
#include <set>

#include <math.h>

#include <openssl/md5.h>
#include <Eigen/Dense>

#include <matrix.h>
#include <mex.h>

#include "common.h"

#include "ofhtCommon.h"

using namespace std;
using namespace Eigen;

#define USAGE_NOTE "TODO: This is mex function!"

//typedef pair<int, vector<int> > IdxPair;
typedef pair<string, vector<int> > IdxPair;

//////////////////////////////////////////////////////////////////
inline float inlQuantization(float val, float quan)
{
    val = val / quan;

    int qval = round(val);

    return qval;
}

vector<int> quantizeVal(vector<float> inputVec)
{

    float quanScale = QUAN_SCALE;
    float quanTheta = QUAN_THETA;
    float quanCoor = QUAN_COOR;

    vector<int> quanVec;

    int qscale = inlQuantization(inputVec[0], quanScale);
    int qtheta = inlQuantization(inputVec[1], quanTheta);
    int qdx = inlQuantization(inputVec[2], quanCoor);
    int qdy = inlQuantization(inputVec[3], quanCoor);

    quanVec.push_back(qscale);
    quanVec.push_back(qtheta);
    quanVec.push_back(qdx);
    quanVec.push_back(qdy);

    return quanVec;
}

vector<vector<float> > deQuantizeVal(vector<IdxPair> inputVec, int noOfRetVal)
{
    vector<vector<float> > retVec(noOfRetVal);

    float quanScale = QUAN_SCALE;
    float quanTheta = QUAN_THETA;
    float quanCoor = QUAN_COOR;

    int noOfEle = inputVec[0].second.size();

    for (int i = 0; i < noOfRetVal; ++i) {
        vector<float> tmpVec(noOfEle);

        tmpVec[0] = inputVec[i].second[0] * quanScale;
        tmpVec[1] = inputVec[i].second[1] * quanTheta;
        tmpVec[2] = inputVec[i].second[2] * quanCoor;
        tmpVec[3] = inputVec[i].second[3] * quanCoor;
        tmpVec[4] = inputVec[i].second[4];

        retVec[i] = tmpVec;
    }

    return retVec;
}

vector<vector<float> > calcLeastSquare(
    vector<IdxPair> sortInd,
    int noOfRetVal,
    map< string, set<int> > hashVoteList,
    float *prevPt,
    float *currPt,
    int noOfPt)
{
    vector<vector<float> > retVec(noOfRetVal);

    for (int i = 0; i < noOfRetVal; ++i) {
        string hashCode = sortInd[i].first;

        set<int> voteList = hashVoteList[hashCode];

        int noOfVote = voteList.size();

        MatrixXf A(2*noOfVote, 4);
        VectorXf b(2*noOfVote);

        int r = 0;
        for (set<int>::iterator it = voteList.begin(); it != voteList.end(); ++it) {

            int idxi = *it;

            A(r*2, 0) = prevPt[idxi];
            A(r*2, 1) = -prevPt[idxi+noOfPt];
            A(r*2, 2) = 1.0f;
            A(r*2, 3) = 0.0f;

            A(r*2+1, 0) = prevPt[idxi+noOfPt];
            A(r*2+1, 1) = prevPt[idxi];
            A(r*2+1, 2) = 0.0f;
            A(r*2+1, 3) = 1.0f;

            b(r*2) = currPt[idxi];
            b(r*2+1) = currPt[idxi + noOfPt];

            r++;
        }

        //solve least square by Eigen
        //VectorXf x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);

        VectorXf x = A.fullPivHouseholderQr().solve(b);

        // cout << "Least square results: " << endl;
        // cout << x << endl;

        //calcuate scale and theta
        retVec[i] = calcScaleAndTheta(x);

        retVec[i].push_back((float)sortInd[i].second[4]);

        //prepare the return value

    } //for (int i = 0; i < noOfRetVal; ++i) { ...

    return retVec;
}

//DESCEND
// inline bool comparator(const IdxPair& l, const IdxPair& r)
// {
//     return l.first > r.first;
// }

inline bool comparator(const IdxPair& l, const IdxPair& r)
{
    return l.second[4] > r.second[4];
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    float minDist = 25.f;

    map< string, vector<int> > hashTable;
    map< string, set<int> > hashVoteList;

    vector<IdxPair> sortInd;
    int loopNum = 0;
    vector<vector<float> > retVal;

    if (nrhs == 3) {
        double *tmp = (double *)mxGetData(prhs[2]);
        minDist = (float)tmp[0];
        //mexPrintf("minDist = %0.02f\n", minDist);
    }

    //check input variable
    if (nrhs == 3 || nrhs == 2) {
        if (mxGetClassID(prhs[0]) != mxSINGLE_CLASS ||
            mxGetClassID(prhs[1]) != mxSINGLE_CLASS) {
            mexErrMsgTxt(USAGE_NOTE);
        }

        int noOfPrevPt = mxGetM(prhs[0]);
        int noOfCurrPt = mxGetM(prhs[1]);

        //mexPrintf("noOfPrevPt = %d, noOfCurrPt = %d\n", noOfPrevPt, noOfCurrPt);

        if (noOfPrevPt != noOfCurrPt) {
            mexErrMsgTxt("The number of matched points is invalid!");
        }

        float *prevPt = (float *)mxGetData(prhs[0]);
        float *currPt = (float *)mxGetData(prhs[1]);

        //DEBUG - show all the points
        // for (int i = 0; i < noOfPrevPt; ++i) {
        //     mexPrintf("%0.02f, %0.02f\n", prevPt[i], prevPt[i+noOfPrevPt]);
        // }
        // mexPrintf("noOfPrevPt: %d, noOfCurrPt: %d\n", noOfPrevPt, noOfCurrPt);

        //fix bug: at least two points should be collected for hough voting
        int noOfValidPts = 0;
        while (1) {
            for (int i = 0; i < noOfPrevPt; ++i) {
                for (int j = i+1; j < noOfCurrPt; ++j) {
                    if (eDist(prevPt[i], prevPt[i+noOfPrevPt],
                          prevPt[j], prevPt[j+noOfPrevPt]) > minDist) {
                        noOfValidPts = noOfValidPts + 2;
                    }
                }
            }
            //mexPrintf("noOfValidPts = %d\n", noOfValidPts);

            if (noOfValidPts > 2) {
                break;
            } else {
                minDist = minDist / 2.0;
                mexPrintf("Shrink! minDist = %0.03f\n", minDist);   //potential problem
            }

            if (minDist <= 2.0f) {
                break;
            }

        }

        //return with empty matrix
        if (minDist <= 2.0) {
            mxArray *retMatrix = mxCreateNumericMatrix(0, 0, mxSINGLE_CLASS, mxREAL);
            mxArray *retLoopNum = mxCreateNumericMatrix(0, 0, mxINT32_CLASS, mxREAL);

            mexPrintf("Not enough valid points for ofht estimation!\n");

            plhs[0] = retMatrix;
            plhs[1] = retLoopNum;

            return;
        }

        for (int i = 0; i < noOfPrevPt; ++i) {
            for (int j = i+1; j < noOfCurrPt; ++j) {

                if (eDist(prevPt[i], prevPt[i+noOfPrevPt],
                          prevPt[j], prevPt[j+noOfPrevPt]) < minDist) {
                    continue;
                }

                loopNum++;
                //mexPrintf("loopNum = %d\n", loopNum);

                vector<float> vec = calcTransMatrix(prevPt, currPt,
                                                    i, j, noOfPrevPt);

                vector<int> quanVec = quantizeVal(vec);

                //DEBUG - show quantified vector results
                // for (int i = 0;  i < quanVec.size(); ++i){
                //     mexPrintf("%0.02f\t%d\n", vec[i], quanVec[i]);
                // }

                string hashCode = genHashCode(quanVec);

                if (hashTable.find(hashCode) == hashTable.end()) {
                    //mexPrintf("Could not find the key!\n");

                    quanVec.push_back(1);
                    hashTable[hashCode] = quanVec;

                    set<int> tmpSet;
                    tmpSet.insert(i);
                    tmpSet.insert(j);
                    hashVoteList[hashCode] = tmpSet;

                } else {
                    hashTable[hashCode][4]++;

                    hashVoteList[hashCode].insert(i);
                    hashVoteList[hashCode].insert(j);

                }

                //DEBUG - check hashing results
                // vector<int> checkVec = hashTable[hashCode];

                // for (vector<int>::iterator i = checkVec.begin();
                //      i != checkVec.end(); ++i) {
                //     cout << *i << endl;
                // }
                //return;

            } //for (int j = i+1; j < noOfCurrPt; ++j) { ...
        }//for (int i = 0; i < noOfPrevPt; ++i) {

        //DEBUG - check all hash code
        // for (map<string, vector<int> >::iterator it = hashTable.begin();
        //     it != hashTable.end(); ++it) {

        //     vector<int> checkVec = it->second;

        //     for (vector<int>::iterator i = checkVec.begin();
        //          i != checkVec.end(); ++i) {
        //         mexPrintf("%d ", *i);
        //     }
        //     mexPrintf("\n");
        // }

        for (map<string, vector<int> >::iterator it = hashTable.begin();
             it != hashTable.end(); ++it) {

            // vector<int> checkVec = it->second;
            // sortInd.push_back(make_pair(checkVec[4], checkVec));

            //vector<int> checkVec = it->second;
            sortInd.push_back(make_pair(it->first, it->second));
        }

        //DEBUG - show vector pair
        // mexPrintf("Before sorting ...\n");
        // for (int i = 0; i < sortInd.size(); ++i) {
        //     mexPrintf("%s: %d\n", sortInd[i].first.c_str(), sortInd[i].second[4]);
        // }

        //Sort pair results
        sort(sortInd.begin(), sortInd.end(), comparator);

        // mexPrintf("After sorting ...\n");
        // for (int i = 0; i < sortInd.size(); ++i) {
        //     mexPrintf("%s: %d\n", sortInd[i].first.c_str(), sortInd[i].second[4]);
        // }

        //dequantize the results
        int noOfRetVal = MIN(NUM_OF_RET, sortInd.size());

        //use Least Square to calculate reture value
        retVal = calcLeastSquare(sortInd, noOfRetVal, hashVoteList,
                                 prevPt, currPt, noOfPrevPt);

        //use deQuantizd hard vote results as return value
        //retVal = deQuantizeVal(sortInd, noOfRetVal);

    } else {
        mexErrMsgTxt(USAGE_NOTE);
    }

    //mexPrintf("Total number of hashTable entry: %d\n", sortInd.size());

    //check output vairable
    if (nlhs == 2) {
        //mexPrintf("The output is valid!\n");

        //int height = MIN(NUM_OF_RET, sortInd.size());
        int height = retVal.size();
        //int width = sortInd[0].second.size();
        int width = retVal[0].size();

        mxArray *retMatrix = mxCreateNumericMatrix(height, width,
                                                   mxSINGLE_CLASS, mxREAL);
        float *retMatrixData = (float *)mxGetData(retMatrix);

        for (int i = 0; i < height; ++i) {
            //mexPrintf("%d: %d\n", sortInd[i].first, sortInd[i].second[4]);

            for (int j = 0; j < width; ++j) {
                int idx = j * height + i;

                //retMatrixData[idx] = sortInd[i].second[j];
                retMatrixData[idx] = retVal[i][j];
            }
        }

        mxArray *retLoopNum = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
        int *retLoopNumData = (int *)mxGetData(retLoopNum);

        retLoopNumData[0] = loopNum;

        plhs[0] = retMatrix;
        plhs[1] = retLoopNum;

    } else {
        mexPrintf("The output is invalid!");
        mexErrMsgTxt(USAGE_NOTE);
    }

    return;

}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef ANALYTICEVALUATE_HPP
#define ANALYTICEVALUATE_HPP

#include <cmath>

#include <Eigen/Core>

// Boost.Math includes
#include <boost/math/special_functions/legendre.hpp>

#include "utils/MathUtils.hpp"

/*! \brief Analytic evaluation of vacuum Green's function and its derivatives
 *
 *  \f[
 *  \begin{align}
 *     G(\vect{r},\vect{r}^\prime) &= \frac{1}{|\vect{r}-\vect{r}^\prime|} \\
 *     \pderiv{}{{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) &= \frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}}{|\vect{r}-\vect{r}^\prime|^3} \\
 *     \pderiv{}{{\vect{n}_{\vect{r}}}}G(\vect{r},\vect{r}^\prime) &= -\frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}}{|\vect{r}-\vect{r}^\prime|^3} \\
 *     \frac{\partial^2}{\partial{\vect{n}_{\vect{r}}}\partial{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) &=
 *     \frac{\vect{n}_{\vect{r}}\cdot \vect{n}_{\vect{r}^\prime}}{|\vect{r}-\vect{r}^\prime|^3}
 *     -3\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]}{|\vect{r}-\vect{r}^\prime|^5}
 *  \end{align}
 *  \f]
 */
inline Eigen::Array4d analyticVacuum(const Eigen::Vector3d & spNormal,
                                const Eigen::Vector3d & sp,
                                const Eigen::Vector3d & ppNormal, const Eigen::Vector3d & pp) {
    Eigen::Array4d result = Eigen::Array4d::Zero();
    double distance = (sp - pp).norm();
    double distance_3 = std::pow(distance, 3);
    double distance_5 = std::pow(distance, 5);

    // Value of the function
    result(0) = 1.0 / distance;
    // Value of the directional derivative wrt probe
    result(1) = (sp - pp).dot(ppNormal) / distance_3 ;
    // Directional derivative wrt source
    result(2) = - (sp - pp).dot(spNormal) / distance_3;
    // Value of the Hessian
    result(3) = spNormal.dot(ppNormal) / distance_3 - 3 * ((
                    sp - pp).dot(spNormal))*((sp - pp).dot(
                                ppNormal)) / distance_5;

    return result;
}

/*! \brief Analytic evaluation of uniform dielectric Green's function and its derivatives
 *
 *  \f[
 *  \begin{align}
 *     G(\vect{r},\vect{r}^\prime) &= \frac{1}{|\diel\vect{r}-\vect{r}^\prime|} \\
 *     \pderiv{}{{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) &= \frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}}{\diel|\vect{r}-\vect{r}^\prime|^3} \\
 *     \pderiv{}{{\vect{n}_{\vect{r}}}}G(\vect{r},\vect{r}^\prime) &= -\frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}}{\diel|\vect{r}-\vect{r}^\prime|^3} \\
 *     \frac{\partial^2}{\partial{\vect{n}_{\vect{r}}}\partial{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) &=
 *     \frac{\vect{n}_{\vect{r}}\cdot \vect{n}_{\vect{r}^\prime}}{\diel|\vect{r}-\vect{r}^\prime|^3}
 *     -3\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]}{\diel|\vect{r}-\vect{r}^\prime|^5}
 *  \end{align}
 *  \f]
 */
inline Eigen::Array4d analyticUniformDielectric(double eps, const Eigen::Vector3d & spNormal,
                                const Eigen::Vector3d & sp,
                                const Eigen::Vector3d & ppNormal, const Eigen::Vector3d & pp) {
    Eigen::Array4d result = Eigen::Array4d::Zero();
    double distance = (sp - pp).norm();
    double distance_3 = std::pow(distance, 3);
    double distance_5 = std::pow(distance, 5);

    // Value of the function
    result(0) = 1.0 / (eps * distance);
    // Value of the directional derivative wrt probe
    result(1) = (sp - pp).dot(ppNormal) / (eps * distance_3);
    // Directional derivative wrt source
    result(2) = - (sp - pp).dot(spNormal) / (eps * distance_3);
    // Value of the Hessian
    result(3) = spNormal.dot(ppNormal) / (eps * distance_3) - 3 * ((
                    sp - pp).dot(spNormal))*((sp - pp).dot(
                                ppNormal)) / (eps * distance_5);

    return result;
}

/*! \brief Analytic evaluation of ionic liquid Green's function and its derivatives
 *
 *  \f[
 *  \begin{align}
 *  \phantom{G(\vect{r},\vect{r}^\prime)}
 *  &\begin{aligned}
 *    G(\vect{r},\vect{r}^\prime) = \frac{\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|}
 *  \end{aligned}\\
 *  &\begin{aligned}
 *    \pderiv{}{{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) =
 *    \frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^3}
 *    +\kappa\frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^2}
 *   \end{aligned}\\
 *   &\begin{aligned}
 *     \pderiv{}{{\vect{n}_{\vect{r}}}}G(\vect{r},\vect{r}^\prime) =
 *     -\frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^3}
 *     -\kappa\frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^2}
 *    \end{aligned}\\
 *   &\begin{aligned}
 *     \frac{\partial^2}{\partial{\vect{n}_{\vect{r}}}\partial{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) &=
 *     \frac{\vect{n}_{\vect{r}}\cdot \vect{n}_{\vect{r}^\prime}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^3}
 *     -\kappa\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^4}\\
 *     &-3\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^5}
 *     + \kappa\frac{\vect{n}_{\vect{r}}\cdot \vect{n}_{\vect{r}^\prime}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^2} \\
 *     &-\kappa^2\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^3}\\
 *     &-2\kappa\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^4}
 *    \end{aligned}
 *  \end{align}
 *  \f]
 */
inline Eigen::Array4d analyticIonicLiquid(double eps, double k,
                                const Eigen::Vector3d & spNormal,
                                const Eigen::Vector3d & sp,
                                const Eigen::Vector3d & ppNormal, const Eigen::Vector3d & pp) {
    Eigen::Array4d result = Eigen::Array4d::Zero();
    double distance = (sp - pp).norm();
    double distance_3 = std::pow(distance, 3);
    double distance_5 = std::pow(distance, 5);

    // Value of the function
    result(0) = std::exp(- k * distance) / (eps * distance);
    // Value of the directional derivative wrt probe
    result(1) = (sp - pp).dot(ppNormal) * (1 + k * distance ) * std::exp(
                    - k * distance) / (eps * distance_3);
    // Directional derivative wrt source
    result(2) = - (sp - pp).dot(spNormal) * (1 + k * distance ) * std::exp(
                    - k * distance) / (eps * distance_3);
    // Value of the Hessian
    result(3) = spNormal.dot(ppNormal) * (1 + k * distance) * std::exp(
                    - k * distance) / (eps * distance_3)
                - std::pow(k, 2) * (sp - pp).dot(spNormal) * (sp - pp).dot(
                    ppNormal) * std::exp(- k * distance) / (eps * distance_3)
                - 3 * (sp - pp).dot(spNormal) * (sp - pp).dot(
                    ppNormal) * (1 + k * distance) * std::exp(- k * distance) /
                (eps * distance_5);

    return result;
}

/*! \brief Analytic evaluation of anisotropic liquid Green's function and its derivatives
 *
 *  \f[
 *  \begin{align}
 *   \phantom{G(\vect{r},\vect{r}^\prime)}
 *   &\begin{aligned}
 *     G(\vect{r},\vect{r}^\prime) = \frac{1}{4\pi\sqrt{\det{\bm{\diel}}}\sqrt{(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)}}
 *     \end{aligned}\\
 *   &\begin{aligned}
 *     \pderiv{}{{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) = \frac{[\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]\cdot \vect{n}_{\vect{r}^\prime}}{4\pi\sqrt{\det{\bm{\diel}}}
 *     [(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]^{\frac{3}{2}}}
 *     \end{aligned}\\
 *   &\begin{aligned}
 *     \pderiv{}{{\vect{n}_{\vect{r}}}}G(\vect{r},\vect{r}^\prime) = -\frac{[\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]\cdot \vect{n}_{\vect{r}}}{4\pi\sqrt{\det{\bm{\diel}}}
 *     [(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]^{\frac{3}{2}}}
 *     \end{aligned}\\
 *   &\begin{aligned}
 *     \frac{\partial^2}{\partial{\vect{n}_{\vect{r}}}\partial{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) &=
 *     \frac{\vect{n}_{\vect{r}}\cdot[\bm{\diel}^{-1} \vect{n}_{\vect{r}^\prime}]}{4\pi\sqrt{\det{\bm{\diel}}}
 *     [(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]^{\frac{3}{2}}} \\
 *     &-3\frac{\lbrace[\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]\cdot \vect{n}_{\vect{r}}\rbrace\lbrace[\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]\cdot \vect{n}_{\vect{r}^\prime}\rbrace}{4\pi\sqrt{\det{\bm{\diel}}}
 *     [(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]^{\frac{5}{2}}}
 *    \end{aligned}
 *   \end{align}
 *   \f]
 */
inline Eigen::Array4d analyticAnisotropicLiquid(const Eigen::Vector3d & epsilon,
    	                    const Eigen::Vector3d & euler,
                                const Eigen::Vector3d & spNormal,
                                const Eigen::Vector3d & sp,
                                const Eigen::Vector3d & ppNormal, const Eigen::Vector3d & pp) {
    Eigen::Array4d result = Eigen::Array4d::Zero();
    Eigen::Matrix3d epsilonInv, R;
    eulerRotation(R, euler);
    Eigen::Vector3d scratch;
    scratch << (1.0/epsilon(0)), (1.0/epsilon(1)), (1.0/epsilon(2));
    epsilonInv = R * scratch.asDiagonal() * R.transpose();
    double detEps = epsilon(0) * epsilon(1) * epsilon(2);
    Eigen::Vector3d right = epsilonInv * (sp - pp);
    Eigen::Vector3d left  = (sp - pp).transpose();
    double distance   = std::sqrt(left.dot(right));
    double distance_3 = std::pow(distance, 3);
    double distance_5 = std::pow(distance, 5);

    // Value of the function
    result(0) = 1.0 / (std::sqrt(detEps) * distance);
    // Value of the directional derivative wrt probe
    result(1) = right.dot(ppNormal) / (std::sqrt(detEps) * distance_3);
    // Directional derivative wrt source
    result(2) = - right.dot(spNormal) / (std::sqrt(detEps) * distance_3);
    // Value of the Hessian
    Eigen::Vector3d eps_ppNormal = epsilonInv * ppNormal;
    result(3) = spNormal.dot(eps_ppNormal) / (std::sqrt(detEps) * distance_3)
    	  - 3 * (right.dot(spNormal)) * (right.dot(ppNormal)) / (std::sqrt(detEps) * distance_5);

    return result;
}

inline double imagePotential(double eps, double epsSolv, double radius, const Eigen::Vector3d & origin,
                        const Eigen::Vector3d & sp, const Eigen::Vector3d & pp) {
    Eigen::Vector3d sp_origin = sp - origin;
    double sp_origin_norm = sp_origin.norm();
    Eigen::Vector3d pp_origin = pp - origin;
    double pp_origin_norm = pp_origin.norm();
    double cos_gamma = sp_origin.dot(pp_origin) / (sp_origin.norm() * pp_origin.norm());
    // Clean-up cos_gamma, Legendre polynomials are only defined for -1 <= x <= 1
    if (numericalZero(cos_gamma - 1)) cos_gamma = 1.0;
    if (numericalZero(cos_gamma + 1)) cos_gamma = -1.0;
    // Image charge position
    Eigen::Vector3d r_img = origin + std::pow(radius / pp_origin_norm, 2) * pp_origin;
    double sp_image_norm = (sp - r_img).norm();
    // Image charge
    double q_img = radius / pp_origin_norm;
    // Permittivity factor
    double factor = (eps - epsSolv) / (eps + epsSolv);

    // Image Green's function
    double G_img = factor * (q_img / sp_image_norm - q_img / sp_origin_norm);
    // Image Green's function
    // Accumulate Legendre polynomial expansion of image potential
    double f_0 = radius / (sp_origin_norm * pp_origin_norm);
    double f_l = f_0;
    for (int l = 1; l <= 200; ++l) {
        f_l = f_l * radius * f_0;
        double C_0_l = (eps - epsSolv) * l / ((eps + epsSolv) * l + epsSolv);
        double pl_x = boost::math::legendre_p(l, cos_gamma);
        G_img += f_l * (C_0_l - factor) * pl_x;
    }

    return G_img / epsSolv;
}

inline double derivativeImagePotential(double eps, double epsSolv, double radius, const Eigen::Vector3d & origin,
                        const Eigen::Vector3d & sp,
                        const Eigen::Vector3d & ppNormal, const Eigen::Vector3d & pp) {
    Eigen::Vector3d sp_origin = sp - origin;
    double sp_origin_norm = sp_origin.norm();
    Eigen::Vector3d pp_origin = pp - origin;
    double pp_origin_norm = pp_origin.norm();
    double cos_gamma = sp_origin.dot(pp_origin) / (sp_origin_norm * pp_origin_norm);
    // Clean-up cos_gamma, Legendre polynomials are only defined for -1 <= x <= 1
    if (numericalZero(cos_gamma - 1)) cos_gamma = 1.0;
    if (numericalZero(cos_gamma + 1)) cos_gamma = -1.0;
    double pp_origin_norm_3 = std::pow(pp_origin_norm, 3);

    Eigen::Vector3d r_img = origin + std::pow(radius / pp_origin_norm, 2) * pp_origin;
    Eigen::Vector3d pp_image = pp - r_img;
    double pp_image_norm_3 = std::pow(pp_image.norm(), 3);
    double factor = (eps - epsSolv) / (eps + epsSolv);

    double der_G_img = factor * (radius / sp_origin_norm) * (pp_origin.dot(ppNormal) / pp_origin_norm_3 - pp_image.dot(ppNormal) / pp_image_norm_3);
    /*
    // Accumulate Legendre polynomial expansion of image potential
    double f_0 = radius / (sp_origin_norm * pp_origin_norm);
    double f_l = f_0;
    double pp_origin_norm_l_3 = pp_origin_norm_3; // To accumulate (pp-origin).norm()^(l+3)
    double pp_origin_norm_l_1 = pp_origin_norm;   // To accumulate (pp-origin).norm()^(l+1)
    for (int l = 1; l <= 200; ++l) {
        f_l = f_l * radius * f_0;
        pp_origin_norm_l_3 *= pp_origin_norm;
        pp_origin_norm_l_1 *= pp_origin_norm;

        double pl_x = boost::math::legendre_p(l, cos_gamma); // P_l(cos_gamma)
        double pl_1_x = boost::math::legendre_p(l+1, cos_gamma); // P_(l+1)(cos_gamma)
        double cos_denom = std::pow(cos_gamma, 2) - 1;

        double tmp_a = ((l+1) * pl_x * pp_origin.dot(ppNormal)) / pp_origin_norm_l_3;
        double tmp_b = ((l+1) * (pl_1_x - cos_gamma * pl_x) ) / (pp_origin_norm_l_1 * cos_denom);
        double tmp_c = sp_origin.dot(ppNormal) / (sp_origin_norm * pp_origin_norm);
        double tmp_d = (pp_origin.dot(sp_origin) * pp_origin.dot(ppNormal)) / (sp_origin_norm * pp_origin_norm_3);
        double tmp_e = tmp_b * (tmp_c - tmp_d);

        double C_0_l = (eps - epsSolv) * l / ((eps + epsSolv) * l + epsSolv);
        double C_l = C_0_l - factor;

        der_G_img += f_l * C_l * (-tmp_a + tmp_e);
    }
    */

    return der_G_img / epsSolv;
}

/*! \brief Analytic evaluation of spherical sharp Green's function and its derivatives
 *  Derivation details in J. Chem. Phys. 139, 0224105 (2013)
 */
inline Eigen::Array4d analyticSphericalSharp(double eps, double epsSolv, double radius, const Eigen::Vector3d & origin,
                        const Eigen::Vector3d & spNormal, const Eigen::Vector3d & sp,
                        const Eigen::Vector3d & ppNormal, const Eigen::Vector3d & pp) {
    Eigen::Array4d result = Eigen::Array4d::Zero();
    double distance = (sp - pp).norm();
    double distance_3 = std::pow(distance, 3);

    double G_img = imagePotential(eps, epsSolv, radius, origin, sp, pp);
    // Value of the function
    result(0) = 1.0 / (epsSolv * distance) - G_img;

    double d_probe_G_img = derivativeImagePotential(eps, epsSolv, radius, origin, sp, ppNormal, pp);
    // Value of the directional derivative wrt probe
    result(1) = (sp - pp).dot(ppNormal) / (epsSolv * distance_3) - d_probe_G_img;

    double d_source_G_img = 0.0;
    // Directional derivative wrt source
    result(2) = - (sp - pp).dot(spNormal) / (epsSolv * distance_3) - d_source_G_img;
    // Value of the Hessian
    result(3) = 0.0;

    return result;
}

#endif // ANALYTICEVALUATE_HPP


//
// Copyright (c) 2018-2019 INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/centroidal-derivatives.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"

#include <iostream>

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

template<typename JointModel>
static void addJointAndBody(pinocchio::Model & model,
                            const pinocchio::JointModelBase<JointModel> & joint,
                            const std::string & parent_name,
                            const std::string & name,
                            const pinocchio::SE3 placement = pinocchio::SE3::Random(),
                            bool setRandomLimits = true)
{
  using namespace pinocchio;
  typedef typename JointModel::ConfigVector_t CV;
  typedef typename JointModel::TangentVector_t TV;

  Model::JointIndex idx;

  if (setRandomLimits)
    idx = model.addJoint(model.getJointId(parent_name),joint,
                         SE3::Random(),
                         name + "_joint",
                         TV::Random() + TV::Constant(1),
                         TV::Random() + TV::Constant(1),
                         CV::Random() - CV::Constant(1),
                         CV::Random() + CV::Constant(1)
                         );
  else
    idx = model.addJoint(model.getJointId(parent_name),joint,
                         placement, name + "_joint");

  model.addJointFrame(idx);

  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
  model.addBodyFrame(name + "_body", idx);
}

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_centroidal_derivatives)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  const std::string parent_name = model.names.back();
  const std::string joint_name = "ee_spherical_joint";
  addJointAndBody(model, pinocchio::JointModelSpherical(), parent_name , joint_name);

  pinocchio::Data data(model), data_ref(model);

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

  Eigen::VectorXd q = pinocchio::randomConfiguration(model);
  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);

  pinocchio::Data::Matrix6x
    dh_dq(6,model.nv),dhdot_dq(6,model.nv), dhdot_dv(6,model.nv), dhdot_da(6,model.nv);
  pinocchio::computeCentroidalDynamicsDerivatives(model,data,q,v,a,
                                                  dh_dq,dhdot_dq,dhdot_dv,dhdot_da);
  pinocchio::ccrba(model,data_ref,q,v);

  for(size_t k = 0; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
    BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
  }
  BOOST_CHECK(dhdot_da.isApprox(data_ref.Ag));

  pinocchio::computeCentroidalMomentumTimeVariation(model,data_ref,q,v,a);
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.ov[k].isApprox(data.oMi[k].act(data_ref.v[k])));
    BOOST_CHECK(data.oa[k].isApprox(data.oMi[k].act(data_ref.a[k])));
    BOOST_CHECK(data.oh[k].isApprox(data.oMi[k].act(data_ref.h[k])));
  }

  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));

  BOOST_CHECK(data.oh[0].isApprox(data_ref.h[0]));
  BOOST_CHECK(data.of[0].isApprox(data_ref.f[0]));

  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
  BOOST_CHECK(data.dhg.isApprox(data_ref.dhg));

  pinocchio::Data data_fd(model);

  const double eps = 1e-8;
  const pinocchio::Force dhg = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v,a);
  const pinocchio::Force hg = data_fd.hg;
  BOOST_CHECK(data.hg.isApprox(data_ref.hg));

  // Check dhdot_dq and dh_dq with finite differences
  Eigen::VectorXd q_plus(model.nq,1);
  Eigen::VectorXd v_eps(model.nv,1); v_eps.setZero();
  pinocchio::Data::Matrix6x dhdot_dq_fd(6,model.nv);
  pinocchio::Data::Matrix6x dh_dq_fd(6,model.nv);

  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_eps[k] = eps;
    q_plus = pinocchio::integrate(model,q,v_eps);

    const pinocchio::Force & dhg_plus
    = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q_plus,v,a);
    const pinocchio::Force hg_plus = data_fd.hg;

    dhdot_dq_fd.col(k) = (dhg_plus - dhg).toVector()/eps;
    dh_dq_fd.col(k) = (hg_plus - hg).toVector()/eps;
    v_eps[k] = 0.;
  }

  BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_fd,sqrt(eps)));
  BOOST_CHECK(dh_dq.isApprox(dh_dq_fd,sqrt(eps)));
  // Check dhdot_dv with finite differences
  Eigen::VectorXd v_plus(v);
  pinocchio::Data::Matrix6x dhdot_dv_fd(6,model.nv);

  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_plus[k] += eps;

    const pinocchio::Force & dhg_plus
    = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v_plus,a);
    dhdot_dv_fd.col(k) = (dhg_plus - dhg).toVector()/eps;

    v_plus[k] -= eps;
  }

  BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_fd,sqrt(eps)));

  // Check dhdot_da with finite differences
  Eigen::VectorXd a_plus(a);
  pinocchio::Data::Matrix6x dhdot_da_fd(6,model.nv);

  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    a_plus[k] += eps;

    const pinocchio::Force & dhg_plus
    = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v,a_plus);
    dhdot_da_fd.col(k) = (dhg_plus - dhg).toVector()/eps;

    a_plus[k] -= eps;
  }

  BOOST_CHECK(dhdot_da.isApprox(dhdot_da_fd,sqrt(eps)));

  pinocchio::computeRNEADerivatives(model,data_ref,q,v,a);
  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
}

BOOST_AUTO_TEST_CASE(test_retrieve_centroidal_derivatives)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model), data_ref(model);

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

  Eigen::VectorXd q = pinocchio::randomConfiguration(model);
  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);

  pinocchio::Data::Matrix6x
    dh_dq(6,model.nv), dhdot_dq(6,model.nv), dhdot_dv(6,model.nv), dhdot_da(6,model.nv);
  pinocchio::Data::Matrix6x
    dh_dq_ref(6,model.nv), dhdot_dq_ref(6,model.nv), dhdot_dv_ref(6,model.nv), dhdot_da_ref(6,model.nv);

  pinocchio::computeCentroidalDynamicsDerivatives(model,data_ref,q,v,a,
                                                  dh_dq_ref, dhdot_dq_ref,dhdot_dv_ref,dhdot_da_ref);

  pinocchio::computeRNEADerivatives(model,data,q,v,a);
  pinocchio::getCentroidalDynamicsDerivatives(model,data,
                                              dh_dq, dhdot_dq,dhdot_dv,dhdot_da);

  BOOST_CHECK(data.J.isApprox(data_ref.J));

  for(pinocchio::Model::JointIndex k = 1; k < (pinocchio::Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
    pinocchio::Force force_ref = data_ref.of[k];
    pinocchio::Force gravity_contribution = data.oYcrb[k] * (-model.gravity);
    pinocchio::Force force = data.of[k] - gravity_contribution;
    BOOST_CHECK(force.isApprox(force_ref));
  }

  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));

  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
  BOOST_CHECK(data.dhg.isApprox(data_ref.dhg));

  BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdq));
  BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv));
  BOOST_CHECK(data.dFda.isApprox(data_ref.dFda));
  BOOST_CHECK(dh_dq.isApprox(dh_dq_ref));
  BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_ref));
  BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_ref));
  BOOST_CHECK(dhdot_da.isApprox(dhdot_da_ref));

}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <Eigen/Dense>

#include "pinocchio/parsers/urdf.hpp"

#include "crocoddyl/core/utils/timer.hpp"

#include "example-robot-data/path.hpp"

#include "multicopter_mpc/ocp/trajectory-generator.hpp"
#include "multicopter_mpc/path.h"

void applyRandomNoise(Eigen::VectorXd& vector) {
  Eigen::VectorXd r_vector = Eigen::VectorXd::Random(vector.size());

  vector += r_vector * 0.05;

  Eigen::Quaterniond quat;
  quat.vec() = vector.segment(3, 3);
  quat.w() = vector(6);
  quat.normalize();

  vector.segment(3, 3) = quat.vec();
  vector(6) = quat.w();
}

void printNodeCombination(const int& nodes_number) { std::cout << "NUMBER OF NODES: " << nodes_number << std::endl; }

void printTrial(const int& trial_number, const int& iter_number, const double& elapsed_time, const double& stop,
                const double& cost) {
  std::string separator = ". ";

  std::cout << "Trial # " << trial_number << ": ";

  std::cout << "Iterations: " << iter_number << separator;
  std::cout << "Elapsed time: " << elapsed_time << separator;
  std::cout << "Freq. per iteration: " << iter_number / elapsed_time * 1000 << separator;
  std::cout << "Stop value: " << stop << separator;
  std::cout << "Final cost: " << cost << std::endl;
}

void printOverview(const Eigen::ArrayXd& iterations, const Eigen::ArrayXd& durations, const int& trial_num) {
  std::string separator = ". ";

  std::cout << "Average: ";

  std::cout << "Iterations: " << iterations.sum() / trial_num << separator;
  std::cout << "Elapsed time: " << durations.sum() / trial_num << separator;

  Eigen::ArrayXd time_weighted;
  time_weighted = durations / iterations;
  std::cout << "Freq. per iteration: " << trial_num / time_weighted.sum() * 1000.0 << std::endl;
}

int main(void) {
  // int nodes_max = 200;
  // int nodes_min = 30;
  // int nodes_inc = 10;

  // int solve_trials = 10;

  // for (int n = nodes_min; n < nodes_max; n += nodes_inc) {
  //   std::string model_description_path;
  //   std::string model_yaml_path;
  //   std::string mission_yaml_path = MULTICOPTER_MPC_MISSION_DIR "/hover.yaml";

  //   model_description_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/iris_description/robots/iris_simple.urdf";
  //   model_yaml_path = MULTICOPTER_MPC_MULTIROTOR_DIR "/iris.yaml";

  //   yaml_parser::ParserYAML yaml_mc(model_yaml_path, "", true);
  //   yaml_parser::ParamsServer server_params(yaml_mc.getParams());

  //   yaml_parser::ParserYAML yaml_mission(mission_yaml_path, "", true);
  //   yaml_parser::ParamsServer server_mission(yaml_mission.getParams());

  //   pinocchio::Model model;
  //   pinocchio::urdf::buildModel(model_description_path, pinocchio::JointModelFreeFlyer(), model);
  //   boost::shared_ptr<pinocchio::Model> model_ = boost::make_shared<pinocchio::Model>(model);
  //   boost::shared_ptr<multicopter_mpc::MultiCopterBaseParams> mc_params_ =
  //       boost::make_shared<multicopter_mpc::MultiCopterBaseParams>();
  //   mc_params_->fill(server_params);
  //   boost::shared_ptr<multicopter_mpc::Mission> mission_ = boost::make_shared<multicopter_mpc::Mission>(model_->nq + model_->nv);
  //   mission_->fillWaypoints(server_mission);
  //   mission_->fillInitialState(server_mission);

  //   double dt_ = 4e-3;
  //   multicopter_mpc::TrajectoryGenerator trajectory_generator(model_, mc_params_,dt_,mission_);

  //   // mission_->getWaypoints()[0].knots = n;

  //   trajectory_generator.createProblem(multicopter_mpc::SolverTypes::BoxFDDP);

  //   printNodeCombination(n);
  //   Eigen::ArrayXd durations(solve_trials);
  //   Eigen::ArrayXd iterations(solve_trials);
  //   trajectory_generator.solve();
  //   for (int i = 0; i < solve_trials; i++) {
  //     Eigen::VectorXd initial_state = mission_->getInitialState();
  //     applyRandomNoise(initial_state);
  //     crocoddyl::Timer timer;
  //     trajectory_generator.setInitialState(initial_state);
  //     trajectory_generator.solve();
  //     durations[i] = timer.get_duration();
  //     iterations[i] = trajectory_generator.getSolver()->get_iter();
  //     printTrial(i, trajectory_generator.getSolver()->get_iter(), durations[i], trajectory_generator.getSolver()->get_stop(),
  //                trajectory_generator.getSolver()->get_cost());
  //   }
  //   printOverview(iterations, durations, solve_trials);
  // }
}