#include <iostream>
#include <vector>
#include <chrono>

//#define _RANSAC_STATS_ 1
#ifdef _RANSAC_STATS_
#include <limits>
#endif

#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/calib3d.hpp>

#include <opencv2/core/eigen.hpp>

#include "pose3d.h"
#include "Optimization.h"
#include "Ransac.hh"

#define USE_ROTATION_MATRIX
//#define USE_QUATERNION

#define USE_SVD
//#define USE_QR

namespace pose3d
{
   inline double mrhs1(double X_1, double Y_1, double Z_1, double y_1, double r_10, double r_11, double r_12,
                       double r_20, double r_21, double r_22)
//---------------------------------------------------------------------------------------------------------
   {
      return X_1 * r_10 - X_1 * r_20 * y_1 + Y_1 * r_11 - Y_1 * r_21 * y_1 + Z_1 * r_12 - Z_1 * r_22 * y_1;
   }

   inline double mrhs2(double X_1, double Y_1, double Z_1, double x_1,
                       double r_00, double r_01, double r_02, double r_20, double r_21, double r_22)
//------------------------------------------------------------------------------------------------
   {
      return -X_1 * r_00 + X_1 * r_20 * x_1 - Y_1 * r_01 + Y_1 * r_21 * x_1 - Z_1 * r_02 + Z_1 * r_22 * x_1;
   }

   inline double mrhs3(double X_1, double Y_1, double Z_1, double x_1, double y_1,
                       double r_00, double r_01, double r_02, double r_10, double r_11, double r_12)
//-----------------------------------------------------------------------------------------------
   {
      return X_1 * r_00 * y_1 - X_1 * r_10 * x_1 + Y_1 * r_01 * y_1 - Y_1 * r_11 * x_1 + Z_1 * r_02 * y_1 -
             Z_1 * r_12 * x_1;
   }

   inline double qrhs1(double Qw, double Qx, double Qy, double Qz, double X_1, double Y_1, double Z_1, double y_1)
//-------------------------------------------------------------------------------------------------------------
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, Qwx = Qw * Qx;
      return -Qw2 * Z_1 * y_1 - 2 * Qwx * Y_1 * y_1 + 2 * Qw * Qy * X_1 * y_1 + Qx2 * Z_1 * y_1 -
             2 * Qx * Qz * X_1 * y_1 +
             Qy2 * Z_1 * y_1 - 2 * Qy * Qz * Y_1 * y_1 - Qz2 * Z_1 * y_1 + 2 * X_1 * (Qw * Qz + Qx * Qy) + Y_1 * (Qw2 -
                                                                                Qx2 +
                                                                                Qy2 -
                                                                                Qz2) -
             2 * Z_1 * (Qwx - Qy * Qz);
   }

   inline double qrhs2(double Qw, double Qx, double Qy, double Qz, double X_1, double Y_1, double Z_1, double x_1)
//------------------------------------------------------------------------------------------------------------
   {
      double Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, Qwy = Qw * Qy;
      return Qw * Qw * Z_1 * x_1 + 2 * Qw * Qx * Y_1 * x_1 - 2 * Qwy * X_1 * x_1 - Qx2 * Z_1 * x_1 +
             2 * Qx * Qz * X_1 * x_1 -
             Qy2 * Z_1 * x_1 + 2 * Qy * Qz * Y_1 * x_1 + Qz2 * Z_1 * x_1 - X_1 * (Qw * Qw + Qx2 -
                                                                                Qy2 - Qz2) -
             2 * Y_1 * (-Qw * Qz + Qx * Qy) - 2 * Z_1 * (Qwy + Qx * Qz);
   }

   inline double
   qrhs3(double Qw, double Qx, double Qy, double Qz, double X_1, double Y_1, double Z_1, double x_1, double y_1)
//-----------------------------------------------------------------------------------------------------------------
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, Qwz = Qw * Qz;
      return Qw2 * X_1 * y_1 - Qw2 * Y_1 * x_1 + 2 * Qw * Qx * Z_1 * x_1 + 2 * Qw * Qy * Z_1 * y_1 -
             2 * Qwz * X_1 * x_1 -
             2 * Qwz * Y_1 * y_1 + Qx2 * X_1 * y_1 + Qx2 * Y_1 * x_1 - 2 * Qx * Qy * X_1 * x_1 +
             2 * Qx * Qy * Y_1 * y_1 +
             2 * Qx * Qz * Z_1 * y_1 - Qy2 * X_1 * y_1 - Qy2 * Y_1 * x_1 - 2 * Qy * Qz * Z_1 * x_1 - Qz2 * X_1 * y_1 +
             Qz2 * Y_1 * x_1;
   }

   inline double dotg(double X_1, double Y_1, double Z_1, double X_2, double Y_2, double Z_2,
                      double g_x, double g_y, double g_z, const Eigen::Matrix3d& R)
   {
      return g_x * (X_1 * R(0, 0) - X_2 * R(0, 0) + Y_1 * R(0, 1) - Y_2 * R(0, 1) + Z_1 * R(0, 2) - Z_2 * R(0, 2)) +
             g_y * (X_1 * R(1, 0) - X_2 * R(1, 0) +
                    Y_1 * R(1, 1) - Y_2 * R(1, 1) + Z_1 * R(1, 2) - Z_2 * R(1, 2)) +
             g_z * (X_1 * R(2, 0) - X_2 * R(2, 0) + Y_1 * R(2, 1) - Y_2 * R(2, 1) +
                    Z_1 * R(2, 2) - Z_2 * R(2, 2));
   }

   inline Eigen::Quaterniond rotation(const Eigen::Vector3d &from, const Eigen::Vector3d &to,
                                      const Eigen::Vector3d &fallbackAxis = Eigen::Vector3d(0, 0, 0))
   //-----------------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond q;
      Eigen::Vector3d v0 = from;
      Eigen::Vector3d v1 = to;
      v0.normalize();
      v1.normalize();

      double d = v0.dot(v1);
      if (d >= 1.0f)
         return Eigen::Quaterniond(1, 0, 0, 0);

      if (d < (1e-6f - 1.0f))
      {
         if (fallbackAxis != Eigen::Vector3d(0, 0, 0))
            q = Eigen::AngleAxis<double>(PI, fallbackAxis);
         else
         {
            // Generate an axis
            Eigen::Vector3d axis = Eigen::Vector3d(1, 0, 0).cross(from);
            if (axis.norm() < 0.000000001) // pick another if colinear
               axis = Eigen::Vector3d(0, 1, 0).cross(from);
            axis.normalize();
            q = Eigen::AngleAxis<double>(PI, axis);
         }
      }
      else
      {
         double s = sqrt((1 + d) * 2);
         double invs = 1 / s;

         Eigen::Vector3d c = v0.cross(v1);

         q.x() = c.x() * invs;
         q.y() = c.y() * invs;
         q.z() = c.z() * invs;
         q.w() = s * 0.5f;
         q.normalize();
      }
      return q;
   }

#ifdef USE_ROTATION_MATRIX
   bool pose(const std::vector<std::pair<cv::Point3d, cv::Point2d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
             const Eigen::Matrix3d& KI, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
//-----------------------------------------------------------------------------------------------------------
   {
      const size_t m = pts.size();
//      if (m > 3)
//      {
//         std::cerr << "Use pose_ransac for more than 3 points" << std::endl;
//         // pose_ransac(world_pts, image_pts, train_g, query_g, KI, Q, translation);
//         return false;
//      }
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera

      Eigen::Matrix3d R = Q.toRotationMatrix();
      const cv::Point3d &world_pt0 = pts[0].first, &world_pt1 = pts[1].first,
                        &world_pt2 = pts[2].first;
      const cv::Point2d &image_pt0 = pts[0].second, &image_pt1 = pts[1].second,
                        &image_pt2 = pts[2].second;
      Eigen::Vector3d query_ray1 = KI*Eigen::Vector3d(image_pt0.x, image_pt0.y, 1),
                      query_ray2 = KI*Eigen::Vector3d(image_pt1.x, image_pt1.y, 1),
                      query_ray3 = KI*Eigen::Vector3d(image_pt2.x, image_pt2.y, 1);
      double Xt1 = world_pt0.x, Yt1 = world_pt0.y, Zt1 = world_pt0.z,
             Xt2 = world_pt1.x, Yt2 = world_pt1.y, Zt2 = world_pt1.z,
             Xt3 = world_pt2.x, Yt3 = world_pt2.y, Zt3 = world_pt2.z,
             xq1 = query_ray1[0], yq1 = query_ray1[1], xq2 = query_ray2[0], yq2 = query_ray2[1],
             xq3 = query_ray3[0], yq3 = query_ray3[1];

      Eigen::Matrix<double, 9, 3> A;
      Eigen::Matrix<double, 9, 1> b;
//   Eigen::Matrix<double, 6, 3> A;
//   Eigen::Matrix<double, 6, 1> b;
      A << 0, -1, yq1,
            1, 0, -xq1,
            -yq1, xq1, 0,
            0, -1, yq2,
            1, 0, -xq2,
            -yq2, xq2, 0,
            0, -1, yq3,
            1, 0, -xq3,
            -yq3, xq3, 0;
      double r_00 = R(0, 0), r_01 = R(0, 1), r_02 = R(0, 2),
             r_10 = R(1, 0), r_11 = R(1, 1), r_12 = R(1, 2),
             r_20 = R(2, 0), r_21 = R(2, 1), r_22 = R(2, 2);
      b <<  mrhs1(Xt1, Yt1, Zt1, yq1, r_10, r_11, r_12, r_20, r_21, r_22),
            mrhs2(Xt1, Yt1, Zt1, xq1, r_00, r_01, r_02, r_20, r_21, r_22),
            mrhs3(Xt1, Yt1, Zt1, xq1, yq1, r_00, r_01, r_02, r_10, r_11, r_12),
            mrhs1(Xt2, Yt2, Zt2, yq2, r_10, r_11, r_12, r_20, r_21, r_22),
            mrhs2(Xt2, Yt2, Zt2, xq2, r_00, r_01, r_02, r_20, r_21, r_22),
            mrhs3(Xt2, Yt2, Zt2, xq2, yq2, r_00, r_01, r_02, r_10, r_11, r_12),
            mrhs1(Xt3, Yt3, Zt3, yq3, r_10, r_11, r_12, r_20, r_21, r_22),
            mrhs2(Xt3, Yt3, Zt3, xq3, r_00, r_01, r_02, r_20, r_21, r_22),
            mrhs3(Xt3, Yt3, Zt3, xq3, yq3, r_00, r_01, r_02, r_10, r_11, r_12);
//      std::cout << A << std::endl << "Rank " << A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).rank() << std::endl;
#ifdef USE_SVD
      translation = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
#endif
#ifdef USE_QR
      Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 9, 3>> MQR(A);
      translation = MQR.solve(b);
#endif

//   Eigen::MatrixXd A(m*3, 3);
//   Eigen::VectorXd b(m*3);
//    for (size_t row=0, ri=0; row<m; row++)
//    {
//       if (row == 3)
//       {
//          std::cout << row << " " << ri << std::endl;
//          break;
//       }

//       cv::Point2d& pt = const_cast<cv::Point2d &>(image_pts[row]);
//       Eigen::Vector3d ray = KI*Eigen::Vector3d(pt.x, pt.y, 1);
//       const double Xt = world_pts[row].x, Yt = world_pts[row].y, Zt = world_pts[row].z, xq = ray[0], yq = ray[1];

//       A(ri, 0) = 0;
//       A(ri, 1) = -1;
//       A(ri, 2) = yq;
//       b[ri++] = mrhs1(Xt, Yt, Zt, yq, r_10, r_11,  r_12, r_20, r_21, r_22);

//       A(ri, 0) = 1.0;
//       A(ri, 1) = 0.0;
//       A(ri, 2) = -xq;
//       b[ri++] = mrhs2(Xt, Yt, Zt, xq, r_00, r_01, r_02, r_20, r_21, r_22);

//       A(ri, 0) = -yq;
//       A(ri, 1) = xq;
//       A(ri, 2) = 0;
//       b[ri++] = mrhs3(Xt, Yt, Zt, xq, yq, r_00, r_01, r_02, r_10, r_11, r_12);
//    }
// //   std::cout << A << std::endl << b << std::endl;
//    translation = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
      return true;
   }

#endif

#ifdef USE_QUATERNION
   bool pose(const std::vector<std::pair<cv::Point3d, cv::Point2d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
             const Eigen::Matrix3d& KI, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
   //-----------------------------------------------------------------------------------------------------------
   {
      const size_t m = pts.size();
      if (m > 3)
      {
         std::cout << "Use pose_ransac for more than 3 points" << std::endl;
         return false;
      }
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
   //   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      std::vector<cv::Point3d> world_pts;
      std::vector<cv::Point2d> image_pts;
      for (const std::pair<cv::Point3d, cv::Point2d>& pp : pts)
      {
         const cv::Point3d &wpt = pp.first;
         const cv::Point2d &ipt = pp.second;
         world_pts.emplace_back(wpt.x, wpt.y, wpt.z);
         image_pts.emplace_back(ipt.x, ipt.y);
      }
      pose3d::pose_translation(world_pts, image_pts, KI, Q, translation);
   /*
      Eigen::MatrixXd A(m*3, 3);
      Eigen::VectorXd b(m*3);
   //   Eigen::MatrixXd A(m*2, 3);
   //   Eigen::VectorXd b(m*2);
      const double Qw = Q.w(), Qx = Q.x(), Qy = Q.y(), Qz = Q.z();
      for (size_t row=0, ri=0; row<m; row++)
      {
         cv::Point2d& pt = const_cast<cv::Point2d &>(image_pts[row]);
         Eigen::Vector3d ray = KI*Eigen::Vector3d(pt.x, pt.y, 1);
         const double Xt = world_pts[row].x, Yt = world_pts[row].y, Zt = world_pts[row].z, xq = ray[0], yq = ray[1];

         A(ri, 0) = 0;
         A(ri, 1) = -1;
         A(ri, 2) = yq;
         b[ri++] = qrhs1(Qw, Qx, Qy, Qz, Xt, Yt, Zt, xq);

         A(ri, 0) = 1.0;
         A(ri, 1) = 0.0;
         A(ri, 2) = -xq;
         b[ri++] = qrhs2(Qw, Qx, Qy, Qz, Xt, Yt, Zt, xq);

         A(ri, 0) = -yq;
         A(ri, 1) = xq;
         A(ri, 2) = 0;
         b[ri++] = qrhs3(Qw, Qx, Qy, Qz, Xt, Yt, Zt, xq, yq);
      }
      translation = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
   */
      return true;
   }
#endif

   void refine(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& image_pts,
               const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation, int& iterations)
//---------------------------------------------------------------------------------------------------
   {
      translation_levenberg_marquardt3d(world_pts, image_pts, K, Q, translation, iterations);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point2d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const Eigen::Matrix3d& KI, Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void *RANSAC_params, int samples)
//----------------------------------------------------------------------------------------------------------
   {
      if (RANSAC_params == nullptr) throw std::logic_error("pose3d::pose_ransac: RANSAC params are null");
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
      //   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      double confidence = -1;
#ifdef USE_THEIA_RANSAC
      Grav3DRansacEstimator estimator(KI, Q, samples);
      theia::RansacParameters* parameters = static_cast<theia::RansacParameters*>(RANSAC_params);
      theia::RansacSummary summary;
      std::unique_ptr<theia::SampleConsensusEstimator<Grav3DRansacEstimator>> ransac =
            theia::CreateAndInitializeRansacVariant(theia::RansacType::RANSAC, *parameters, estimator);
      if (ransac)
      {
         GravPoseRansacModel best_model;
         ransac->Estimate(pts, &best_model, &summary);
         confidence = summary.confidence;
         if (confidence > 0)
         {
            Q = best_model.rotation;
            translation = best_model.translation;
         }
      }
#else
      templransac::RANSACParams* parameters = static_cast<templransac::RANSACParams*>(RANSAC_params);
      Grav3DRansacEstimator estimator(KI, Q);
      Grav3DRansacData data(pts);
      std::vector<std::pair<double, GravPoseRansacModel> > results;
      std::vector<std::vector<size_t>> inlier_indices;
      std::stringstream errs;
      confidence = templransac::RANSAC(*parameters, estimator, data, pts.size(), samples, 1, results,
                                        inlier_indices, &errs);
      if (confidence > 0)
      {
         GravPoseRansacModel& model = results[0].second;
         translation = model.translation;
      }
#endif
      return confidence;
   }

   //Called by RANSAC estimation
   void pose_translation(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& query_image_pts,
                         const Eigen::Matrix3d& KI, const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
   //-----------------------------------------------------------------------------------------------------------
   {
      const cv::Point2d &qpt0 = query_image_pts[0], &qpt1 = query_image_pts[1], &qpt2 = query_image_pts[2];
      Eigen::Vector3d query_ray1 = KI*Eigen::Vector3d(qpt0.x, qpt0.y, 1),
            query_ray2 = KI*Eigen::Vector3d(qpt1.x, qpt1.y, 1),
            query_ray3 = KI*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double Xt1 = world_pts[0].x, Yt1 = world_pts[0].y, Zt1 = world_pts[0].z,
            Xt2 = world_pts[1].x, Yt2 = world_pts[1].y, Zt2 = world_pts[1].z,
            Xt3 = world_pts[2].x, Yt3 = world_pts[2].y, Zt3 = world_pts[2].z,
            xq1 = query_ray1[0], yq1 = query_ray1[1], xq2 = query_ray2[0], yq2 = query_ray2[1],
            xq3 = query_ray3[0], yq3 = query_ray3[1];

      Eigen::Matrix<double, 9, 3> A;
      Eigen::Matrix<double, 9, 1> b;
//   Eigen::Matrix<double, 6, 3> A;
//   Eigen::Matrix<double, 6, 1> b;
      A << 0, -1, yq1,
            1, 0, -xq1,
            -yq1, xq1, 0,
            0, -1, yq2,
            1, 0, -xq2,
            -yq2, xq2, 0,
            0, -1, yq3,
            1, 0, -xq3,
            -yq3, xq3, 0;
      double r_00 = R(0, 0), r_01 = R(0, 1), r_02 = R(0, 2),
            r_10 = R(1, 0), r_11 = R(1, 1), r_12 = R(1, 2),
            r_20 = R(2, 0), r_21 = R(2, 1), r_22 = R(2, 2);
      b << mrhs1(Xt1, Yt1, Zt1, yq1, r_10, r_11, r_12, r_20, r_21, r_22),
            mrhs2(Xt1, Yt1, Zt1, xq1, r_00, r_01, r_02, r_20, r_21, r_22),
            mrhs3(Xt1, Yt1, Zt1, xq1, yq1, r_00, r_01, r_02, r_10, r_11, r_12),
            mrhs1(Xt2, Yt2, Zt2, yq2, r_10, r_11, r_12, r_20, r_21, r_22),
            mrhs2(Xt2, Yt2, Zt2, xq2, r_00, r_01, r_02, r_20, r_21, r_22),
            mrhs3(Xt2, Yt2, Zt2, xq2, yq2, r_00, r_01, r_02, r_10, r_11, r_12),
            mrhs1(Xt3, Yt3, Zt3, yq3, r_10, r_11, r_12, r_20, r_21, r_22),
            mrhs2(Xt3, Yt3, Zt3, xq3, r_00, r_01, r_02, r_20, r_21, r_22),
            mrhs3(Xt3, Yt3, Zt3, xq3, yq3, r_00, r_01, r_02, r_10, r_11, r_12);
//   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).rank() << std::endl;
#ifdef USE_SVD
      translation = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
#endif
#ifdef USE_QR
      Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 9, 3>> MQR(A);
      translation = MQR.solve(b);
#endif
   }

   //Called by RANSAC estimation
   void pose_translation(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& query_image_pts,
                         const Eigen::Matrix3d& KI, const Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
//-----------------------------------------------------------------------------------------------------------
   {
      const cv::Point2d &qpt0 = query_image_pts[0], &qpt1 = query_image_pts[1], &qpt2 = query_image_pts[2];
      Eigen::Vector3d query_ray1 = KI*Eigen::Vector3d(qpt0.x, qpt0.y, 1),
            query_ray2 = KI*Eigen::Vector3d(qpt1.x, qpt1.y, 1),
            query_ray3 = KI*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double Xt1 = world_pts[0].x, Yt1 = world_pts[0].y, Zt1 = world_pts[0].z,
            Xt2 = world_pts[1].x, Yt2 = world_pts[1].y, Zt2 = world_pts[1].z,
            Xt3 = world_pts[2].x, Yt3 = world_pts[2].y, Zt3 = world_pts[2].z,
            xq1 = query_ray1[0], yq1 = query_ray1[1], xq2 = query_ray2[0], yq2 = query_ray2[1],
            xq3 = query_ray3[0], yq3 = query_ray3[1];

      Eigen::Matrix<double, 9, 3> A;
      Eigen::Matrix<double, 9, 1> b;
//   Eigen::Matrix<double, 6, 3> A;
//   Eigen::Matrix<double, 6, 1> b;
      A << 0, -1, yq1,
            1, 0, -xq1,
            -yq1, xq1, 0,
            0, -1, yq2,
            1, 0, -xq2,
            -yq2, xq2, 0,
            0, -1, yq3,
            1, 0, -xq3,
            -yq3, xq3, 0;

      double Qw = Q.w(), Qx = Q.x(), Qy = Q.y(), Qz = Q.z();
      b << qrhs1(Qw, Qx, Qy, Qz, Xt1, Yt1, Zt1, yq1),
            qrhs2(Qw, Qx, Qy, Qz, Xt1, Yt1, Zt1, xq1),
            qrhs3(Qw, Qx, Qy, Qz, Xt1, Yt1, Zt1, xq1, yq1),
            qrhs1(Qw, Qx, Qy, Qz, Xt2, Yt2, Zt2, yq2),
            qrhs2(Qw, Qx, Qy, Qz, Xt2, Yt2, Zt2, xq2),
            qrhs3(Qw, Qx, Qy, Qz, Xt2, Yt2, Zt2, xq2, yq2),
            qrhs1(Qw, Qx, Qy, Qz, Xt3, Yt3, Zt3, yq3),
            qrhs2(Qw, Qx, Qy, Qz, Xt3, Yt3, Zt3, xq3),
            qrhs3(Qw, Qx, Qy, Qz, Xt3, Yt3, Zt3, xq3, yq3);

//   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).rank() << std::endl;
#ifdef USE_SVD
      translation = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
#endif
#ifdef USE_QR
      Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 9, 3>> MQR(A);
      translation = MQR.solve(b);
#endif
   }
}


/*
 * Triangle.hpp
 *
 *  Created on: 20/apr/2013
 *      Author: alessandro
 */

#ifndef TRIANGLE_HPP_
#define TRIANGLE_HPP_

#include <opencv2/core/core.hpp>
#include <iostream>
#include <boost/lexical_cast.hpp>

using namespace cv;

/*
 * Class Triangle
 */
class Triangle {
private:
	Point p1;
	Point p2;
	Point p3;

public:
	/*
	 * Constructors
	 */
	Triangle() {
	}

	Triangle(Point _p1, Point _p2, Point _p3) :
		p1(_p1), p2(_p2), p3(_p3) {
	}

	/*
	 * Getters and Setters
	 */
	Point getP1() const {
		return p1;
	}

	Point getP2() const {
		return p2;
	}

	Point getP3() const {
		return p3;
	}

	void setP1(Point _p1) {
		p1 = _p1;
	}

	void setP2(Point _p2) {
		p2 = _p2;
	}

	void setP3(Point _p3) {
		p3 = _p3;
	}

	/*
	 * Area given 2D-points
	 * From "http://www.mathopenref.com/coordtrianglearea.html"
	 */
	double getArea() const {
		double area;
		area = p1.x * (p2.y - p3.y) + p2.x * (p3.y - p1.y) + p3.x * (p1.y - p2.y);
		area = fabs(area / 2);

		return area;
	}
};

#endif /* TRIANGLE_HPP_ */


#include "matplotlibcpp.h"
#include <Eigen/Dense>
#include <iostream>

namespace plt = matplotlibcpp;

int main() {

  Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(200, 0, 6);
  Eigen::VectorXd y, z;

  // y = exp(sin(x)), z = exp(cos(z))
  y = x.array().sin().exp().matrix();
  z = x.array().cos().exp().matrix();

  plt::figure();

  plt::loglog(x, y, "tab:red");
  plt::loglog(x, z, "tab:blue", {{"linestyle", "--"}});

  plt::xlabel("Time in lecture");
  plt::ylabel("Student confusion");

  plt::grid();
  plt::savefig("eigen.pdf");
  // plt::show(); // show the figure instead of saving it
}


#include <iostream>

#include "datastructures/Queue.h"
#include "parallel/ThreadPoolExecutor.h"
#include "datastructures/FibonacciHeap.h"
#include "datastructures/Heap.h"

#include <array>
#include <queue>
#include <ctime>
#include <cstdlib>
#include <chrono>
#include <vector>

#include <boost/heap/fibonacci_heap.hpp>

using namespace std::chrono;

using data_type = int;

void testStdQueue(std::vector<int>& arr, data_type& data, int push_threshold) {
    std::queue<data_type> std_q;
    for(int i = 0; i< arr.size(); i++) {
        int r = arr[i];
        if(r < push_threshold) {
            std_q.push(data);
        } else {
            size_t size = std_q.size();
            if( size > 0) {
                data_type a = std_q.front();
                std_q.pop();
            }
        }
    }
}

void testMyQueue(std::vector<int>& arr, data_type& data, int push_threshold) {

    Queue<data_type,20> q;
    for(int i = 0; i< arr.size(); i++) {
        int r = arr[i];
        if(r < push_threshold) {
            q.push(data);
        } else {
            size_t size = q.size();
            if( size > 0) {
                data_type a = q.front();
                q.pop();
            }
        }
    }
}

void testMyFibonacci(std::vector<int>& arr, data_type& data, int push_threshold) {

    FibonacciHeap<int> q;
    for(int i = 0; i< arr.size(); i++) {
        int r = arr[i];
        if(r < push_threshold) {
            q.push(r);
        } else {
            size_t size = q.size();
            if( size > 0) {
                data_type a = q.top();
                q.pop();
            }
        }
    }
}

void testStdPrioQueu(std::vector<int>& arr, data_type& data, int push_threshold) {

    std::priority_queue<int, std::vector<int>, std::greater<int>> q;
    for(int i = 0; i< arr.size(); i++) {
        int r = arr[i];
        if(r < push_threshold) {
            q.push(r);
        } else {
            size_t size = q.size();
            if( size > 0) {
                data_type a = q.top();
                q.pop();
            }
        }
    }
}

void testMyPrioQueu(std::vector<int>& arr, data_type& data, int push_threshold) {

    Heap<int> q;
    for(int i = 0; i< arr.size(); i++) {
        int r = arr[i];
        if(r < push_threshold) {
            q.push(r);
        } else {
            size_t size = q.size();
            if( size > 0) {
                data_type a = q.top();
                q.pop();
            }
        }
    }
}

void testBoostFibonacciHeap(std::vector<int>& arr, data_type& data, int push_threshold) {
    boost::heap::fibonacci_heap<int> q;
    for(int i = 0; i< arr.size(); i++) {
        int r = arr[i];
        if(r < push_threshold) {
            q.push(r);
        } else {
            size_t size = q.size();
            if( size > 0) {
                data_type a = q.top();
                q.pop();
            }
        }
    }
}

void ValidateFibonacciHeap(std::vector<int>& arr, data_type& data, int push_threshold) {
    std::priority_queue<int, std::vector<int>, std::greater<int>> q;
    Heap<int> q2;
    for(int i = 0; i< arr.size(); i++) {
        int r = arr[i];
        if(r < push_threshold) {
            q.push(r);
            q2.push(r);
        } else {
            size_t size = q.size();
            if( size > 0) {
                data_type a = q.top();
                if(a != q2.top()) {
                    std::cout << "WRONG" << std::endl;
                }
                q.pop();
                q2.pop();

            }
        }
    }
    std::cout << "Validated" << std::endl;
}

int timeIt(std::function<void()> f){
    milliseconds start = duration_cast< milliseconds >(
            system_clock::now().time_since_epoch()
    );
    f();
    milliseconds end = duration_cast< milliseconds >(
            system_clock::now().time_since_epoch()
    );
    return (end - start).count();
}

void threadPoolQueusTest() {
    int N = 5000000;
    // Should be between 1 and 100;
    int push_threshold = 80;
    data_type data;

    std::vector<int>  arr;
    std::srand(std::time(nullptr));
    for(int i = 0; i< N; i++) {
        arr.push_back(std::rand() % 100);
    }

    std::function<void()> test_std_queue = std::bind(testStdQueue, arr, data, push_threshold);
    std::function<void()> test_my_queue = std::bind(testMyQueue, arr, data, push_threshold);
    std::function<void()> test_std_prioqueue = std::bind(testStdPrioQueu, arr, data, push_threshold);
    std::function<void()> test_my_prioqueue = std::bind(testStdPrioQueu, arr, data, push_threshold);
    std::function<void()> test_my_fibonacciHeap = std::bind(testMyFibonacci, arr, data, push_threshold);
    std::function<void()> test_boost_fibonacciHeap = std::bind(testBoostFibonacciHeap, arr, data, push_threshold);

    ThreadPoolExecutor<int> tpe(1);

    //std::future<int> f1 = tpe.execute(std::bind(timeIt, test_std_queue));
    //std::future<int> f2 = tpe.execute(std::bind(timeIt, test_my_queue));
    std::future<int> f3 = tpe.execute(std::bind(timeIt, test_std_prioqueue));
    std::future<int> f4 = tpe.execute(std::bind(timeIt, test_my_prioqueue));
    std::future<int> f5 = tpe.execute(std::bind(timeIt, test_my_fibonacciHeap));
    std::future<int> f6 = tpe.execute(std::bind(timeIt, test_boost_fibonacciHeap));
    tpe.join();
    //std::cout << "STD QUEUE: " << f1.get() << std::endl;
    //std::cout << "MY QUEUE: " << f2.get() << std::endl;
    std::cout << "STD PRIO QUEUE: " << f3.get() << std::endl;
    std::cout << "MY HEAP: " << f4.get() << std::endl;
    std::cout << "FIBONACCI HEAP: " << f5.get()  << std::endl;
    std::cout << "BOOST FIBONACCI HEAP: " << f6.get()  << std::endl;
}

int main() {
  threadPoolQueusTest();

    return 0;
}


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

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

// Use, modification and distribution is subject to 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 <array>

#include <geometry_test_common.hpp>

#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/approximate.hpp>
#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/expression_tree.hpp>

template <typename CalculationType>
void test_all()
{
    using bg::detail::generic_robust_predicates::_1;
    using bg::detail::generic_robust_predicates::_2;
    using bg::detail::generic_robust_predicates::_3;
    using bg::detail::generic_robust_predicates::_4;
    using bg::detail::generic_robust_predicates::sum;
    using bg::detail::generic_robust_predicates::difference;
    using bg::detail::generic_robust_predicates::product;
    using bg::detail::generic_robust_predicates::max;
    using bg::detail::generic_robust_predicates::abs;
    using bg::detail::generic_robust_predicates::post_order;
    using bg::detail::generic_robust_predicates::approximate_value;
    using bg::detail::generic_robust_predicates::get_approx;
    using bg::detail::generic_robust_predicates::approximate_interim;
    using ct = CalculationType;
    ct r1 = approximate_value<sum<_1, _2>, ct>(
            std::array<ct, 2>{1.0, 2.0});
    BOOST_CHECK_EQUAL(3.0, r1);
    ct r2 = approximate_value<max<abs<_1>, abs<_2>>, ct>(
            std::array<ct, 2>{-10.0, 2.0});
    BOOST_CHECK_EQUAL(10.0, r2);

    using expression = product
        <
            difference<_1, _2>,
            difference<_3, _4>
        >;
    using evals = post_order<expression>;
    std::array<ct, boost::mp11::mp_size<evals>::value> r;
    std::array<ct, 4> input {5.0, 3.0, 2.0, 8.0};
    approximate_interim<evals, evals, ct>(r, input);
    ct r3 = get_approx<evals, typename expression::left, ct>(r, input);
    BOOST_CHECK_EQUAL(2.0, r3);
    ct r4 = get_approx<evals, typename expression::right, ct>(r, input);
    BOOST_CHECK_EQUAL(-6.0, r4);
    ct r5 = get_approx<evals, expression, ct>(r, input);
    BOOST_CHECK_EQUAL(-12.0, r5);
}

int test_main(int, char* [])
{
    test_all<double>();
    return 0;
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-ieee
    Function object implementing ulp capabilities

    Returns the ulp distance to the nearest (distinct) element of the same type.

    @par Semantic:

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

    is similar to:

    @code
    T r = min(x-prev(x), next(x)-x)/Eps<T>();
    @endcode

    @par Note
    ulp stands for Unit in the Last Place.

    @see ulpdist, eps, Eps

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

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

#endif


#include <stdio.h>
#include <iostream>
#include "dbscan.h"

#include <dlib/clustering.h>
#include <dlib/matrix.h>
#include "plot.h"
#include <experimental/filesystem>
#include <utility>
#include <unordered_map>
namespace fs = std::experimental::filesystem;

const std::vector<std::string> data_names{
    "dataset0.csv"
    //, "dataset1.csv",
    //"dataset2.csv", "dataset3.csv",
    //"dataset4.csv", "dataset5.csv"
};

const std::vector<std::string> colors{"black", "red", "blue", "green",
                "cyan", "yellow", "brown", "magenta", "gray", "chartreuse", "honeydew"};

using DataType = double;
using Coords = std::vector<DataType>;
using PointCoords = std::pair<Coords, Coords>;
using Clusters = std::unordered_map<int, PointCoords>;

void PlotClusters(const Clusters& clusters,
                 const std::string& name,
                 const std::string& file_name) {

    plotcpp::Plot plt;
    plt.SetTerminal("png");
    plt.SetOutput(file_name);
    plt.SetTitle(name);
    plt.SetXLabel("x");
    plt.SetYLabel("y");
    plt.SetAutoscale();
    plt.GnuplotCommand("set grid");
    auto draw_state = plt.StartDraw2D<Coords::const_iterator>();
    for (const auto& cluster : clusters) {
        std::stringstream params;
        if(cluster.first == -1) continue;
        params << "lc rgb '" << colors[cluster.first] << "'pt 7";
        plt.AddDrawing(draw_state,
                        plotcpp::Points(
                        cluster.second.first.cbegin(), cluster.second.first.cend(),
                        cluster.second.second.cbegin(),
                        std::to_string(static_cast<size_t>(cluster.first)) + " cls", params.str()));

    }

    plt.EndDraw2D(draw_state);
    plt.Flush();
}

//For debug here:
/*
void printResults(vector<Point>& points, int num_points)
{
    int i = 0;
    printf("Number of points: %u\n"
        " x     y   cluster_id\n"
        "-----------------------------\n"
        , num_points);
    while (i < num_points)
    {
          printf("%5.2lf %5.2lf %d\n",
                 points[i].x,
                 points[i].y,
                 points[i].clusterID);
          ++i;
    }
}
*/

int main(int argc, char** argv)
{
    if(argc > 3){
    auto base_dir = fs::path(argv[1]);
    const double epsilon = atof(argv[2]);
    const double minimum_points = atoi(argv[3]);
    for(auto & dataset : data_names ) {
        auto dataset_name = base_dir / dataset;

        if (fs::exists(dataset_name)){
        std::ifstream file(dataset_name);
        dlib::matrix<DataType> data;
        file >> data;
        vector<Point> points;
        for ( int i = 0; i < data.nr(); i++){
            points.emplace_back(data(i, 1), data(i, 2), UNCLASSIFIED);
        }

        Clusters clusters;
        DBSCAN ds(minimum_points, epsilon, points);
        ds.run();

        for (const auto& point : ds.m_points){

            clusters[point.clusterID].first.push_back(point.x);
            clusters[point.clusterID].second.push_back(point.y);
            //For debug here:
            //std::cout << point.clusterID << point.x << point.y << std::endl;
         }

        //std::cout << " file name is: "<< dataset_name << "\n\n\n";
        //printResults(ds.m_points, ds.getTotalPointSize());  //for debug here
        //std::cout << "\n\n\n";
        //std::cout <<__LINE__ << std::endl << std::endl;
        PlotClusters(clusters, "DBSCAN clustering", "../results/" +  dataset + "-dbscans.png");
       }
      else{
        std::cerr << "Dataset file " << dataset_name << "missed. Please provide in the correct form.\n";
      }
     }
    }
    else {
        std::cerr <<" Please provide the data's folder!!!\n\n";
    }
    return 0;
}


#ifndef __BLACKSCHOLES_H
#define __BLACKSCHOLES_H

#include <iostream>
#include <cmath>
#include <armadillo>

namespace bsc{

        class BlackScholes{

        private:
                double S_;
                double K_;
                double r_;
                double T_;

        public:
                BlackScholes(double S_,double K_,double r_,double T_);
                double OptionEurop(double);
                double OptionVega(double);

};

}

#endif


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#include <rw/math/Vector3D.hpp>

#include <Eigen/Core>
#include <gtest/gtest.h>

using namespace rw::math;

TEST (Vector3D, MiscTest)
{
    const Vector3D<> v1 (1.0, 2.0, 3.0);
    const Vector3D<> v2 (v1);
    const Vector3D<> v3 = v1 + v2;
    const Vector3D<> v4 = Vector3D<> (2.0, 4.0, 6.0);
    EXPECT_EQ ((v3 - v4).normInf (), 0);

    const Vector3D<> v5 (3.0, 4.0, 5.0);
    const Vector3D<> v5_norm = normalize (v5);
    EXPECT_LT (fabs (v5_norm.norm2 () - 1), 1e-15);

    const double len = v5.norm2 ();
    EXPECT_LT (fabs (v5_norm (0) - v5 (0) / len), 1e-15);
    EXPECT_LT (fabs (v5_norm (1) - v5 (1) / len), 1e-15);
    (fabs (v5_norm (2) - v5 (2) / len) < 1e-15);

    Vector3D<> v6;
    v6 (0) = len;
    EXPECT_EQ (v6 (0), len);

    const Vector3D< std::string > vs1 ("x1", "y1", "z1");

    EXPECT_EQ (vs1[0], "x1");
    EXPECT_EQ (vs1[1], "y1");
    EXPECT_EQ (vs1[2], "z1");

    EXPECT_EQ (cross (v1, v2).normInf (), 0);

    const Vector3D< double > vd (1.1, 5.51, -10.3);
    const Vector3D< int > vi = cast< int > (vd);
    EXPECT_EQ (vi (0), 1);
    EXPECT_EQ (vi (1), 5);
    EXPECT_EQ (vi (2), -10);

    /* Test comparison operators operator== and operator!= */
    const Vector3D< double > comp1 (1.1, -2.2, 3.3);
    const Vector3D< double > comp2 (1.1, -2.2, 3.3);
    EXPECT_TRUE (comp1 == comp2);
    EXPECT_TRUE (!(comp1 != comp2));
    const Vector3D< double > comp3 (1.1, 2.2, -3.3);
    EXPECT_TRUE (comp1 != comp3);
    EXPECT_TRUE (!(comp1 == comp3));
}

TEST (Vector3D, scalarOperatorTest)
{
    Vector3D<> obj (3, 3, 3);

    auto test1 = obj * 2;
    auto test2 = 2 * obj;
    auto test3 = obj / 2;
    auto test4 = 2 / obj;
    auto test5 = obj.elemAdd(2);
    auto test7 = obj.elemSubtract(2);
    auto test9 = obj;
    test9 *= 2;
    auto test10 = obj;
    test10 /= 2;
    for (size_t i = 0; i < obj.size (); i++) {
        EXPECT_DOUBLE_EQ (test1[i], 6.0);
        EXPECT_DOUBLE_EQ (test2[i], 6.0);
        EXPECT_DOUBLE_EQ (test3[i], 3.0 / 2.0);
        EXPECT_DOUBLE_EQ (test4[i], 2.0 / 3.0);
        EXPECT_DOUBLE_EQ (test5[i], 5.0);
        EXPECT_DOUBLE_EQ (test7[i], 1.0);
        EXPECT_DOUBLE_EQ (test9[i], 6.0);
        EXPECT_DOUBLE_EQ (test10[i], 3.0 / 2.0);
    }
}

TEST (Vector3D, Vector3DOperatorTest)
{
    Vector3D<> obj1 (3, 3, 3);
    Vector3D<> obj2 (2, 2, 2);

    auto test1 = obj1.elemMultiply(obj2);
    auto test2 = obj1.elemDivide(obj2);
    auto test3 = obj1 + obj2;
    auto test4 = obj1 - obj2;
    auto test7 = obj1;
    test7 += obj2;
    auto test8 = obj1;
    test8 -= obj2;

    for (size_t i = 0; i < obj1.size (); i++) {
        EXPECT_DOUBLE_EQ (test1[i], 6.0);
        EXPECT_DOUBLE_EQ (test2[i], 3.0 / 2.0);
        EXPECT_DOUBLE_EQ (test3[i], 5.0);
        EXPECT_DOUBLE_EQ (test4[i], 1.0);
        EXPECT_DOUBLE_EQ (test7[i], 5.0);
        EXPECT_DOUBLE_EQ (test8[i], 1.0);
    }
}

TEST (Vector3D, EigenOperatorTest)
{
    Vector3D<> obj1 (3, 3, 3);
    Eigen::Vector3d obj2 (2, 2, 2);

    auto test1  = obj1.elemMultiply(obj2);
    auto test2  = obj1.elemDivide(obj2);
    auto test3  = obj1 + obj2;
    auto test3x = obj2 + obj1;
    auto test4  = obj1 - obj2;
    auto test4x = obj2 - obj1;
    auto test7 = obj1;
    test7 += obj2;
    auto test8 = obj1;
    test8 -= obj2;

    for (size_t i = 0; i < obj1.size (); i++) {
        EXPECT_DOUBLE_EQ (test1[i], 6.0);
        EXPECT_DOUBLE_EQ (test2[i], 3.0 / 2.0);
        EXPECT_DOUBLE_EQ (test3[i], 5.0);
        EXPECT_DOUBLE_EQ (test3x[i], 5.0);
        EXPECT_DOUBLE_EQ (test4[i], 1.0);
        EXPECT_DOUBLE_EQ (test4x[i], -1.0);
        EXPECT_DOUBLE_EQ (test7[i], 5.0);
        EXPECT_DOUBLE_EQ (test8[i], 1.0);
    }

    EXPECT_TRUE (obj1 != obj2);
    EXPECT_TRUE (obj2 != obj1);
    EXPECT_FALSE (obj1 == obj2);
    EXPECT_FALSE (obj2 == obj1);
    obj2 = obj1.e();
    EXPECT_TRUE (obj1 == obj2);
    EXPECT_TRUE (obj2 == obj1);
    EXPECT_FALSE (obj1 != obj2);
    EXPECT_FALSE (obj2 != obj1);
}

TEST (Vector3D, ComparisonTest)
{
    const Vector3D< double > comp1 (1.1, -2.2, 3.3);
    auto comp2 = comp1;
    auto comp3 = -comp1;
    EXPECT_TRUE (comp1 == comp2);
    EXPECT_FALSE (comp1 != comp2);
    EXPECT_TRUE (comp1 != comp3);
    EXPECT_FALSE (comp1 == comp3);
}

TEST (Vector3D, MathOperators)
{
    Vector3D<> obj1 (1, 2, 3);
    Vector3D<> obj2 (3, 2, 1);

    auto test1 = obj1.cross (obj2);
    auto test2 = obj1.dot (obj2);
    auto test3 = obj1.normalize ();

    EXPECT_EQ (obj1.e ().cross (obj2.e ()), test1);
    EXPECT_EQ (obj1.e ().dot (obj2.e ()), test2);
    EXPECT_EQ (test3.norm2 (), 1.0);
    EXPECT_EQ (test3 * obj1.norm2 (), obj1);
}


/*! \file demo_2d_bezier.cpp
\brief Simple 2D plot of trig functions,
contrived to show optionally
  using Bezier for curves,
  data-points markers,
  lines joining data-points,
and
  legend identifying point marks and/or lines.

  A few color, widths and shapes options are demonstrated,
  to produce a somewhat lurid effect.

  Demonstrates that the legend marks line only shown if a dataset is plotted with a line joining points,
  and a data-point value marker is only shown if marker shapes are used,
  and not if the shape == none.
*/

// Copyright Paul A. Bristow 2018

// Use, modification and distribution are subject to 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/svg_plot/svg_2d_plot.hpp>
// using namespace boost::svg;
// Very convenient to allow easy access to colors and other items.

#include <map>
// using std::map;
#include <cmath>
//using std::sin;
//using std::cos;
//using std::tan;

double f(double x)
{
  return std::sin(x);
}

double g(double x)
{
  return std::cos(x);
}

double h(double x)
{
  return std::tan(x);
}

int main()
{

  using namespace boost::svg; // Very convenient to allow easy access to colors and many other items.

  std::map<double, double> sin_data, cos_data, tan_data, sincos_data;

  double step = 3.14159265 / 8.;  // Interval between function data-points.

  // Generate some trigonmetric data to plot.
  for(double i = 0; i <= 10.; i += step)
  {
    sin_data[i] = f(i); // sin
    cos_data[i] = g(i); // cos
    tan_data[i] = h(i); // tan
    sincos_data[i] = std::sin(g(i)); // sincos
 } // for i

  svg_2d_plot my_plot; // Data structure to hold the plot.

  // Size/scale settings.
  my_plot.size(700, 500) // SVG image size (pixel).
    .x_range(-0.5, 10.5) // Range of x and y axes,
    .y_range(-1.1, 1.1); // chosen to ensure that the maxima and minimax
                         // are not just on the edge of the plot window.

  // Text settings.
  my_plot.title("Plot of sin, cos & tan and sincos functions")
    // Note: for ampersand must use Unicode & because it is a reserved symbol in SVG XML.
    // Search engines will provide Unicodes by querying "Unicode ampersand"
    // at sites like https://unicode.org/,
    // http://www.fileformat.info/info/unicode/char/0026/index.htm and others.
    .title_font_size(28)
    .x_label("x Axis Units")
    .y_major_labels_side(left)
    .y_major_grid_on(true);

  // Layout options:
  my_plot.legend_on(true) // Want a legend box.
    .plot_window_on(true) // want a plot window with axis labels etc outside.
    .x_label_on(true) // Label X-axis ticks with their values.
    //.y_label_on(false)  // false is default.
    ;

  // Plot color settings.
  // (Note use of chaining to add settings).
  my_plot
    .background_color(darkgreen)
    .legend_background_color(lightgray)
    .legend_border_color(black)
    .plot_background_color(lightgoldenrodyellow)
    .title_color(white)
    .y_major_grid_color(black);

  // X axis settings.
  my_plot.x_major_interval(2)
    .x_major_tick_length(14)
    .x_major_tick_width(1)
    .x_minor_tick_length(7)
    .x_minor_tick_width(1)
    .x_num_minor_ticks(3)

    // Y axis settings.
    .y_major_interval(25)
    .y_num_minor_ticks(5);

  // Legend settings.
  my_plot.legend_title_font_size(15)
    .legend_title("Legend");

  my_plot.plot(sin_data, "sin(x)")
    .line_on(true) // Line joining data-points, using default color black.
    .shape(circlet) // and circle marker showing data-points.
    .size(10) // Size (diameter pixels) of circlet data-point marker.
    .fill_color(yellow) // Outline is default black and centre yellow.
  // Default is no bezier.  Note angularity at the minima and maxima.
    ;

  my_plot.plot(cos_data, "cos(x)")
    .line_color(blue) // Defaults to showing line, but not in legend.
    .line_on(true) // Needed to show in the legend.
    .line_width(1) // thinner line.
    .shape(square) // Center of square has the data-point coordinate.
    .size(5)
    .fill_color(red)  // Center of square.
    ;

  my_plot.plot(tan_data, "tan(x)")
    .line_on(false)  // No line joining points.
    .shape(cone) // bottom point of cone has the coordinate of the data point.
    .size(5).fill_color(blue); // Just show data-point markers.

  my_plot.plot(sincos_data, "sincos(x)")
    .line_on(true)  // Just line joining points.
    .line_color(purple)
    .line_width(0.5)
    .bezier_on(true) // Note smoother at the minima and maxima.
    .shape(none) ; // NO data-point markers (and only shows a line in the legend).

  my_plot.write("demo_2d_bezier.svg"); // Final plot.

  std::cout << "demo_2d_bezier plot written to " << "demo_2d_bezier.svg" << std::endl;
  return 0;
} // int main()


#pragma once
/*!	\file	real.hpp
	\brief	Real classes declarations.
	\author	Garth Santor
	\date	2021-10-29
	\copyright	Garth Santor, Trinh Han

=============================================================
Declarations of the Real classes derived from Operand.  Includes
the subclasses Pi and E.

=============================================================
Revision History
-------------------------------------------------------------

Version 2021.10.26
	C++ 20 validated

Version 2019.11.05
	C++ 17 cleanup

Version 2014.10.29
	C++ 11 refactor.
	Switched value_type to boost::multiprecision::cpp_dec_float_100

Version 2012.11.13
	C++ 11 cleanup

Version 2010.11.09
	Switched boost::shared_ptr<> to std::shared_ptr<>.
	Added TOKEN_PTR_TYPE macro.

Version 2009.11.25
	Alpha release.

=============================================================

Copyright Garth Santor/Trinh Han

The copyright to the computer program(s) herein
is the property of Garth Santor/Trinh Han, Canada.
The program(s) may be used and/or copied only with
the written permission of Garth Santor/Trinh Han
or in accordance with the terms and conditions
stipulated in the agreement/contract under which
the program(s) have been supplied.
=============================================================*/

#include <ee/operand.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/constants/constants.hpp>

/*! Real number token. */
class Real : public Operand {
public:
	DEF_POINTER_TYPE(Real)
	using value_type = boost::multiprecision::number<boost::multiprecision::cpp_dec_float<1000, int32_t, void>>;
private:
	value_type	value_;
public:
	Real(value_type value = value_type(0)) : value_(value) { }
	[[nodiscard]] value_type	value() const { return value_; };
	[[nodiscard]] string_type	str() const override;
};

/*! Pi constant token. */
class Pi : public Real {
public:
	Pi() : Real(boost::math::constants::pi<value_type>()) { }
};

/*! Euler constant token. */
class E : public Real {
public:
	E() : Real(boost::math::constants::e<value_type>()) { }
};


#include <iostream>
#include <Eigen/Dense>
#include "file_manage.h"
#include "decision_tree.h"
#include "model_selection.h"
using namespace std;
using namespace Eigen;

int main()
{
	MatrixXd features;
	VectorXd labels;

	read_csv("data/iris.csv", features, labels, 150, 5);
	//read_csv("data/seeds.csv", features, labels, 210, 8);

	DecisionTree model;
	model.fit(features, labels);
	model.printTree();

	double accuracy = evaluate_model(model, features, labels, 5);
	cout << "Model accuracy : " << accuracy << endl;

	return 0;
}


#ifndef GQ_CHARACTERIZE_H
#define GQ_CHARACTERIZE_H

#include <Eigen/Eigen>
#include <cfloat>
#include <iostream>
#include <string>

#include "geometry.hpp"

enum GQ_TYPE {UNKNOWN = 0,
              ELLIPSOID,
              ONE_SHEET_HYPERBOLOID,
              TWO_SHEET_HYPERBOLOID,
              ELLIPTIC_CONE,
              ELLIPTIC_PARABOLOID,
              HYPERBOLIC_PARABOLOID,
              ELLIPTIC_CYL,
              HYPERBOLIC_CYL,
              PARABOLIC_CYL};

const std::vector<std::string> _gq_names = {"UNKNOWN",
                                            "ELLIPSOID",
                                            "ONE_SHEET_HYPERBOLOID",
                                            "TWO_SHEET_HYPERBOLOID",
                                            "ELLIPTIC_CONE",
                                            "ELLIPTIC_PARABOLOID",
                                            "HYPERBOLIC_PARABOLOID",
                                            "ELLIPTIC_CYL",
                                            "HYPERBOLIC_CYL",
                                            "PARABOLIC_CYL"};

extern const std::vector<std::string> _gq_names;

class GQ_Characterize{

protected:
  // coefficients of the GQ
  double A_,B_,C_,D_,E_,F_,G_,H_,J_,K_;
  // the cannonical GQ type
  GQ_TYPE type;
  // translation from the canoncial GQ to final GQ
  Vector3d translation;
  // rotation matrix from canonical GQ to final GQ
  double rotation_mat[9];
  // gq transform
  Transform transform_;

  // tolerance used to determine
  // if matrix determinant should be considered zero
  const double gq_tol = 1e-6;
  const double equivalence_tol = 1e-8;

public:
  // Constructor
  // Characterizes a general quadratic (GQ) equation of the form:
  // A x^2 + B y^2 + C z^2 + D xy + E yz + F xz + G x + H y + J z + K = 0
  // into a special quadratic (SQ) with a transformation
  GQ_Characterize(double A, double B, double C,
                  double D, double E, double F,
                  double G, double H, double J,
                  double K);

  // GQ type accessor
  GQ_TYPE get_type() { return type; }

protected:
  // reduces GQ coefficients to 2nd order terms, a constant, and a transformation
  // to the original origntation when needed
  void make_canonical();

  // this method reduces a complex GQ to a geometrically equivalent
  // and more CAD-friendly form if appropriate
  void reduce_type();

  // determines GQ type based on characteristic parameters
  GQ_TYPE find_type(int rt, int rf, int del, int s, int d);
};

#endif


/* Boost libs/numeric/odeint/examples/simple1d.cpp

 Copyright 2012-2013 Mario Mulansky
 Copyright 2012 Karsten Ahnert

 example for a simple one-dimensional 1st order ODE

 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 <iostream>
#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

/* we solve the simple ODE x' = 3/(2t^2) + x/(2t)
 * with initial condition x(1) = 0.
 * Analytic solution is x(t) = sqrt(t) - 1/t
 */

void rhs( const double x , double &dxdt , const double t )
{
    dxdt = 3.0/(2.0*t*t) + x/(2.0*t);
}

void write_cout( const double &x , const double t )
{
    cout << t << '\t' << x << endl;
}

// state_type = double
typedef runge_kutta_dopri5< double > stepper_type;

int main()
{
    double x = 0.0; //initial value x(1) = 0
    // use dopri5 with stepsize control and allowed errors 10^-12, integrate t=1...10
    integrate_adaptive( make_controlled( 1E-12 , 1E-12 , stepper_type() ) , rhs , x , 1.0 , 10.0 , 0.1 , write_cout );
}


#include <string>
#include <unordered_map>
#include <Eigen/Dense>
#include <cmath>
#include "../include/optimizer.h"

namespace MyDL
{

    // ------------------------------------------------------------
    //                  SGD
    // ------------------------------------------------------------
    SGD::SGD(double learning_rate): _learning_rate(learning_rate)
    {
    }

    void SGD::update(unordered_map<string, MatrixXd>& params, unordered_map<string, MatrixXd>& grads)
    {
        for (auto grad : grads)
        {
            params[grad.first] -= _learning_rate * grad.second;
        }
    }

    void SGD::update(unordered_map<string, shared_ptr<MatrixXd>> & params, unordered_map<string, MatrixXd> &grads)
    {
        for (auto grad : grads)
        {
            *(params[grad.first]) -= _learning_rate * grad.second;
        }
    }

    // ------------------------------------------------------------
    //                  Momentum
    // ------------------------------------------------------------
    Momentum::Momentum(double learning_rate, double momentum) : _learning_rate(learning_rate), _momentum(momentum)
    {
    }

    void Momentum::update(unordered_map<string, MatrixXd>& params, unordered_map<string, MatrixXd>& grads)
    {
        if (_v.empty())
        {
            for (auto param : params)
            {
                _v[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            _v[key] = _momentum * _v[key] - _learning_rate * grads[key];
            params[key] += _v[key];
        }
    }

    void Momentum::update(unordered_map<string, shared_ptr<MatrixXd>> &params, unordered_map<string, MatrixXd> &grads)
    {
        if (_v.empty())
        {
            for (auto param : params)
            {
                MatrixXd tmp_mat = *(param.second);
                _v[param.first] = MatrixXd::Zero(tmp_mat.rows(), tmp_mat.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            _v[key] = _momentum * _v[key] - _learning_rate * grads[key];
            *(params[key]) += _v[key];
        }
    }

    // ------------------------------------------------------------
    //                  AdaGrad
    // ------------------------------------------------------------
    AdaGrad::AdaGrad(double learning_rate) : _learning_rate(learning_rate)
    {
    }

    void AdaGrad::update(unordered_map<string, MatrixXd> &params, unordered_map<string, MatrixXd> & grads)
    {
        if (_h.empty())
        {
            for (auto param : params)
            {
                _h[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            // 勾配変化の大きかったパラメータの二乗平方根で割る → 要は勾配の絶対値が大きかったパラメータの更新幅を小さくするということ
            // ※ 厳密には勾配の絶対値で割っているわけではない(直前の内部状態に加算して平方根を取っている)
            _h[key].array() += grads[key].array() * grads[key].array();
            params[key].array() -= _learning_rate * _h[key].unaryExpr([](double p){return 1/(sqrt(p)+1e-7);}).array() * grads[key].array();
        }
    }

    void AdaGrad::update(unordered_map<string, shared_ptr<MatrixXd>> &params, unordered_map<string, MatrixXd> &grads)
    {
        if (_h.empty())
        {
            for (auto param : params)
            {
                MatrixXd tmp_mat = *(param.second);
                _h[param.first] = MatrixXd::Zero(tmp_mat.rows(), tmp_mat.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            // 勾配変化の大きかったパラメータの二乗平方根で割る → 要は勾配の絶対値が大きかったパラメータの更新幅を小さくするということ
            // ※ 厳密には勾配の絶対値で割っているわけではない(直前の内部状態に加算して平方根を取っている)
            _h[key].array() += grads[key].array() * grads[key].array();
            params[key]->array() -= _learning_rate * _h[key].unaryExpr([](double p) { return 1 / (sqrt(p) + 1e-7); }).array() * grads[key].array();
        }
    }

    // ------------------------------------------------------------
    //                  RMSProp
    // ------------------------------------------------------------
    RMSprop::RMSprop(double learning_rate, double decay_rate): _learning_rate(learning_rate), _decay_rate(decay_rate)
    {
    }

    void RMSprop::update(unordered_map<string, MatrixXd>& params, unordered_map<string, MatrixXd>& grads)
    {
        if (_h.empty())
        {
            for (auto param : params)
            {
                _h[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            // AdaGradの内部状態hを指数移動平均に置き換えたものがRMSprop -> 指数移動平均の減衰率(平滑化係数)がdecay_rate
            _h[key].array() *= _decay_rate;
            _h[key].array() += (1 - _decay_rate) * grads[key].array() * grads[key].array();
            params[key].array() -= _learning_rate * _h[key].unaryExpr([](double p) { return 1 / (sqrt(p) + 1e-7); }).array() * grads[key].array();
        }
    }

    void RMSprop::update(unordered_map<string, shared_ptr<MatrixXd>> &params, unordered_map<string, MatrixXd> &grads)
    {
        if (_h.empty())
        {
            for (auto param : params)
            {
                _h[param.first] = MatrixXd::Zero(param.second->rows(), param.second->cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            // AdaGradの内部状態hを指数移動平均に置き換えたものがRMSprop -> 指数移動平均の減衰率(平滑化係数)がdecay_rate
            _h[key].array() *= _decay_rate;
            _h[key].array() += (1 - _decay_rate) * grads[key].array() * grads[key].array();
            params[key]->array() -= _learning_rate * _h[key].unaryExpr([](double p) { return 1 / (sqrt(p) + 1e-7); }).array() * grads[key].array();
        }

    }

    // ------------------------------------------------------------
    //                  Adam
    // ------------------------------------------------------------
    Adam::Adam(double learning_rate, double beta1, double beta2): _learning_rate(learning_rate), _beta1(beta1), _beta2(beta2)
    {
        _iter = 0;
    }

    void Adam::update(unordered_map<string, MatrixXd>& params, unordered_map<string, MatrixXd>& grads)
    {
        if (_m.empty())
        {
            for (auto param : params)
            {
                _m[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
                _v[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
            }
        }

        _iter++;
        double lr_t = _learning_rate * sqrt(1.0 - std::pow(_beta2, _iter)) / (1.0 - std::pow(_beta1, _iter));

        for (auto param : params)
        {
            string key = param.first;
            _m[key] += (1 - _beta1) * (grads[key] - _m[key]);
            _v[key] += (1 - _beta2) * (grads[key].unaryExpr([](double p){return p*p;}) - _v[key]);

            params[key].array() -= lr_t * _m[key].array() * _v[key].unaryExpr([](double p){return 1 / (sqrt(p) + 1e-7);}).array();
        }
    }

    void Adam::update(unordered_map<string, shared_ptr<MatrixXd>>& params, unordered_map<string, MatrixXd>& grads)
    {
        if (_m.empty())
        {
            for (auto param : params)
            {
                _m[param.first] = MatrixXd::Zero(param.second->rows(), param.second->cols());
                _v[param.first] = MatrixXd::Zero(param.second->rows(), param.second->cols());
            }
        }

        _iter++;
        double lr_t = _learning_rate * sqrt(1.0 - std::pow(_beta2, _iter)) / (1.0 - std::pow(_beta1, _iter));

        for (auto param : params)
        {
            string key = param.first;
            _m[key] += (1 - _beta1) * (grads[key] - _m[key]);
            _v[key] += (1 - _beta2) * (grads[key].unaryExpr([](double p) { return p * p; }) - _v[key]);

            params[key]->array() -= lr_t * _m[key].array() * _v[key].unaryExpr([](double p) { return 1 / (sqrt(p) + 1e-7); }).array();
        }

    }

}


#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

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

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;

// baseline
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png");    // other files

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

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

Eigen::Vector3d get_3D_point_from_depth(const Eigen::Vector2d& p, double depth, const Eigen::Matrix3d& K)
{
    return Eigen::Vector3d(depth * (p.x() - K(0, 2)) / K(0, 0),
                           depth * (p.y() - K(1, 2)) / K(1, 1),
                           depth);
}

using namespace Sophus;
// Local parameterization needed to handle SE3 from Sophus (from Sophus/test/ceres/)
class LocalParameterizationSE3 : public ceres::LocalParameterization {
 public:
  virtual ~LocalParameterizationSE3() {}

  // SE3 plus operation for Ceres
  //
  //  T * exp(x)
  //
  virtual bool Plus(double const* T_raw, double const* delta_raw,
                    double* T_plus_delta_raw) const {
    Eigen::Map<SE3d const> const T(T_raw);
    Eigen::Map<Vector6d const> const delta(delta_raw);
    Eigen::Map<SE3d> T_plus_delta(T_plus_delta_raw);
    T_plus_delta = T * SE3d::exp(delta);
    return true;
  }

  // Jacobian of SE3 plus operation for Ceres
  //
  // Dx T * exp(x)  with  x=0
  //
  virtual bool ComputeJacobian(double const* T_raw,
                               double* jacobian_raw) const {
    Eigen::Map<SE3d const> T(T_raw);
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
        jacobian_raw);
    jacobian = T.Dx_this_mul_exp_x_at_0();
    return true;
  }

  virtual int GlobalSize() const { return SE3d::num_parameters; }

  virtual int LocalSize() const { return SE3d::DoF; }
};

struct PhotometricError: public ceres::SizedCostFunction<9, 7>
{
    PhotometricError(const cv::Mat& img1, const cv::Mat& img2, const Eigen::Vector2d& p1, const Eigen::Vector3d& P1, const Eigen::Matrix3d& K, int half_w_size)
    : _img1(img1), _img2(img2), _p1(p1), _P1(P1), _K(K), _half_w_size(half_w_size) {}

    virtual bool Evaluate(double const* const *params,
                          double *residuals,
                          double **jacobians) const {
        const Eigen::Map<const Sophus::SE3d> Rt(params[0]);

        Eigen::Vector3d P2 = Rt * _P1;
        Eigen::Vector3d p2 = _K * P2;
        p2 /= p2.z();

        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double cx = _K(0, 2);
        double cy = _K(1, 2);
        double X = P2.x();
        double Y = P2.y();
        double Z = P2.z();
        double X2 = X * X;
        double Y2 = Y * Y;
        double Z2 = Z * Z;

        double x3 = _P1.x();
        double y3 = _P1.y();
        double z3 = _P1.z();
        double qx = params[0][0];
        double qy = params[0][1];
        double qz = params[0][2];
        double qw = params[0][3];

        if (P2.z() < 0) // invalid depth
        {
            // fill residuals and jacobians to 0
            for (int i = 0; i < 9; ++i)
            {
                residuals[i] = 0;
                if (jacobians!=nullptr && jacobians[0]!=nullptr)
                {
                    for (int j = 0; j < 7; ++j)
                    {
                        jacobians[0][i*7+j] = 0.0;
                    }
                }
            }
            return true;
        }

        if (p2.x() < _half_w_size || p2.x() > _img2.cols - _half_w_size
            || p2.y() < _half_w_size || p2.y() > _img2.rows - _half_w_size)
        {
            // fill residuals and jacobians to 0
            for (int i = 0; i < 9; ++i)
            {
                residuals[i] = 0;
                if (jacobians!=nullptr && jacobians[0]!=nullptr)
                {
                    for (int j = 0; j < 7; ++j)
                    {
                        jacobians[0][i*7+j] = 0.0;
                    }
                }
            }
            return true;
        }

        int cnt = 0;
        for (int xx = -_half_w_size; xx <= _half_w_size; ++xx)
        {
            for (int yy = -_half_w_size; yy <= _half_w_size; ++yy)
            {

                double v1 = get(_img1, _p1.x() + xx, _p1.y() + yy);
                double v2 = get(_img2,  p2.x() + xx, p2.y() + yy);
                double err = v1 - v2;
                residuals[cnt] = err;

                if (jacobians && jacobians[0])
                {
                    double dx = 0.5 * (get(_img2, p2.x() + xx + 1, p2.y() + yy) - get(_img2, p2.x() + xx - 1, p2.y() + yy));
                    double dy = 0.5 * (get(_img2, p2.x() + xx, p2.y() + yy + 1) - get(_img2, p2.x() + xx, p2.y() + yy - 1));
                    Eigen::Vector2d dIdu(dx, dy);

                    Eigen::Matrix<double, 2, 3> dudXc;
                    dudXc << fx / Z, 0.0, -X * fx / Z2,
                            0.0, fy / Z, -Y * fy / Z2;

                    Eigen::Matrix<double, 3, 4> dXcdq; // derivative of Xcam wrt. quaternions
                    dXcdq(0, 0) = 2*qy*y3 + 2*qz*z3;
                    dXcdq(0, 1) = 2*qw*z3 + 2*qx*y3 - 4*qy*x3;
                    dXcdq(0, 2) = -2*qw*y3 + 2*qx*z3 - 4*qz*x3;
                    dXcdq(0, 3) = 2*qy*z3 - 2*qz*y3;

                    dXcdq(1, 0) = -2*qw*z3 - 4*qx*y3 + 2*qy*x3;
                    dXcdq(1, 1) = 2*qx*x3 + 2*qz*z3;
                    dXcdq(1, 2) = 2*qw*x3 + 2*qy*z3 - 4*qz*y3;
                    dXcdq(1, 3) = -2*qx*z3 + 2*qz*x3;

                    dXcdq(2, 0) = 2*qw*y3 - 4*qx*z3 + 2*qz*x3;
                    dXcdq(2, 1) = -2*qw*x3 - 4*qy*z3 + 2*qz*y3;
                    dXcdq(2, 2) = 2*qx*x3 + 2*qy*y3;
                    dXcdq(2, 3) = 2*qx*y3 - 2*qy*x3;

                    Eigen::Matrix<double, 1, 7, Eigen::RowMajor> J;
                    J.block<1, 4>(0, 0) = -dIdu.transpose() * dudXc * dXcdq;
                    J.block<1, 3>(0, 4) = -dIdu.transpose() * dudXc;

                    for (int i = 0; i < 7; ++i)
                    {
                        jacobians[0][cnt*7 + i] = J(0, i);
                    }
                }
                ++cnt;
            }
        }

        return true;
    }

    private:
        cv::Mat _img1, _img2;
        Eigen::Vector2d _p1;
        Eigen::Vector3d _P1;
        Eigen::Matrix3d _K;
        int _half_w_size;

};

// TO TEST with autodiff or numeric diff, but not easy to differentiate wrt. image (maybe try to combine Jets + image gradient with chain rule)
// struct PhotometricError
// {
//     PhotometricError(const cv::Mat& img1, const cv::Mat& img2, const Eigen::Vector2d& p1, const Eigen::Vector3d& P1, const Eigen::Matrix3d& K, int half_w_size)
//     : _img1(img1), _img2(img2), _p1(p1), _P1(P1), _K(K), _half_w_size(half_w_size) {}

//     virtual bool operator() (const double* const params,
//                              double *residuals) const {
//         const Eigen::Map<const Sophus::SE3d> Rt(params);

//         Eigen::Vector3d P2 = Rt * _P1;
//         Eigen::Vector3d p2 = _K * P2;
//         p2 /= p2.z();

//         int j = 0;
//         for (int xx = -_half_w_size; xx <= _half_w_size; ++xx)
//         {
//             for (int yy = -_half_w_size; yy <= _half_w_size; ++yy)
//             {
//                 double v1 = get(_img1, _p1.x() + xx, _p1.y() + yy);
//                 double v2 = get(_img2, p2.x() + xx, p2.y() + yy);
//                 // debug << _p1.x() << " " << _p1.y() << " " << p2.x() << " " << p2.y() << "\n";
//                 debug << v1 << " " << v2 << "\n";
//                 double err = v1 - v2;
//                 residuals[j++] = err;
//             }
//         }

//         return true;
//     }

//     private:
//         cv::Mat _img1, _img2;
//         Eigen::Vector2d _p1;
//         Eigen::Vector3d _P1;
//         Eigen::Matrix3d _K;
//         int _half_w_size;
// };

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt // points from cam1 reference frame to cam2
)
{
    int nb_iters = 11;
    int half_w_size = 1;

    ceres::Problem problem;

    problem.AddParameterBlock(Rt.data(), 7, new LocalParameterizationSE3());
    for (int i = 0; i < px_ref.size(); ++i)
    {
        const auto& p1 = px_ref[i];
        Eigen::Vector3d P1 = get_3D_point_from_depth(p1, depth_ref[i], K);
        problem.AddResidualBlock(
            // new ceres::NumericDiffCostFunction<PhotometricError, ceres::CENTRAL, 9, 7>(
            //     new PhotometricError(img1, img2, p1, P1, K, half_w_size)
            // ),
            new PhotometricError(img1, img2, p1, P1, K, half_w_size),
            nullptr,
            Rt.data()
        );
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = false;
    options.max_num_iterations = 11;

    ceres::Solver::Summary summary;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double >>(t2 - t1);
    cout << "optimization with ceres costs time: " << time_used.count() << " seconds." << endl;

    std::cout << "translation: " << Rt.translation().transpose() << "\n";
    std::cout << "rotation: " << Rt.so3().unit_quaternion().toRotationMatrix() << "\n";

}

void DirectPoseEstimationPyramidal(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt)
{
    int nb_levels = 4;
    double factor = 0.5;

    std::vector<cv::Mat> pyr1, pyr2;
    std::vector<double> scales;
    for (int i = 0; i < nb_levels; ++i)
    {
        if (i == 0)
        {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
            scales.push_back(1.0);
        }
        else
        {
            cv::Mat img1_r, img2_r;
            cv::resize(pyr1[i-1], img1_r, cv::Size(pyr1[i-1].cols * factor, pyr1[i-1].rows * factor));
            cv::resize(pyr2[i-1], img2_r, cv::Size(pyr2[i-1].cols * factor, pyr2[i-1].rows * factor));
            pyr1.push_back(img1_r);
            pyr2.push_back(img2_r);
            scales.push_back(scales[i-1] * factor);
        }
    }

    for (int l = nb_levels-1; l >= 0; l--)
    {

        cv::Mat img1_r = pyr1[l];
        cv::Mat img2_r = pyr2[l];
        double scale = scales[l];

        Eigen::Matrix3d K_r = K;
        K_r(0, 0) *= scale;
        K_r(1, 1) *= scale;
        K_r(0, 2) *= scale;
        K_r(1, 2) *= scale;
        auto p_r = px_ref;
        for (auto& p : p_r)
        {
            p *= scale;
        }

        DirectPoseEstimationSingleLayer(img1_r, img2_r, p_r, depth_ref, K_r, Rt);
    }
}

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

Sophus::SE3d r;
Eigen::Matrix<double, 6, 1> d;
d << 1, 2, 3, 0, 0, 0;
r =  Sophus::SE3d::exp(d) * r;
double* rr = r.data();
for (int i = 0; i<7;++i)
std::cout << rr[i] << " ";
cout << "\n\n";

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

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

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

    // estimates 01~05.png's pose using this information
    Sophus::SE3d Rt;
    Eigen::Matrix3d K;
    K << fx, 0.0, cx,
         0.0, fy, cy,
         0.0, 0.0, 1.0;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // try single layer by uncomment this line
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, K, Rt);
        DirectPoseEstimationPyramidal(left_img, img, pixels_ref, depth_ref, K, Rt);

        // plot the projected pixels here
        cv::Mat img2_show;
        cv::cvtColor(img, img2_show, CV_GRAY2BGR);
        std::vector<Eigen::Vector2d> projections(pixels_ref.size());
        for (int i = 0; i < pixels_ref.size(); ++i)
        {
            Eigen::Vector3d P_ref = get_3D_point_from_depth(pixels_ref[i], depth_ref[i], K);
            Eigen::Vector3d uv = K * (Rt * P_ref);
            projections[i] = uv.hnormalized();
        }

        for (size_t i = 0; i < pixels_ref.size(); ++i) {
            auto p_ref = pixels_ref[i];
            auto p_cur = projections[i];
            if (p_cur[0] > 0 && p_cur[1] > 0 && p_cur[0] < img2_show.cols && p_cur[1] < img2_show.rows) {
                cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
                cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                        cv::Scalar(0, 250, 0));
            }
        }
        // cv::imshow("current", img2_show);
        // cv::waitKey();
        cv::imwrite("img_"+std::to_string(i) + "_ceres.png", img2_show);

    }
    // debug.close();
    return 0;
}


#include "plane_param.h"

#include <Eigen/Dense>

PlaneParam::PlaneParam()
{}

PlaneParam::PlaneParam(
		const Eigen::Vector3d& v0,
		const Eigen::Vector3d& v1,
		const Eigen::Vector3d& v2) {

	Eigen::Vector3d V0(v0[0] * v0[2], v0[1] * v0[2], v0[2]);
	Eigen::Vector3d V1(v1[0] * v1[2], v1[1] * v1[2], v1[2]);
	Eigen::Vector3d V2(v2[0] * v2[2], v2[1] * v2[2], v2[2]);
	/*
	Eigen::Vector3d V0(v0);
	Eigen::Vector3d V1(v1);
	Eigen::Vector3d V2(v2);
	*/
	Eigen::Vector3d norm = (V1 - V0).cross(V2 - V0);
	if (norm.norm() < 1e-10 || norm[2] == 0) {
		invalid_ = true;
	}
	norm.normalize();
	d_ = -V0.dot(norm);
	n1_ = norm[0];
	n2_ = norm[1];
	n3_ = norm[2];
	invalid_ = false;
	if (d_ == 0)
		invalid_ = true;
}

bool PlaneParam::ProjectiveIntersection(const PlaneParam& other, LineSegment* l) const {
	if (invalid_ || other.invalid_)
		return false;
	//figure out the intersection line direction
	l->n1 = n2_ * other.n3_ - n3_ * other.n2_;
	l->n2 = n3_ * other.n1_ - n1_ * other.n3_;
	l->n3 = n1_ * other.n2_ - n2_ * other.n1_;

	//figure out one intersection point
	if (l->n1 != 0) {
		l->x = 0;
		l->z = (other.n2_ * d_ - n2_ * other.d_) / l->n1;
		l->y = (-other.n3_ * d_ + n3_ * other.d_) / l->n1;
	}
	else if (l->n2 != 0) {
		l->y = 0;
		l->x = (other.n3_ * d_ - n3_ * other.d_) / l->n2;
		l->z = (-other.n1_ * d_ + n1_ * other.d_) / l->n2;
	}
	else {
		return false;
	}

	if (l->x == 0 && l->y == 0)
		return false;

	if (l->z == 0) {
		l->x += l->n1;
		l->y += l->n2;
		l->z += l->n3;
	}
	K nx, ny;
	if (l->z + l->n3 != 0) {
		nx = (l->x+l->n1) / (l->z+l->n3);
		ny = (l->y+l->n2) / (l->z+l->n3);
	} else if (l->z - l->n3 != 0) {
		nx = (l->x-l->n1) / (l->z-l->n3);
		ny = (l->y-l->n2) / (l->z-l->n3);
	} else {
		return false;
	}

	l->x /= l->z;
	l->y /= l->z;
	l->n1 = nx - l->x;
	l->n2 = ny - l->y;
	return true;
}


#include <stdio.h>  /* for sprintf */
#include <stdlib.h> /* for strtod */
#include <string>
#include <cassert>
#include <iostream>
#include <fstream>
#include <math.h>
#include <time.h>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>
#include <boost/unordered_map.hpp>

using namespace std;
using namespace boost;

const float pi = acos(-1);

int numTerms;
int numVars;
int Order;
vector<vector<double> > multipliers;
vector<vector<double> > obsRanges;

/********** GDK's Fourier Code (Ported from Java) **************/
/**                                                                                * This method iterates through a coefficient vector                                                                                * up to a given degree. (like counting in a base of                                                                                * that degree).                                                                                *                                                                                * @param cthe coefficient vector.                                                                                * @param NVariablesthe number of variables in c.                                                                                * @param Degreethe degree up to which to increment.                                                                                */
void Iterate(int *c, int NVariables, int Degree)
{
  c[NVariables - 1] = c[NVariables-1] + 1;

  if(c[NVariables - 1] > Degree)
    {
      if(NVariables > 1)
        {
          c[NVariables - 1]  = 0;
          Iterate(c, NVariables - 1, Degree);
        }
    }
}

/*
 * Compute the full Fourier Basis coefficient matrix for
 * a given number of variables up to a given order.
 *
 * @param nvarsthe number of variables.
 * @param orderthe highest coefficient value for any individual variable.
 * @returna two dimensional array of doubles. The first dimension length is
 * the number of basis functions, and the second is the number of state variables.
 */
void computeFourierCoefficients(int nvars, int order) {
  int nterms = (int)pow(order + 1.0, nvars);
  numTerms = nterms;
  numVars = nvars;
  Order = order;

  int pos = 0;
  multipliers.resize(nterms);
  for (int i=0; i<nterms; i++)
    multipliers[i].resize(nvars);
  int *c = new int[nvars];
  for(int j = 0; j < nvars; j++)
    c[j] = 0;

  do
    {
      for(int k = 0; k < nvars; k++)
        {
          multipliers[pos][k] = c[k];
        }

      pos++;
      // Iterate c
      Iterate(c, nvars, order);
    }
  while(c[0] <= order);
}

/**
 * Scale a state variable to between 0 and 1.
 * (this is required for the Fourier Basis).
 *
 * @param valthe state variable.
 * @param posthe state variable number.
 * @returnthe normalized state variable.
 */
double scale(double val, int pos)
{
  //  cout << pos << "," << val << " " << obsRanges[pos][0] << "," << obsRanges[pos][1] << " : " << (val - obsRanges[pos][0])/(obsRanges[pos][1] - obsRanges[pos][0]) << endl;
  return (val - obsRanges[pos][0]) / (obsRanges[pos][1] - obsRanges[pos][0]);
}

/**
 * Compute the feature vector for a given state.
 * This is achieved by evaluating each Fourier Basis function
 * at that state.
 *
 * @param sthe state in question.
 * @return a vector of doubles representing each basis function evaluated at s.
 */
vector<double> computeFeatures(vector<double> features)
{
  vector<double> phi(numTerms);
  for(int pos = 0; pos < numTerms; pos++)
    {
      double dsum = 0;
      for(int j = 0; j < numVars; j++)
        {
          double sval = scale(features[j], j);
          dsum += sval*multipliers[pos][j];
        }

      phi[pos] = cos((pi) * dsum);
    }

  return phi;
}

  typedef boost::unordered_map<std::string, int> map;

int main(int argc, char **argv) {
  char input[4096];
  char_separator<char> sep(", "); // lets start with csv and move to VW input later
  char_separator<char> vwsep(":");
  double order = lexical_cast<int>(*++argv);
  //  vector<string> features;
  int vw_offset = 0;
  bool skip_normal = false;
  boost::unordered_map<string, int> features;
  // Default to CSV conversion
  // Take parameter flag: --vw
  // to switch to vw input based
  while(*++argv) {
      if(strcmp(*argv, "--vw") == 0)
          vw_offset = 1;
      else if(strcmp(*argv, "--nonorm") == 0)
  	  skip_normal = true;
      else {
	cout << "Unknown argument: " << *argv << endl;
	exit(1);
      }
  }

  //cout << "Fourier Order: " << order << endl;
  int line_count = 0;
  bool translating = false;

  while(!cin.eof()) {
    bool line_has_data = false;
    cin.getline(input,4096);

    string line(input);
    tokenizer< char_separator<char> > tokens(line, sep);
    vector<double> vanilla_features;
    int feature_counter = 0;

    if(vw_offset > 0 && line_count > 0)  {
      vanilla_features.resize(features.size());
    }
    BOOST_FOREACH (const string& t, tokens) {
      try {
	if(line_count == 0) {
	  obsRanges.push_back(vector<double>(2));
	  obsRanges[feature_counter][0] = 0.0;
	  obsRanges[feature_counter][1] = 1.0;
	}
	// min then max then features
	if(line_count < vw_offset) { // first line, and we ARE doing VW inputs
	  features[t] = feature_counter;
	} else if(line_count == vw_offset && !skip_normal) { // ready for the min-max values
	  obsRanges[feature_counter][0] = lexical_cast<double>(t);
	} else if(line_count == vw_offset+1 && !skip_normal) {
	  obsRanges[feature_counter][1] = lexical_cast<double>(t);
	} else {
	  translating = true;
	  if(vw_offset > 0) {
	    boost::tokenizer< char_separator<char> > vwtoken(t, vwsep);
	    boost::tokenizer< char_separator<char> >::iterator beg=vwtoken.begin();
	    string fname(*beg);
	    if(features.find(*beg) != features.end()) {
	      int findex = features.at(*beg);
	      vanilla_features[findex] = lexical_cast<double>(*++beg);
	    } else {
	      cout << t << " ";
	      feature_counter--;
	      line_has_data = true;
	    }
	  } else {
	    vanilla_features.push_back(lexical_cast<double>(t));
	  }
	  if(feature_counter >= 0 && (vanilla_features[feature_counter] < obsRanges[feature_counter][0] || vanilla_features[feature_counter] > obsRanges[feature_counter][1])) {
	    cout << "ERROR: feature " << feature_counter << " is out of supplied range"<<endl;
	    exit(1);
	  } //else cout << vanilla_features[feature_counter] << endl;
	}
      }
      catch(bad_lexical_cast&)
	{}

      feature_counter++;
    }
    //    cout << "Counts: " << feature_counter << " " << line_count << " " << vanilla_features.size() << endl;
    //head ../uk3day/train.vw | sed '1d' | ./fourie 3 --vw
    if(line_count == 0) {
      if(vw_offset > 0 && features.size() == 0) {
	cout << "ERROR: feature labels not provided" << endl;
	exit(1);
      }
      //      cout << feature_counter << endl;
      //cout << "Num Features: " << feature_counter << endl;
      computeFourierCoefficients(feature_counter, order);
      //      cout << "Num Fourier Terms: " << numTerms << endl;
    }

    if(translating && feature_counter > 0) {
      //cout << "Computing features... " <<endl;
      vector<double> fourie_features = computeFeatures(vanilla_features);
      if(vw_offset > 0) {
	cout <<"FOURIER0:" << fourie_features[0] << " ";
	for(int i=1; i<(int)fourie_features.size(); i++) {
	  cout << "FOURIER" << i << ":" << fourie_features[i] << " ";
	}
	cout << endl;
      } else {
	cout << fourie_features[0];
	for(int i=1; i<(int)fourie_features.size(); i++) {
	  cout << "," << fourie_features[i];
	}
	cout << endl;
      }
    } else if(line_has_data) {
      cout << endl;
    }
    line_count++;
  }
  return 0;
}


#include <algorithm>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <stdexcept>
#include <string>

#ifndef CPPDEBUG /* Ubuntu's Boost does not provide binaries compatible with libstdc++'s debug mode so we just reduce functionality here */
#include <boost/program_options.hpp>
#endif

#include "boost_profile.cpp"
#include "libiop/algebra/fft.hpp"
#include "libiop/algebra/fields/gf64.hpp"
#include "libiop/algebra/fields/gf128.hpp"
#include "libiop/algebra/fields/gf192.hpp"
#include "libiop/algebra/fields/gf256.hpp"
#include "libiop/algebra/field_subset/subspace.hpp"
#include "libiop/common/profiling.hpp"

#ifndef CPPDEBUG
bool process_prover_command_line(const int argc, const char** argv,
                                 std::size_t &log_n_min,
                                 std::size_t &log_n_max,
                                 std::size_t &field_size)
{
    namespace po = boost::program_options;

    try
    {
        po::options_description desc("Usage");
        desc.add_options()
        ("help", "print this help message")
        ("log_n_min", po::value<std::size_t>(&log_n_min)->default_value(8))
        ("log_n_max", po::value<std::size_t>(&log_n_max)->default_value(20))
        ("field_size", po::value<std::size_t>(&field_size)->default_value(64));

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);

        if (vm.count("help"))
        {
            std::cout << desc << "\n";
            return false;
        }

        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << "\n";
        return false;
    }

    return true;
}
#endif

using namespace libiop;

template<typename FieldT>
void instrument_algebra(std::size_t log_n_min,
                        std::size_t log_n_max)
{
    if (log_n_min % 2 != 0)
    {
        log_n_min += 1;
    }
    if (log_n_max % 2 != 0)
    {
        log_n_max += 1;
    }

    for (std::size_t log_n = log_n_min; log_n <= log_n_max; log_n += 2)
    {
        print_separator();
        const std::size_t n = 1ul << log_n;
        print_indent(); printf("* size of n: %zu\n", n);
        const std::size_t sqrt_n = 1ul << (log_n / 2);
        print_indent(); printf("* size of sqrt(n): %zu\n", sqrt_n);

        /* FFT(n) */
        std::vector<FieldT> n_vec;
        enter_block("n");
        for (size_t i = 0; i < n; ++i)
        {
            n_vec.push_back(FieldT::random_element());
        }
        leave_block("n");
        affine_subspace<FieldT> n_subspace = linear_subspace<FieldT>::standard_basis(libiop::log2(n));
        enter_block("FFT(n)");
        std::vector<FieldT> fft_results = additive_FFT<FieldT>(n_vec, n_subspace);
        leave_block("FFT(n)");

        /* sqrt(n) * FFT(sqrt(n)) */
        std::vector<FieldT> sqrt_n_vec;
        enter_block("sqrt(n)");
        for (size_t i = 0; i < sqrt_n; ++i)
        {
            sqrt_n_vec.push_back(FieldT::random_element());
        }
        leave_block("sqrt(n)");
        affine_subspace<FieldT> sqrt_n_subspace = linear_subspace<FieldT>::standard_basis(libiop::log2(sqrt_n));
        enter_block("sqrt(n) * FFT(sqrt(n))");
        for (size_t i = 0; i < sqrt_n; ++i)
        {
            std::vector<FieldT> sqrt_results = additive_FFT<FieldT>(sqrt_n_vec, sqrt_n_subspace);
        }
        leave_block("sqrt(n) * FFT(sqrt(n))");
    }
}

int main(int argc, const char * argv[])
{
    std::size_t log_n_min;
    std::size_t log_n_max;
    std::size_t field_size;

#ifdef CPPDEBUG
    /* set reasonable defaults */
    if (argc > 1)
    {
        printf("There is no argument parsing in CPPDEBUG mode.");
        exit(1);
    }
    libiop::UNUSED(argv);

    log_n_min = 8;
    log_n_max = 20;
    field_size = 64;
#else
    if (!process_prover_command_line(argc, argv, log_n_min, log_n_max, field_size))
    {
        return 1;
    }
#endif

    printf("Selected parameters:\n");
    printf("* log_n_min = %zu\n", log_n_min);
    printf("* log_n_max = %zu\n", log_n_max);

    switch (field_size)
    {
        case 64:
            instrument_algebra<gf64>(log_n_min, log_n_max);
            break;
        case 128:
            instrument_algebra<gf128>(log_n_min, log_n_max);
            break;
        case 192:
            instrument_algebra<gf192>(log_n_min, log_n_max);
            break;
        case 256:
            instrument_algebra<gf256>(log_n_min, log_n_max);
            break;
        default:
            throw std::invalid_argument("Field size not supported.");
    }
}


/*
 * NormalVectorsFilter.hpp
 *
 *  Created on: May 05, 2015
 *      Author: Peter Fankhauser, Martin Wermelinger
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include <filters/filter_base.h>
#include <grid_map_core/grid_map_core.hpp>

#include <Eigen/Core>
#include <string>

namespace grid_map {

/*!
 * Compute the normal vectors of a layer in a map.
 */
template <typename T>
class NormalVectorsFilter : public filters::FilterBase<T> {
 public:
  /*!
   * Constructor
   */
  NormalVectorsFilter();

  /*!
   * Destructor.
   */
  virtual ~NormalVectorsFilter();

  /*!
   * Configures the filter from parameters on the Parameter Server
   */
  virtual bool configure();

  /*!
   * Compute the normal vectors of a layer in a map and
   * saves it as additional grid map layer.
   * @param mapIn grid map containing the layer for which the normal vectors are computed for.
   * @param mapOut grid map containing mapIn and the new layers for the normal vectors.
   */
  virtual bool update(const T& mapIn, T& mapOut);

 private:
  /*!
   * Estimate the normal vector at each point of the input layer by using points within a circle of specified radius.
   *
   * The eigen decomposition of the covariance matrix (3x3) of all data points is used to establish the normal direction.
   * Four cases can be identified when the eigenvalues are ordered in ascending order:
   *    1) The data is in a cloud -> all eigenvalues are non-zero
   *    2) The data is on a plane -> The first eigenvalue is zero
   *    3) The data is on a line -> The first two eigenvalues are zero.
   *    4) The data is in one point -> All eigenvalues are zero
   *
   * Only case 1 & 2 provide enough information the establish a normal direction.
   * The degenerate cases (3 or 4) are identified by checking if the second eigenvalue is zero.
   *
   * The numerical threshold (1e-8) for the eigenvalue being zero is given by the accuracy of the decomposition, as reported by Eigen:
   * https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html
   *
   * Finally, the sign normal vector is correct to be in the same direction as the user defined "normal vector positive axis"
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   */
  void computeWithArea(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  void computeWithRaster(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  enum class Method { Area, Raster };

  Method method_;

  //! Radius of submap for normal vector estimation.
  double estimationRadius_;

  //! Normal vector positive axis.
  Eigen::Vector3d normalVectorPositiveAxis_;

  //! Input layer name.
  std::string inputLayer_;

  //! Output layer name.
  std::string outputLayersPrefix_;
};

}  // namespace grid_map


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

#include "TestsCatchRequire.h"
#include "../core/NeuralNetwork.h"

TEST_CASE("NeuralNetwork can be initialised", "[NeuralNetwork]") {
    // create a basic layout 2 inputs, 2 nodes in the 1st layer 1 in the 2nd
    int nodes[3] = { 2, 2, 1};
    std::vector<int> layout(&nodes[0], &nodes[0]+3);

    SECTION("Creating a network with valid parameters") {
        REQUIRE_NOTHROW(NeuralNetwork(layout, "step"));
    }

    SECTION("Creating a network with sigmoid function") {
        REQUIRE_NOTHROW(NeuralNetwork(layout, "sigmoid"));
    }

    SECTION("Creating a network with invalid function") {
        REQUIRE_THROWS(NeuralNetwork(layout, "somefunc"));
    }

    SECTION("Creating a network without a random seed") {
        REQUIRE_NOTHROW(NeuralNetwork(layout, "step", false));
    }
}

TEST_CASE("Testing NeuralNetwork feedForward", "[NeuralNetwork]") {

    SECTION("Check feedForward with valid parameters") {
        int nodes[3] = { 3, 2, 1};
        std::vector<int> layout(&nodes[0], &nodes[0]+3);

        NeuralNetwork ann(layout, "step", false);

        Eigen::MatrixXd input(1, 3);
        input << 1, 1, 1;

        Eigen::MatrixXd output = ann.feedForward(input);

        REQUIRE(output.value() == 0);
    }

    SECTION("Check feedForward with multiple outputs") {
        int nodes[3] = { 3, 2, 2};
        std::vector<int> layout(&nodes[0], &nodes[0]+3);

        NeuralNetwork ann(layout, "sigmoid", false);

        Eigen::MatrixXd input(1, 3);
        input << 1, 1, 1;

        Eigen::MatrixXd output = ann.feedForward(input);

        REQUIRE(output(0, 0) == Approx(0.276810));
        REQUIRE(output(1, 0) == Approx(0.757047));
    }

    SECTION("Check feedForward single perceptron") {
        int nodes[2] = { 2, 1};
        std::vector<int> layout(&nodes[0], &nodes[0]+2);

        NeuralNetwork ann(layout, "sigmoid", false);

        Eigen::MatrixXd input(1, 2);
        input << 1, 1;

        Eigen::MatrixXd output = ann.feedForward(input);
        REQUIRE(output.value() == Approx(0.16148));
    }

    SECTION("Check feedForward with multiple inputs", "[NeuralNetwork]") {
        int nodes[3] = { 2, 2, 1 };
        std::vector<int> layout(&nodes[0], &nodes[0]+3);

        NeuralNetwork ann(layout, "sigmoid", false);

        Eigen::MatrixXd input(4, 2);
        input << 1, 1,
                 0, 1,
                 1, 0,
                 0, 0;

        Eigen::MatrixXd output = ann.feedForward(input);
        REQUIRE(output.size() == 4);

        Eigen::MatrixXd expectedResult(1, 4);
        expectedResult << 0.36363, 0.323174, 0.350235, 0.30499;
        REQUIRE(output.size() == expectedResult.size());

        for (int i=0; i < output.cols(); ++i) {
            for (int j=0; j < output.rows(); ++j) {
                REQUIRE(output(j, i) == Approx(expectedResult(j, i)));
            }
        }

    }

}

TEST_CASE("Testing NeuralNetwork backPropagate", "[NeuralNetwork]") {
    // create a basic layout 2 inputs, 2 nodes in the 1st layer 1 in the 2nd

    SECTION("Check backprop with normal input", "[NeuralNetwork]") {
        int nodes[3] = { 3, 2, 1};
        std::vector<int> layout(&nodes[0], &nodes[0]+3);

        NeuralNetwork ann(layout, "sigmoid", false);

        Eigen::MatrixXd input(1, 3);
        input << 1, 1, 1;

        Eigen::VectorXd actual(1);
        actual << 1;

        Eigen::MatrixXd output = ann.feedForward(input);

        REQUIRE(output.size() == 1);
        REQUIRE(output.value() == Approx(0.27681));

        REQUIRE_NOTHROW(ann.backPropagate(output, actual));
    }

    SECTION("Check backprop with two outputs", "[NeuralNetwork]") {
        int nodes[3] = { 3, 2, 2};
        std::vector<int> layout(&nodes[0], &nodes[0]+3);

        NeuralNetwork ann(layout, "sigmoid", false);

        Eigen::MatrixXd input(1, 3);
        input << 1, 1, 1;

        Eigen::MatrixXd output = ann.feedForward(input);

        REQUIRE(output.cols() == 1);
        REQUIRE(output.rows() == 2);

        REQUIRE(output(0, 0) == Approx(0.276810));
        REQUIRE(output(1, 0) == Approx(0.757047));

        Eigen::MatrixXd actual(2, 1);
        actual << 1, 1;

        REQUIRE_NOTHROW(ann.backPropagate(output, actual));
    }
}

TEST_CASE("Testing NeuralNetwork multiple inputs", "[NeuralNetwork]") {
    int nodes[3] = { 3, 3, 1 };
    std::vector<int> layout(&nodes[0], &nodes[0]+3);
    NeuralNetwork ann(layout, "sigmoid", false);

    Eigen::MatrixXd input(4, 3);
    input << 1, 1, 1,
             1, 1, 0,
             1, 0, 1,
             1, 0, 0;

    Eigen::MatrixXd output = ann.feedForward(input);
    REQUIRE(output.cols() == 4);
    REQUIRE(output.rows() == 1);

    Eigen::MatrixXd expectedResult(1, 4);
    expectedResult << 0.39573, 0.64993, 0.29571, 0.48559;

    REQUIRE(output.cols() == expectedResult.cols());
    REQUIRE(output.rows() == expectedResult.rows());

    for (int i=0; i < output.cols(); ++i) {
        for (int j=0; j < output.rows(); ++j) {
            REQUIRE(output(j, i) == Approx(expectedResult(j, i)));
        }
    }

    Eigen::MatrixXd actual(1, 4);
    actual << 0, 1, 1, 0;

    REQUIRE_NOTHROW(ann.backPropagate(output, actual));
}

TEST_CASE("Testing NeuralNetwork training", "[NeuralNetwork]") {
    int nodes[3] = { 3, 3, 1 };
    std::vector<int> layout(&nodes[0], &nodes[0]+3);

    NeuralNetwork ann(layout, "sigmoid", false);

    Eigen::MatrixXd input(4, 3);
    input << 1, 1, 1,
             1, 1, 0,
             1, 0, 1,
             1, 0, 0;

    Eigen::MatrixXd actual(1, 4);
    actual << 0, 1, 1, 0;

    REQUIRE_NOTHROW(ann.train(input, actual, 5000, 1));

    Eigen::MatrixXd example(1, 3);
    example << 1, 0, 0;

    Eigen::MatrixXd output = ann.feedForward(example);
    REQUIRE(output.value() == Approx(0.0328009245));
}