// 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_VARIATIONALMONTECARLO_HPP
#define NETKET_VARIATIONALMONTECARLO_HPP

#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <complex>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>
#include "Machine/machine.hpp"
#include "Observable/observable.hpp"
#include "Optimizer/optimizer.hpp"
#include "Sampler/sampler.hpp"
#include "Stats/stats.hpp"
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "json_output_writer.hpp"
#include "matrix_replacement.hpp"

namespace netket {

// Variational Monte Carlo schemes to learn the ground state
// Available methods:
// 1) Stochastic reconfiguration optimizer
//   both direct and sparse version
// 2) Gradient Descent optimizer
class VariationalMonteCarlo {
  using GsType = std::complex<double>;

  using VectorT =
      Eigen::Matrix<typename Machine<GsType>::StateType, Eigen::Dynamic, 1>;
  using MatrixT = Eigen::Matrix<typename Machine<GsType>::StateType,
                                Eigen::Dynamic, Eigen::Dynamic>;

  Hamiltonian &ham_;
  Sampler<Machine<GsType>> &sampler_;
  Machine<GsType> &psi_;

  std::vector<std::vector<int>> connectors_;
  std::vector<std::vector<double>> newconfs_;
  std::vector<std::complex<double>> mel_;

  Eigen::VectorXcd elocs_;
  MatrixT Ok_;
  VectorT Okmean_;

  Eigen::MatrixXd vsamp_;

  Eigen::VectorXcd grad_;
  Eigen::VectorXcd gradprev_;

  double sr_diag_shift_;
  bool sr_rescale_shift_;
  bool use_iterative_;

  int totalnodes_;
  int mynode_;

  // This optional will contain a value iff the MPI rank is 0.
  nonstd::optional<JsonOutputWriter> output_;

  Optimizer &opt_;

  std::vector<Observable> obs_;
  ObsManager obsmanager_;

  bool dosr_;

  bool use_cholesky_;

  int nsamples_;
  int nsamples_node_;
  int ninitsamples_;
  int ndiscardedsamples_;
  int niter_opt_;

  std::complex<double> elocmean_;
  double elocvar_;
  int npar_;

 public:
  // JSON constructor
  VariationalMonteCarlo(Hamiltonian &ham, Sampler<Machine<GsType>> &sampler,
                        Optimizer &opt, const json &pars)
      : ham_(ham),
        sampler_(sampler),
        psi_(sampler.Psi()),
        opt_(opt),
        obs_(Observable::FromJson(ham.GetHilbert(), pars)),
        elocvar_(0.) {
    // DEPRECATED (to remove for v2.0.0)
    if (FieldExists(pars, "Learning")) {
      auto pars1 = pars;
      pars1["GroundState"] = pars["Learning"];
      Init(pars1);
    } else {
      Init(pars);
    }
    InitOutput(pars);
  }

  void InitOutput(const json &pars) {
    // DEPRECATED (to remove for v2.0.0)
    auto pars_gs = FieldExists(pars, "GroundState") ? pars["GroundState"]
                                                    : pars["Learning"];
    if (mynode_ == 0) {
      output_ = JsonOutputWriter::FromJson(pars_gs);
    }
  }

  void Init(const json &pars) {
    npar_ = psi_.Npar();

    opt_.Init(psi_.GetParameters());

    grad_.resize(npar_);
    Okmean_.resize(npar_);

    setSrParameters();

    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    nsamples_ = FieldVal(pars["GroundState"], "Nsamples", "GroundState");

    nsamples_node_ = int(std::ceil(double(nsamples_) / double(totalnodes_)));

    ninitsamples_ =
        FieldOrDefaultVal(pars["GroundState"], "DiscardedSamplesOnInit", 0.);

    ndiscardedsamples_ = FieldOrDefaultVal(
        pars["GroundState"], "DiscardedSamples", 0.1 * nsamples_node_);

    niter_opt_ = FieldVal(pars["GroundState"], "NiterOpt", "GroundState");

    if (pars["GroundState"]["Method"] == "Gd") {
      dosr_ = false;
    } else {
      double diagshift =
          FieldOrDefaultVal(pars["GroundState"], "DiagShift", 0.01);
      bool rescale_shift =
          FieldOrDefaultVal(pars["GroundState"], "RescaleShift", false);
      bool use_iterative =
          FieldOrDefaultVal(pars["GroundState"], "UseIterative", false);
      use_cholesky_ =
          FieldOrDefaultVal(pars["GroundState"], "UseCholesky", true);
      setSrParameters(diagshift, rescale_shift, use_iterative);
    }

    if (dosr_) {
      InfoMessage() << "Using the Stochastic reconfiguration method"
                    << std::endl;

      if (use_iterative_) {
        InfoMessage() << "With iterative solver" << std::endl;
      } else {
        if (use_cholesky_) {
          InfoMessage() << "Using Cholesky decomposition" << std::endl;
        }
      }
    } else {
      InfoMessage() << "Using a gradient-descent based method" << std::endl;
    }

    InfoMessage() << "Variational Monte Carlo running on " << totalnodes_
                  << " processes" << std::endl;

    MPI_Barrier(MPI_COMM_WORLD);
  }

  void InitSweeps() {
    sampler_.Reset();

    for (int i = 0; i < ninitsamples_; i++) {
      sampler_.Sweep();
    }
  }

  void Sample() {
    sampler_.Reset();

    for (int i = 0; i < ndiscardedsamples_; i++) {
      sampler_.Sweep();
    }

    vsamp_.resize(nsamples_node_, psi_.Nvisible());

    for (int i = 0; i < nsamples_node_; i++) {
      sampler_.Sweep();
      vsamp_.row(i) = sampler_.Visible();
    }
  }

  void Gradient() {
    obsmanager_.Reset("Energy");
    obsmanager_.Reset("EnergyVariance");

    for (const auto &ob : obs_) {
      obsmanager_.Reset(ob.Name());
    }

    const int nsamp = vsamp_.rows();
    elocs_.resize(nsamp);
    Ok_.resize(nsamp, psi_.Npar());

    for (int i = 0; i < nsamp; i++) {
      elocs_(i) = Eloc(vsamp_.row(i));
      Ok_.row(i) = psi_.DerLog(vsamp_.row(i));
      obsmanager_.Push("Energy", elocs_(i).real());

      for (const auto &ob : obs_) {
        obsmanager_.Push(ob.Name(), ObSamp(ob, vsamp_.row(i)));
      }
    }

    elocmean_ = elocs_.mean();
    SumOnNodes(elocmean_);
    elocmean_ /= double(totalnodes_);

    Okmean_ = Ok_.colwise().mean();
    SumOnNodes(Okmean_);
    Okmean_ /= double(totalnodes_);

    Ok_ = Ok_.rowwise() - Okmean_.transpose();

    elocs_ -= elocmean_ * Eigen::VectorXd::Ones(nsamp);

    for (int i = 0; i < nsamp; i++) {
      obsmanager_.Push("EnergyVariance", std::norm(elocs_(i)));
    }

    grad_ = 2. * (Ok_.adjoint() * elocs_);

    // Summing the gradient over the nodes
    SumOnNodes(grad_);
    grad_ /= double(totalnodes_ * nsamp);
  }

  std::complex<double> Eloc(const Eigen::VectorXd &v) {
    ham_.FindConn(v, mel_, connectors_, newconfs_);

    assert(connectors_.size() == mel_.size());

    auto logvaldiffs = (psi_.LogValDiff(v, connectors_, newconfs_));

    assert(mel_.size() == std::size_t(logvaldiffs.size()));

    std::complex<double> eloc = 0;

    for (int i = 0; i < logvaldiffs.size(); i++) {
      eloc += mel_[i] * std::exp(logvaldiffs(i));
    }

    return eloc;
  }

  double ObSamp(const Observable &ob, const Eigen::VectorXd &v) {
    ob.FindConn(v, mel_, connectors_, newconfs_);

    assert(connectors_.size() == mel_.size());

    auto logvaldiffs = (psi_.LogValDiff(v, connectors_, newconfs_));

    assert(mel_.size() == std::size_t(logvaldiffs.size()));

    std::complex<double> obval = 0;

    for (int i = 0; i < logvaldiffs.size(); i++) {
      obval += mel_[i] * std::exp(logvaldiffs(i));
    }

    return obval.real();
  }

  double ElocMean() { return elocmean_.real(); }

  double Elocvar() { return elocvar_; }

  void Run() {
    opt_.Reset();

    InitSweeps();

    for (int i = 0; i < niter_opt_; i++) {
      Sample();

      Gradient();

      UpdateParameters();

      PrintOutput(i);
    }
  }

  void UpdateParameters() {
    auto pars = psi_.GetParameters();

    if (dosr_) {
      const int nsamp = vsamp_.rows();

      Eigen::VectorXcd b = Ok_.adjoint() * elocs_;
      SumOnNodes(b);
      b /= double(nsamp * totalnodes_);

      if (!use_iterative_) {
        // Explicit construction of the S matrix
        Eigen::MatrixXcd S = Ok_.adjoint() * Ok_;
        SumOnNodes(S);
        S /= double(nsamp * totalnodes_);

        // Adding diagonal shift
        S += Eigen::MatrixXd::Identity(pars.size(), pars.size()) *
             sr_diag_shift_;

        Eigen::VectorXcd deltaP;
        if (use_cholesky_ == false) {
          Eigen::FullPivHouseholderQR<Eigen::MatrixXcd> qr(S.rows(), S.cols());
          qr.setThreshold(1.0e-6);
          qr.compute(S);
          deltaP = qr.solve(b);
        } else {
          Eigen::LLT<Eigen::MatrixXcd> llt(S.rows());
          llt.compute(S);
          deltaP = llt.solve(b);
        }
        // Eigen::VectorXcd deltaP=S.jacobiSvd(ComputeThinU |
        // ComputeThinV).solve(b);

        assert(deltaP.size() == grad_.size());
        grad_ = deltaP;

        if (sr_rescale_shift_) {
          std::complex<double> nor = (deltaP.dot(S * deltaP));
          grad_ /= std::sqrt(nor.real());
        }

      } else {
        Eigen::ConjugateGradient<MatrixReplacement, Eigen::Lower | Eigen::Upper,
                                 Eigen::IdentityPreconditioner>
            it_solver;
        // Eigen::GMRES<MatrixReplacement, Eigen::IdentityPreconditioner>
        // it_solver;
        it_solver.setTolerance(1.0e-3);
        MatrixReplacement S;
        S.attachMatrix(Ok_);
        S.setShift(sr_diag_shift_);
        S.setScale(1. / double(nsamp * totalnodes_));

        it_solver.compute(S);
        auto deltaP = it_solver.solve(b);

        grad_ = deltaP;
        if (sr_rescale_shift_) {
          auto nor = deltaP.dot(S * deltaP);
          grad_ /= std::sqrt(nor.real());
        }

        // if(mynode_==0){
        //   std::cerr<<it_solver.iterations()<<"
        //   "<<it_solver.error()<<std::endl;
        // }
        MPI_Barrier(MPI_COMM_WORLD);
      }
    }

    opt_.Update(grad_, pars);

    SendToAll(pars);

    psi_.SetParameters(pars);
    MPI_Barrier(MPI_COMM_WORLD);
  }

  void PrintOutput(int i) {
    // Note: This has to be called in all MPI processes, because converting
    // the ObsManager to JSON performs a MPI reduction.
    auto obs_data = json(obsmanager_);
    obs_data["Acceptance"] = sampler_.Acceptance();

    if (output_.has_value()) {  // output_.has_value() iff the MPI rank is 0, so
                                // the output is only written once
      output_->WriteLog(i, obs_data);
      output_->WriteState(i, psi_);
    }
    MPI_Barrier(MPI_COMM_WORLD);
  }

  void setSrParameters(double diagshift = 0.01, bool rescale_shift = false,
                       bool use_iterative = false) {
    sr_diag_shift_ = diagshift;
    sr_rescale_shift_ = rescale_shift;
    use_iterative_ = use_iterative;
    dosr_ = true;
  }
};

}  // namespace netket

#endif


#include "self_msgs_and_srvs/GlbObsRcv.h"

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/kdtree.hpp>

#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <math.h>
#include <random>

using namespace std;
using namespace Eigen;

ros::Publisher _all_map_pub;
ros::Subscriber _odom_sub;

int _obs_num, _cir_num;
double _x_size, _y_size, _z_size, _init_x, _init_y, _resolution, _sense_rate;
double _x_l, _x_h, _y_l, _y_h, _w_l, _w_h, _h_l, _h_h, _w_c_l, _w_c_h;

bool _has_map = false;

sensor_msgs::PointCloud2 globalMap_pcd;
pcl::PointCloud<pcl::PointXYZ> cloudMap;

pcl::search::KdTree<pcl::PointXYZ> kdtreeMap;
vector<int> pointIdxSearch;
vector<float> pointSquaredDistance;

void RandomMapGenerate()
{
   random_device rd;
   default_random_engine eng(rd());

   uniform_real_distribution<double> rand_theta = uniform_real_distribution<double>(-M_PI, M_PI);

   uniform_real_distribution<double> rand_x = uniform_real_distribution<double>(_x_l, _x_h);
   uniform_real_distribution<double> rand_y = uniform_real_distribution<double>(_y_l, _y_h);
   uniform_real_distribution<double> rand_w = uniform_real_distribution<double>(_w_l, _w_h);
   uniform_real_distribution<double> rand_h = uniform_real_distribution<double>(_h_l, _h_h);

   uniform_real_distribution<double> rand_x_circle = uniform_real_distribution<double>(_x_l + 1.0, _x_h - 1.0);
   uniform_real_distribution<double> rand_y_circle = uniform_real_distribution<double>(_y_l + 1.0, _y_h - 1.0);
   uniform_real_distribution<double> rand_r_circle = uniform_real_distribution<double>(_w_c_l, _w_c_h);

   uniform_real_distribution<double> rand_roll = uniform_real_distribution<double>(-M_PI, +M_PI);
   uniform_real_distribution<double> rand_pitch = uniform_real_distribution<double>(+M_PI / 4.0, +M_PI / 2.0);
   uniform_real_distribution<double> rand_yaw = uniform_real_distribution<double>(+M_PI / 4.0, +M_PI / 2.0);
   uniform_real_distribution<double> rand_ellipse_c = uniform_real_distribution<double>(0.5, 2.0);
   uniform_real_distribution<double> rand_num = uniform_real_distribution<double>(0.0, 1.0);

   pcl::PointXYZ pt_random;

   int base2(2), base3(3), base4(4); //Halton base
   // firstly, we put some circles
   for (int i = 0; i < _cir_num; i++)
   {
      double x0, y0, z0, R;
      std::vector<Vector3d> circle_set;

      // x0 = rand_x_circle(eng);
      // y0 = rand_y_circle(eng);
      z0 = rand_h(eng);

      //Halton sequence for x(0, 1)
      double f = 1;
      x0 = 0;
      int ii = i;
      while (ii > 0)
      {
         f = f / base2;
         x0 = x0 + f * (ii % base2);
         ii = floor(ii / base2);
      }
      x0 *= _x_size;
      x0 -= _x_size / 2;

      //Halton sequence for y(0, 1)
      f = 1;
      y0 = 0;
      ii = i;
      while (ii > 0)
      {
         f = f / base3;
         y0 = y0 + f * (ii % base3);
         ii = floor(ii / base3);
      }
      y0 *= _y_size;
      y0 -= _y_size / 2;

      R = rand_r_circle(eng);

      if (sqrt(pow(x0 - _init_x, 2) + pow(y0 - _init_y, 2)) < 1.5)
         continue;

      double a, b;
      a = rand_ellipse_c(eng);
      b = rand_ellipse_c(eng);

      double x, y, z;
      Vector3d pt3, pt3_rot;
      for (double theta = -M_PI; theta < M_PI; theta += 0.025)
      {
         x = a * cos(theta) * R;
         y = b * sin(theta) * R;
         z = 0;
         pt3 << x, y, z;
         circle_set.push_back(pt3);
      }
      // Define a random 3d rotation matrix
      Matrix3d Rot;
      double roll, pitch, yaw;
      double alpha, beta, gama;
      roll = rand_roll(eng);   // alpha
      pitch = rand_pitch(eng); // beta
      yaw = rand_yaw(eng);     // gama

      alpha = roll;
      beta = pitch;
      gama = yaw;

      double p = rand_num(eng);
      if (p < 0.5)
      {
         beta = M_PI / 2.0;
         gama = M_PI / 2.0;
      }

      Rot << cos(alpha) * cos(gama) - cos(beta) * sin(alpha) * sin(gama), -cos(beta) * cos(gama) * sin(alpha) - cos(alpha) * sin(gama), sin(alpha) * sin(beta),
          cos(gama) * sin(alpha) + cos(alpha) * cos(beta) * sin(gama), cos(alpha) * cos(beta) * cos(gama) - sin(alpha) * sin(gama), -cos(alpha) * sin(beta),
          sin(beta) * sin(gama), cos(gama) * sin(beta), cos(beta);

      for (auto pt : circle_set)
      {
         pt3_rot = Rot * pt;
         pt_random.x = pt3_rot(0) + x0 + 0.001;
         pt_random.y = pt3_rot(1) + y0 + 0.001;
         pt_random.z = pt3_rot(2) + z0 + 0.001 - 1;

         if (pt_random.z >= 0.0)
            cloudMap.points.push_back(pt_random);
      }
   }

   bool is_kdtree_empty = false;
   if (cloudMap.points.size() > 0)
      kdtreeMap.setInputCloud(cloudMap.makeShared());
   else
      is_kdtree_empty = true;

   // then, we put some pilar
   for (int i = 0; i < _obs_num; i++)
   {
      double x, y, w, h;
      // x    = rand_x(eng);
      // y    = rand_y(eng);
      w = rand_w(eng);

      //Halton sequence for x(0, 1)
      double f = 1;
      x = 0;
      int ii = i;
      while (ii > 0)
      {
         f = f / base2;
         x = x + f * (ii % base2);
         ii = floor(ii / base2);
      }
      x *= _x_size;
      x -= _x_size / 2;

      //Halton sequence for y(0, 1)
      f = 1;
      y = 0;
      ii = i;
      while (ii > 0)
      {
         f = f / base3;
         y = y + f * (ii % base3);
         ii = floor(ii / base3);
      }
      y *= _y_size;
      y -= _y_size / 2;

      double d_theta = rand_theta(eng);

      if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 2.0)
         continue;

      pcl::PointXYZ searchPoint(x, y, (_h_l + _h_h) / 2.0);
      pointIdxSearch.clear();
      pointSquaredDistance.clear();

      if (is_kdtree_empty == false)
      {
         if (kdtreeMap.nearestKSearch(searchPoint, 1, pointIdxSearch, pointSquaredDistance) > 0)
         {
            if (sqrt(pointSquaredDistance[0]) < 1.0)
               continue;
         }
      }

      x = floor(x / _resolution) * _resolution + _resolution / 2.0;
      y = floor(y / _resolution) * _resolution + _resolution / 2.0;

      int widNum = ceil(w / _resolution);
      int halfWidNum = widNum / 2.0;
      for (int r = -halfWidNum; r < halfWidNum; r++)
      {
         for (int s = -halfWidNum; s < halfWidNum; s++)
         {
            // make pilars hollow
            if (r > -halfWidNum + 2 && r < (halfWidNum - 3))
            {
               if (s > -halfWidNum + 2 && s < (halfWidNum - 3))
               {
                  continue;
               }
            }
            // rotate
            double th = atan2((double)s, (double)r);
            int len = sqrt(s * s + r * r);
            th += d_theta;
            int rr = cos(th) * len;
            int ss = sin(th) * len;

            h = rand_h(eng);
            int heiNum = 2.0 * ceil(h / _resolution);
            for (int t = 0; t < heiNum; t++)
            {
               pt_random.x = x + (rr + 0.0) * _resolution + 0.001;
               pt_random.y = y + (ss + 0.0) * _resolution + 0.001;
               pt_random.z = (t + 0.0) * _resolution * 0.5 - 1.0 + 0.001;
               cloudMap.points.push_back(pt_random);
            }
         }
      }
   }

   cloudMap.width = cloudMap.points.size();
   cloudMap.height = 1;
   cloudMap.is_dense = true;

   _has_map = true;

   pcl::toROSMsg(cloudMap, globalMap_pcd);
   globalMap_pcd.header.frame_id = "map";
}

void pubSensedPoints()
{
   if (!_has_map)
      return;
   _all_map_pub.publish(globalMap_pcd);
}

bool pubGlbObs(self_msgs_and_srvs::GlbObsRcv::Request &req, self_msgs_and_srvs::GlbObsRcv::Response &res)
{
   pubSensedPoints();
   return true;
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "random_map_sensing");
   ros::NodeHandle n("~");

   _all_map_pub = n.advertise<sensor_msgs::PointCloud2>("all_map", 1);

   n.param("init_state_x", _init_x, 0.0);
   n.param("init_state_y", _init_y, 0.0);

   n.param("map/x_size", _x_size, 50.0);
   n.param("map/y_size", _y_size, 50.0);
   n.param("map/z_size", _z_size, 5.0);

   n.param("map/obs_num", _obs_num, 30);
   n.param("map/circle_num", _cir_num, 30);
   n.param("map/resolution", _resolution, 0.2);

   n.param("ObstacleShape/lower_rad", _w_l, 0.3);
   n.param("ObstacleShape/upper_rad", _w_h, 0.8);
   n.param("ObstacleShape/lower_hei", _h_l, 3.0);
   n.param("ObstacleShape/upper_hei", _h_h, 7.0);

   n.param("CircleShape/lower_circle_rad", _w_c_l, 0.3);
   n.param("CircleShape/upper_circle_rad", _w_c_h, 0.8);

   n.param("sensing/rate", _sense_rate, 1.0);

   _x_l = -_x_size / 2.0;
   _x_h = +_x_size / 2.0;

   _y_l = -_y_size / 2.0;
   _y_h = +_y_size / 2.0;

   RandomMapGenerate();
   //only pub map pointcloud on request
   ros::ServiceServer pub_glb_obs_service = n.advertiseService("/pub_glb_obs", pubGlbObs);
   ros::spin();

   // ros::Rate loop_rate(_sense_rate);
   // while (ros::ok())
   // {
   //    ros::spinOnce();
   //    loop_rate.sleep();
   // }
}


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

#include "Open3D/Geometry/TriangleMesh.h"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <algorithm>

#include "Open3D/Utility/Console.h"

namespace open3d {
namespace geometry {

std::shared_ptr<TriangleMesh> TriangleMesh::DeformAsRigidAsPossible(
        const std::vector<int> &constraint_vertex_indices,
        const std::vector<Eigen::Vector3d> &constraint_vertex_positions,
        size_t max_iter) const {
    auto prime = std::make_shared<TriangleMesh>();
    prime->vertices_ = this->vertices_;
    prime->triangles_ = this->triangles_;

    utility::LogDebug("[DeformAsRigidAsPossible] setting up S'");
    prime->ComputeAdjacencyList();
    auto edges_to_vertices = prime->GetEdgeToVerticesMap();
    auto edge_weights =
            prime->ComputeEdgeWeightsCot(edges_to_vertices, /*min_weight=*/0);
    std::vector<Eigen::Matrix3d> Rs(vertices_.size());
    utility::LogDebug("[DeformAsRigidAsPossible] done setting up S'");

    std::unordered_map<int, Eigen::Vector3d> constraints;
    for (size_t idx = 0; idx < constraint_vertex_indices.size() &&
                         idx < constraint_vertex_positions.size();
         ++idx) {
        constraints[constraint_vertex_indices[idx]] =
                constraint_vertex_positions[idx];
    }

    // Build system matrix L and its solver
    utility::LogDebug("[DeformAsRigidAsPossible] setting up system matrix L");
    std::vector<Eigen::Triplet<double>> triplets;
    for (int i = 0; i < int(vertices_.size()); ++i) {
        if (constraints.count(i) > 0) {
            triplets.push_back(Eigen::Triplet<double>(i, i, 1));
        } else {
            double W = 0;
            for (int j : prime->adjacency_list_[i]) {
                double w = edge_weights[GetOrderedEdge(i, j)];
                triplets.push_back(Eigen::Triplet<double>(i, j, -w));
                W += w;
            }
            if (W > 0) {
                triplets.push_back(Eigen::Triplet<double>(i, i, W));
            }
        }
    }
    Eigen::SparseMatrix<double> L(vertices_.size(), vertices_.size());
    L.setFromTriplets(triplets.begin(), triplets.end());
    utility::LogDebug(
            "[DeformAsRigidAsPossible] done setting up system matrix L");

    utility::LogDebug("[DeformAsRigidAsPossible] setting up sparse solver");
    Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
    // Eigen::SuperLU<Eigen::SparseMatrix<double>> solver;
    solver.analyzePattern(L);
    solver.factorize(L);
    if (solver.info() != Eigen::Success) {
        utility::LogError(
                "[DeformAsRigidAsPossible] Failed to build solver (factorize)");
    } else {
        utility::LogDebug(
                "[DeformAsRigidAsPossible] done setting up sparse solver");
    }

    std::vector<Eigen::VectorXd> b = {Eigen::VectorXd(vertices_.size()),
                                      Eigen::VectorXd(vertices_.size()),
                                      Eigen::VectorXd(vertices_.size())};
    for (size_t iter = 0; iter < max_iter; ++iter) {
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
        for (int i = 0; i < int(vertices_.size()); ++i) {
            // Update rotations
            Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
            for (int j : prime->adjacency_list_[i]) {
                Eigen::Vector3d e0 = vertices_[i] - vertices_[j];
                Eigen::Vector3d e1 = prime->vertices_[i] - prime->vertices_[j];
                double w = edge_weights[GetOrderedEdge(i, j)];
                S += w * (e0 * e1.transpose());
            }
            Eigen::JacobiSVD<Eigen::Matrix3d> svd(
                    S, Eigen::ComputeFullU | Eigen::ComputeFullV);
            Eigen::Matrix3d U = svd.matrixU();
            Eigen::Matrix3d V = svd.matrixV();
            Eigen::Vector3d D(1, 1, (V * U.transpose()).determinant());
            // ensure rotation:
            // http://graphics.stanford.edu/~smr/ICP/comparison/eggert_comparison_mva97.pdf
            Rs[i] = V * D.asDiagonal() * U.transpose();
            if (Rs[i].determinant() <= 0) {
                utility::LogError(
                        "[DeformAsRigidAsPossible] something went wrong with "
                        "updateing R");
            }
        }

#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
        for (int i = 0; i < int(vertices_.size()); ++i) {
            // Update Positions
            Eigen::Vector3d bi(0, 0, 0);
            if (constraints.count(i) > 0) {
                bi = constraints[i];
            } else {
                for (int j : prime->adjacency_list_[i]) {
                    double w = edge_weights[GetOrderedEdge(i, j)];
                    bi += w / 2 *
                          ((Rs[i] + Rs[j]) * (vertices_[i] - vertices_[j]));
                }
            }
            b[0](i) = bi(0);
            b[1](i) = bi(1);
            b[2](i) = bi(2);
        }
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
        for (int comp = 0; comp < 3; ++comp) {
            Eigen::VectorXd p_prime = solver.solve(b[comp]);
            if (solver.info() != Eigen::Success) {
                utility::LogError(
                        "[DeformAsRigidAsPossible] Cholesky solve failed");
            }
            for (int i = 0; i < int(vertices_.size()); ++i) {
                prime->vertices_[i](comp) = p_prime(i);
            }
        }

        // Compute energy and log
        double energy = 0;
        for (int i = 0; i < int(vertices_.size()); ++i) {
            for (int j : prime->adjacency_list_[i]) {
                double w = edge_weights[GetOrderedEdge(i, j)];
                Eigen::Vector3d e0 = vertices_[i] - vertices_[j];
                Eigen::Vector3d e1 = prime->vertices_[i] - prime->vertices_[j];
                Eigen::Vector3d diff = e1 - Rs[i] * e0;
                energy += w * diff.squaredNorm();
            }
        }
        utility::LogDebug("[DeformAsRigidAsPossible] iter={}, energy={:e}",
                          iter, energy);
    }

    return prime;
}

}  // namespace geometry
}  // namespace open3d


#include <iostream>

#include <El.hpp>
#include <boost/mpi.hpp>
#include <boost/format.hpp>

#include "../utility/typer.hpp"

#define SKYLARK_NO_ANY
#include <skylark.hpp>

/*******************************************/
namespace bmpi =  boost::mpi;
namespace skybase = skylark::base;
namespace skysk =  skylark::sketch;
namespace skynla = skylark::nla;
namespace skyalg = skylark::algorithms;
namespace skyutil = skylark::utility;
/*******************************************/

// Parameters
#if 0
const int m = 2000;
const int n = 10;
const int t = 500;
#else
const int m = 30000;
const int n = 500;
const int t = 2000;
#endif

typedef El::DistMatrix<double, El::VC, El::STAR> matrix_type;
typedef El::DistMatrix<double, El::VC, El::STAR> rhs_type;
typedef El::DistMatrix<double, El::STAR, El::STAR> sol_type;
typedef El::DistMatrix<double, El::STAR, El::STAR> sketch_type;
typedef El::DistMatrix<double, El::STAR, El::STAR> precond_type;

typedef skyalg::regression_problem_t<matrix_type,
                                     skyalg::linear_tag,
                                     skyalg::l2_tag,
                                     skyalg::no_reg_tag> regression_problem_type;

template<typename AlgTag>
struct exact_solver_type :
    public skyalg::regression_solver_t<
    regression_problem_type, rhs_type, sol_type, AlgTag> {

    typedef skyalg::regression_solver_t<
        regression_problem_type, rhs_type, sol_type, AlgTag> base_type;

    exact_solver_type(const regression_problem_type& problem) :
        base_type(problem) {

    }
};

// Just a temporary small example on using "computed matrices"
class cmatrix : public skybase::computed_matrix_t<matrix_type> {
    const matrix_type &_A;

public:
    cmatrix(const matrix_type& A) : _A(A) { };
    typedef typename skyutil::typer_t<matrix_type>::index_type index_type;

    index_type height() const { return _A.Height(); }
    index_type width() const { return _A.Width(); }

    void materialize(matrix_type& Z) const { Z = _A; }
    matrix_type materialize() const { matrix_type Z(_A); return Z; }
};

typedef skyalg::regression_problem_t<skybase::computed_matrix_t<matrix_type>,
                                     skyalg::linear_tag,
                                     skyalg::l2_tag,
                                     skyalg::no_reg_tag> regression_problem_type1;

template<typename AlgTag>
struct exact_solver_type1 :
    public skyalg::regression_solver_t<
    regression_problem_type1, rhs_type, sol_type, AlgTag> {

    typedef skyalg::regression_solver_t<
        regression_problem_type1, rhs_type, sol_type, AlgTag> base_type;

    exact_solver_type1(const regression_problem_type1& problem) :
        base_type(problem) {

    }
};

template<template <typename, typename> class TransformType >
struct accelerated_exact_solver_type_sb :
    public skyalg::accelerated_regression_solver_t<
    regression_problem_type, rhs_type, sol_type,
    skyalg::simplified_blendenpik_tag<TransformType,
                                      skyalg::svd_precond_tag> > {

    typedef  skyalg::accelerated_regression_solver_t<
        regression_problem_type, rhs_type, sol_type,
        skyalg::simplified_blendenpik_tag<TransformType,
                                          skyalg::svd_precond_tag > > base_type;

    accelerated_exact_solver_type_sb(const regression_problem_type& problem,
        skybase::context_t& context) :
        base_type(problem, context) {

    }
};

struct accelerated_exact_solver_type_blendenpik :
    public skyalg::accelerated_regression_solver_t<
    regression_problem_type, rhs_type, sol_type,
    skyalg::blendenpik_tag<skyalg::qr_precond_tag> > {

    typedef  skyalg::accelerated_regression_solver_t<
        regression_problem_type, rhs_type, sol_type,
        skyalg::blendenpik_tag<skyalg::qr_precond_tag > > base_type;

    accelerated_exact_solver_type_blendenpik(const regression_problem_type& problem,
        skybase::context_t& context) :
        base_type(problem, context) {

    }
};

struct accelerated_exact_solver_type_lsrn :
    public skyalg::accelerated_regression_solver_t<
    regression_problem_type, rhs_type, sol_type,
    skyalg::lsrn_tag<skyalg::svd_precond_tag> > {

    typedef  skyalg::accelerated_regression_solver_t<
        regression_problem_type, rhs_type, sol_type,
        skyalg::lsrn_tag<skyalg::svd_precond_tag > > base_type;

    accelerated_exact_solver_type_lsrn(const regression_problem_type& problem,
        skybase::context_t& context) :
        base_type(problem, context) {

    }
};

template<>
template<typename KT>
struct exact_solver_type< skyalg::iterative_l2_solver_tag<KT> >:
    public skyalg::regression_solver_t<
    regression_problem_type, rhs_type, sol_type,
    skyalg::iterative_l2_solver_tag<KT> > {

    typedef skyalg::regression_solver_t<
        regression_problem_type, rhs_type, sol_type,
        skyalg::iterative_l2_solver_tag<KT> > base_type;

    exact_solver_type(const regression_problem_type& problem,
        skyalg::krylov_iter_params_t iter_params) :
        base_type(problem, iter_params) {

    }

    exact_solver_type(const regression_problem_type& problem,
        const skyalg::inplace_precond_t<sol_type>& R,
        skyalg::krylov_iter_params_t iter_params) :
        base_type(problem, R, iter_params) {

    }

};

template<template <typename, typename> class TransformType >
struct sketched_solver_type :
    public skyalg::sketched_regression_solver_t<
    regression_problem_type, matrix_type, sol_type,
    skyalg::linear_tag,
    sketch_type,
    sketch_type,
    TransformType,
    skyalg::qr_l2_solver_tag> {

    typedef skyalg::sketched_regression_solver_t<
        regression_problem_type, matrix_type, sol_type,
        skyalg::linear_tag,
        sketch_type,
        sketch_type,
        TransformType,
        skyalg::qr_l2_solver_tag> base_type;

    sketched_solver_type(const regression_problem_type& problem,
        int sketch_size,
        skybase::context_t& context) :
        base_type(problem, sketch_size, context) {

    }

};

template<typename ProblemType, typename RhsType, typename SolType>
void check_solution(const ProblemType &pr, const RhsType &b, const SolType &x,
    const RhsType &r0,
    double &res, double &resAtr, double &resFac) {
    RhsType r(b);
    skybase::Gemv(El::NORMAL, -1.0, pr.input_matrix, x, 1.0, r);
    res = skybase::Nrm2(r);

    SolType Atr(x.Height(), x.Width(), x.Grid());
    skybase::Gemv(El::TRANSPOSE, 1.0, pr.input_matrix, r, 0.0, Atr);
    resAtr = skybase::Nrm2(Atr);

    skybase::Axpy(-1.0, r0, r);
    RhsType dr(b);
    skybase::Axpy(-1.0, r0, dr);
    resFac = skybase::Nrm2(r) / skybase::Nrm2(dr);
}

int main(int argc, char** argv) {
    double res, resAtr, resFac;

    El::Initialize(argc, argv);

    bmpi::communicator world;
    int rank = world.rank();

    skybase::context_t context(23234);

    // Setup problem and righthand side
    // Using Skylark's uniform generator (as opposed to Elemental's)
    // will insure the same A and b are generated regardless of the number
    // of processors.
    matrix_type A, b;
    skylark::base::UniformMatrix(A, m, n, context);
    skylark::base::UniformMatrix(b, m, 1, context);

    regression_problem_type problem(m, n, A);

    boost::mpi::timer timer;
    double telp;

    sol_type x(n,1);

    rhs_type r(b);

    // Using QR
    timer.restart();
    exact_solver_type<skyalg::qr_l2_solver_tag> exact_solver(problem);
    exact_solver.solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Exact (QR):\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << "\t\t\t\t\t\t\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
    double res_opt = res;

    skybase::Gemv(El::NORMAL, -1.0, problem.input_matrix, x, 1.0, r);

    // Using SNE (semi-normal equations)
    timer.restart();
    exact_solver_type<skyalg::sne_l2_solver_tag>(problem).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Exact (SNE):\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << "\t\t\t\t\t\t\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
    res_opt = res;

    // Again, using SNE, only with the computed interface (example; to be removed.)
    cmatrix CA(A);
    regression_problem_type1 problem1(m, n, CA);
    timer.restart();
    exact_solver_type1<skyalg::sne_l2_solver_tag>(problem1).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Exact (SNE) (COMPUTED):\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << "\t\t\t\t\t\t\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
    res_opt = res;

    // Using SVD
    timer.restart();
    exact_solver_type<skyalg::svd_l2_solver_tag>(problem).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Exact (SVD):\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << "\t\t\t\t\t\t\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
    res_opt = res;

    // Using LSQR
    skyalg::krylov_iter_params_t lsqrparams;
    lsqrparams.am_i_printing = rank == 0;
    lsqrparams.log_level = 0;
    timer.restart();
    exact_solver_type<
        skyalg::iterative_l2_solver_tag<
            skyalg::lsqr_tag > >(problem, lsqrparams)
        .solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Exact (LSQR):\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << "\t\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;

    // Using sketch-and-solve

#if 0
    timer.restart();
    sketched_solver_type<skysk::JLT_t>(problem, t, context).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Sketch-and-Solve (JLT):\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
#endif

    timer.restart();
    sketched_solver_type<skysk::CWT_t>(problem, t, context).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Sketch-and-Solve (CWT):\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;

#if SKYLARK_HAVE_FFTW || SKYLARK_HAVE_FFTWF
    timer.restart();
    sketched_solver_type<skysk::FJLT_t>(problem, t, context).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Sketch-and-Solve (FJLT):\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
#endif

    // Accelerate-using-sketching
#if 0
    timer.restart();
    accelerated_exact_solver_type_sb<skysk::JLT_t>(problem, context).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Simplified Blendenpik (JLT):\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
#endif

#if SKYLARK_HAVE_FFTW || SKYLARK_HAVE_FFTWF
    timer.restart();
    accelerated_exact_solver_type_sb<skysk::FJLT_t>(problem, context).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Simplified Blendenpik (FJLT):\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
#endif

    timer.restart();
    accelerated_exact_solver_type_sb<skysk::CWT_t>(problem, context).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Simplified Blendenpik (CWT):\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;

#if SKYLARK_HAVE_FFTW || SKYLARK_HAVE_FFTWF
    timer.restart();
    accelerated_exact_solver_type_blendenpik(problem, context).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Blendenpik:\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
#endif

    timer.restart();
    accelerated_exact_solver_type_lsrn(problem, context).solve(b, x);
    telp = timer.elapsed();
    check_solution(problem, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "LSRN:\t\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;

    return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
	The initial source for the model's creation came from the document of
	Z. Qawaqneh et al.: "Deep Convolutional Neural Network for Age Estimation
	based on VGG-Face Model". However, our research has led us to significant
	improvements in the CNN model, allowing us to estimate the age of a person
	outperforming the state-of-the-art results in terms of the exact accuracy
	and for 1-off accuracy.

	This model is thus an age predictor leveraging a ResNet-10 architecture and
	trained using a private dataset of about 110k different labelled images.
	During the training, we used an optimization and data augmentation pipeline
	and considered several sizes for the entry image.

	Finally, this age predictor model is provided for free by Cydral and is
	licensed under the Creative Commons Zero v1.0 Universal.
*/

#include "dlib/data_io.h"
#include "dlib/string.h"
#include <dlib/cmd_line_parser.h>
#include <dlib/image_transforms.h>
#include <dlib/dir_nav.h>
#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/image_processing/frontal_face_detector.h>

#include <iostream>
#include <iterator>

using namespace std;
using namespace dlib;

const char* VERSION = "1.0";

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

// This block of statements defines a Resnet-10 architecture for the age predictor.
// We will use 81 classes (0-80 years old) to predict the age of a face.
const unsigned long number_of_age_classes = 81;

// The resnet basic block.
template<
	int num_filters,
	template<typename> class BN,  // some kind of batch normalization or affine layer
	int stride,
	typename SUBNET
>
using basicblock = BN<con<num_filters, 3, 3, 1, 1, relu<BN<con<num_filters, 3, 3, stride, stride, SUBNET>>>>>;

// A residual making use of the skip layer mechanism.
template<
	template<int, template<typename> class, int, typename> class BLOCK,  // a basic block defined before
	int num_filters,
	template<typename> class BN,  // some kind of batch normalization or affine layer
	typename SUBNET
> // adds the block to the result of tag1 (the subnet)
using residual = add_prev1<BLOCK<num_filters, BN, 1, tag1<SUBNET>>>;

// A residual that does subsampling (we need to subsample the output of the subnet, too).
template<
	template<int, template<typename> class, int, typename> class BLOCK,  // a basic block defined before
	int num_filters,
	template<typename> class BN,
	typename SUBNET
>
using residual_down = add_prev2<avg_pool<2, 2, 2, 2, skip1<tag2<BLOCK<num_filters, BN, 2, tag1<SUBNET>>>>>>;

// Residual block with optional downsampling and batch normalization.
template<
	template<template<int, template<typename> class, int, typename> class, int, template<typename>class, typename> class RESIDUAL,
	template<int, template<typename> class, int, typename> class BLOCK,
	int num_filters,
	template<typename> class BN,
	typename SUBNET
>
using residual_block = relu<RESIDUAL<BLOCK, num_filters, BN, SUBNET>>;

template<int num_filters, typename SUBNET>
using aresbasicblock_down = residual_block<residual_down, basicblock, num_filters, affine, SUBNET>;

// Some useful definitions to design the affine versions for inference.
template<typename SUBNET> using aresbasicblock256 = residual_block<residual, basicblock, 256, affine, SUBNET>;
template<typename SUBNET> using aresbasicblock128 = residual_block<residual, basicblock, 128, affine, SUBNET>;
template<typename SUBNET> using aresbasicblock64  = residual_block<residual, basicblock, 64, affine, SUBNET>;

// Common input for standard resnets.
template<typename INPUT>
using aresnet_input = max_pool<3, 3, 2, 2, relu<affine<con<64, 7, 7, 2, 2, INPUT>>>>;

// Resnet-10 architecture for estimating.
template<typename SUBNET>
using aresnet10_level1 = aresbasicblock256<aresbasicblock_down<256, SUBNET>>;
template<typename SUBNET>
using aresnet10_level2 = aresbasicblock128<aresbasicblock_down<128, SUBNET>>;
template<typename SUBNET>
using aresnet10_level3 = aresbasicblock64<SUBNET>;
// The resnet 10 backbone.
template<typename INPUT>
using aresnet10_backbone = avg_pool_everything<
	aresnet10_level1<
	aresnet10_level2<
	aresnet10_level3<
	aresnet_input<INPUT>>>>>;

using apredictor_t = loss_multiclass_log<fc<number_of_age_classes, aresnet10_backbone<input_rgb_image>>>;

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

// Helper function to estimage the age
uint8_t get_estimated_age(matrix<float, 1, number_of_age_classes>& p, float& confidence)
{
	float estimated_age = (0.25f * p(0));
	confidence = p(0);

	for (uint16_t i = 1; i < number_of_age_classes; i++) {
		estimated_age += (static_cast<float>(i) * p(i));
		if (p(i) > confidence) confidence = p(i);
	}

	return std::lround(estimated_age);
}

int main(int argc, char** argv) try {
	// Use a parser to set parameters.
	command_line_parser parser;

	parser.add_option("h", "");
	parser.add_option("help", "Displays this information");
	parser.add_option("version", "Display version");
	parser.add_option("predict-age", "Predict the age of a person ./image.jpg", 1);
	parser.parse(argc, argv);

	parser.check_incompatible_options("help", "version");
	parser.check_incompatible_options("help", "predict-age");
	parser.check_incompatible_options("version", "predict-age");

	if (parser.option("help") || parser.option("h"))
	{
		cout << "Usage: ./dnn_age_predictor_v1_ex [options]\n";
		parser.print_options(cout);
		cout << endl << "Note:" << endl << "You will need two different models to predict the age.";
		cout << endl << "First, download and then decompress the age predictor model from: " << endl;
		cout << "http://dlib.net/files/dnn_age_predictor_v1.dat.bz2" << endl;
		cout << "Then, you also need the face landmarking model file from:" << endl;
		cout << "http://dlib.net/files/shape_predictor_5_face_landmarks.dat.bz2" << endl;
		cout << endl << endl;
		return EXIT_SUCCESS;
	}
	if (parser.option("version"))
	{
		cout << "dnn_age_predictor_v1_ex v" << VERSION
			 << "\nCompiled: " << __TIME__ << " " << __DATE__ << endl << endl;
		return EXIT_SUCCESS;
	}

	if (parser.option("predict-age"))
	{
		// Initialize networks.
		frontal_face_detector detector = get_frontal_face_detector();
		shape_predictor sp;
		deserialize("shape_predictor_5_face_landmarks.dat") >> sp;

		static const char* model_net_filename = "dnn_age_predictor_v1.dat";
		apredictor_t net;
		deserialize(model_net_filename) >> net;

		// Load the source image.
		matrix<rgb_pixel> in;
		load_image(in, parser.option("predict-age").argument(0));

		// Usea Softmax for the last layer to estimate the age.
		softmax<apredictor_t::subnet_type> snet;
		snet.subnet() = net.subnet();

		// Age prediction using machine learning.
		int32_t cur_face = 0;
		for (auto face : detector(in))
		{
			auto shape = sp(in, face);
			if (shape.num_parts())
			{
				float confidence;
				matrix<rgb_pixel> face_chip;
				extract_image_chip(in, get_face_chip_details(shape, 64), face_chip);
				matrix<float, 1, number_of_age_classes> p = mat(snet(face_chip));
				cout << "face#" << cur_face++ << " - age prediction: " << to_string(get_estimated_age(p, confidence)) << " years old";
				cout << std::fixed << std::setprecision(1) << " [" << (confidence * 100.0f) << "%]" << endl;
			}
		}
		return EXIT_SUCCESS;
	}
}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


#include "Collisions.h"
#include "Box.h"
#include "Points.h"

#define EIGEN_DONT_ALIGN_STATICALLY
#include <Eigen\Dense>

using namespace std;
using namespace Eigen;

void CD(const Mesh& mesh, const shared_ptr<Obstacles> obs, std::vector<std::shared_ptr<btc::Collision> > &cls)
{
	MatrixXd verts2(3, mesh.nodes.size());
	MatrixXi faces2(3, mesh.faces.size());
	//VectorXi EoLs(1, mesh.nodes.size());
	VectorXi EoLs;
	EoLs.resize(mesh.nodes.size());

	for (int i = 0; i < mesh.nodes.size(); i++) {
		verts2.col(i) = Vector3d(mesh.nodes[i]->x[0], mesh.nodes[i]->x[1], mesh.nodes[i]->x[2]);
		if (mesh.nodes[i]->EoL) EoLs(i) = 1;
	}
	for (int i = 0; i < mesh.faces.size(); i++) {
		faces2.col(i) = Vector3i(mesh.faces[i]->v[0]->node->index, mesh.faces[i]->v[1]->node->index, mesh.faces[i]->v[2]->node->index);
	}

	// Compute these first so they form the base of our collision list
	btc::pointTriCollision(cls, obs->cdthreshold, obs->points->pxyz, obs->points->norms, verts2, faces2, true);

	int c = cls.size();
	for (int b = 0; b < obs->num_boxes; b++) {
		vector<shared_ptr<btc::Collision> > clst;
		btc::boxTriCollision(clst, obs->cdthreshold, obs->boxes[b]->dim, obs->boxes[b]->E1, verts2, faces2, EoLs, false);
		cls.insert(cls.end(), clst.begin(), clst.end());
		// We need to augment the indices of the box geometry by the object number
		// TODO:: Internally?
		for (c; c < cls.size(); c++) {
			if (cls[c]->count1 == 1 && cls[c]->count2 == 3) {
				//cls[c]->verts1(0) = obs->points->num_points + (obs->boxes[b]->num_points * b) + cls[c]->verts1(0);
				cls[c]->verts1(0) = obs->points->num_points + (b* obs->boxes[b]->num_points) + (b* obs->boxes[b]->num_edges) + cls[c]->verts1(0);
			}
			for (int e = 0; e < cls[c]->edge1.size(); e++) {
				//cls[c]->edge1[e] = obs->points->num_points + (obs->boxes[b]->num_edges * b) + cls[c]->edge1[e];
				//cls[c]->edge1[e] = (obs->boxes[b]->num_edges * b) + cls[c]->edge1[e];
				cls[c]->edge1[e] = obs->points->num_points + (b* obs->boxes[b]->num_points) + (b* obs->boxes[b]->num_edges) + (obs->boxes[b]->num_points + cls[c]->edge1[e]);
			}
		}
	}

}

void CD2(const Mesh& mesh, const shared_ptr<Obstacles> obs, std::vector<std::shared_ptr<btc::Collision> > &cls)
{
	MatrixXd verts2(3, mesh.nodes.size());
	MatrixXi faces2(3, mesh.faces.size());
	VectorXi EoLs;
	EoLs.resize(mesh.nodes.size());

	for (int i = 0; i < mesh.nodes.size(); i++) {
		verts2.col(i) = Vector3d(mesh.nodes[i]->x[0], mesh.nodes[i]->x[1], mesh.nodes[i]->x[2]);
		if (mesh.nodes[i]->EoL) EoLs(i) = 1;
	}
	for (int i = 0; i < mesh.faces.size(); i++) {
		faces2.col(i) = Vector3i(mesh.faces[i]->v[0]->node->index, mesh.faces[i]->v[1]->node->index, mesh.faces[i]->v[2]->node->index);
	}

	// Compute these first so they form the base of our collision list
	btc::pointTriCollision(cls, obs->cdthreshold, obs->points->pxyz, obs->points->norms, verts2, faces2, false);

	for (int b = 0; b < obs->num_boxes; b++) {
		vector<shared_ptr<btc::Collision> > clst;
		btc::boxTriCollision(clst, obs->cdthreshold, obs->boxes[b]->dim, obs->boxes[b]->E1, verts2, faces2, EoLs, false);
		cls.insert(cls.end(), clst.begin(), clst.end());
	}
}


/* Boost test/overflow.cpp
 * test if extended precision exponent does not disturb interval computation
 *
 * Copyright 2002-2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/interval.hpp>
#include <boost/test/minimal.hpp>
#include "bugs.hpp"

template<class I>
void test_one(typename I::base_type x, typename I::base_type f) {
  I y = x;
  typename I::base_type g = 1 / f;
  const int nb = 10000;
  for(int i = 0; i < nb; i++) y *= f;
  for(int i = 0; i < nb; i++) y *= g;
  BOOST_CHECK(in(x, y));
# ifdef BOOST_BORLANDC
  ::detail::ignore_unused_variable_warning(nb);
# endif
}

template<class I>
void test() {
  test_one<I>(1., 25.);
  test_one<I>(1., 0.04);
  test_one<I>(-1., 25.);
  test_one<I>(-1., 0.04);
}

int test_main(int, char *[]) {
  test<boost::numeric::interval<float> >();
  test<boost::numeric::interval<double> >();
  //test<boost::numeric::interval<long double> >();
# ifdef BOOST_BORLANDC
  ::detail::ignore_warnings();
# endif
  return 0;
}


#include <stdexcept>
#include <boost/test/unit_test.hpp>
#include "circle.hpp"
#include "base-types.hpp"

const double EPSILON = 0.00000001;

BOOST_AUTO_TEST_SUITE(CircleTest)

  BOOST_AUTO_TEST_SUITE(Circle_CircleConstructor)

    BOOST_AUTO_TEST_CASE(Circle_NegativeNumberAsRadiusParameter_ExceptionThrown)
    {
      double invalidRadius = -4.0;
      double x = 9.8;
      double y = 1.4;
      BOOST_CHECK_THROW(samokhin::Circle(invalidRadius, {x, y}), std::invalid_argument);
    }

    BOOST_AUTO_TEST_CASE(Circle_PositiveNumberAsRadiusParameter_NoException)
    {
      double validRadius = 8.5;
      double x = 1.0;
      double y = 3.4;
      BOOST_CHECK_NO_THROW(samokhin::Circle(validRadius, {x, y}));
    }

    BOOST_AUTO_TEST_CASE(Circle_ZeroAsRadiusParameter_NoException)
    {
      double validRadius = 0.0;
      double x = 1.0;
      double y = 3.4;
      BOOST_CHECK_NO_THROW(samokhin::Circle(validRadius, {x, y}));
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Circle_Scale)

    BOOST_AUTO_TEST_CASE(Scale_NegativeNumberAsScaleCoefficient_ExceptionThrown)
    {
      double radius = 9.1;
      double x = 4.1;
      double y = 3.8;
      samokhin::Circle circle(radius, {x, y});
      double k = -4.0;
      BOOST_CHECK_THROW(circle.scale(k), std::invalid_argument);
    }

    BOOST_AUTO_TEST_CASE(Scale_PositiveNumberAsScaleCoefficient_NoException)
    {
      double radius = 9.3;
      double x = 5.2;
      double y = 1.2;
      samokhin::Circle circle(radius, {x, y});
      double k = 6.0;
      BOOST_CHECK_NO_THROW(circle.scale(k));
    }

    BOOST_AUTO_TEST_CASE(Scale_ZeroAsScaleCoefficient_NoException)
    {
      double radius = 9.3;
      double x = 5.2;
      double y = 1.2;
      samokhin::Circle circle(radius, {x, y});
      double k = 0.0;
      BOOST_CHECK_NO_THROW(circle.scale(k));
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Circle_Move_ByCords)

    BOOST_AUTO_TEST_CASE(Move_CirclesRectangleFrameAtMovingByCords_ConstantFrame)
    {
      double radius = 7.2;
      samokhin::point_t centre = {2.4, 9.6};
      samokhin::Circle circle(radius, centre);
      double dx = 3.0;
      double dy = 2.6;
      samokhin::rectangle_t frame = circle.getFrameRect();
      circle.move(dx, dy);

      samokhin::point_t newCentre = circle.getFrameRect().pos;
      samokhin::rectangle_t newFrame = circle.getFrameRect();

      BOOST_CHECK_CLOSE(frame.width, newFrame.width, EPSILON);
      BOOST_CHECK_CLOSE(frame.height, newFrame.height, EPSILON);

      BOOST_CHECK_CLOSE(newCentre.x, newFrame.pos.x, EPSILON);
      BOOST_CHECK_CLOSE(newCentre.y, newFrame.pos.y, EPSILON);
    }

    BOOST_AUTO_TEST_CASE(Move_CirclesAreaAtMovingByCords_ConstantArea)
    {
      double radius = 6.3;
      double x = 5.7;
      double y = 1.9;
      samokhin::Circle circle(radius, {x, y});
      double area = circle.getArea();
      double dx = 2.0;
      double dy = 9.1;
      circle.move(dx, dy);
      BOOST_CHECK_CLOSE(circle.getArea(), area, EPSILON);
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Circle_Move_AtPoint)

    BOOST_AUTO_TEST_CASE(Move_CirclesRectangleFrameAtMovingAtPoint_ConstantFrame)
    {
      double radius = 7.2;
      double x = 2.4;
      double y = -9.6;
      samokhin::Circle circle(radius, {x, y});
      samokhin::point_t newCentre = {3.0, 2.6};
      samokhin::rectangle_t frame = circle.getFrameRect();
      circle.move(newCentre);
      samokhin::rectangle_t newFrame = circle.getFrameRect();

      BOOST_CHECK_CLOSE(frame.width, newFrame.width, EPSILON);
      BOOST_CHECK_CLOSE(frame.height, newFrame.height, EPSILON);

      BOOST_CHECK_CLOSE(newCentre.x, newFrame.pos.x, EPSILON);
      BOOST_CHECK_CLOSE(newCentre.y, newFrame.pos.y, EPSILON);
    }

    BOOST_AUTO_TEST_CASE(Move_CirclesAreaAtMovingAtPoint_ConstantArea)
    {
      double radius = 6.3;
      double x = 5.7;
      double y = 1.9;
      samokhin::Circle circle(radius, {x, y});
      double area = circle.getArea();
      samokhin::point_t newCentre = {2.0, 9.1};
      circle.move(newCentre);
      BOOST_CHECK_CLOSE(circle.getArea(), area, EPSILON);
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Circle_Scale)

    BOOST_AUTO_TEST_CASE(Scale_CirclesArea_SquareAreaChange)
    {
      double radius = 6.3;
      double x = 5.7;
      double y = 1.9;
      samokhin::Circle circle(radius, {x, y});
      double area = circle.getArea();
      double k = 1.4;
      circle.scale(k);

      BOOST_CHECK_CLOSE(circle.getArea(), area * k * k, EPSILON);
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Circle_Rotate)

    BOOST_AUTO_TEST_CASE(Rotate_AnyAngleAsParameter_ConstantArea)
    {
      double radius = 2.3;
      double x = 3.1;
      double y = 6.9;
      samokhin::Circle circle(radius, {x, y});
      double area = circle.getArea();
      double angle = 33;
      circle.rotate(angle);

      BOOST_CHECK_CLOSE(circle.getArea(), area, EPSILON);
    }

    BOOST_AUTO_TEST_CASE(Rotate_AnyAngleAsParameter_ConstantCenter)
    {
      double radius = 1.3;
      double x = 5.4;
      double y = 1.1;
      samokhin::Circle circle(radius, {x, y});
      double angle = -33;

      samokhin::point_t center = circle.getCenter();
      circle.rotate(angle);
      samokhin::point_t newCenter = circle.getCenter();
      BOOST_CHECK_CLOSE(center.x, newCenter.x, EPSILON);
      BOOST_CHECK_CLOSE(center.y, newCenter.y, EPSILON);
    }

    BOOST_AUTO_TEST_CASE(Rotate_AnyAngleAsParameter_ConstantRadius)
    {
      double radius = 1.3;
      double x = 5.4;
      double y = 1.1;
      samokhin::Circle circle(radius, {x, y});
      double angle = 13;

      circle.rotate(angle);
      samokhin::rectangle_t frame = circle.getFrameRect();
      BOOST_CHECK_CLOSE(radius, frame.width / 2.0, EPSILON);
      BOOST_CHECK_CLOSE(radius, frame.height / 2.0, EPSILON);
    }

  BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2::meta::permute_view"

#include <boost/array.hpp>
#include <boost/fusion/adapted/array.hpp>

#include <nt2/sdk/meta/permute_view.hpp>

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

struct some_permutation
{
  template<class Size, class Index>
  struct apply : boost::mpl::size_t<Size::value - 1 - Index::value>
  {};
};

////////////////////////////////////////////////////////////////////////////////
// Test permute view
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(permute_view)
{
  using boost::array;
  using boost::is_same;
  using nt2::meta::permute_view;

  typedef boost::array<std::size_t,4> Seq;
  Seq sizes = {{5,2,4,3}};

  permute_view<Seq,some_permutation> permuted(sizes);

  NT2_TEST_EQUAL( boost::fusion::at_c<0>(sizes), boost::fusion::at_c<3>(permuted));
  NT2_TEST_EQUAL( boost::fusion::at_c<1>(sizes), boost::fusion::at_c<2>(permuted));
  NT2_TEST_EQUAL( boost::fusion::at_c<2>(sizes), boost::fusion::at_c<1>(permuted));
  NT2_TEST_EQUAL( boost::fusion::at_c<3>(sizes), boost::fusion::at_c<0>(permuted));

}


#include <napi.h>
#include <sstream>

#include <Eigen/Dense>

void console_log(Napi::Env env, std::initializer_list<napi_value> const& values) {
    using Napi::Function;

    auto console = env.Global().Get("console").ToObject();
    auto log = console.Get("log").As<Function>();
    log.Call(console, values);
}

void run(Napi::CallbackInfo const& callback) {
    using Eigen::Matrix;
    using Napi::String;
    using std::ostringstream;
    using std::string;

    auto A = Matrix<int, 2, 1>::Random();
    auto B = Matrix<int, 1, 2>::Random();
    auto C = A * B;

    ostringstream os;
    os << C;
    string output = os.str();

    auto env = callback.Env();
    console_log(env, { String::New(env, output) });
}

Napi::Object init(Napi::Env env, Napi::Object exports) {
    using Napi::String;
    using Napi::Function;

    exports.Set(String::New(env, "run"), Function::New(env, run));
    return exports;
}

NODE_API_MODULE(NODE_GYP_MODULE_NAME, init);


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

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

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

/**
 * @file testGeneralSFMFactor_Cal3Bundler.cpp
 * @date Dec 27, 2010
 * @author nikai
 * @brief unit tests for GeneralSFMFactor
 */

#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/base/Testable.h>

#include <boost/shared_ptr.hpp>
#include <CppUnitLite/TestHarness.h>

#include <iostream>
#include <vector>

using namespace std;
using namespace gtsam;

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;

typedef PinholeCamera<Cal3Bundler> GeneralCamera;
typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef NonlinearEquality<GeneralCamera> CameraConstraint;
typedef NonlinearEquality<Point3> Point3Constraint;

/* ************************************************************************* */
class Graph: public NonlinearFactorGraph {
public:
  void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) {
    push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
  }

  void addCameraConstraint(int j, const GeneralCamera& p) {
    boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(X(j), p));
    push_back(factor);
  }

  void addPoint3Constraint(int j, const Point3& p) {
    boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(L(j), p));
    push_back(factor);
  }

};

static double getGaussian()
{
    double S,V1,V2;
    // Use Box-Muller method to create gauss noise from uniform noise
    do
    {
        double U1 = rand() / (double)(RAND_MAX);
        double U2 = rand() / (double)(RAND_MAX);
        V1 = 2 * U1 - 1;           /* V1=[-1,1] */
        V2 = 2 * U2 - 1;           /* V2=[-1,1] */
        S  = V1 * V1 + V2 * V2;
    } while(S>=1);
    return sqrt(-2.0f * (double)log(S) / S) * V1;
}

static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, equals )
{
  // Create two identical factors and make sure they're equal
  Vector z = Vector_(2,323.,240.);
  const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
  const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
  boost::shared_ptr<Projection>
    factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));

  boost::shared_ptr<Projection>
    factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));

  EXPECT(assert_equal(*factor1, *factor2));
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, error ) {
  Point2 z(3.,0.);
  const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
  boost::shared_ptr<Projection>
  factor(new Projection(z, sigma, X(1), L(1)));
  // For the following configuration, the factor predicts 320,240
  Values values;
  Rot3 R;
  Point3 t1(0,0,-6);
  Pose3 x1(R,t1);
  values.insert(X(1), GeneralCamera(x1));
  Point3 l1;  values.insert(L(1), l1);
  EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
}

static const double baseline = 5.0 ;

/* ************************************************************************* */
static vector<Point3> genPoint3() {
  const double z = 5;
  vector<Point3> landmarks ;
  landmarks.push_back(Point3 (-1.0,-1.0, z));
  landmarks.push_back(Point3 (-1.0, 1.0, z));
  landmarks.push_back(Point3 ( 1.0, 1.0, z));
  landmarks.push_back(Point3 ( 1.0,-1.0, z));
  landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z));
  landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z));
  landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z));
  landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z));
  landmarks.push_back(Point3 (-2.0,-2.0, 2*z));
  landmarks.push_back(Point3 (-2.0, 2.0, 2*z));
  landmarks.push_back(Point3 ( 2.0, 2.0, 2*z));
  landmarks.push_back(Point3 ( 2.0,-2.0, 2*z));
  return landmarks ;
}

static vector<GeneralCamera> genCameraDefaultCalibration() {
  vector<GeneralCamera> cameras ;
  cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0))));
  cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0))));
  return cameras ;
}

static vector<GeneralCamera> genCameraVariableCalibration() {
  const Cal3Bundler K(500,1e-3,1e-3);
  vector<GeneralCamera> cameras ;
  cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K));
  cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K));
  return cameras ;
}

static boost::shared_ptr<Ordering> getOrdering(const std::vector<GeneralCamera>& cameras, const std::vector<Point3>& landmarks) {
  boost::shared_ptr<Ordering> ordering(new Ordering);
  for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
  for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
  return ordering ;
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) {

  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraDefaultCalibration();

  // add measurement with noise
  Graph graph;
  for ( size_t j = 0 ; j < cameras.size() ; ++j) {
    for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
      Point2 pt = cameras[j].project(landmarks[i]) ;
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = cameras.size()*landmarks.size() ;

  // add initial
  const double noise = baseline*0.1;
  Values values;
  for ( size_t i = 0 ; i < cameras.size() ; ++i )
    values.insert(X(i), cameras[i]) ;

  for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
    Point3 pt(landmarks[i].x()+noise*getGaussian(),
              landmarks[i].y()+noise*getGaussian(),
              landmarks[i].z()+noise*getGaussian());
    values.insert(L(i), pt) ;
  }

  graph.addCameraConstraint(0, cameras[0]);

  // Create an ordering of the variables
  Ordering ordering = *getOrdering(cameras,landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) {
  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraVariableCalibration();
  // add measurement with noise
  Graph graph;
  for ( size_t j = 0 ; j < cameras.size() ; ++j) {
    for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
      Point2 pt = cameras[j].project(landmarks[i]) ;
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = cameras.size()*landmarks.size() ;

  // add initial
  const double noise = baseline*0.1;
  Values values;
  for ( size_t i = 0 ; i < cameras.size() ; ++i )
    values.insert(X(i), cameras[i]) ;

  // add noise only to the first landmark
  for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
    if ( i == 0 ) {
      Point3 pt(landmarks[i].x()+noise*getGaussian(),
                landmarks[i].y()+noise*getGaussian(),
                landmarks[i].z()+noise*getGaussian());
      values.insert(L(i), pt) ;
    }
    else {
      values.insert(L(i), landmarks[i]) ;
    }
  }

  graph.addCameraConstraint(0, cameras[0]);
  const double reproj_error = 1e-5;

  Ordering ordering = *getOrdering(cameras,landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) {

  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraVariableCalibration();

  // add measurement with noise
  const double noise = baseline*0.1;
  Graph graph;
  for ( size_t j = 0 ; j < cameras.size() ; ++j) {
    for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
      Point2 pt = cameras[j].project(landmarks[i]) ;
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = landmarks.size()*cameras.size();

  Values values;
  for ( size_t i = 0 ; i < cameras.size() ; ++i )
    values.insert(X(i), cameras[i]) ;

  for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
    Point3 pt(landmarks[i].x()+noise*getGaussian(),
              landmarks[i].y()+noise*getGaussian(),
              landmarks[i].z()+noise*getGaussian());
    //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
    values.insert(L(i), pt) ;
  }

  for ( size_t i = 0 ; i < cameras.size() ; ++i )
    graph.addCameraConstraint(i, cameras[i]);

  const double reproj_error = 1e-5 ;

  Ordering ordering = *getOrdering(cameras,landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {

  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraVariableCalibration();

  // add measurement with noise
  Graph graph;
  for ( size_t j = 0 ; j < cameras.size() ; ++j) {
    for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
      Point2 pt = cameras[j].project(landmarks[i]) ;
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = landmarks.size()*cameras.size();

  Values values;
  for ( size_t i = 0 ; i < cameras.size() ; ++i ) {
    const double
      rot_noise = 1e-5, trans_noise = 1e-3,
      focal_noise = 1, distort_noise = 1e-3;
    if ( i == 0 ) {
      values.insert(X(i), cameras[i]) ;
    }
    else {

      Vector delta = Vector_(9,
          rot_noise, rot_noise, rot_noise, // rotation
          trans_noise, trans_noise, trans_noise, // translation
          focal_noise, distort_noise, distort_noise // f, k1, k2
          ) ;
      values.insert(X(i), cameras[i].retract(delta)) ;
    }
  }

  for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
    values.insert(L(i), landmarks[i]) ;
  }

  // fix X0 and all landmarks, allow only the cameras[1] to move
  graph.addCameraConstraint(0, cameras[0]);
  for ( size_t i = 0 ; i < landmarks.size() ; ++i )
    graph.addPoint3Constraint(i, landmarks[i]);

  const double reproj_error = 1e-5 ;

  Ordering ordering = *getOrdering(cameras,landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraVariableCalibration();

  // add measurement with noise
  Graph graph;
  for ( size_t j = 0 ; j < cameras.size() ; ++j) {
    for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
      Point2 pt = cameras[j].project(landmarks[i]) ;
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = cameras.size()*landmarks.size() ;

  // add initial
  const double noise = baseline*0.1;
  Values values;
  for ( size_t i = 0 ; i < cameras.size() ; ++i )
    values.insert(X(i), cameras[i]) ;

  // add noise only to the first landmark
  for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
    Point3 pt(landmarks[i].x()+noise*getGaussian(),
              landmarks[i].y()+noise*getGaussian(),
              landmarks[i].z()+noise*getGaussian());
    values.insert(L(i), pt) ;
  }

  // Constrain position of system with the first camera constrained to the origin
  graph.addCameraConstraint(0, cameras[0]);

  // Constrain the scale of the problem with a soft range factor of 1m between the cameras
  graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));

  const double reproj_error = 1e-5 ;

  Ordering ordering = *getOrdering(cameras,landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */