#include <Eigen/Dense>
#include <fmt/core.h>
#include <fmt/ostream.h>
#include <iostream>

#include "HittableList.hh"
#include "Sphere.hh"
#include "color.hh"
#include "ray.hh"

auto main(int /*argc*/, char** /*argv*/) -> int
{
    // Image
    constexpr auto aspect_ratio = 16.0 / 9.0;
    constexpr int image_width = 400;
    constexpr int image_height = static_cast<int>(image_width / aspect_ratio);

    // Camera
    constexpr auto viewport_height = 2.0;
    constexpr auto viewport_width = aspect_ratio * viewport_height;
    constexpr auto focal_length = 1.0;

    Eigen::Vector3d origin{0, 0, 0};
    Eigen::Vector3d horizontal{viewport_width, 0, 0};
    Eigen::Vector3d vertical{0, viewport_height, 0};
    Eigen::Vector3d lower_left_corner = origin - horizontal / 2 - vertical / 2 -
                                        Eigen::Vector3d{0, 0, focal_length};

    // World
    hittable::HittableList world;
    world.add(
        std::make_shared<hittable::Sphere>(Eigen::Vector3d{0, 0, -1}, 0.5));
    world.add(std::make_shared<hittable::Sphere>(Eigen::Vector3d{0, -100.5, -1},
                                                 100));

    fmt::print("P3{} {}\n255\n", image_width, image_height);

    for (int j = image_height - 1; j >= 0; j--)
    {
        fmt::print(std::cerr, "\rScanlines remaining: {} ", j);
        for (int i = 0; i < image_width; i++)
        {
            auto u = double(i) / (image_width - 1);
            auto v = double(j) / (image_height - 1);
            ray::Ray r{origin, lower_left_corner + u * horizontal +
                                   v * vertical - origin};
            color::Color pixColor = ray::ray_color(r, world);
            color::write_color(std::cout, ray::ray_color(r));
        }
    }

    return 0;
}


#include "gradient_sh.hpp"

#include <Eigen/Dense>
#include <cmath>
#include <complex>
#include <fmt/format.h>
#include <fmt/ostream.h>
#include <fstream>
#include <memory>
#include <utility>

using namespace Eigen;
using namespace std::complex_literals;

using std::exp;
using std::pow;
using std::sqrt;
using complex_d = std::complex<double>;

const double PI = 3.14159265358979323846;

namespace grad_sh {

GRTCoeff::GRTCoeff(const Ref<const ArrayXXd> model, const double freq,
                   const double c)
    : z_(model.col(1)), rho_(model.col(2)), beta_(model.col(3)),
      alpha_(model.col(4)), mu_(rho_ * beta_.pow(2)), nl_(model.rows()),
      angfreq_(2.0 * PI * freq), c_(c), nv_(VectorXcd::Zero(nl_)),
      e_(nl_, Matrix2cd::Zero()), t_d_(VectorXcd::Zero(nl_)),
      r_ud_(VectorXcd::Zero(nl_)), r_du_(VectorXcd::Zero(nl_)),
      t_u_(VectorXcd::Zero(nl_)), gt_d_(VectorXcd::Zero(nl_)),
      gr_ud_(VectorXcd::Zero(nl_)), gr_du_(VectorXcd::Zero(nl_ + 1)),
      gt_u_(VectorXcd::Zero(nl_)), cd_(VectorXcd::Zero(nl_)),
      cu_(VectorXcd::Zero(nl_)) {

  initialize_nv();
  initialize_E();

  compute_rtc();
  compute_grtc();
  compute_CdCu();
}

void GRTCoeff::initialize_E() {
  for (int i = 0; i < nl_; ++i) {
    complex_d e21 = -mu_(i) * nv_(i);
    e_[i] << 1.0, 1.0, e21, -e21;
  }
}

void GRTCoeff::initialize_nv() {
  for (int i = 0; i < nl_; ++i) {
    complex_d val =
        std::sqrt(pow(angfreq_ / c_, 2) - pow(angfreq_ / beta_(i), 2));
    if (val.real() < 0) {
      val = -val;
    }
    nv_(i) = val;
  }
}

complex_d GRTCoeff::get_Ad(const double z, const int ind_layer) const {
  return exp(-nv_(ind_layer) * (z - z_(ind_layer)));
}

complex_d GRTCoeff::get_Au(const double z, const int ind_layer) const {
  return exp(-nv_(ind_layer) * (z_(ind_layer + 1) - z));
}

complex_d GRTCoeff::get_Ad_der(const double z, const int ind_layer) const {
  return -nv_(ind_layer) * exp(-nv_(ind_layer) * (z - z_(ind_layer)));
}

complex_d GRTCoeff::get_Au_der(const double z, const int ind_layer) const {
  if (ind_layer == nl_ - 1) {
    return 0.0;
  }

  return nv_(ind_layer) * exp(-nv_(ind_layer) * (z_(ind_layer + 1) - z));
}

void GRTCoeff::compute_rtc() {
  for (int i = 1; i < nl_ - 1; ++i) {
    auto &e0 = e_[i - 1];
    auto &e1 = e_[i];

    Matrix2cd mat1;
    mat1 << e1(0, 0), -e0(0, 1), e1(1, 0), -e0(1, 1);

    // complex_d ad = exp(-nv_(i - 1) * (z_(i) - z_(i - 1)));
    // complex_d au = exp(-nv_(i) * (z_(i + 1) - z_(i)));
    complex_d ad = get_Ad(z_(i), i - 1);
    complex_d au = get_Au(z_(i), i);
    Matrix2cd mat2;
    mat2 << e0(0, 0) * ad, -e1(0, 1) * au, e0(1, 0) * ad, -e1(1, 1) * au;

    Matrix2cd result = mat1.inverse() * mat2;
    t_d_(i) = result(0, 0);
    r_ud_(i) = result(0, 1);
    r_du_(i) = result(1, 0);
    t_u_(i) = result(1, 1);
  }

  int i = nl_ - 1;
  auto &e0 = e_[i - 1];
  auto &e1 = e_[i];
  Matrix2cd mat1;
  mat1 << e1(0, 0), -e0(0, 1), e1(1, 0), -e0(1, 1);
  // complex_d ad = exp(-nv_(i - 1) * (z_(i) - z_(i - 1)));
  complex_d ad = get_Ad(z_(nl_ - 1), nl_ - 2);
  Matrix<complex_d, 2, 1> mat2;
  mat2 << e0(0, 0) * ad, e0(1, 0) * ad;

  Matrix<complex_d, 2, 1> result = mat1.inverse() * mat2;
  t_d_(i) = result(0, 0);
  r_du_(i) = result(1, 0);
}

void GRTCoeff::compute_grtc() {
  for (int i = nl_ - 1; i >= 1; --i) {
    gt_d_(i) = 1.0 / (1.0 - r_ud_(i) * gr_du_(i + 1)) * t_d_(i);
    gr_du_(i) = r_du_(i) + t_u_(i) * gr_du_(i + 1) * gt_d_(i);
  }
  auto &e0 = e_[0];
  // complex_d au = exp(-nv_(0) * (z_(1) - z_(0)));
  complex_d au = get_Au(z_(0), 0);
  gr_ud_(0) = -1.0 / e0(1, 0) * e0(1, 1) * au;
  for (int i = 1; i < nl_; ++i) {
    gt_u_(i) = 1.0 / (1.0 - r_du_(i) * gr_ud_(i - 1)) * t_u_(i);
    gr_ud_(i) = r_ud_(i) + t_d_(i) * gr_ud_(i - 1) * gt_u_(i);
  }
}

void GRTCoeff::compute_CdCu() {
  cd_(0) = 1.0;
  cu_(0) = gr_du_(1) * cd_(0);
  for (int i = 1; i < nl_ - 1; ++i) {
    cd_(i) = gt_d_(i) * cd_(i - 1);
    cu_(i) = gr_du_(i + 1) * cd_(i);
  }
  cd_(nl_ - 1) = gt_d_(nl_ - 1) * cd_(nl_ - 2);
}

IntegralLayer::IntegralLayer(const Ref<const ArrayXXd> model, const double freq,
                             const double c)
    : grtc_(std::make_unique<GRTCoeff>(model, freq, c)), nl_(model.rows()),
      k_(2.0 * PI * freq / c), pvel_(c), z_(model.col(1)), beta_(model.col(3)),
      rho_(model.col(2)), mu_(rho_ * beta_ * beta_), nv_(grtc_->nv_),
      thickness_(nl_ - 1), cd_(grtc_->cd_), cu_(grtc_->cu_),
      matP_u_u_(nl_, Array22cd::Zero()), matP_uc_u_(nl_, Array22cd::Zero()),
      matP_uc_uc_(nl_, Array22cd::Zero()), matP_du_du_(nl_, Array22cd::Zero()),
      matP_duc_du_(nl_, Array22cd::Zero()),
      matP_duc_duc_(nl_, Array22cd::Zero()),
      sigma_x_sigma_top_(nl_, Array22cd::Zero()),
      sigmac_x_sigma_top_(nl_, Array22cd::Zero()),
      sigma_x_sigmac_top_(nl_, Array22cd::Zero()),
      sigmac_x_sigmac_top_(nl_, Array22cd::Zero()),
      sigma_x_sigma_bottom_(nl_, Array22cd::Zero()),
      sigmac_x_sigma_bottom_(nl_, Array22cd::Zero()),
      sigma_x_sigmac_bottom_(nl_, Array22cd::Zero()),
      sigmac_x_sigmac_bottom_(nl_, Array22cd::Zero()),
      int_ut2_(ArrayXd::Zero(nl_)), int_dut2_(ArrayXd::Zero(nl_)) {
  for (int i = 0; i < nl_ - 1; ++i) {
    thickness_(i) = z_(i + 1) - z_(i);
  }

  initialize_P();
  initialize_sigma();

  integrate_ut2();
  integrate_dut2();
}

void IntegralLayer::initialize_P() {
  for (auto i = 0; i < nl_; ++i) {
    const complex_d nv = nv_(i);
    const complex_d nvc = std::conj(nv_(i));

    matP_u_u_[i] << 1.0 / (-nv - nv), 1.0, 1.0, 1.0 / (nv + nv);
    matP_uc_u_[i] << 1.0 / (-nvc - nv), 1.0 / (-nvc + nv), 1.0 / (nvc - nv),
        1.0 / (nvc + nv);
    matP_uc_uc_[i] << 1.0 / (-nvc - nvc), 1.0, 1.0, 1.0 / (nvc + nvc);
    matP_du_du_[i] << nv / (-2.0), -nv * nv, -nv * nv, nv / 2.0;
    matP_duc_du_[i] << nvc * nv / (-nvc - nv), -nvc * nv / (-nvc + nv),
        -nvc * nv / (nvc - nv), nvc * nv / (nvc + nv);
    matP_duc_duc_[i] << nvc / (-2.0), -nvc * nvc, -nvc * nvc, nvc / 2.0;

    if (pvel_ > beta_(i)) {
      matP_uc_u_[i](0, 0) = 1.0;
      matP_uc_u_[i](1, 1) = 1.0;
      matP_duc_du_[i](0, 0) = nvc * nv;
      matP_duc_du_[i](1, 1) = nvc * nv;
    } else {
      matP_uc_u_[i](0, 1) = 1.0;
      matP_uc_u_[i](1, 0) = 1.0;
      matP_duc_du_[i](0, 1) = -nvc * nv;
      matP_duc_du_[i](1, 0) = -nvc * nv;
    }
  }
}

void IntegralLayer::initialize_sigma() {
  MatrixXcd sigma_bottom = MatrixXcd::Zero(2, nl_);
  MatrixXcd sigma_top = MatrixXcd::Zero(2, nl_);
  for (auto i = 0; i < nl_ - 1; ++i) {
    sigma_bottom.col(i) << cd_(i), cu_(i) * exp(-nv_(i) * thickness_(i));
    sigma_top.col(i) << cd_(i) * exp(-nv_(i) * thickness_(i)), cu_(i);
  }
  sigma_bottom.col(nl_ - 1) << cd_(nl_ - 1), 0;
  sigma_top.col(nl_ - 1).fill(0);

  for (auto id_layer = 0; id_layer < nl_; ++id_layer) {
    for (auto i = 0; i < 2; ++i) {
      for (auto j = 0; j < 2; ++j) {
        sigma_x_sigma_top_[id_layer](i, j) =
            sigma_top(i, id_layer) * sigma_top(j, id_layer);
        sigmac_x_sigma_top_[id_layer](i, j) =
            conj(sigma_top(i, id_layer)) * sigma_top(j, id_layer);
        sigma_x_sigmac_top_[id_layer](i, j) =
            sigma_top(i, id_layer) * conj(sigma_top(j, id_layer));
        sigmac_x_sigmac_top_[id_layer](i, j) =
            conj(sigma_top(i, id_layer)) * conj(sigma_top(j, id_layer));

        sigma_x_sigma_bottom_[id_layer](i, j) =
            sigma_bottom(i, id_layer) * sigma_bottom(j, id_layer);
        sigmac_x_sigma_bottom_[id_layer](i, j) =
            conj(sigma_bottom(i, id_layer)) * sigma_bottom(j, id_layer);
        sigma_x_sigmac_bottom_[id_layer](i, j) =
            sigma_bottom(i, id_layer) * conj(sigma_bottom(j, id_layer));
        sigmac_x_sigmac_bottom_[id_layer](i, j) =
            conj(sigma_bottom(i, id_layer)) * conj(sigma_bottom(j, id_layer));
      }
    }
    if (id_layer != nl_ - 1) {
      sigma_x_sigma_top_[id_layer](0, 1) *= thickness_(id_layer);
      sigma_x_sigma_top_[id_layer](1, 0) *= thickness_(id_layer);
      sigmac_x_sigmac_top_[id_layer](0, 1) *= thickness_(id_layer);
      sigmac_x_sigmac_top_[id_layer](1, 0) *= thickness_(id_layer);
      if (pvel_ > beta_(id_layer)) {
        sigmac_x_sigma_top_[id_layer](0, 0) =
            conj(cd_(id_layer)) * cd_(id_layer) * thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](1, 1) =
            conj(cu_(id_layer)) * cu_(id_layer) * thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](0, 0) =
            cd_(id_layer) * conj(cd_(id_layer)) * thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](1, 1) =
            cu_(id_layer) * conj(cu_(id_layer)) * thickness_(id_layer);
      } else {
        sigmac_x_sigma_top_[id_layer](0, 1) *= thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](1, 0) *= thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](0, 1) *= thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](1, 0) *= thickness_(id_layer);
      }
      sigma_x_sigma_bottom_[id_layer](0, 1) = 0.;
      sigma_x_sigma_bottom_[id_layer](1, 0) = 0.;
      sigmac_x_sigmac_bottom_[id_layer](0, 1) = 0.;
      sigmac_x_sigmac_bottom_[id_layer](1, 0) = 0.;

      if (pvel_ > beta_(id_layer)) {
        sigmac_x_sigma_bottom_[id_layer](0, 0) = 0.;
        sigmac_x_sigma_bottom_[id_layer](1, 1) = 0.;
        sigma_x_sigmac_bottom_[id_layer](0, 0) = 0.;
        sigma_x_sigmac_bottom_[id_layer](1, 1) = 0.;
      } else {
        sigmac_x_sigma_bottom_[id_layer](0, 1) = 0.;
        sigmac_x_sigma_bottom_[id_layer](1, 0) = 0.;
        sigma_x_sigmac_bottom_[id_layer](0, 1) = 0.;
        sigma_x_sigmac_bottom_[id_layer](1, 0) = 0.;
      }
    }
  }
}

double IntegralLayer::intker_ut2_top(int id_layer) {
  Array22cd auu = matP_u_u_[id_layer] * sigma_x_sigma_top_[id_layer];
  Array22cd aucu = matP_uc_u_[id_layer] * sigmac_x_sigma_top_[id_layer];
  Array22cd aucuc = matP_uc_uc_[id_layer] * sigmac_x_sigmac_top_[id_layer];
  complex_d result = auu.sum() + 2.0 * aucu.sum() + aucuc.sum();
  return result.real() / 4.0;
}

double IntegralLayer::intker_ut2_bottom(int id_layer) {
  Array22cd auu = matP_u_u_[id_layer] * sigma_x_sigma_bottom_[id_layer];
  Array22cd aucu = matP_uc_u_[id_layer] * sigmac_x_sigma_bottom_[id_layer];
  Array22cd aucuc = matP_uc_uc_[id_layer] * sigmac_x_sigmac_bottom_[id_layer];
  complex_d result = auu.sum() + 2.0 * aucu.sum() + aucuc.sum();
  return result.real() / 4.0;
}

double IntegralLayer::intker_dut2_top(int id_layer) {
  Array22cd auu = matP_du_du_[id_layer] * sigma_x_sigma_top_[id_layer];
  Array22cd aucu = matP_duc_du_[id_layer] * sigmac_x_sigma_top_[id_layer];
  Array22cd aucuc = matP_duc_duc_[id_layer] * sigmac_x_sigmac_top_[id_layer];
  complex_d result = auu.sum() + 2.0 * aucu.sum() + aucuc.sum();
  return result.real() / 4.0;
}

double IntegralLayer::intker_dut2_bottom(int id_layer) {
  Array22cd auu = matP_du_du_[id_layer] * sigma_x_sigma_bottom_[id_layer];
  Array22cd aucu = matP_duc_du_[id_layer] * sigmac_x_sigma_bottom_[id_layer];
  Array22cd aucuc = matP_duc_duc_[id_layer] * sigmac_x_sigmac_bottom_[id_layer];
  complex_d result = auu.sum() + 2.0 * aucu.sum() + aucuc.sum();
  return result.real() / 4.0;
}

void IntegralLayer::integrate_ut2() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_ut2_(id_layer) = intker_ut2_top(id_layer) - intker_ut2_bottom(id_layer);
  }
}

void IntegralLayer::integrate_dut2() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_dut2_(id_layer) =
        intker_dut2_top(id_layer) - intker_dut2_bottom(id_layer);
  }
}

double IntegralLayer::compute_I1() {
  ArrayXd ker = rho_ * int_ut2_;
  double i2 = 0.5 * ker.sum();
  return i2;
}

double IntegralLayer::compute_I2() {
  ArrayXd ker = mu_ * int_ut2_;
  double i2 = 0.5 * ker.sum();
  return i2;
}

double IntegralLayer::compute_I3() {
  ArrayXd ker = mu_ * int_dut2_;
  double i2 = 0.5 * ker.sum();
  return i2;
}

ArrayXd IntegralLayer::compute_kvs() {
  ArrayXd kvs(nl_);
  for (int i = 0; i < nl_; ++i) {
    kvs(i) =
        0.5 * rho_(i) * beta_(i) * (int_ut2_(i) + int_dut2_(i) / pow(k_, 2));
  }
  return kvs;
}

GradientSH::~GradientSH() = default;

ArrayXd GradientSH::compute(const double freq, const double c) const {
  IntegralLayer intl(model_, freq, c);
  double I2 = intl.compute_I2();
  ArrayXd kvs = intl.compute_kvs();

  kvs *= c / I2;

  return kvs;
}

} // namespace grad_sh


//
// Copyright 2014 Mitsubishi Electric Research Laboratories All
// Rights Reserved.
//
// Permission to use, copy and modify this software and its
// documentation without fee for educational, research and non-profit
// purposes, is hereby granted, provided that the above copyright
// notice, this paragraph, and the following three paragraphs appear
// in all copies.
//
// To request permission to incorporate this software into commercial
// products contact: Director; Mitsubishi Electric Research
// Laboratories (MERL); 201 Broadway; Cambridge, MA 02139.
//
// IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT,
// INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING
// LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
// DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF
// SUCH DAMAGES.
//
// MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN
// "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE,
// SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
//
#pragma once
//
// Note:
// If you use dsyevh3 library [http://www.mpi-hd.mpg.de/personalhomes/globes/3x3/],
// this function can be accelerated by uncommenting the following line
//#define USE_DSYEVH3
//
#ifdef USE_DSYEVH3
	#include "dsyevh3/dsyevh3.h"
#else
	#include <Eigen/Core>
	#include <Eigen/Dense>
#endif

#include <cmath>
#include <limits>

namespace LA {
	//s[0]<=s[1]<=s[2], V[:][i] correspond to s[i]
	inline static bool eig33sym(double K[3][3], double s[3], double V[3][3])
	{
#ifdef USE_DSYEVH3
		double tmpV[3][3];
		if(dsyevh3(K, tmpV, s)!=0) return false;

		int order[]={0,1,2};
		for(int i=0; i<3; ++i) {
			for(int j=i+1; j<3; ++j) {
				if(s[i]>s[j]) {
					double tmp=s[i];
					s[i]=s[j];
					s[j]=tmp;
					int tmpor=order[i];
					order[i]=order[j];
					order[j]=tmpor;
				}
			}
		}
		V[0][0]=tmpV[0][order[0]]; V[0][1]=tmpV[0][order[1]]; V[0][2]=tmpV[0][order[2]];
		V[1][0]=tmpV[1][order[0]]; V[1][1]=tmpV[1][order[1]]; V[1][2]=tmpV[1][order[2]];
		V[2][0]=tmpV[2][order[0]]; V[2][1]=tmpV[2][order[1]]; V[2][2]=tmpV[2][order[2]];
#else
		//below we did not specify row major since it does not matter, K==K'
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(
			Eigen::Map<Eigen::Matrix3d>(K[0], 3, 3) );
		Eigen::Map<Eigen::Vector3d>(s,3,1)=es.eigenvalues();
		//below we need to specify row major since V!=V'
		Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>>(V[0],3,3)=es.eigenvectors();
#endif
		return true;
	}
}//end of namespace LA


/*****************************************************************************
*
* Copyright (C) 2015-2017 by Synge Todo <wistaria@phy.s.u-tokyo.ac.jp>
*
* 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)
*
*****************************************************************************/

// Calculating free energy density of square lattice Ising model by the transfer matrix method

#include <cmath>
#include <vector>
#include <boost/array.hpp>
#include <lattice/square.hpp>
#include <lse/exp_number.hpp>

#ifndef ISING_SQUARE_TRANSFER_MATRIX_HPP
#define ISING_SQUARE_TRANSFER_MATRIX_HPP

namespace ising {
namespace square {

struct transfer_matrix {
public:
  typedef lse::exp_double exp_double;
  template<typename VEC>
  static exp_double product_D(double beta, std::vector<double> const& inter_x,
                                   std::vector<double> const& field, VEC& v) {
    int width = inter_x.size();
    int dim = 1 << width;
    exp_double normal = 1;
    std::vector<boost::array<double, 2> > weight(width);
    for (int b = 0; b < width; ++b) {
      double offset = std::abs(beta * inter_x[b]);
      normal *= lse::exp_value(offset);
      weight[b][0] = std::exp(beta * inter_x[b] - offset);
      weight[b][1] = std::exp(-beta * inter_x[b] - offset);
    }
    for (int c = 0; c < dim; ++c) {
      double elem = v[c];
      for (int b = 0; b < width; ++b) {
        int s0 = b;
        int s1 = (b+1) % width;
        elem *= weight[b][((c >> s0) & 1) ^ ((c >> s1) & 1)];
      }
      v[c] = elem;
    }
    if (field.size()) {
      for (int s = 0; s < width; ++s) {
        double offset = std::abs(beta * field[s]);
        normal *= lse::exp_value(offset);
        weight[s][0] = std::exp(beta * field[s] - offset);
        weight[s][1] = std::exp(-beta * field[s] - offset);
      }
      for (int c = 0; c < dim; ++c) {
        double elem = v[c];
        for (int s = 0; s < width; ++s) elem *= weight[s][((c >> s) & 1)];
        v[c] = elem;
      }
    }
    return normal;
  }

  template<typename VEC>
  static exp_double product_D(double beta, std::vector<double> const& inter_x, VEC& v) {
    std::vector<double> field(0);
    return product_D(beta, inter_x, field, v);
  }

  template<typename VEC>
  static exp_double product_U(double beta, std::vector<double> const& inter_y, VEC& v) {
    int width = inter_y.size();
    int dim = 1 << width;
    exp_double normal = 1;
    boost::array<double, 2> weight;
    for (int s = 0; s < width; ++s) {
      double offset = std::abs(beta * inter_y[s]);
      normal *= lse::exp_value(offset);
      weight[0] = std::exp(beta * inter_y[s] - offset);
      weight[1] = std::exp(-beta * inter_y[s] - offset);
      for (int c0 = 0; c0 < dim; ++c0) {
        if (((c0 >> s) & 1) == 0) {
          int c1 = c0 ^ (1 << s);
          double v0 = v[c0];
          double v1 = v[c1];
          v[c0] = weight[0] * v0 + weight[1] * v1;
          v[c1] = weight[0] * v1 + weight[1] * v0;
        }
      }
    }
    return normal;
  }

  static double free_energy(double beta, lattice::square const& lat,
                            std::vector<double> const& inter,
                            std::vector<double> const& field = std::vector<double>(0)) {
    int Lx = lat.get_length_x();
    int Ly = lat.get_length_y();
    std::vector<double> inter_x(Lx), inter_y(Lx), field_x(field.size() > 0 ? Lx : 0);
    int dim = 1 << Lx;
    std::vector<double> v(dim);
    exp_double sum = 0;
    for (int i = 0; i < dim; ++i) {
      exp_double weight = 1;
      for (int j = 0; j < dim; ++j) v[j] = 0;
      v[i] = 1;
      for (int y = 0; y < Ly; ++y) {
        for (int x = 0; x < Lx; ++x) {
          inter_x[x] = inter[2 * (Lx * y + x)];
          inter_y[x] = inter[2 * (Lx * y + x) + 1];
        }
        if (field_x.size()) {
          for (int x = 0; x < Lx; ++x) {
            field_x[x] = field[Lx * y + x];
          }
        }
        weight *= ising::square::transfer_matrix::product_D(beta, inter_x, field_x, v);
        weight *= ising::square::transfer_matrix::product_U(beta, inter_y, v);
      }
      weight *= v[i];
      sum += weight;
    }
    return -log(sum) / beta;
  }

  static double free_energy(double beta, lattice::square const& lat, double J, double H = 0.0) {
    std::vector<double> inter(lat.num_bonds(), J);
    std::vector<double> field((H != 0.0) ? lat.num_sites() : 0, H);
    return free_energy(beta, lat, inter, field);
  }

  static double free_energy_density(double beta, lattice::square const& lat,
                                    std::vector<double> const& inter,
                                    std::vector<double> const& field = std::vector<double>(0)) {
    return free_energy(beta, lat, inter, field) / lat.num_sites();
  }

  static double free_energy_density(double beta, lattice::square const& lat, double J,
                                    double H = 0.0) {
    return free_energy(beta, lat, J, H) / lat.num_sites();
  }
};

} // end namespace square
} // end namespace ising

#endif // ISING_SQUARE_TRANSFER_MATRIX_HPP


//
//  dt_util.h
//  Classifer_RF
//
//  Created by jimmy on 2017-02-16.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#ifndef __Classifer_RF__dt_util__
#define __Classifer_RF__dt_util__

// decision tree util
#include <stdio.h>
#include <vector>
#include <Eigen/Dense>
#include <unordered_map>
#include <string>

using std::vector;
using std::string;
using Eigen::VectorXf;
using Eigen::VectorXd;
using std::vector;

class DTUtil
{
public:
    // spatial variance objective
    template <class T>
    static double spatialVariance(const vector<T> & labels, const vector<unsigned int> & indices);

    // mean and standard deviation
    template <class T>
    static void meanStddev(const vector<T> & labels, const vector<unsigned int> & indices, T & mean, T & sigma);

    // mean value of data
    // mask: index of data
    template <class T>
    static T mean(const vector<T> & data,
                  const vector<unsigned int> & mask);

    // mean value of data
    template <class T>
    static T mean(const vector<T> & data);

    // mean value and median value of errors
    // median value: each dimension is independently computed
    template <class T>
    static void meanMedianError(const vector<T> & errors, T & mean, T & median);

    // balance objective
    static double balanceLoss(const int leftNodeSize, const int rightNodeSize);

    // [start, step, end)
    template <class T>
    static vector<T> range(int start, int end, int step)
    {
        assert((end - start) * step >= 0);
        vector<T> ret;
        for (int i = start; i < end; i += step) {
            ret.push_back((T)i);
        }
        return ret;
    }

};

#endif /* defined(__Classifer_RF__dt_util__) */


#include <test_common.h>
#include <igl/per_face_normals.h>
#include <Eigen/Geometry>

TEST_CASE("per_face_normals: dot", "[igl]")
{
  const auto test_case = [](const std::string &param)
  {
	  Eigen::MatrixXd V,N;
	  Eigen::MatrixXi F;
	  // Load example mesh: GetParam() will be name of mesh file
	  test_common::load_mesh(param, V, F);
	  igl::per_face_normals(V,F,N);
	  REQUIRE (N.rows() == F.rows());
	  for(int f = 0;f<N.rows();f++)
	  {
	    for(int c = 0;c<3;c++)
	    {
	      // Every half-edge dot the normal should be 0
	      REQUIRE(std::abs((V.row(F(f,c))-V.row(F(f,(c+1)%3))).dot(N.row(f))) < 1e-12);
	    }
	  }
	  // REQUIRE (b == a);
	  // REQUIRE (a==b);
	  // ASSERT_NEAR(a,b,1e-15)
	  // REQUIRE (1e-12 > a);
  };

  test_common::run_test_cases(test_common::all_meshes(), test_case);
}


/*
 * bulirsch_stoer.cpp
 *
 * Copyright 2011-2013 Mario Mulansky
 * Copyright 2011-2012 Karsten Ahnert
 *
 * 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 <fstream>
#define _USE_MATH_DEFINES
#include <cmath>

#include <boost/array.hpp>
#include <boost/ref.hpp>

#include <boost/numeric/odeint/config.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp>

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

typedef boost::array< double , 1 > state_type;

/*
 * x' = ( - x*sin t  + 2 tan x ) y
 * with x( pi/6 ) = 2/sqrt(3) the analytic solution is 1/cos t
 */

void rhs( const state_type &x , state_type &dxdt , const double t )
{
    dxdt[0] = ( - x[0] * sin( t ) + 2.0 * tan( t ) ) * x[0];
}

void rhs2( const state_type &x , state_type &dxdt , const double t )
{
    dxdt[0] = sin(t);
}

ofstream out;

void write_out( const state_type &x , const double t )
{
    out << t << '\t' << x[0] << endl;
}

int main()
{
    bulirsch_stoer_dense_out< state_type > stepper( 1E-8 , 0.0 , 0.0 , 0.0 );
    bulirsch_stoer< state_type > stepper2( 1E-8 , 0.0 , 0.0 , 0.0 );

    state_type x = {{ 2.0 / sqrt(3.0) }};

    double t = M_PI/6.0;
    //double t = 0.0;
    double dt = 0.01;
    double t_end = M_PI/2.0 - 0.1;
    //double t_end = 100.0;

    out.open( "bs.dat" );
    out.precision(16);
    integrate_const( stepper , rhs , x , t , t_end , dt , write_out );
    out.close();

    x[0] = 2.0 / sqrt(3.0);

    out.open( "bs2.dat" );
    out.precision(16);
    integrate_adaptive( stepper , rhs , x , t , t_end , dt , write_out );
    out.close();

    x[0] = 2.0 / sqrt(3.0);

    out.open( "bs3.dat" );
    out.precision(16);
    integrate_adaptive( stepper2 , rhs , x , t , t_end , dt , write_out );
    out.close();

    typedef runge_kutta_dopri5< state_type > dopri5_type;
    typedef controlled_runge_kutta< dopri5_type > controlled_dopri5_type;
    typedef dense_output_runge_kutta< controlled_dopri5_type > dense_output_dopri5_type;

    dense_output_dopri5_type dopri5 = make_dense_output( 1E-9 , 1E-9 , dopri5_type() );

    x[0] = 2.0 / sqrt(3.0);

    out.open( "bs4.dat" );
    out.precision(16);
    integrate_adaptive( dopri5 , rhs , x , t , t_end , dt , write_out );
    out.close();

}


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

#include <Eigen/Dense>
#include <cmath>

namespace cupoch {
namespace visualization {
namespace gl_helper {

GLMatrix4f LookAt(const Eigen::Vector3f &eye,
                  const Eigen::Vector3f &lookat,
                  const Eigen::Vector3f &up) {
    Eigen::Vector3f front_dir = (eye - lookat).normalized();
    Eigen::Vector3f up_dir = up.normalized();
    Eigen::Vector3f right_dir = up_dir.cross(front_dir).normalized();
    up_dir = front_dir.cross(right_dir).normalized();

    Eigen::Matrix4f mat = Eigen::Matrix4f::Zero();
    mat.block<1, 3>(0, 0) = right_dir.transpose();
    mat.block<1, 3>(1, 0) = up_dir.transpose();
    mat.block<1, 3>(2, 0) = front_dir.transpose();
    mat(0, 3) = -right_dir.dot(eye);
    mat(1, 3) = -up_dir.dot(eye);
    mat(2, 3) = -front_dir.dot(eye);
    mat(3, 3) = 1.0;
    return mat.cast<GLfloat>();
}

GLMatrix4f Perspective(float field_of_view_,
                       float aspect,
                       float z_near,
                       float z_far) {
    Eigen::Matrix4f mat = Eigen::Matrix4f::Zero();
    float fov_rad = field_of_view_ / 180.0 * M_PI;
    float tan_half_fov = std::tan(fov_rad / 2.0);
    mat(0, 0) = 1.0 / aspect / tan_half_fov;
    mat(1, 1) = 1.0 / tan_half_fov;
    mat(2, 2) = -(z_far + z_near) / (z_far - z_near);
    mat(3, 2) = -1.0;
    mat(2, 3) = -2.0 * z_far * z_near / (z_far - z_near);
    return mat.cast<GLfloat>();
}

GLMatrix4f Ortho(float left,
                 float right,
                 float bottom,
                 float top,
                 float z_near,
                 float z_far) {
    Eigen::Matrix4f mat = Eigen::Matrix4f::Zero();
    mat(0, 0) = 2.0 / (right - left);
    mat(1, 1) = 2.0 / (top - bottom);
    mat(2, 2) = -2.0 / (z_far - z_near);
    mat(0, 3) = -(right + left) / (right - left);
    mat(1, 3) = -(top + bottom) / (top - bottom);
    mat(2, 3) = -(z_far + z_near) / (z_far - z_near);
    mat(3, 3) = 1.0;
    return mat.cast<GLfloat>();
}

Eigen::Vector3f Project(const Eigen::Vector3f &point,
                        const GLMatrix4f &mvp_matrix,
                        const int width,
                        const int height) {
    Eigen::Vector4f pos = mvp_matrix.cast<float>() *
                          Eigen::Vector4f(point(0), point(1), point(2), 1.0);
    if (pos(3) == 0.0) {
        return Eigen::Vector3f::Zero();
    }
    pos /= pos(3);
    return Eigen::Vector3f((pos(0) * 0.5 + 0.5) * (float)width,
                           (pos(1) * 0.5 + 0.5) * (float)height,
                           (1.0 + pos(2)) * 0.5);
}

Eigen::Vector3f Unproject(const Eigen::Vector3f &screen_point,
                          const GLMatrix4f &mvp_matrix,
                          const int width,
                          const int height) {
    Eigen::Vector4f point =
            mvp_matrix.cast<float>().inverse() *
            Eigen::Vector4f(screen_point(0) / (float)width * 2.0 - 1.0,
                            screen_point(1) / (float)height * 2.0 - 1.0,
                            screen_point(2) * 2.0 - 1.0, 1.0);
    if (point(3) == 0.0) {
        return Eigen::Vector3f::Zero();
    }
    point /= point(3);
    return point.block<3, 1>(0, 0);
}

int ColorCodeToPickIndex(const Eigen::Vector4i &color) {
    if (color(0) == 255) {
        return -1;
    } else {
        return ((color(0) * 256 + color(1)) * 256 + color(2)) * 256 + color(3);
    }
}

}  // namespace gl_helper
}  // namespace visualization
}  // namespace cupoch


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
cpp_int Combination(int n, int r) {
    cpp_int u = 1, d = 1;
    for (int i = 2; i <= n; i++) {
        u *= i;
        if (i <= n - r) d *= i;
    }
    for (int i = 2; i <= r; i++) d *= i;
    return u / d;
}
int main() {
    int l; cin >> l;
    cpp_int ans = Combination(l - 1, 11);
    cout << ans << endl;
}


#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

#include <opencv2/opencv.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>

#include "imgProc.hpp"
#include "sfm.hpp"
#include "tapl/pte/ptEngine.hpp"

inline double deg2rad(double deg) { return deg * M_PI / 180.0; }
inline double rad2deg(double rad) { return rad * 180.0 / M_PI; }

/**
 * Estimate four possible R and T from the Essential Matrix
 */
std::vector<Eigen::MatrixXd> computeInitialRTfromE(const Eigen::MatrixXd &E) {

    // SVD decomposition
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // build 'Z' and 'W'
    Eigen::MatrixXd Z(3,3);
    Z << 0, 1, 0,
        -1, 0, 0,
         0, 0, 0;
    Eigen::MatrixXd W(3,3);
    W << 0, -1, 0,
         1,  0, 0,
         0,  0, 1;

    // E can be re-written as E=MQ; where M=U.Z.U^T and Q=U.W.V^T or Q=U.W^T.V^T
    Eigen::MatrixXd M = svd.matrixU() * Z * svd.matrixU().transpose();
    Eigen::MatrixXd Q1 = svd.matrixU() * W * svd.matrixV().transpose();
    Eigen::MatrixXd Q2 = svd.matrixU() * W.transpose() * svd.matrixV().transpose();

    // R can be computed as R=(det Q)Q
    Eigen::MatrixXd R1 = Q1.determinant() * Q1;
    Eigen::MatrixXd R2 = Q2.determinant() * Q2;

    // E = UΣV^T, T is simply either u3 or −u3, where u3 is the third column vector of U
    Eigen::MatrixXd T1 = svd.matrixU().col(2);
    Eigen::MatrixXd T2 = -svd.matrixU().col(2);

    // compose four possible RT
    std::vector<Eigen::MatrixXd> RT;
    // R1 and T1
    Eigen::MatrixXd R1T1(3,4);
    R1T1.block<3,3>(0,0) = R1;
    R1T1.block<3,1>(0,3) = T1;
    RT.push_back(R1T1);
    // R1 and T2
    Eigen::MatrixXd R1T2(3,4);
    R1T2.block<3,3>(0,0) = R1;
    R1T2.block<3,1>(0,3) = T2;
    RT.push_back(R1T2);
    // R2 and T1
    Eigen::MatrixXd R2T1(3,4);
    R2T1.block<3,3>(0,0) = R2;
    R2T1.block<3,1>(0,3) = T1;
    RT.push_back(R2T1);
    // R2 and T2
    Eigen::MatrixXd R2T2(3,4);
    R2T2.block<3,3>(0,0) = R2;
    R2T2.block<3,1>(0,3) = T2;
    RT.push_back(R2T2);

    // return
    return RT;
}

/**
 * Linear estimate of the 3d point
 */
tapl::Point3d tapl::cve::StructureFromMotion::linearEstimate3dPt(
                    const std::vector<tapl::Point2d> &point2d,
                    const std::vector<Eigen::MatrixXd> &projectionMatrices ) {
    // We can re-write [p x MP = 0] in the form [AP = 0] and solve for P
    // by decomposing A using SVD
    // let's formulate the A matrix
    Eigen::MatrixXd A(point2d.size()*2, 4);
    for (auto i=0; i<point2d.size(); ++i) {
        A.row(i*2) << (point2d.at(i).x * projectionMatrices.at(i).row(2)) -
                                    projectionMatrices.at(i).row(0);
        A.row((i*2)+1) << (point2d.at(i).y * projectionMatrices.at(i).row(2)) -
                                    projectionMatrices.at(i).row(1);
    }
    // SVD decomposition
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    // P can be obtained as the last column of v or last row of v_transpose
    Eigen::MatrixXd pt3d_homogeneous = svd.matrixV().col(3);
    // homogeneous to cartesian coordinate conversion
    tapl::Point3d pt3d( pt3d_homogeneous(0,0) / pt3d_homogeneous(3,0),
                        pt3d_homogeneous(1,0) / pt3d_homogeneous(3,0),
                        pt3d_homogeneous(2,0) / pt3d_homogeneous(3,0) );
    return pt3d;
}

/**
 * Non-linear estimate of the 3d point
 */
tapl::Point3d tapl::cve::StructureFromMotion::nonLinearEstimate3dPt(
                    const std::vector<tapl::Point2d> &point2d,
                    const std::vector<Eigen::MatrixXd> &projectionMatrices,
                    std::pair<std::vector<float>,std::vector<float>> &reprojectionErrors,
                    const uint16_t nIterations,
                    const float reprErrorThresh ) {
    // compute linear estimate of the 3d point
    tapl::Point3d pt3d_initial = linearEstimate3dPt(point2d, projectionMatrices);
    // perform non-linear least squares optimization
    tapl::Point3d pt3d_optimize;
    reprojectionErrors = \
        gnOptim.optimize(pt3d_initial, point2d, projectionMatrices, nIterations, reprErrorThresh, pt3d_optimize);
    return pt3d_optimize;
}

/**
 * Estimate R, T, and triengulated points
 */
std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>>
    tapl::cve::StructureFromMotion::computeSFM(
                    const Eigen::MatrixXd &E,
                    const std::vector<std::vector<tapl::Point2d>> &points2d,
                    const Eigen::MatrixXd &projectionMat1,
                    const float &maxReprojectionErr ) {
    // compute four possible RT
    std::vector<Eigen::MatrixXd> RT = computeInitialRTfromE(E);
    // projection matrices
    std::vector<Eigen::MatrixXd> projectionMatrices(2);
    projectionMatrices.at(0) = projectionMat1;
    // set of triangulated 3d points
    std::vector<std::vector<tapl::Point3d>> pointsTriangulatedRef(4, std::vector<tapl::Point3d>(points2d.size()));
    std::vector<std::vector<tapl::Point3d>> pointsTriangulated(4, std::vector<tapl::Point3d>(points2d.size()));
    // reprojection errors
    std::vector<std::vector<std::pair<std::vector<float>,std::vector<float>>>>
        reprojectionErrors(4, std::vector<std::pair<std::vector<float>,std::vector<float>>>(points2d.size()));
    // iterate over 4 possible R & T
    for (auto it1=RT.begin(); it1!= RT.end(); ++it1) {
        auto rtIdx = std::distance(RT.begin(), it1);
        // build projection matrix
        Eigen::MatrixXd projectionMat2 = projectionMat1 * (*it1);
        // TODO: Update the implementation so that we have 1 projection matrix per camera
        projectionMatrices.at(1) = projectionMat2;
        // go through each point and triangulate it through multiple-views
        for (auto it2=points2d.begin(); it2!=points2d.end(); ++it2) {
            auto ptIdx = std::distance(points2d.begin(), it2);
            // triangulate 3d point
            std::pair<std::vector<float>,std::vector<float>> reprojectionError;
            auto pt3d = this->nonLinearEstimate3dPt(*it2, projectionMatrices, reprojectionError);
            reprojectionErrors[rtIdx][ptIdx] = reprojectionError;
            pointsTriangulatedRef[rtIdx][ptIdx] = pt3d;

            // convert to homogeneous coordinate system
            Eigen::MatrixXd point3dHomogeneous(4,1);
            point3dHomogeneous << pt3d.x, pt3d.y, pt3d.z, 1.0;
            auto ptsTransformed = (*it1) * point3dHomogeneous;
            pointsTriangulated[rtIdx][ptIdx] = *(new tapl::Point3d(ptsTransformed(0), ptsTransformed(1), ptsTransformed(2)));
        }
    }

    // for each RT, count number of points that fall in front of all cameras.
    // the one with maximum number of points will correspond to the correct RT
    size_t correctIdx = 0;
    uint16_t maxCount = 0;
    for (auto i=0; i<RT.size(); ++i) {
        uint16_t count = 0;
        for (auto j=0; j<points2d.size(); ++j) {
            if ((pointsTriangulatedRef[i][j].z > 0.0) &&
                (pointsTriangulated[i][j].z > 0.0)) {
                   count ++;
               }
        }
        if (count > maxCount) {
            maxCount = count;
            correctIdx = i;
        }
    }

    Eigen::MatrixXd correctRT = RT[correctIdx];
    std::vector<tapl::Point3d> correctTriangulatedPtsRef = pointsTriangulatedRef[correctIdx];
    std::vector<tapl::Point3d> correctTriangulatedPts = pointsTriangulated[correctIdx];

    // use PnP to get refined pose
    // get pose using ransac
    cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
    cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
    const int iterationsCount = 500;       // number of Ransac iterations. default 100
    const float reprojectionError = 1.0;    // maximum allowed distance to consider it an inlier. default 8.0
    const float confidence = 0.99;          // RANSAC successful confidence. default 0.99
    const bool useExtrinsicGuess = true;   // default false
    const int flags = cv::SOLVEPNP_ITERATIVE;
    cv::Mat inliers;
    std::vector<cv::Point2f> kpts2d_;
    std::vector<cv::Point3f> kpts3d_;
    std::vector<uint16_t> idxRemove;
    for (auto i=0; i<points2d.size(); ++i) {
        // filter out the points outside of region-of-interest and the points with high reprojection error
        if ((correctTriangulatedPts[i].z > 0.0) && (correctTriangulatedPtsRef[i].z > 0.0) &&
            (correctTriangulatedPtsRef[i].z > this->minXYZ.at(2)) && (correctTriangulatedPtsRef[i].z < this->maxXYZ.at(2)) &&
            (fabs(correctTriangulatedPtsRef[i].x) > this->minXYZ.at(0)) && (fabs(correctTriangulatedPtsRef[i].x) < this->maxXYZ.at(0)) &&
            (fabs(correctTriangulatedPtsRef[i].y) > this->minXYZ.at(1)) && (fabs(correctTriangulatedPtsRef[i].y) < this->maxXYZ.at(1)) &&
            reprojectionErrors[correctIdx][i].second.at(0) < maxReprojectionErr ) { // post-optimization reprojection error in the first camera frame

            // compute epipolar lines
            Eigen::MatrixXf pt2(3,1);
            pt2 << points2d[i][1].x, points2d[i][1].y, 1.0;
            Eigen::MatrixXf epipolarLine1 = (E.transpose().cast <float> ()) * pt2.cast <float> ();
            epipolarLine1 = epipolarLine1 / epipolarLine1(2,0); // normalize
            // compute distance between epipolar line to the point in the other image
            auto dist2ep = fabs(static_cast<float>(epipolarLine1(0,0)) * points2d[i][0].x +
                                static_cast<float>(epipolarLine1(1,0)) * points2d[i][0].y +
                                static_cast<float>(epipolarLine1(2,0))) /
                            sqrt(pow(static_cast<float>(epipolarLine1(0,0)), 2) +
                                 pow(static_cast<float>(epipolarLine1(1,0)), 2));
            // if (dist2ep < 50.0) {
            // points in the camera plane for which the pose needs to be computed
            kpts2d_.push_back(cv::Point2f(points2d[i][1].x, points2d[i][1].y));
            // 3d points in the coordinate of reference camera frame
            kpts3d_.push_back(cv::Point3f(correctTriangulatedPtsRef[i].x, correctTriangulatedPtsRef[i].y, correctTriangulatedPtsRef[i].z));
            // }
            // else idxRemove.push_back(i);
        }
        else idxRemove.push_back(i);
    }
    // remove outliers
    for (auto it_idx=idxRemove.end()-1; it_idx!=idxRemove.begin()-1; --it_idx) {
        correctTriangulatedPtsRef.erase(correctTriangulatedPtsRef.begin()+(*it_idx));
    }
    if(kpts3d_.size() >= 6) {
        cv::solvePnPRansac(kpts3d_, kpts2d_, this->K, this->dist_coeff, rvec, tvec,
                            useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
                            inliers, flags );
        float inliers_ratio = static_cast<float>(inliers.size().height) / static_cast<float>(kpts3d_.size());
        // TLOG_DEBUG << "Inliers ratio [" << kpts3d_.size() << "/" << inliers.size().height << "] = [" << inliers_ratio << "]";

        // get global pose in world reference frame
        // check for NaNs
        rvec.convertTo(rvec, CV_32F);
        tvec.convertTo(tvec, CV_32F);
        cv::patchNaNs(rvec, 0.0); // replace NaN with 0.0
        cv::patchNaNs(tvec, 0.0); // replace NaN with 0.0
        // rotation matrix
        cv::Mat R;
        cv::Rodrigues(rvec, R);
        // translation matrix
        cv::Mat t = cv::Mat::eye(4,4, CV_32FC1);
        tvec.copyTo(t(cv::Rect(3,0,1,3)));
        // build an RT matrix
        Eigen::MatrixXd R_eigen(3,3);
        cv2eigen(R, R_eigen);
        Eigen::MatrixXd RT_pnp(3,4);
        RT_pnp.block<3,3>(0,0) << R_eigen;
        RT_pnp.block<3,1>(0,3) << tvec.at<float>(0,0), tvec.at<float>(0,1), tvec.at<float>(0,2);

        return std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>>(RT_pnp, correctTriangulatedPtsRef);
    }
    else {
        TLOG_WARN << "PnP failed";
        return std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>>(correctRT, correctTriangulatedPtsRef);
    }
}

/**
 * Structure-from-Motion constructor
 */
tapl::cve::StructureFromMotion::StructureFromMotion(
                                     const std::vector<cv::Mat> &images,
                                     const cv::Mat &K,
                                     const std::vector<float> &minXYZ,
                                     const std::vector<float> &maxXYZ,
                                     const bool verbose) {
    // copy inputs to private variable
    this->images = images;
    this->K = K;
    this->minXYZ = minXYZ;
    this->maxXYZ = maxXYZ;
    this->verbose = verbose;
    // make bundles of 'm' images
}

/**
 * Structure-from-Motion implementation
 */
tapl::ResultCode tapl::cve::StructureFromMotion::process(
                                std::vector<tapl::Point3dColor> &points,
                                std::vector<tapl::Pose6dof> &poses,
                                std::vector<tapl::CameraPairs> &framePairs) {

    // global pose
    Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
    // Go through each camera frame
    for (auto it=this->images.begin()+1; it!=this->images.end(); ++it) {
        if (this->verbose) TLOG_INFO << "processing frames [" <<
                                        std::distance(images.begin(), it) <<
                                        "] and [" <<
                                        std::distance(images.begin(), it+1) << "]";

        tapl::CameraPairs camPairs(*(it-1), *it, this->K);

        // compute fundamental matrix
        if (tapl::cve::computeFundamentalMatrix(camPairs) != tapl::SUCCESS) {
            TLOG_ERROR << "could not compute fundamental matrix";
            return tapl::FAILURE;
        }

        // retrieve fundamental matrix
        cv::Mat F;
        if (camPairs.getFundamentalMatrix(F) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve fundamental matrix";
            return tapl::FAILURE;
        }

        // compute essential matrix
        cv::Mat E = this->K.t() * F * this->K;

        // get keypoints
        std::vector<cv::KeyPoint> kpts1;
        if (camPairs.first->getKeypoints(kpts1) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve keypoints";
            return tapl::FAILURE;
        }
        std::vector<cv::KeyPoint> kpts2;
        if (camPairs.second->getKeypoints(kpts2) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve keypoints";
            return tapl::FAILURE;
        }

        // get keypoints matches
        std::vector<cv::DMatch> kptMatches;
        if (camPairs.getKptsMatches(kptMatches) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve keypoints matches";
            return tapl::FAILURE;
        }

        // build vector of 'n' points in 'm' camera frames (n x m - Point2d)
        std::vector<std::vector<tapl::Point2d>> kptSet;
        std::vector<tapl::Point2d> trackedKpts;
        for (auto it_match=kptMatches.begin(); it_match!=kptMatches.end(); ++it_match) {
            if (it_match->distance < 100.0) {
                tapl::Point2d pt1 = tapl::Point2d( kpts1.at((*it_match).queryIdx).pt.x,
                                                   kpts1.at((*it_match).queryIdx).pt.y );
                tapl::Point2d pt2 = tapl::Point2d( kpts2.at((*it_match).trainIdx).pt.x,
                                                   kpts2.at((*it_match).trainIdx).pt.y );
                trackedKpts.push_back(pt1);
                // TODO: compute corresponding lines, find and reject outliers

                // this point in all cameras
                std::vector<tapl::Point2d> ptCameras = {pt1, pt2};
                kptSet.push_back(ptCameras);
            }
        }

        // draw the matches
        cv::Mat matchImg;
        cv::drawMatches(*(it-1), kpts1, *(it), kpts2,
                        kptMatches, matchImg,
                        cv::Scalar::all(-1), cv::Scalar::all(-1),
                        std::vector<char>(), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

        // convert E to eigen format
        Eigen::MatrixXd eigenE(3,3);
        cv2eigen(E, eigenE);
        // convert K to eigen format
        Eigen::MatrixXd eigenK(3,3);
        Eigen::MatrixXd projectionMat1(3,4);
        cv2eigen(this->K, eigenK);
        projectionMat1.block<3,3>(0,0) = eigenK;
        projectionMat1(3,3) = 1.0;
        auto sfm = computeSFM(eigenE, kptSet, projectionMat1);
        Eigen::MatrixXd RT = sfm.first;
        std::vector<tapl::Point3d> triangulatedPts = sfm.second;
        // Get point color
        std::vector<tapl::Point3dColor> triangulatedPtsColor;
        cv::Mat imgProj = cv::Mat::zeros((it-1)->rows, (it-1)->cols, CV_8UC3);
        for ( auto &pt3d : triangulatedPts) {
            Eigen::MatrixXd eigenPt3d(3,1);
            eigenPt3d << pt3d.x, pt3d.y, pt3d.z;
            auto eigenPt2dHomogeneous = eigenK * eigenPt3d;
            float pxX, pxY;
            pxX = eigenPt2dHomogeneous(0,0) / eigenPt2dHomogeneous(2,0);
            pxY = eigenPt2dHomogeneous(1,0) / eigenPt2dHomogeneous(2,0);
            // check if the coordinate is within limits
            cv::Vec3b rgb;
            if ((pxX >= 0) && (pxX < (it-1)->cols) &&
                (pxY >= 0) && (pxY < (it-1)->rows)) {
                rgb = (it-1)->at<cv::Vec3b>(static_cast<int>(pxY),static_cast<int>(pxX));
                imgProj.at<cv::Vec3b>(static_cast<int>(pxY),static_cast<int>(pxX)) = rgb;
            }
            else rgb = cv::Vec3b(0, 0, 0);
            tapl::Point3dColor pt3dColor( pt3d.x, pt3d.y, pt3d.z, static_cast<uint8_t>(rgb[0]),
                                          static_cast<uint8_t>(rgb[1]), static_cast<uint8_t>(rgb[2]));
            triangulatedPtsColor.push_back(pt3dColor);
        }

        // plot
        Eigen::Matrix4f P = Eigen::Matrix4f::Identity();
        P.block<3,4>(0,0) = RT.cast<float>();
        Eigen::Matrix4f Pi = Eigen::Matrix4f::Identity();
        Pi.block<3,3>(0,0) = P.block<3,3>(0,0).transpose();
        Pi.block<3,1>(0,3) = -P.block<3,3>(0,0).transpose() * P.block<3,1>(0,3);
        globalPose = globalPose * Pi;

        // compute pose
        cv::Mat cvPose;
        eigen2cv(globalPose, cvPose);
        tapl::Pose6dof pose(cvPose);
        // add to camera
        camPairs.pushPose(pose);
        camPairs.pushTrackedKpts(trackedKpts);
        camPairs.pushTriangulatedPts(triangulatedPtsColor);
        // push to the output vector
        poses.push_back(pose);
        for (auto &point : triangulatedPtsColor) points.push_back(point);
        // push camera pairs
        framePairs.push_back(camPairs);
    }

    // return success
    return tapl::SUCCESS;
}


#define BOOST_TEST_MODULE "Test Chebyshev Normalization class"

#include <boost/test/unit_test.hpp>

#include "distance/Chebyshev.hpp"

using namespace genex;

#define TOLERANCE 1e-9

struct MockData
{
  data_t dat_1[5] = {1, 2, 3, 4, 5};
  data_t dat_2[5] = {11, 2, 3, 4, 5};
};

BOOST_AUTO_TEST_CASE( time_series_length, *boost::unit_test::tolerance(TOLERANCE)  )
{
  MockData data;
  TimeSeries ts_1(data.dat_1, 0, 0, 5);
  TimeSeries ts_2(data.dat_2, 0, 0, 5);
  Chebyshev dist;

  data_t total = dist.init();

  for (int i = 0; i < ts_1.getLength(); i++) {
    total = dist.reduce(total, total, ts_1[i], ts_2[i]);
  }

  BOOST_TEST( dist.norm(total, ts_1, ts_2), 10.0 );

  dist.clean(total);
}


/*
LICENSE: see isogeometric_application/LICENSE.txt
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: hbui $
//   Date:                $Date: Nov 11, 2017 $
//   Revision:            $Revision: 1.1 $
//
//

// System includes
#include <string>

// External includes
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/operators.hpp>

// Project includes
#include "includes/define.h"
#include "containers/array_1d.h"
#include "includes/ublas_interface.h"
#include "custom_utilities/trans/transformation.h"
#include "custom_utilities/trans/translation.h"
#include "custom_utilities/trans/rotation.h"
#include "custom_utilities/trans/mirror.h"
#include "custom_utilities/trans/transformation_utility.h"
#include "custom_python/add_transformation_to_python.h"

namespace Kratos
{

namespace Python
{

using namespace boost::python;

//////////////////////////////////////////////////

template<typename TDataType>
Transformation<TDataType> TransformationUtility_CreateAlignTransformation(TransformationUtility<TDataType>& rDummy,
    const typename Transformation<TDataType>::VectorType& a, const typename Transformation<TDataType>::VectorType& b)
{
    Transformation<TDataType> T;
    T = rDummy.CreateAlignTransformation(a, b);
    return T;
}

template<typename TDataType>
void Transformation_SetValue(Transformation<TDataType>& rDummy, const int& i, const int& j, const TDataType& v)
{
    rDummy(i, j) = v;
}

template<typename TDataType>
TDataType Transformation_GetValue(Transformation<TDataType>& rDummy, const int& i, const int& j)
{
    return rDummy(i, j);
}

template<typename TDataType, typename TVectorType>
TVectorType Transformation_Apply(Transformation<TDataType>& rDummy, const TVectorType& v)
{
    TVectorType newv = v;
    rDummy.template ApplyTransformation<TVectorType>(newv);
    return newv;
}

template<typename TDataType>
boost::python::list Transformation_Apply2(Transformation<TDataType>& rDummy, boost::python::list v)
{
    std::vector<TDataType> newv;
    typedef boost::python::stl_input_iterator<TDataType> iterator_value_type;
    BOOST_FOREACH(const typename iterator_value_type::value_type& d, std::make_pair(iterator_value_type(v), iterator_value_type() ) )
        newv.push_back(d);

    rDummy.template ApplyTransformation<std::vector<TDataType> >(newv);

    boost::python::list res;
    for (std::size_t i = 0; i < newv.size(); ++i)
        res.append(newv[i]);

    return res;
}

template<typename TDataType>
array_1d<TDataType, 3> Transformation_P(Transformation<TDataType>& rDummy)
{
    return rDummy.P();
}

template<typename TDataType>
array_1d<TDataType, 3> Transformation_V1(Transformation<TDataType>& rDummy)
{
    return rDummy.V1();
}

template<typename TDataType>
array_1d<TDataType, 3> Transformation_V2(Transformation<TDataType>& rDummy)
{
    return rDummy.V2();
}

template<typename TDataType>
array_1d<TDataType, 3> Transformation_V3(Transformation<TDataType>& rDummy)
{
    return rDummy.V3();
}

//////////////////////////////////////////////////

void IsogeometricApplication_AddTransformationToPython()
{
    typedef Transformation<double>::VectorType VectorType;

    class_<Transformation<double>, Transformation<double>::Pointer>
    ("Transformation", init<>())
    .def(init<const VectorType&, const VectorType&, const VectorType&>())
    .def(init<const array_1d<double, 3>&, const array_1d<double, 3>&, const array_1d<double, 3>&>())
    .def(init<const VectorType&, const VectorType&, const VectorType&, const VectorType&>())
    .def(init<const array_1d<double, 3>&, const array_1d<double, 3>&, const array_1d<double, 3>&, const array_1d<double, 3>&>())
    .def("AppendTransformation", &Transformation<double>::AppendTransformation)
    .def("PrependTransformation", &Transformation<double>::PrependTransformation)
    .def("Inverse", &Transformation<double>::Inverse)
    // .def(boost::python::operators<boost::python::op_mul>());
    .def("P", &Transformation_P<double>)
    .def("V1", &Transformation_V1<double>)
    .def("V2", &Transformation_V2<double>)
    .def("V3", &Transformation_V3<double>)
    .def("SetValue", &Transformation_SetValue<double>)
    .def("GetValue", &Transformation_GetValue<double>)
    .def("Apply", &Transformation_Apply<double, Vector>)
    .def("Apply", &Transformation_Apply<double, array_1d<double, 3> >)
    .def("Apply", &Transformation_Apply2<double>)
    .def(self_ns::str(self))
    ;

    class_<Translation<double>, Translation<double>::Pointer, bases<Transformation<double> >, boost::noncopyable>
    ("Translation", init<const double&, const double&, const double&>())
    .def(self_ns::str(self))
    ;

    class_<Rotation<0, double>, Rotation<0, double>::Pointer, bases<Transformation<double> >, boost::noncopyable>
    ("RotationX", init<const double&>())
    .def(self_ns::str(self))
    ;

    class_<Rotation<1, double>, Rotation<1, double>::Pointer, bases<Transformation<double> >, boost::noncopyable>
    ("RotationY", init<const double&>())
    .def(self_ns::str(self))
    ;

    class_<Rotation<2, double>, Rotation<2, double>::Pointer, bases<Transformation<double> >, boost::noncopyable>
    ("RotationZ", init<const double&>())
    .def(self_ns::str(self))
    ;

    class_<Mirror<0, double>, Mirror<0, double>::Pointer, bases<Transformation<double> >, boost::noncopyable>
    ("MirrorX", init<>())
    .def(self_ns::str(self))
    ;

    class_<Mirror<1, double>, Mirror<1, double>::Pointer, bases<Transformation<double> >, boost::noncopyable>
    ("MirrorY", init<>())
    .def(self_ns::str(self))
    ;

    class_<Mirror<2, double>, Mirror<2, double>::Pointer, bases<Transformation<double> >, boost::noncopyable>
    ("MirrorZ", init<>())
    .def(self_ns::str(self))
    ;

    class_<TransformationUtility<double>, TransformationUtility<double>::Pointer, boost::noncopyable>
    ("TransformationUtility", init<>())
    .def("CreateAlignTransformation", &TransformationUtility_CreateAlignTransformation<double>)
    ;

}

}  // namespace Python.

} // Namespace Kratos


/**
 * \file dcs/math/stats/distribution/pareto.hpp
 *
 * \brief The Pareto probability distribution.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_PARETO_HPP
#define DCS_MATH_STATS_DISTRIBUTION_PARETO_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(103500) // 1.35
# 	error "Required Boost library version >= 1.35"
#endif

#include <boost/math/distributions/pareto.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/random/uniform_01_adaptor.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

/**
 * \brief The Pareto distribution with shape parameter \f$\alpha\f$ and scale
 *  parameter \f$\beta\f$.
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * The probability density function (pdf):
 * \f[
 *   \Pr(x|\lambda) = \frac{\alpha\,\beta^\alpha}{x^{\alpha+1}}, \quad
 *                    \text{ for }x>\beta
 * \f]
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class pareto_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit pareto_distribution(support_type shape=1, support_type scale=1)
		: dist_(shape, scale)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a random number distributed according to this
	 * pareto distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \return A random number distributed according to this pareto
	 * distribution.
	 *
	 * A \c pareto random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\alpha) = \frac{\alpha\,k^\alpha}{x^{\alpha+1}}, \quad
	 *                   \text{ for }x>k
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		::dcs::math::random::uniform_01_adaptor<UniformRandomGeneratorT&, support_type> eng(rng);

		// Use the inversion method:
    	//    x=\frac{k}{(1-p)^{1/\alpha}}
    	// => x=k(1-p)^{-1/\alpha}
    	// => x=ku^{-1/\alpha}, where u is a uniform random number in [0,1)

#if DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(104000)
		return dist_.scale()*::std::pow(value_type(1)-eng(), -value_type(1)/dist_.shape());
//		return dist_.scale()*::std::pow(eng(), -value_type(1)/dist_.shape());
#else
		return dist_.location()*::std::pow(value_type(1)-eng(), -value_type(1)/dist_.shape());
//		return dist_.location()*::std::pow(eng(), -value_type(1)/dist_.shape());
#endif // DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * pareto distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * pareto distribution.
	 *
	 * A \c pareto random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\lambda) = \frac{\alpha\,\beta^\alpha}{x^{\alpha+1}}, \quad
	 *                    \text{ for }x>\beta
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, ::std::size_t n)
	{
		::std::vector<support_type> rnds(n);

        for ( ; n > 0; --n)
		{
			rnds.push_back(rand());
		}

		return rnds;
	}

	public: support_type shape() const
	{
		return dist_.shape();
	}

	public: support_type scale() const
	{
#if DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(104000)
		return dist_.scale();
#else
		return dist_.location();
#endif // DCS_DETAIL_CONFIG_BOOST_VERSION
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

	public: value_type mean() const
	{
#if DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(104000)
		return dist_.scale()*dist_.shape()/(dist_.shape()-1);
#else
		return dist_.location()*dist_.shape()/(dist_.shape()-1);
#endif // DCS_DETAIL_CONFIG_BOOST_VERSION
	}

	public: value_type variance() const
	{
#if DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(104000)
		return dist_.scale()*dist_.scale()*dist_.shape()/((dist_.shape()-1)*(dist_.shape()-2));
#else
		return dist_.location()*dist_.location()*dist_.shape()/((dist_.shape()-1)*(dist_.shape()-2));
#endif // DCS_DETAIL_CONFIG_BOOST_VERSION
	}

	private: ::boost::math::pareto_distribution<value_type,policy_type> dist_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, pareto_distribution<RealT,PolicyT> const& dist)
{
	return os << "Pareto("
			  << "shape=" <<  dist.shape()
			  << ", scale=" <<  dist.scale()
			  << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_PARETO_HPP


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <stdexcept>

TEST(MathFunctions, erf) {
  using stan::math::erf;
  EXPECT_FLOAT_EQ(-0.3286267594591274, erf(-0.3));
  EXPECT_FLOAT_EQ(0, erf(0));
  EXPECT_FLOAT_EQ(0.9999939742388482, erf(3.2));
}

TEST(MathFunctions, erfOverflow) {
  EXPECT_FLOAT_EQ(-1, erf(-100));
  EXPECT_FLOAT_EQ(1, erf(100));
}

TEST(MathFunctions, erfNan) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::erf(nan));
}


/**
 * @file    LossFunction.cpp
 * @brief   Gaussian loss function with mahalanobis distance, and robust loss
 * @author  Jing Dong
 * @date    Oct 14, 2017
 */

#include <minisam/core/LossFunction.h>

#include <Eigen/Dense>

namespace minisam {

/* ************************************************************************** */
std::shared_ptr<LossFunction> GaussianLoss::SqrtInformation(
    const Eigen::MatrixXd& R) {
  assert(R.rows() == R.cols() &&
         "[GaussianLoss::SqrtInformation] non-square sqrt-root matrix");
  return std::shared_ptr<LossFunction>(new GaussianLoss(R));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> GaussianLoss::Information(
    const Eigen::MatrixXd& I) {
  assert(I.rows() == I.cols() &&
         "[GaussianLoss::Information] non-square information matrix");
  Eigen::LLT<Eigen::MatrixXd> llt(I.selfadjointView<Eigen::Upper>());
  return std::shared_ptr<LossFunction>(new GaussianLoss(llt.matrixU()));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> GaussianLoss::Covariance(
    const Eigen::MatrixXd& Sigma) {
  assert(Sigma.rows() == Sigma.cols() &&
         "[GaussianLoss::Covariance] non-square covariance matrix");
  return Information(Sigma.inverse());
}

/* ************************************************************************** */
void GaussianLoss::print(std::ostream& out) const {
  out << "Gaussian loss function : R =" << std::endl << sqrt_info_ << std::endl;
}

/* ************************************************************************** */
void GaussianLoss::weightInPlace(Eigen::VectorXd& b) const {
  assert(sqrt_info_.cols() == b.size() &&
         "[GaussianLoss::weightInPlace] error size wrong");
  b = sqrt_info_ * b;
}

/* ************************************************************************** */
void GaussianLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                                 Eigen::VectorXd& b) const {
  assert(sqrt_info_.cols() == b.size() &&
         "[GaussianLoss::weightInPlace] error size wrong");
  b = sqrt_info_ * b;
  for (auto& A : As) {
    assert(sqrt_info_.cols() == A.rows() &&
           "[GaussianLoss::weightInPlace] jacobian size wrong");
    A = sqrt_info_ * A;
  }
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> DiagonalLoss::Precisions(
    const Eigen::VectorXd& I_diag) {
  return std::shared_ptr<LossFunction>(new DiagonalLoss(I_diag.cwiseSqrt()));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> DiagonalLoss::Sigmas(
    const Eigen::VectorXd& S_diag) {
  return std::shared_ptr<LossFunction>(new DiagonalLoss(S_diag.cwiseInverse()));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> DiagonalLoss::Variances(
    const Eigen::VectorXd& V_diag) {
  return std::shared_ptr<LossFunction>(
      new DiagonalLoss((V_diag.cwiseInverse()).cwiseSqrt()));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> DiagonalLoss::Scales(const Eigen::VectorXd& s) {
  return std::shared_ptr<LossFunction>(new DiagonalLoss(s));
}

/* ************************************************************************** */
void DiagonalLoss::print(std::ostream& out) const {
  out << "Diagonal loss function : R_diag = [" << sqrt_info_diag_.transpose()
      << "]'" << std::endl;
}

/* ************************************************************************** */
void DiagonalLoss::weightInPlace(Eigen::VectorXd& b) const {
  assert(sqrt_info_diag_.size() == b.size() &&
         "[DiagonalLoss::weightInPlace] error size wrong");
  b = b.cwiseProduct(sqrt_info_diag_);
}

/* ************************************************************************** */
void DiagonalLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                                 Eigen::VectorXd& b) const {
  assert(sqrt_info_diag_.size() == b.size() &&
         "[DiagonalLoss::weightInPlace] error size wrong");
  b = b.cwiseProduct(sqrt_info_diag_);
  for (auto& A : As) {
    assert(sqrt_info_diag_.size() == A.rows() &&
           "[DiagonalLoss::weightInPlace] jacobian size wrong");
    for (int i = 0; i < A.rows(); i++) {
      A.row(i) *= sqrt_info_diag_(i);
    }
  }
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> ScaleLoss::Precision(double prec) {
  return std::shared_ptr<LossFunction>(new ScaleLoss(std::sqrt(prec)));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> ScaleLoss::Sigma(double sigma) {
  return std::shared_ptr<LossFunction>(new ScaleLoss(1.0 / sigma));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> ScaleLoss::Variance(double var) {
  return std::shared_ptr<LossFunction>(new ScaleLoss(1.0 / std::sqrt(var)));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> ScaleLoss::Scale(double s) {
  return std::shared_ptr<LossFunction>(new ScaleLoss(s));
}

/* ************************************************************************** */
void ScaleLoss::print(std::ostream& out) const {
  out << "Scale loss function : inv_sigma = " << inv_sigma_ << std::endl;
}

/* ************************************************************************** */
void ScaleLoss::weightInPlace(Eigen::VectorXd& b) const { b *= inv_sigma_; }

/* ************************************************************************** */
void ScaleLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                              Eigen::VectorXd& b) const {
  b *= inv_sigma_;
  for (auto& A : As) {
    A *= inv_sigma_;
  }
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> CauchyLoss::Cauchy(double k) {
  return std::shared_ptr<LossFunction>(new CauchyLoss(k));
}

/* ************************************************************************** */
void CauchyLoss::print(std::ostream& out) const {
  out << "Cauchy loss function : k = " << k_ << std::endl;
}

/* ************************************************************************** */
void CauchyLoss::weightInPlace(Eigen::VectorXd& b) const {
  double sqrtw = std::sqrt(weight(b.norm()));
  b *= sqrtw;
}

/* ************************************************************************** */
void CauchyLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                               Eigen::VectorXd& b) const {
  const double sqrtw = std::sqrt(weight(b.norm()));
  b *= sqrtw;
  for (auto& A : As) {
    A *= sqrtw;
  }
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> HuberLoss::Huber(double k) {
  return std::shared_ptr<LossFunction>(new HuberLoss(k));
}

/* ************************************************************************** */
void HuberLoss::print(std::ostream& out) const {
  out << "Huber loss function : k = " << k_ << std::endl;
}

/* ************************************************************************** */
void HuberLoss::weightInPlace(Eigen::VectorXd& b) const {
  double sqrtw = std::sqrt(weight(b.norm()));
  b *= sqrtw;
}

/* ************************************************************************** */
void HuberLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                              Eigen::VectorXd& b) const {
  const double sqrtw = std::sqrt(weight(b.norm()));
  b *= sqrtw;
  for (auto& A : As) {
    A *= sqrtw;
  }
}

/* ************************************************************************** */
void ComposedLoss::print(std::ostream& out) const {
  out << "Composed loss function : " << std::endl << "Loss 1 : ";
  l1_->print(out);
  out << "Loss 2 : ";
  l2_->print(out);
}

/* ************************************************************************** */
void ComposedLoss::weightInPlace(Eigen::VectorXd& b) const {
  l1_->weightInPlace(b);
  l2_->weightInPlace(b);
}

/* ************************************************************************** */
void ComposedLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                                 Eigen::VectorXd& b) const {
  l1_->weightInPlace(As, b);
  l2_->weightInPlace(As, b);
}
}  // namespace minisam


#ifndef YANNQ_HYPERCUBECONVLAYER_HH
#define YANNQ_HYPERCUBECONVLAYER_HH

#include <time.h>
#include <Eigen/Dense>
#include <algorithm>
#include <complex>
#include <fstream>
#include <memory>
#include <random>
#include <vector>

#include "AbstractLayer.hpp"

#include <Utilities/Utility.hpp>
#include <Utilities/Exceptions.hpp>

namespace yannq {

template<typename T>
class Conv1D : public AbstractLayer<T> {
	static_assert(!AbstractLayer<T>::Matrix::IsRowMajor, "Matrix must be column-major");

public:
	using Scalar = T;
	using RealScalar = remove_complex_t<T>;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

private:
	const bool useBias_;  // boolean to turn or off bias

	const uint32_t inChannels_;   // number of input channels
	const uint32_t outChannels_;  // number of output channels

	const uint32_t kernelSize_;
	const uint32_t stride_;         // convolution stride

	const uint32_t npar_;          // number of parameters in layer

	Matrix kernel_;  // Weight parameters, W((inChannels_ * kernelSize)x(outChannels))
	Vector bias_;     // Bias parameters, b(outChannels)

	static uint32_t numParams(bool useBias, const uint32_t inChannels,
			const uint32_t outChannels, const uint32_t kernelSize)
	{
		uint32_t np = inChannels * outChannels * kernelSize;
		if(useBias)
			np += outChannels;
		return np;
	}

public:
	/// Constructor
	Conv1D(	const uint32_t inChannels, const uint32_t outChannels,
			const uint32_t kernelSize, const uint32_t stride = 1,
			const bool useBias = false)
		: useBias_(useBias), inChannels_(inChannels), outChannels_(outChannels),
		kernelSize_(kernelSize), stride_(stride),
		npar_(numParams(useBias, inChannels, outChannels, kernelSize)),
		kernel_(inChannels*kernelSize, outChannels), bias_(outChannels)
	{
	}

	Conv1D(const Conv1D& rhs) = default;
	Conv1D(Conv1D&& rhs) = default;

	Conv1D& operator=(const Conv1D& rhs) = default;
	Conv1D& operator=(Conv1D&& rhs) = default;

	bool operator==(const Conv1D& rhs) const
	{
		if(useBias_ != rhs.useBias_)
			return false;

		bool res = (inChannels_ == rhs.inChannels_) &&
				(outChannels_ == rhs.outChannels_) &&
				(kernelSize_ == rhs.kernelSize_) &&
				(stride_ == rhs.stride_) &&
				(kernel_ == rhs.kernel_);

		if(!useBias_)
			return res;
		else
			return res && (bias_ == rhs.bias_);
	}

	bool operator!=(const Conv1D& rhs) const
	{
		return !(*this == rhs);
	}

	std::string name() const override { return "Convolutional 1D Layer"; }

	template<class RandomEngine>
	void randomizeParams(RandomEngine&& re, RealScalar sigma)
	{
		setParams(randomVector<T>(std::forward<RandomEngine>(re), sigma, npar_));
	}

	uint32_t paramDim() const override { return npar_; }
	uint32_t outputDim(uint32_t inputDim) const override {
		return (inputDim / stride_ / inChannels_) * outChannels_;
	}

	Vector getParams() const override
	{
		Vector pars(npar_);
		pars.head(kernel_.size()) = Eigen::Map<const Vector>(kernel_.data(), kernel_.size());
		if(useBias_)
		{
			pars.tail(outChannels_) = bias_;
		}
		return pars;
	}

	void setParams(VectorConstRef pars) override
	{
		assert(pars.size() == npar_);
		Eigen::Map<Vector>(kernel_.data(), kernel_.size()) = pars.head(kernel_.size());
		if(useBias_)
		{
			bias_ = pars.tail(outChannels_);
		}
	}

	void updateParams(VectorConstRef ups) override
	{
		assert(ups.size() == npar_);
		Eigen::Map<Vector>(kernel_.data(), kernel_.size()) += ups.head(kernel_.size());
		if(useBias_)
		{
			bias_ += ups.tail(outChannels_);
		}
	}

	/**
	 * Feedforward
	 * @input: inChannels*size
	 * @output: outChannels*size
	 */
	void forward(const VectorConstRef& input, VectorRef output) override
	{
		assert(input.size() % inChannels_ == 0);
		uint32_t inSize = input.size() / inChannels_;
		uint32_t outSize = inSize / stride_;

		output.setZero();

		// y = Wx+b
		for (uint32_t oc = 0; oc < outChannels_; oc++)
		for (uint32_t r = 0; r < outSize; r ++)
		{
			for (uint32_t ic = 0; ic < inChannels_; ic++)
			for (uint32_t ki = 0; ki < kernelSize_; ki++)
			{
				output(r + oc*outSize) += kernel_(ki + ic*kernelSize_, oc)
					*input(((r*stride_+ki-kernelSize_/2+inSize)%inSize) + ic*inSize);
			}
		}

		if (useBias_) {
			for (uint32_t oc = 0; oc < outChannels_; ++oc) {
				for (uint32_t i = 0; i < outSize; ++i) {
					output(i + oc*outSize) += bias_(oc);
				}
			}
		}
	}

	void backprop(const VectorConstRef& prev_layer_output,
			const VectorConstRef& /*this_layer_output*/,
			const VectorConstRef& dout,
			VectorRef din,
			VectorRef der) override
	{
		assert(prev_layer_output.size() % inChannels_ == 0);
		uint32_t inSize = prev_layer_output.size() / inChannels_;
		uint32_t outSize = inSize / stride_;

		din.resize(inSize*inChannels_);
		din.setZero();
		der.setZero();

		// propagate delta to prev-layer
		for (uint32_t oc = 0; oc < outChannels_; oc++)
		for (uint32_t r = 0; r < outSize; r ++)
		for (uint32_t ic = 0; ic < inChannels_; ic++)
		for (uint32_t ki = 0; ki < kernelSize_; ki++)
		{
			din(((r*stride_+ki-kernelSize_/2+inSize)%inSize) + ic*inSize)
				+= kernel_(ki + ic*kernelSize_, oc)*dout[r + oc*outSize];
		}

		// weight der
		Matrix dw(inChannels_*kernelSize_, outChannels_);
		dw.setZero();

		// accumulate weight difference
		for (uint32_t oc = 0; oc < outChannels_; oc++)
		for (uint32_t r = 0; r < outSize; r ++)
		{
			for (uint32_t ic = 0; ic < inChannels_; ic++)
			for (uint32_t ki = 0; ki < kernelSize_; ki++)
			{
				dw(ki + ic*kernelSize_, oc) +=
					prev_layer_output(((r*stride_+ki-kernelSize_/2+inSize)%inSize) + ic*inSize)*
					dout[r + oc*outSize];
			}
		}
		dw.resize(dw.rows()*dw.cols(),1);
		der.head(kernel_.size()) = std::move(dw);

		uint32_t k = kernel_.size();
		if(useBias_)// accumulate bias difference
		{
			for (uint32_t oc = 0; oc < outChannels_; oc++)
			for (uint32_t r = 0; r < outSize; r ++)
			{
				der(oc + k) += dout(r + oc*outSize);
			}
		}
	}

	uint32_t fanIn() override
	{
		return inChannels_*kernelSize_;
	}
	uint32_t fanOut() override
	{
		return outChannels_*kernelSize_;
	}

	nlohmann::json desc() const override {
		nlohmann::json layerpar;
		layerpar["name"] = name();
		layerpar["use_bias"] = useBias_;
		layerpar["input_channels"] = inChannels_;
		layerpar["output_channels"] = outChannels_;
		layerpar["kernel_size"] = kernelSize_;
		layerpar["stride"] = stride_;
		return layerpar;
	}
};
}// namespace yannq

#endif


//
// Environment that supports circle obstacles.
//

#pragma once

#include <Eigen/Dense>

#include <array>
#include <vector>

namespace circle_world
{

constexpr int OBSTACLE_DIM = 2;

class Circle
{
public:
    Circle(const double radius, const double x, double y);
    Circle(const double radius, const Eigen::Vector2d &position);

    // Returns TRUE if the other circle intersects with this circle.
    // The Euclidean distance between the edges of the circles is returned in distance.
    // The distance will be negative if they intersect.
    bool distance(const Circle &other, double &distance) const;

    // Returns the Euclidean distance between the centroids of this circle and the other.
    double centroid_distance(const Circle &other) const;

    double radius() const { return radius_; }
    double& radius() { return radius_; }

    Eigen::Vector2d& position() { return position_; };
    const Eigen::Vector2d& position() const { return position_; };

private:
    double radius_ = 0;
    Eigen::Vector2d position_ = Eigen::Vector2d::Zero();
};

class CircleWorld
{
public:
    // Set the world boundary constraints to the default.
    CircleWorld() = default;

    // Set the world boundary constraints.
    CircleWorld(double min_x, double max_x, double min_y, double max_y);
    // Same order as above, [min_x, max_x, min_y, max_y].
    CircleWorld(const std::array<double, 4> &world_dims);

    void add_obstacle(const double radius, double x, double y);
    void add_obstacle(const double radius, const Eigen::Vector2d& position);
    void add_obstacle(const Circle &obstacle);

    // Returns TRUE if circle intersects with any obstacle.
    // The Euclidean distance from circle's edge to all the obstacles
    // is returned in distance.
    // The distance will be negative if they intersect.
    bool distances(const Circle& circle, std::vector<double> &distances) const;

    const std::vector<Circle> &obstacles() const { return obstacles_; }
    std::vector<Circle> &obstacles() { return obstacles_; }

    std::array<double, 4> dimensions() const { return {{min_x_, max_x_, min_y_, max_y_}}; }

private:
    std::vector<Circle> obstacles_;

    double min_x_ = -20;
    double max_x_ = 20;
    double min_y_ = -20;
    double max_y_ = 20;
};

std::ostream& operator<<(std::ostream& os, const Circle& o);

// First line is the dimensions of the world.
// Following lines are posx posy radius) for each Circle obstacle.
// Everything is set at 13 character width for each entry.
std::ostream& operator<<(std::ostream& os, const CircleWorld& world);

} // namespace circle_world


#pragma once

#include <Eigen/Eigen>
#include <glm/glm.hpp>

namespace gfx
{
template <glm::length_t C, typename T, glm::qualifier Q> struct eigenresults
{
    glm::mat<C, C, T, Q> eigenvectors;
    glm::vec<C, T, Q>    eigenvalues;
};

template <glm::length_t C, typename T, glm::qualifier Q> eigenresults<C, T, Q> eig(const glm::mat<C, C, T, Q>& matrix)
{
    static Eigen::EigenSolver<Eigen::Matrix<T, C, C>> solver;
    Eigen::Matrix3d                            matrix_eigen = reinterpret_cast<const Eigen::Matrix3d&>(matrix);
    solver.compute(matrix_eigen);

    eigenresults<C, T, Q> results;
    Eigen::Matrix3d vectors = solver.eigenvectors().real();
    Eigen::Vector3d values  = solver.eigenvalues().real();
    results.eigenvalues     = reinterpret_cast<const glm::dvec3&>(values);
    results.eigenvectors    = reinterpret_cast<const glm::dmat3&>(vectors);
    return results;
}
}


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   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.
*/

// Free energy, energy, and specific heat of square lattice Ising model

#pragma once

#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

namespace tfi {
namespace energy {
namespace chain {

namespace {

template<typename T>
struct functor {
  typedef T real_t;
  functor(real_t J, real_t Gamma) : J_(J), Gamma_(Gamma) {}
  real_t operator()(real_t k) const {
    using std::abs; using std::cos; using std::sin; using std::sqrt; using std::pow;
    return 2 * abs(J_) * sqrt(pow(cos(k) - (Gamma_ / J_), 2) + pow(sin(k), 2));
  }
  real_t J_, Gamma_;
};

template<typename T>
functor<T> func(T J, T Gamma) { return functor<T>(J, Gamma); }

}

template<typename T>
inline T infinite(T J, T Gamma) {
  using std::abs;
  typedef T real_t;
  if (abs(J)> 0) {
    auto pi = boost::math::constants::pi<real_t>();
    boost::math::quadrature::tanh_sinh<real_t> integrator;
    return -integrator.integrate(func(J, Gamma), 0, pi) / (2 * pi);
  } else {
    return -abs(Gamma);
  }
}

} // end namespace chain
} // end namespace energy
} // end namespace tfi


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the support vector machine
    utilities from the dlib C++ Library.

    This example creates a simple set of data to train on and then shows
    you how to use the cross validation and svm training functions
    to find a good decision function that can classify examples in our
    data set.

    The data used in this example will be 2 dimensional data and will
    come from a distribution where points with a distance less than 10
    from the origin are labeled +1 and all other points are labeled
    as -1.

*/

#include <iostream>
#include <dlib/svm.h>

using namespace std;
using namespace dlib;

int main()
{
    // The svm functions use column vectors to contain a lot of the data on which they
    // operate. So the first thing we do here is declare a convenient typedef.

    // This typedef declares a matrix with 2 rows and 1 column.  It will be the object that
    // contains each of our 2 dimensional samples.   (Note that if you wanted more than 2
    // features in this vector you can simply change the 2 to something else.  Or if you
    // don't know how many features you want until runtime then you can put a 0 here and
    // use the matrix.set_size() member function)
    typedef matrix<double, 2, 1> sample_type;

    // This is a typedef for the type of kernel we are going to use in this example.  In
    // this case I have selected the radial basis kernel that can operate on our 2D
    // sample_type objects
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Now we make objects to contain our samples and their respective labels.
    std::vector<sample_type> samples;
    std::vector<double> labels;

    // Now let's put some data into our samples and labels objects.  We do this by looping
    // over a bunch of points and labeling them according to their distance from the
    // origin.
    for (int r = -20; r <= 20; ++r)
    {
        for (int c = -20; c <= 20; ++c)
        {
            sample_type samp;
            samp(0) = r;
            samp(1) = c;
            samples.push_back(samp);

            // if this point is less than 10 from the origin
            if (sqrt((double)r*r + c*c) <= 10)
                labels.push_back(+1);
            else
                labels.push_back(-1);

        }
    }

    // Here we normalize all the samples by subtracting their mean and dividing by their
    // standard deviation.  This is generally a good idea since it often heads off
    // numerical stability problems and also prevents one large feature from smothering
    // others.  Doing this doesn't matter much in this example so I'm just doing this here
    // so you can see an easy way to accomplish this with the library.
    vector_normalizer<sample_type> normalizer;
    // let the normalizer learn the mean and standard deviation of the samples
    normalizer.train(samples);
    // now normalize each sample
    for (unsigned long i = 0; i < samples.size(); ++i)
        samples[i] = normalizer(samples[i]);

    // Now that we have some data we want to train on it.  However, there are two
    // parameters to the training.  These are the nu and gamma parameters.  Our choice for
    // these parameters will influence how good the resulting decision function is.  To
    // test how good a particular choice of these parameters is we can use the
    // cross_validate_trainer() function to perform n-fold cross validation on our training
    // data.  However, there is a problem with the way we have sampled our distribution
    // above.  The problem is that there is a definite ordering to the samples.  That is,
    // the first half of the samples look like they are from a different distribution than
    // the second half.  This would screw up the cross validation process but we can fix it
    // by randomizing the order of the samples with the following function call.
    randomize_samples(samples, labels);

    // The nu parameter has a maximum value that is dependent on the ratio of the +1 to -1
    // labels in the training data.  This function finds that value.
    const double max_nu = maximum_nu(labels);

    // here we make an instance of the svm_nu_trainer object that uses our kernel type.
    svm_nu_trainer<kernel_type> trainer;

    // Now we loop over some different nu and gamma values to see how good they are.  Note
    // that this is a very simple way to try out a few possible parameter choices.  You
    // should look at the model_selection_ex.cpp program for examples of more sophisticated
    // strategies for determining good parameter choices.
    cout << "doing cross validation" << endl;
    for (double gamma = 0.00001; gamma <= 1; gamma *= 5)
    {
        for (double nu = 0.00001; nu < max_nu; nu *= 5)
        {
            // tell the trainer the parameters we want to use
            trainer.set_kernel(kernel_type(gamma));
            trainer.set_nu(nu);

            cout << "gamma: " << gamma << "    nu: " << nu;
            // Print out the cross validation accuracy for 3-fold cross validation using
            // the current gamma and nu.  cross_validate_trainer() returns a row vector.
            // The first element of the vector is the fraction of +1 training examples
            // correctly classified and the second number is the fraction of -1 training
            // examples correctly classified.
            cout << "     cross validation accuracy: " << cross_validate_trainer(trainer, samples, labels, 3);
        }
    }

    // From looking at the output of the above loop it turns out that a good value for nu
    // and gamma for this problem is 0.15625 for both.  So that is what we will use.

    // Now we train on the full set of data and obtain the resulting decision function.  We
    // use the value of 0.15625 for nu and gamma.  The decision function will return values
    // >= 0 for samples it predicts are in the +1 class and numbers < 0 for samples it
    // predicts to be in the -1 class.
    trainer.set_kernel(kernel_type(0.15625));
    trainer.set_nu(0.15625);
    typedef decision_function<kernel_type> dec_funct_type;
    typedef normalized_function<dec_funct_type> funct_type;

    // Here we are making an instance of the normalized_function object.  This object
    // provides a convenient way to store the vector normalization information along with
    // the decision function we are going to learn.
    funct_type learned_function;
    learned_function.normalizer = normalizer;  // save normalization information
    learned_function.function = trainer.train(samples, labels); // perform the actual SVM training and save the results

    // print out the number of support vectors in the resulting decision function
    cout << "\nnumber of support vectors in our learned_function is "
         << learned_function.function.basis_vectors.size() << endl;

    // Now let's try this decision_function on some samples we haven't seen before.
    sample_type sample;

    sample(0) = 3.123;
    sample(1) = 2;
    cout << "This is a +1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 3.123;
    sample(1) = 9.3545;
    cout << "This is a +1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This is a -1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This is a -1 class example, the classifier output is " << learned_function(sample) << endl;

    // We can also train a decision function that reports a well conditioned probability
    // instead of just a number > 0 for the +1 class and < 0 for the -1 class.  An example
    // of doing that follows:
    typedef probabilistic_decision_function<kernel_type> probabilistic_funct_type;
    typedef normalized_function<probabilistic_funct_type> pfunct_type;

    pfunct_type learned_pfunct;
    learned_pfunct.normalizer = normalizer;
    learned_pfunct.function = train_probabilistic_decision_function(trainer, samples, labels, 3);
    // Now we have a function that returns the probability that a given sample is of the +1 class.

    // print out the number of support vectors in the resulting decision function.
    // (it should be the same as in the one above)
    cout << "\nnumber of support vectors in our learned_pfunct is "
         << learned_pfunct.function.decision_funct.basis_vectors.size() << endl;

    sample(0) = 3.123;
    sample(1) = 2;
    cout << "This +1 class example should have high probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 3.123;
    sample(1) = 9.3545;
    cout << "This +1 class example should have high probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This -1 class example should have low probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This -1 class example should have low probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    // Another thing that is worth knowing is that just about everything in dlib is
    // serializable.  So for example, you can save the learned_pfunct object to disk and
    // recall it later like so:
    serialize("saved_function.dat") << learned_pfunct;

    // Now let's open that file back up and load the function object it contains.
    deserialize("saved_function.dat") >> learned_pfunct;

    // Note that there is also an example program that comes with dlib called the
    // file_to_code_ex.cpp example.  It is a simple program that takes a file and outputs a
    // piece of C++ code that is able to fully reproduce the file's contents in the form of
    // a std::string object.  So you can use that along with the std::istringstream to save
    // learned decision functions inside your actual C++ code files if you want.

    // Lastly, note that the decision functions we trained above involved well over 200
    // basis vectors.  Support vector machines in general tend to find decision functions
    // that involve a lot of basis vectors.  This is significant because the more basis
    // vectors in a decision function, the longer it takes to classify new examples.  So
    // dlib provides the ability to find an approximation to the normal output of a trainer
    // using fewer basis vectors.

    // Here we determine the cross validation accuracy when we approximate the output using
    // only 10 basis vectors.  To do this we use the reduced2() function.  It takes a
    // trainer object and the number of basis vectors to use and returns a new trainer
    // object that applies the necessary post processing during the creation of decision
    // function objects.
    cout << "\ncross validation accuracy with only 10 support vectors: "
         << cross_validate_trainer(reduced2(trainer,10), samples, labels, 3);

    // Let's print out the original cross validation score too for comparison.
    cout << "cross validation accuracy with all the original support vectors: "
         << cross_validate_trainer(trainer, samples, labels, 3);

    // When you run this program you should see that, for this problem, you can reduce the
    // number of basis vectors down to 10 without hurting the cross validation accuracy.

    // To get the reduced decision function out we would just do this:
    learned_function.function = reduced2(trainer,10).train(samples, labels);
    // And similarly for the probabilistic_decision_function:
    learned_pfunct.function = train_probabilistic_decision_function(reduced2(trainer,10), samples, labels, 3);
}


//
//  main.cpp
//  LR-x
//
//  Created by zhuangqh on 2017/6/16.
//  Copyright © 2017年 zhuangqh. All rights reserved.
//

#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <omp.h>
#include "io.h"
#include "LR.hpp"

using Eigen::MatrixXd;
using Eigen::VectorXd;

void timer_wrapper(std::function<void()> func) {
  double startTime = omp_get_wtime();
  func();
  double stopTime = omp_get_wtime();

  std::cout << stopTime - startTime << std::endl;
}

void save_res(const char *filename, VectorXd &res) {
  std::ofstream f(filename);
  f << res;
  f.close();
}

int main() {

//  auto mat = LR::IO::load_txt("./train.txt", 1866819, 201);

  auto train = LR::IO::load_csv("../spamtrain.csv", 2760, 57);

  auto test = LR::IO::load_csv("../spamtest.csv", 1841, 57);

  LR::LogisticRegression lr(1e-5, 1000, 0);

  VectorXd res;

  timer_wrapper([&]() {
    lr.fit_naive(train);
  });
  res = lr.predict(test.first);
  save_res("naive.txt", res);

  timer_wrapper([&]() {
    lr.fit_vec(train);
  });
  res = lr.predict(test.first);
  save_res("vectorization.txt", res);

  timer_wrapper([&]() {
    lr.fit_parallel(train);
  });
  res = lr.predict(test.first);
  save_res("parallel.txt", res);

  return 0;
}