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

#include <industrial_extrinsic_cal/ceres_costs_utils.hpp>

#include <gtest/gtest.h>
#include <yaml-cpp/yaml.h>
#include <fstream>
#include <iostream>

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

using namespace industrial_extrinsic_cal;

Point3d transformPoint(Point3d &original_point, double &ax, double &ay, double &az, double &x, double&y, double &z);

std::vector<Point3d> created_points;
double aa[3]; // angle axis known/set
double p[3]; // point rotated known/set
std::vector<Point3d> transformed_points;

struct PointReprjErrorNoDistortion
{
  PointReprjErrorNoDistortion(double ob_x, double ob_y, double fx) :
      px_(ob_x), py_(ob_y), pz_(fx)
  {
  }

  template<typename T>
    bool operator()(const T* const c_p1, /** extrinsic parameters */
                    //const T* c_p2, /** intrinsic parameters */
                    const T* point, /** point being projected, yes this is has 3 parameters */
                    T* resid) const
    {
      /** extract the variables from the camera parameters */
      int q = 0; /** extrinsic block of parameters */
      const T& x = c_p1[0]; /**  angle_axis x for rotation of camera           */
      const T& y = c_p1[1]; /**  angle_axis y for rotation of camera */
      const T& z = c_p1[2]; /**  angle_axis z for rotation of camera */
      const T& tx = c_p1[3]; /**  translation of camera x */
      const T& ty = c_p1[4]; /**  translation of camera y */
      const T& tz = c_p1[5]; /**  translation of camera z */

      //std::cout<<"x, y, z: "<<c_p1[3]<<", "<<c_p1[4]<<", "<<c_p1[5]<<std::endl;

      //q = 0; /** intrinsic block of parameters */
      //const T& fx = c_p2[q++]; /**  focal length x */
      //const T& fy = c_p2[q++]; /**  focal length x */
      //const T& cx = c_p2[q++]; /**  center point x */
      //const T& cy = c_p2[q++]; /**  center point y */

      /** rotate and translate points into camera frame*/
      T aa[3]; /** angle axis*/
      T p[3]; /** point rotated*/
      aa[0] = x;
      aa[1] = y;
      aa[2] = z;
      ceres::AngleAxisRotatePoint(aa, point, p);

      /** apply camera translation*/
      T xp1 = p[0] + tx; /** point rotated and translated*/
      T yp1 = p[1] + ty;
      T zp1 = p[2] + tz;

      /** scale into the image plane by distance away from camera
      T xp = xp1 / zp1;
      T yp = yp1 / zp1;*/
      T xp=xp1;
      T yp=yp1;
      T zp=zp1;

      /** perform projection using focal length and camera center into image plane
      resid[0] = T(fx_) * xp + T(cx_) - T(ox_);
      resid[1] = T(fy_) * yp + T(cy_) - T(oy_);*/
      resid[0] = T(xp)-T(px_);
      resid[1] = T(yp)-T(py_);
      resid[2] = T(zp)-T(pz_);

      return true;
    } /** end of operator() */

  /** Factory to hide the construction of the CostFunction object from */
  /** the client code. */
  static ceres::CostFunction* Create(const double p_x, const double p_y, const double p_z)
  {
    return (new ceres::AutoDiffCostFunction<PointReprjErrorNoDistortion, 3, 6, 3>(new PointReprjErrorNoDistortion(p_x, p_y, p_z)));
  }
  double px_; /** observed x location of object in image */
  double py_; /** observed y location of object in image */
  double pz_; /*!< known focal length of camera in x */
};

TEST(IndustrialExtrinsicCalCeresSuite, create_points)
{
  Point3d x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14, x15, x16, x17, x18, x19, x20;
  x1.pb[0]=1.0;x1.pb[1]=1.0;x1.pb[2]=1;
  created_points.push_back(x1);
  x2.pb[0]=0;x2.pb[1]=0;x2.pb[2]=0.01;
  created_points.push_back(x2);
  x3.pb[0]=0.65;x3.pb[1]=0.94;x3.pb[2]=0.01;
  created_points.push_back(x3);
  x4.pb[0]=0.2;x4.pb[1]=0.3;x4.pb[2]=0.01;
  created_points.push_back(x4);
  x5.pb[0]=0.372;x5.pb[1]=0.4;x5.pb[2]=0.4;
  created_points.push_back(x5);
  x6.pb[0]=0.3762;x6.pb[1]=0.8;x6.pb[2]=0.3;
  created_points.push_back(x6);
  x7.pb[0]=0.4;x7.pb[1]=0.389;x7.pb[2]=0.4;
  created_points.push_back(x7);
  x8.pb[0]=0.431;x8.pb[1]=0.7;x8.pb[2]=0.7;
  created_points.push_back(x8);
  x9.pb[0]=0.535;x9.pb[1]=0.9;x9.pb[2]=0.01;
  created_points.push_back(x9);
  x10.pb[0]=0.596;x10.pb[1]=1;x10.pb[2]=1.0;
  created_points.push_back(x10);
  x11.pb[0]=0.24;x11.pb[1]=1.0;x11.pb[2]=0.01;
  created_points.push_back(x11);
  x12.pb[0]=0.673;x12.pb[1]=1.7;x12.pb[2]=0.8;
  created_points.push_back(x12);
  x13.pb[0]=0.552;x13.pb[1]=1.15;x13.pb[2]=0.01;
  created_points.push_back(x13);
  x14.pb[0]=0.56;x14.pb[1]=0.81;x14.pb[2]=0.01;
  created_points.push_back(x14);
  x15.pb[0]=.70;x15.pb[1]=1.10;x15.pb[2]=0.01;
  created_points.push_back(x15);
  x16.pb[0]=0.3762;x16.pb[1]=0.02435;x16.pb[2]=0.3;
  created_points.push_back(x16);
  x17.pb[0]=0.0234;x17.pb[1]=0.389;x17.pb[2]=0.132;
  created_points.push_back(x17);
  x18.pb[0]=0.431;x18.pb[1]=0.245;x18.pb[2]=0.0235;
  created_points.push_back(x18);
  x19.pb[0]=0.535;x19.pb[1]=0.673;x19.pb[2]=0.01;
  created_points.push_back(x19);
  x20.pb[0]=0.76;x20.pb[1]=0.453;x20.pb[2]=1.0;
  created_points.push_back(x20);
  std::cout<<"Original Point 1: "<<x1.pb[0]<<" "<<x1.pb[1]<<" "<<x1.pb[2]<<std::endl;

  //create known transform
  aa[0] = 2.7;
  aa[1] = 0.3;
  aa[2] = 0.1;
  p[0]=0.2;
  p[1]=0.4;
  p[2]=0.5;
  Point3d t_point;
  for (int i=0; i<created_points.size();i++)
  {
    t_point=transformPoint(created_points.at(i), aa[0], aa[1], aa[2], p[0], p[1], p[2]);
    transformed_points.push_back(t_point);
  }

}

TEST(IndustrialExtrinsicCalCeresSuite, points_costfunction)
{
double extrinsics[6];
ceres::Problem problem;
  for (int j = 0; j < transformed_points.size(); ++j)
  {
    ceres::CostFunction* cost_function = PointReprjErrorNoDistortion::Create(transformed_points.at(j).pb[0], transformed_points.at(j).pb[1], transformed_points.at(j).pb[2]);

    problem.AddResidualBlock(cost_function, NULL, extrinsics, created_points[j].pb);
    //problem.SetParameterBlockConstant(C.PB_intrinsics);
    problem.SetParameterBlockConstant(created_points[j].pb);
  }
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.max_num_iterations = 1000;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  //std::cout << summary.FullReport() << "\n";
  std::cout << summary.BriefReport() << "\n";

  EXPECT_FLOAT_EQ(-2.7,extrinsics[0]);
  EXPECT_FLOAT_EQ(-0.3,extrinsics[1]);
  EXPECT_FLOAT_EQ(-0.1,extrinsics[2]);
  EXPECT_FLOAT_EQ(0.2,extrinsics[3]);
  EXPECT_FLOAT_EQ(0.4,extrinsics[4]);
  EXPECT_FLOAT_EQ(0.5,extrinsics[5]);
  std::cout<<"Original aa rot and translation: "<<aa[0]<<" "<<aa[1]<<" "<<aa[2]
                                     <<" "<<p[0]<<" "<<p[1]<<" "<<p[2]<<std::endl;

  std::cout<<"Optimized Extrinsics rot/transl: "<<extrinsics[0]<<" "<<extrinsics[1]<<" "<<extrinsics[2]<<" "
      <<extrinsics[3]<<" "<<extrinsics[4]<<" "<<extrinsics[5]<<std::endl;
}

TEST(DISABLED_IndustrialExtrinsicCalCeresSuite, camera_costfunction)
//void test()//
{
  CameraParameters c_parameters;
  c_parameters.angle_axis[0]=0.0;//aa[0];//0.3;//
  c_parameters.angle_axis[1]=0.0;//aa[1];//0.7;//
  c_parameters.angle_axis[2]=0.0;//aa[2];//2.9;//
  c_parameters.position[0]=0.0;//p[0];//0.9;//
  c_parameters.position[1]=0.0;//p[1];//0.3;//
  c_parameters.position[2]=0.0;//p[2];//1.8;//
  c_parameters.focal_length_x=525;
  c_parameters.focal_length_y=525;
  c_parameters.center_x=320;
  c_parameters.center_y=240;
  c_parameters.distortion_k1=0.01;
  c_parameters.distortion_k2=0.02;
  c_parameters.distortion_k3=0.03;
  c_parameters.distortion_p1=0.01;
  c_parameters.distortion_p2=0.01;

  std::vector<Observation> projected_observations;
  Observation proj_point;
  for (int m=0; m<transformed_points.size(); m++)
  {
    proj_point=industrial_extrinsic_cal::projectPointWithDistortion(c_parameters, transformed_points.at(m));
    projected_observations.push_back(proj_point);
  }

  double extrinsics[6];
  extrinsics[0]=c_parameters.pb_extrinsics[0];//0.2;//
  extrinsics[1]=c_parameters.pb_extrinsics[1];//0.1;//
  extrinsics[2]=c_parameters.pb_extrinsics[2];//0.9;//
  extrinsics[3]=c_parameters.pb_extrinsics[3];//1.3;//
  extrinsics[4]=c_parameters.pb_extrinsics[4];//0.1;//
  extrinsics[5]=c_parameters.pb_extrinsics[5];//2.0;//
  ceres::Problem problem;
  double original_points[3];
    for (int j = 0; j < projected_observations.size(); ++j)
    {
      ceres::CostFunction* cost_function = CameraReprjErrorWithDistortion::Create(projected_observations.at(j).image_loc_x,
             projected_observations.at(j).image_loc_y);

      problem.AddResidualBlock(cost_function, NULL, extrinsics, c_parameters.pb_intrinsics, created_points[j].pb);
      problem.SetParameterBlockConstant(c_parameters.pb_intrinsics);
      problem.SetParameterBlockConstant(created_points[j].pb);
    }
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 1000;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    //std::cout << summary.FullReport() << "\n";
    std::cout << summary.BriefReport() << "\n";

    std::cout<<"Expected aa rot and translation: "<<aa[0]<<" "<<aa[1]<<" "<<aa[2]
                                       <<" "<<p[0]<<" "<<p[1]<<" "<<p[2]<<std::endl;

    std::cout<<"Optimized Extrinsics rot/transl: "<<extrinsics[0]<<" "<<extrinsics[1]<<" "
        <<extrinsics[2]<<" "<<extrinsics[3]<<" "<<extrinsics[4]<<" "
        <<extrinsics[5]<<std::endl;
}

//void test()
TEST(DISABLED_IndustrialExtrinsicCalCeresSuite, camera_no_dist_costfunction)
{
  CameraParameters c_parameters;
  c_parameters.angle_axis[0]=0.0;//aa[0];//0.3;//
  c_parameters.angle_axis[1]=0.0;//aa[1];//0.7;//
  c_parameters.angle_axis[2]=0.0;//aa[2];//2.9;//
  c_parameters.position[0]=0.0;//p[0];//0.9;//
  c_parameters.position[1]=0.0;//p[1];//0.3;//
  c_parameters.position[2]=0.0;//p[2];//1.8;//
  c_parameters.focal_length_x=525;
  c_parameters.focal_length_y=525;
  c_parameters.center_x=320;
  c_parameters.center_y=240;
  c_parameters.distortion_k1=0.01;
  c_parameters.distortion_k2=0.02;
  c_parameters.distortion_k3=0.03;
  c_parameters.distortion_p1=0.01;
  c_parameters.distortion_p2=0.01;

  std::vector<Observation> projected_observations;
  Observation proj_point;
  for (int m=0; m<transformed_points.size(); m++)
  {

    proj_point=industrial_extrinsic_cal::projectPointNoDistortion(c_parameters, transformed_points.at(m));
    projected_observations.push_back(proj_point);
  }

  double extrinsics[6];
  extrinsics[0]=c_parameters.pb_extrinsics[0];//0.2;//
  extrinsics[1]=c_parameters.pb_extrinsics[1];//0.1;//
  extrinsics[2]=c_parameters.pb_extrinsics[2];//0.9;//
  extrinsics[3]=c_parameters.pb_extrinsics[3];//1.3;//
  extrinsics[4]=c_parameters.pb_extrinsics[4];//0.1;//
  extrinsics[5]=c_parameters.pb_extrinsics[5];//2.0;//
  ceres::Problem problem;
    for (int j = 0; j < projected_observations.size(); ++j)
    {
      ceres::CostFunction* cost_function = CameraReprjErrorNoDistortion::Create(projected_observations.at(j).image_loc_x,
             projected_observations.at(j).image_loc_y, c_parameters.focal_length_x, c_parameters.focal_length_y,
             c_parameters.center_x, c_parameters.center_y);

      problem.AddResidualBlock(cost_function, NULL, extrinsics, c_parameters.pb_intrinsics, created_points.at(j).pb);
      problem.SetParameterBlockConstant(c_parameters.pb_intrinsics);
      problem.SetParameterBlockConstant(created_points.at(j).pb);
    }
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 1000;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    //std::cout << summary.FullReport() << "\n";
    std::cout << summary.BriefReport() << "\n";

    std::cout<<"Expected aa rot and translation: "<<aa[0]<<" "<<aa[1]<<" "<<aa[2]
                                       <<" "<<p[0]<<" "<<p[1]<<" "<<p[2]<<std::endl;

    std::cout<<"Optimized Extrinsics rot/transl: "<<extrinsics[0]<<" "<<extrinsics[1]<<" "
        <<extrinsics[2]<<" "<<extrinsics[3]<<" "<<extrinsics[4]<<" "
        <<extrinsics[5]<<std::endl;
}

Point3d transformPoint(Point3d &original_point, double &ax, double &ay, double &az, double &x, double&y, double &z)
{
  //std::cout<<"ange axis inputs ax, ay, az: "<<ax<<", "<<ay<<", "<<az<<std::endl;
  Eigen::Matrix3f m_ceres;
  Eigen::Matrix3f m_eigen;
  m_eigen = Eigen::AngleAxisf(ax, Eigen::Vector3f::UnitX())
  * Eigen::AngleAxisf(ay, Eigen::Vector3f::UnitY())
  * Eigen::AngleAxisf(az, Eigen::Vector3f::UnitZ());
  //std::cout<<"m_eigen : "<<std::endl<< m_eigen <<std::endl;
  double aa[3]; // angle axis
  double p[3]; // point rotated
  aa[0] = ax;
  aa[1] = ay;
  aa[2] = az;
  Eigen::Vector3f orig_point(original_point.pb[0], original_point.pb[1], original_point.pb[2]);
  //std::cout<<"within transformPoint, original point Vector3f x, y, z: "<<orig_point.x()<<", "<<orig_point.y()<<", "<<orig_point.z()<<std::endl;

  double R[9];
  ceres::AngleAxisToRotationMatrix(aa, R);
  m_ceres << R[0], R[1],R[2],
      R[3], R[4], R[5],
      R[6], R[7], R[8];
  //std::cout<<"m_ceres : "<<std::endl<< m_ceres <<std::endl;
  Eigen::Vector3f rot_point=m_ceres*orig_point;
  //std::cout<<"within transformPoint, rotated point Vector3f x, y, z: "<<rot_point.x()<<", "<<rot_point.y()<<", "<<rot_point.z()<<std::endl;
  double xp1 = rot_point.x() + x; // point rotated and translated
  double yp1 = rot_point.y() + y;
  double zp1 = rot_point.z() + z;
  //std::cout<<"within transformPoint, original point double x, y, z: "<<xp1<<", "<<yp1<<", "<<zp1<<std::endl;
  Point3d t_point;
  t_point.pb[0]=xp1;t_point.pb[1]=yp1;t_point.pb[2]=zp1;
  //std::cout<<"within transformPoint, t_point x, y, z: "<<t_point.pb[0]<<", "<<t_point.pb[1]<<", "<<t_point.pb[2]<<std::endl;
  return t_point;
}

// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
  //ros::init(argc, argv, "test");
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();

}


/**
 * math lib test
 * @author Tobias Weber <tweber@ill.fr>
 * @date 10-jun-20
 * @license GPLv3, see 'LICENSE' file
 *
 * g++ -std=c++20 -I.. -I/usr/include/libqhullcpp -DUSE_QHULL -o hull1 hull1.cpp -lqhull_r -lqhullcpp
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Hull1
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>

#include "libs/maths.h"
using namespace tl2_ops;

using t_types = std::tuple<double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_equals, t_real, t_types)
{
	using t_vec = tl2::vec<t_real, std::vector>;

	std::vector<t_vec> vecs =
	{
		{1, 1, -1},
		{1, -1, -1},
		{-1, 1, -1},
		{-1, -1, -1},

		{1, 1, 1},
		{1, -1, 1},
		{-1, 1, 1},
		{-1, -1, 1},
	};

	auto [hull, norms, dists] = tl2_qh::get_convexhull<t_vec>(vecs);

	for(std::size_t faceidx=0; faceidx<hull.size(); ++faceidx)
	{
		const auto& face = hull[faceidx];
		std::cout << "face " << faceidx << ":\n";

		for(const auto& vert : face)
			std::cout << "vertex: " << vert << "\n";
		std::cout << "normal: " << norms[faceidx] << "\n";

		std::cout << std::endl;
	}

	BOOST_TEST(hull.size() == 6*2);
	for(const auto& face : hull)
		BOOST_TEST(face.size() == 3);

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


#include <utility>
#include <iostream>
#include <Vector>
#include <cmath>
#include <iomanip>
#include <Eigen/Dense>
// pas terminé,

template <typename arg, class func, class jac>
void mod_newt_step_system(const arg & x, arg & x_next, function&& f, jacobian&& df){
	arg y;
	y=x+f(x)/df(x);
	x_next = y - f(y)/df(x);
}

void mod_newt_step_exec(){
	using Vector = Eigen::VectorXd;
	using Matrix = Eingen::MatrixXd;

	Vector x;
	Vector c = Vector::random(4);
	Matrix A = Matrix::random(4,4);
	A = (A*A.transpose());
	c = c.cwiseAbs();

	auto F = [&A,&c] (const Vector & x)
	{ Vector tmp =  A*x + c.cwiseProduct(x.array().exp().Matrix()).eval(); return tmp; };
    std::function<Matrix(const Vector &)> dF = [&A, &c] (const Vector & x)
    { Matrix C = A; Vector temp = c.cwiseProduct(x.array().exp().Matrix()); C += temp.asDiagonal(); return C; };

	double tol = 1.e-15;

	int i=0;

	while (tol){
		i++;
		mod_newt_step_system(x, x_next,f,df );
		std::cout << i << "th interation,    x_next is " << x_next << "        and x_star - x_next : " << eval_err(x_next) << std::endl;
		if (i> 15){
			std::cout << " Broke " << std::endl;
			break;}
			x= x_next;
	}

}

int main () {
	mod_newt_step_exec();
	const double a = 0.123;
	std::cout << "tan(a) : " << std::tan(a) << std::endl;
	return 0;

}

Nummerical Methods for CSE/PS5/ex01_d.cpp


// eig1.cpp

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

int main(int argc, char const *argv[])
{
    // 2x2 matrix, of type float
    Eigen::Matrix<float, 2, 2> A, B;
    A << 2, -1, -1, 3;
    B << 1, 2, 3, 1;

    std::cout << "A:     " << A <<  '\n';
    std::cout << "B:     " << B << '\n';
    std::cout << "A+B:   " << A+B << '\n';
    std::cout << "A-B:   " << A-B << '\n';
    std::cout << "1.6*A: " << 1.6*A << '\n';

    return 0;
}


#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include "covariance.h"

CovObj::CovObj()
{
    clearCov();
}

CovObj::CovObj(const Vector3d &v1, const Vector3d &v2, const Vector3d &v3)
{
    area_ = 0.5 * (v2 - v1).cross(v3 - v1).norm();
    center_ = 1 / 3.0 * (v1 + v2 + v3);
    Matrix3d vm, c;
    vm(0, 0) = v1(0);
    vm(1, 0) = v1(1);
    vm(2, 0) = v1(2);
    vm(0, 1) = v2(0);
    vm(1, 1) = v2(1);
    vm(2, 1) = v2(2);
    vm(0, 2) = v3(0);
    vm(1, 2) = v3(1);
    vm(2, 2) = v3(2);
    c(0, 0) = c(1, 1) = c(2, 2) = 2;
    c(0, 1) = c(0, 2) = c(1, 0) = c(1, 2) = c(2, 0) = c(2, 1) = -1;
    cov_ = area_ / 36.0 * vm * c * vm.transpose();
    size_ = 1;
}

void CovObj::clearCov()
{
    cov_.setZero();
    area_ = 0;
    size_ = 0;
    center_ = normal_ = Vector3d::Zero();
}

CovObj &CovObj::operator+=(const CovObj &Q)
{
    // some models may contain corrupted null faces, ignore them
    if (Q.area_ < 1e-18)
        return *this;
    CovObj Q_old = *this;
    area_ = Q_old.area_ + Q.area_;
    center_ = (Q_old.area_ * Q_old.center_ + Q.area_ * Q.center_) / (area_);
    Vector3d ktoi, ktoj;
    ktoi = Q_old.center_ - center_;
    ktoj = Q.center_ - center_;
    cov_ = Q_old.cov_ + Q.cov_ + Q_old.area_ * ktoi * ktoi.transpose() + Q.area_ * ktoj * ktoj.transpose();
    size_ += Q.size_;
    return *this;
}

CovObj &CovObj::operator-=(const CovObj &Q)
{
    // some models may contain corrupted null faces, ignore them
    if (Q.area_ < 1e-18)
        return *this;
    CovObj Q_old = *this;
    area_ = Q_old.area_ - Q.area_;
    center_ = (Q_old.area_ * Q_old.center_ - Q.area_ * Q.center_) / (area_);
    Vector3d ktoi, ktoj;
    ktoi = Q_old.center_ - center_;
    ktoj = Q_old.center_ - Q.center_;
    cov_ = Q_old.cov_ - Q.cov_ - area_ * ktoi * ktoi.transpose() - Q.area_ * ktoj * ktoj.transpose();
    size_ -= Q.size_;
    return *this;
}

bool CovObj::operator==(const CovObj &Q)
{
    return cov_ == Q.cov_ && normal_ == Q.normal_ && center_ == Q.center_ && area_ == Q.area_ && size_ == Q.size_;
}

CovObj &CovObj::operator=(const CovObj &Q)
{
    // A good habit to check for self-assignment
    if (this == &Q)
        return *this;

    cov_ = Q.cov_;
    normal_ = Q.normal_;
    center_ = Q.center_;
    area_ = Q.area_;
    size_ = Q.size_;
    return *this;
}

double CovObj::energy()
{
    if (area_ < 1e-10 || size_ <= 1) // 1 triangle has 0 energy
        return 0;
    if (cov_.determinant() / pow(area_, 5) < 1e-15)
        return cov_.trace() * area_ * 1e-20;
    else
        return cov_.determinant() / pow(area_, 4);
}

void CovObj::computePlaneNormal()
{
    Eigen::SelfAdjointEigenSolver<Matrix3d> es(cov_);
    int smallest = 0;

    for (int i = 1; i < 3; i++)
    {
        if (abs(es.eigenvalues()[i]) < abs(es.eigenvalues()[smallest]))
            smallest = i;
    }
    // Plane normal is the eigenvector corresponding to the smallest eigenvalue
    normal_ = es.eigenvectors().col(smallest);
    normal_.normalize();
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file linear_algebra_smw_inversion_test.cpp
 * \date 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include <cmath>
#include <iostream>
#include <vector>
#include <ctime>

#include <fl/util/math.hpp>

typedef Eigen::Matrix<double, 3, 1> State;
typedef Eigen::Matrix<double, 1, 1> Observation;

const int SUBSAMPLING_FACTOR = 8;
const int OBSERVATION_DIMENSION = (640*480)/(SUBSAMPLING_FACTOR*SUBSAMPLING_FACTOR);

//const int INV_DIMENSION = 14;
//const int INVERSION_ITERATIONS = OBSERVATION_DIMENSION * 30;

TEST(InversionTests, SMWInversion)
{
    Eigen::MatrixXd cov = Eigen::MatrixXd::Random(15, 15);
    cov = cov * cov.transpose();

    Eigen::MatrixXd A = cov.block(0,   0, 14, 14);
    Eigen::MatrixXd B = cov.block(0,  14, 14,  1);
    Eigen::MatrixXd C = cov.block(14,  0, 1,  14);
    Eigen::MatrixXd D = cov.block(14, 14, 1,   1);

    Eigen::MatrixXd L_A;
    Eigen::MatrixXd L_B;
    Eigen::MatrixXd L_C;
    Eigen::MatrixXd L_D;

    Eigen::MatrixXd cov_inv = cov.inverse();
    Eigen::MatrixXd cov_smw_inv;

    Eigen::MatrixXd A_inv = A.inverse();
    fl::smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D, cov_smw_inv);

    EXPECT_TRUE(cov_smw_inv.isApprox(cov_inv));
}

TEST(InversionTests, SMWInversion_no_Lx)
{
    Eigen::MatrixXd cov = Eigen::MatrixXd::Random(15, 15);
    cov = cov * cov.transpose();

    Eigen::MatrixXd A = cov.block(0,   0, 14, 14);
    Eigen::MatrixXd B = cov.block(0,  14, 14,  1);
    Eigen::MatrixXd C = cov.block(14,  0, 1,  14);
    Eigen::MatrixXd D = cov.block(14, 14, 1,   1);

    Eigen::MatrixXd cov_inv = cov.inverse();
    Eigen::MatrixXd cov_smw_inv;

    Eigen::MatrixXd A_inv = A.inverse();
    fl::smw_inverse(A_inv, B, C, D, cov_smw_inv);

    EXPECT_TRUE(cov_smw_inv.isApprox(cov_inv));
}

// speed performance tests
//TEST(InversionTests, fullMatrixInversionSpeed)
//{
//    Eigen::MatrixXd cov = Eigen::MatrixXd::Random(INV_DIMENSION, INV_DIMENSION);
//    cov = cov * cov.transpose();

//    Eigen::MatrixXd cov_inv;

//    std::clock_t start = std::clock();
//    int number_of_inversions = 0;
//    while ( (( std::clock() - start ) / (double) CLOCKS_PER_SEC) < 1.0 )
//    {
//        cov_inv = cov.inverse();
//        number_of_inversions++;
//    }

////    std::cout << "fullMatrixInversionSpeed::number_of_inversions: "
////              << number_of_inversions
////              << "(" << number_of_inversions/OBSERVATION_DIMENSION << " fps)"
////              << std::endl;

//}

//TEST(InversionTests, SMWMatrixInversionSpeed)
//{
//    Eigen::MatrixXd cov = Eigen::MatrixXd::Random(INV_DIMENSION, INV_DIMENSION);
//    cov = cov * cov.transpose();

//    Eigen::MatrixXd A = cov.block(0, 0, INV_DIMENSION-1, INV_DIMENSION-1);
//    Eigen::MatrixXd B = cov.block(0, INV_DIMENSION-1, INV_DIMENSION-1, 1);
//    Eigen::MatrixXd C = cov.block(INV_DIMENSION-1, 0, 1, INV_DIMENSION-1);
//    Eigen::MatrixXd D = cov.block(INV_DIMENSION-1, INV_DIMENSION-1, 1, 1);
//    Eigen::MatrixXd A_inv = A.inverse();

//    Eigen::MatrixXd L_A = Eigen::MatrixXd(INV_DIMENSION-1, INV_DIMENSION-1);
//    Eigen::MatrixXd L_B = Eigen::MatrixXd(INV_DIMENSION-1, 1);
//    Eigen::MatrixXd L_C = Eigen::MatrixXd(1, INV_DIMENSION-1);
//    Eigen::MatrixXd L_D = Eigen::MatrixXd(1, 1);

//    Eigen::MatrixXd cov_smw_inv;
//    std::clock_t start = std::clock();
//    int number_of_inversions = 0;
//    while ( ((std::clock() - start) / (double) CLOCKS_PER_SEC) < 1.0 )
//    {
//        fl::smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D, cov_smw_inv);
//        number_of_inversions++;
//    }

////    std::cout << "SMWMatrixInversionSpeed::number_of_inversions: "
////              << number_of_inversions
////              << "(" << number_of_inversions/OBSERVATION_DIMENSION << " fps)"
////              << std::endl;
//}

//TEST(InversionTests, SMWBlockMatrixInversionSpeed)
//{
//    Eigen::MatrixXd cov = Eigen::MatrixXd::Random(INV_DIMENSION, INV_DIMENSION);
//    cov = cov * cov.transpose();

//    Eigen::MatrixXd A = cov.block(0, 0, INV_DIMENSION-1, INV_DIMENSION-1);
//    Eigen::MatrixXd B = cov.block(0, INV_DIMENSION-1, INV_DIMENSION-1, 1);
//    Eigen::MatrixXd C = cov.block(INV_DIMENSION-1, 0, 1, INV_DIMENSION-1);
//    Eigen::MatrixXd D = cov.block(INV_DIMENSION-1, INV_DIMENSION-1, 1, 1);
//    Eigen::MatrixXd A_inv = A.inverse();

//    Eigen::MatrixXd L_A = Eigen::MatrixXd(INV_DIMENSION-1, INV_DIMENSION-1);
//    Eigen::MatrixXd L_B = Eigen::MatrixXd(INV_DIMENSION-1, 1);
//    Eigen::MatrixXd L_C = Eigen::MatrixXd(1, INV_DIMENSION-1);
//    Eigen::MatrixXd L_D = Eigen::MatrixXd(1, 1);

//    std::clock_t start = std::clock();
//    int number_of_inversions = 0;
//    while ( (( std::clock() - start ) / (double) CLOCKS_PER_SEC) < 1.0 )
//    {
//        fl::smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D);
//        number_of_inversions++;
//    }

////    std::cout << "SMWMatrixBlockInversionSpeed::number_of_inversions: "
////              << number_of_inversions
////              << "(" << number_of_inversions/OBSERVATION_DIMENSION << " fps)"
////              << std::endl;
//}


//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <cmath>
#include <limits>
#include <vector>

namespace gil = boost::gil;

// Demonstrates the use of a rasterizer to generate an image of a circle
// The various rasterizers available are defined in include/boost/gil/rasterization/circle.hpp,
// include/boost/gil/rasterization/ellipse.hpp and include/boost/gil/rasterization/line.hpp
// This example uses a trigonometric rasterizer; GIL also offers the rasterizer midpoint_circle_rasterizer,
// which implements the Midpoint algorithm.
// See also:
// rasterizer_ellipse.cpp - Demonstrates the use of a rasterizer to generate an image of an ellipse
// rasterizer_line.cpp - Demonstrates the use of a rasterizer to generate an image of a line

int main()
{
    const std::ptrdiff_t size = 256;
    gil::gray8_image_t buffer_image(size, size);
    auto buffer = gil::view(buffer_image);

    const std::ptrdiff_t radius = 64;
    const auto rasterizer = gil::trigonometric_circle_rasterizer{};
    std::vector<gil::point_t> circle_points(rasterizer.point_count(radius));
    rasterizer(radius, {128, 128}, circle_points.begin());
    for (const auto& point : circle_points)
    {
        buffer(point) = std::numeric_limits<gil::uint8_t>::max();
    }

    gil::write_view("circle.png", buffer, gil::png_tag{});
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/either.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/tuple.hpp>

#include <string>
using namespace boost::hana;
using namespace std::literals;

int main() {

{

//! [comparable]
BOOST_HANA_CONSTEXPR_CHECK(left('x') == left('x'));
BOOST_HANA_CONSTANT_CHECK(right('x') != left('x'));
BOOST_HANA_CONSTEXPR_CHECK(right('x') == right('x'));
BOOST_HANA_CONSTEXPR_CHECK(right('x') != right('y'));
//! [comparable]

}{

//! [orderable]
// left is always less than right, regardless of what's in it
BOOST_HANA_CONSTANT_CHECK(left(2000) < right(2));

// when comparing two lefts or two rights, we compare the contents
BOOST_HANA_CONSTEXPR_CHECK(left(2) < left(2000));
BOOST_HANA_CONSTEXPR_CHECK(right(2) < right(2000));
//! [orderable]

}{

//! [functor]
auto safe_div = infix([](auto x, auto y) {
    return eval_if(y == int_<0>,
        always(left("division by zero"s)),
        [=](auto _) { return right(x / _(y)); }
    );
});

BOOST_HANA_CONSTANT_CHECK(
    transform(int_<6> ^safe_div^ int_<3>, succ) == right(int_<3>)
);

BOOST_HANA_RUNTIME_CHECK(
    transform(int_<6> ^safe_div^ int_<0>, succ) == left("division by zero"s)
);
//! [functor]

}{

//! [monad]
auto safe_div = [](auto x, auto y) {
    return eval_if(y == int_<0>,
        always(left("division by zero"s)),
        [=](auto _) { return right(x / _(y)); }
    );
};

auto safe_dec = [](auto x) {
    return eval_if(x == int_<0>,
        always(left("negative value"s)),
        [=](auto _) { return right(_(x) - int_<1>); }
    );
};

BOOST_HANA_RUNTIME_CHECK(
    (safe_div(int_<4>, int_<0>) | safe_dec) == left("division by zero"s)
);

BOOST_HANA_RUNTIME_CHECK(
    (safe_div(int_<0>, int_<2>) | safe_dec) == left("negative value"s)
);

BOOST_HANA_CONSTANT_CHECK(
    (safe_div(int_<4>, int_<2>) | safe_dec) == right(int_<1>)
);
//! [monad]

}{

//! [foldable]
BOOST_HANA_CONSTANT_CHECK(unpack(left('x'), make<Tuple>) == make<Tuple>());
BOOST_HANA_CONSTEXPR_CHECK(unpack(right('x'), make<Tuple>) == make<Tuple>('x'));
//! [foldable]

}{

//! [traversable]
BOOST_HANA_CONSTEXPR_LAMBDA auto duplicate = [](auto x) {
    return make<Tuple>(x, x);
};

BOOST_HANA_CONSTEXPR_CHECK(
    traverse<Tuple>(left(1), duplicate) == make<Tuple>(left(1))
);
BOOST_HANA_CONSTEXPR_CHECK(
    traverse<Tuple>(right(1), duplicate) == make<Tuple>(right(1), right(1))
);
//! [traversable]

}{

//! [either]
BOOST_HANA_CONSTEXPR_CHECK(either(succ, pred, left(1)) == 2);
BOOST_HANA_CONSTEXPR_CHECK(either(succ, pred, right(1)) == 0);
//! [either]

}{

//! [left]
constexpr auto left_value = left('x');
//! [left]

//! [right]
constexpr auto right_value = right('x');
//! [right]

(void)left_value;
(void)right_value;
}

}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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)

//! \file
//!\brief Tests for the hyperbolic arctangent function of (fixed_point) for a small digit range.

#include <cmath>

#define BOOST_TEST_MODULE test_negatable_func_hyperbolic_arctangent_small
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_negatable_func_hyperbolic_arctangent_small)
{
  typedef boost::fixed_point::negatable<7, -24> fixed_point_type;
  typedef fixed_point_type::float_type          float_point_type;

  const fixed_point_type tol = ldexp(fixed_point_type(1), fixed_point_type::resolution + 4);

  BOOST_CONSTEXPR int i_max = 32;

  // Check positive arguments.
  for(int i = 0; i < i_max; ++i)
  {
    const fixed_point_type x = atanh(fixed_point_type(i) / i_max);

    using std::atanh;
    const float_point_type y = atanh(float_point_type(i) / i_max);

    BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
  }

  // Check negative arguments.
  for(int i = 0; i < 9; ++i)
  {
    const fixed_point_type x = atanh(fixed_point_type(-i) / i_max);

    using std::atanh;
    const float_point_type y = atanh(float_point_type(-i) / i_max);

    BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
  }
}


//
// Created by Amir Masoud Abdol on 2019-01-24.
//

#include <spdlog/spdlog.h>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>

#include "TestStrategy.h"

using namespace sam;

using boost::math::students_t;

TestStrategy::~TestStrategy(){
    // Pure destructors
};

std::unique_ptr<TestStrategy> TestStrategy::build(json &test_strategy_config) {

  if (test_strategy_config["name"] == "TTest") {

    auto params = test_strategy_config.get<TTest::Parameters>();
    return std::make_unique<TTest>(params);

  } else if (test_strategy_config["name"] == "YuenTest") {

    auto params = test_strategy_config.get<YuenTest::Parameters>();
    return std::make_unique<YuenTest>(params);

  } else if (test_strategy_config["name"] == "WilcoxonTest") {
    auto params = test_strategy_config.get<WilcoxonTest::Parameters>();
    return std::make_unique<WilcoxonTest>(params);
  } else {
    spdlog::critical("Unknown Test Strategy.");
    exit(1);
  }
}

namespace sam {

///
/// Calculate confidence intervals for the mean. For example if we set the
/// confidence limit to 0.95, we know that if we repeat the sampling 100 times,
/// then we expect that the true mean will be between out limits on 95 occasions.
/// Note: this is not the same as saying a 95% confidence interval means that there
/// is a 95% probability that the interval contains the true mean. The interval
/// computed from a given sample either contains the true mean or it does not.
///
/// @note       Obtained from [Boost Library
///             Example](https://www.boost.org/doc/libs/1_69_0/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/paired_st.html).
///
/// @param      Sm    Sample Mean.
/// @param      Sd    Sample Standard Deviation.
/// @param      Sn    Sample Size.
///
std::pair<float, float>
confidence_limits_on_mean(float Sm, float Sd, unsigned Sn, float alpha,
                          TestStrategy::TestAlternative alternative) {

  using namespace sam;

  students_t dist(Sn - 1);

  // calculate T
  float T =
      quantile(complement(dist, alpha / 2)); // TODO: Implement the side!./sa

  // Calculate width of interval (one sided):
  float w = T * Sd / sqrt(float(Sn));

  // Calculate and return the interval
  return std::make_pair(Sm - w, Sm + w);
}

///
/// Caculate the degress of freedom to achieve a significance result with the given
/// alpha
///
/// @param      M     True Mean.
/// @param      Sm    Sample Mean.
/// @param      Sd    Sample Standard Deviation.
///
float single_sample_find_df(float M, float Sm, float Sd, float alpha,
                             TestStrategy::TestAlternative alternative) {
  using namespace sam;
  using boost::math::students_t;

  // calculate df for one-sided or two-sided test:
  float df = students_t::find_degrees_of_freedom(
      fabs(M - Sm),
      (alternative == TestStrategy::TestAlternative::Greater) ? alpha
                                                              : alpha / 2.,
      alpha, Sd);

  // convert to sample size, always one more than the degrees of freedom:
  return ceil(df) + 1;
}

float win_var(const arma::Row<float> &x, const float trim) {
  return arma::var(win_val(x, trim));
}

std::pair<float, float> win_cor_cov(const arma::Row<float> &x,
                                      const arma::Row<float> &y,
                                      const float trim) {

  arma::Row<float> xvec{win_val(x, trim)};
  arma::Row<float> yvec{win_val(y, trim)};

  arma::Mat<float> wcor{arma::cor(xvec, yvec)};
  float vwcor{static_cast<float>(wcor.at(0, 0))};

  arma::Mat<float> wcov{arma::cov(xvec, yvec)};
  float vwcov{static_cast<float>(wcov.at(0, 0))};

  return std::make_pair(vwcor, vwcov);
}

arma::Row<float> win_val(const arma::Row<float> &x, float trim) {

  arma::Row<float> y{arma::sort(x)};

  auto ibot = floor(trim * x.n_elem) + 1;
  auto itop = x.n_elem - ibot + 1;

  float xbot{y.at(ibot - 1)};
  float xtop{y.at(itop - 1)};

  return arma::clamp(x, xbot, xtop);
}

// TODO: this can be an extention to arma, something like I did for
// nlohmann::json I should basically put it into arma's namespace
float trim_mean(const arma::Row<float> &x, float trim) {
  arma::Row<float> y{arma::sort(x)};

  auto ibot = floor(trim * x.n_elem) + 1;
  auto itop = x.n_elem - ibot + 1;

  return arma::mean(y.subvec(ibot - 1, itop - 1));
}

float tie_correct(const arma::Col<float> &rankvals) {

  arma::Col<float> arr = arma::sort(rankvals);

  arma::uvec vindx = arma::join_cols(
      arma::uvec({1}), arr.tail(arr.n_elem - 1) != arr.head(arr.n_elem - 1));

  arma::uvec vvindx = arma::join_cols(vindx, arma::uvec({1}));

  arma::uvec indx = nonzeros_index(vvindx);

  arma::uvec cnt = arma::diff(indx);

  auto size = arr.n_elem;

  if (size < 2) {
    return 1.0;
  }

  return 1.0 - arma::accu(arma::pow(cnt, 3) - cnt) / (std::pow(size, 3) - size);
}

arma::Col<float> rankdata(const arma::Row<float> &arr,
                   const std::string method = "average") {

  // if method not in ('average', 'min', 'max', 'dense', 'ordinal'):
  //     raise ValueError('unknown method "{0}"'.format(method))

  // arr = np.ravel(np.asarray(a))
  // > This is always true, for now

  // algo = 'mergesort' if method == 'ordinal' else 'quicksort'
  arma::uvec sorter = arma::stable_sort_index(arr);

  // inv = np.empty(sorter.size, dtype=np.intp)
  // inv[sorter] = np.arange(sorter.size, dtype=np.intp)

  arma::uvec inv(sorter.n_elem);

  inv.elem(sorter) = arma::regspace<arma::uvec>(0, sorter.n_elem - 1);

  //        if (method == "ordinal")
  //            return inv + 1;

  arma::Col<float> arr_sorted(arr.n_elem);
  arr_sorted = arr(sorter);

  // obs = np.r_[True, arr[1:] != arr[:-1]]
  arma::uvec obs = arma::join_cols(arma::uvec({1}),
                                   arr_sorted.tail(arr_sorted.n_elem - 1) !=
                                       arr_sorted.head(arr_sorted.n_elem - 1));

  // dense = obs.cumsum()[inv]
  arma::uvec dense = arma::cumsum(obs); //.elem(inv);
  arma::uvec dense_sorted(dense.n_elem);
  dense_sorted = dense(inv);

  if (method == "dense")
    return arma::conv_to<arma::Col<float>>::from(dense_sorted);

  // cumulative counts of each unique value
  // count = [np.r_[np.nonzero(obs)[0], len(obs)]]
  arma::uvec count =
      arma::join_cols(nonzeros_index(obs), arma::uvec({obs.n_elem}));

  //        if (method == "max")
  //            return count(dense_sorted);
  //
  //        if (method == "min")
  //            return count(dense_sorted - 1) + 1;

  // average method
  return .5 * arma::conv_to<arma::Col<float>>::from(
                  (count(dense_sorted) + count(dense_sorted - 1) + 1));
}

} // namespace sam


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[simplify
//` Example showing how to simplify a linestring

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

/*< For this example we use Boost.Assign to add points >*/
#include <boost/assign.hpp>

using namespace boost::assign;

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> xy;

    boost::geometry::model::linestring<xy> line;
    line += xy(1.1, 1.1), xy(2.5, 2.1), xy(3.1, 3.1), xy(4.9, 1.1), xy(3.1, 1.9); /*< With Boost.Assign >*/

    // Simplify it, using distance of 0.5 units
    boost::geometry::model::linestring<xy> simplified;
    boost::geometry::simplify(line, simplified, 0.5);
    std::cout
        << "  original: " << boost::geometry::dsv(line) << std::endl
        << "simplified: " << boost::geometry::dsv(simplified) << std::endl;

    return 0;
}

//]

//[simplify_output
/*`
Output:
[pre
  original: ((1.1, 1.1), (2.5, 2.1), (3.1, 3.1), (4.9, 1.1), (3.1, 1.9))
simplified: ((1.1, 1.1), (3.1, 3.1), (4.9, 1.1), (3.1, 1.9))
]
*/
//]


/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * @date 2015
 * @author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 * Max-Planck-Institute for Intelligent Systems
 */

#include <gtest/gtest.h>

#include <Eigen/Core>

#include <fl/util/math/linear_algebra.hpp>

#include <fl/filter/particle/particle_filter.hpp>
#include <fl/filter/gaussian/gaussian_filter_kf.hpp>
#include <fl/model/sensor/linear_sensor.hpp>
#include <fl/model/process/linear_transition.hpp>

template<typename Vector, typename Matrix>
bool moments_are_similar(Vector mean_a, Matrix cov_a,
                         Vector mean_b, Matrix cov_b, double epsilon = 0.1)
{
    Matrix cov_delta = cov_a.inverse() * cov_b;
    bool are_similar = cov_delta.isApprox(Matrix::Identity(), epsilon);

    Matrix square_root = fl::matrix_sqrt(cov_a);
    double max_mean_delta =
            (square_root.inverse() * (mean_a-mean_b)).cwiseAbs().maxCoeff();

    are_similar = are_similar && max_mean_delta < epsilon;

    return are_similar;
}

Eigen::Matrix<double, 3, 3> some_rotation()
{
    double angle = 2 * M_PI * double(rand()) / double(RAND_MAX);

    Eigen::Matrix<double, 3, 3> R = Eigen::Matrix<double, 3, 3>::Identity();

    R = R * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
    R = R * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
    R = R * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY());
    return R;
}

TEST(particle_filter, predict)
{
    typedef Eigen::Matrix<double, 3, 1> State;
    typedef Eigen::Matrix<double, 3, 1> Observation;
    typedef Eigen::Matrix<double, 3, 1> Input;

    typedef Eigen::Matrix<double, 3, 3> Matrix;

    typedef fl::LinearGaussianProcessModel<State, Input> ProcessModel;
    typedef fl::LinearObservationModel<Observation, State> ObservationModel;

    // particle filter
    typedef fl::ParticleFilter<ProcessModel, ObservationModel> ParticleFilter;
    typedef ParticleFilter::Belief ParticleBelief;

    // gaussian filter
    typedef fl::GaussianFilter<ProcessModel, ObservationModel> GaussianFilter;
    typedef GaussianFilter::Belief GaussianBelief;

    srand(0);
    size_t N_particles = 10000;
    size_t N_steps = 10;
    size_t delta_time = 1;

    // create process model
    ProcessModel transition;
    {
        transition.A(some_rotation());
        Matrix R = some_rotation();
        Matrix D = Eigen::DiagonalMatrix<double, 3>(1, 3.5, 1.2);
        transition.covariance(R*D*R.transpose());
    }

    // create observation model
    /// \todo this is a hack because the GF does not currently work with the new
    /// observation model interface
    ObservationModel sensor;
    {
        sensor.sensor_matrix(some_rotation());
        Matrix R = some_rotation();
        Matrix D = Eigen::DiagonalMatrix<double, 3>(3.1, 1.0, 1.3);
        D = D.cwiseSqrt();
        sensor.noise_matrix(R*D);
    }

    // create filters
    ParticleFilter particle_filter(transition, sensor);
    GaussianFilter gaussian_filter(transition, sensor);

    // create intial beliefs
    GaussianBelief gaussian_belief;
    {
        gaussian_belief.mean(State::Zero());
        gaussian_belief.covariance(Matrix::Identity());
    }
    ParticleBelief particle_belief;
    particle_belief.from_distribution(gaussian_belief, N_particles);

    // run prediction
    for(size_t i = 0; i < N_steps; i++)
    {
        particle_filter.predict(delta_time, State::Zero(),
                                particle_belief, particle_belief);

        gaussian_filter.predict(delta_time, State::Zero(),
                                gaussian_belief, gaussian_belief);

        EXPECT_TRUE(moments_are_similar(
                        particle_belief.mean(), particle_belief.covariance(),
                        gaussian_belief.mean(), gaussian_belief.covariance()));
    }
}

TEST(particle_filter, update)
{
    typedef Eigen::Matrix<double, 3, 1> State;
    typedef Eigen::Matrix<double, 3, 1> Observation;
    typedef Eigen::Matrix<double, 3, 1> Input;

    typedef Eigen::Matrix<double, 3, 3> Matrix;

    typedef fl::LinearGaussianProcessModel<State, Input> ProcessModel;
    typedef fl::LinearObservationModel<Observation, State> ObservationModel;
    // particle filter
    typedef fl::ParticleFilter<ProcessModel, ObservationModel> ParticleFilter;
    typedef ParticleFilter::Belief ParticleBelief;

    // gaussian filter
    typedef fl::GaussianFilter<ProcessModel, ObservationModel> GaussianFilter;
    typedef GaussianFilter::Belief GaussianBelief;

    srand(0);
    size_t N_particles = 10000;
    size_t N_steps = 10;

    // create process model
    ProcessModel transition;
    {
        transition.A(some_rotation());
        Matrix R = some_rotation();
        Matrix D = Eigen::DiagonalMatrix<double, 3>(1, 3.5, 1.2);
        transition.covariance(R*D*R.transpose());
    }

    // create observation model
    /// \todo this is a hack because the GF does not currently work with the new
    /// observation model interface
    ObservationModel sensor;
    {
        sensor.sensor_matrix(some_rotation());
        Matrix R = some_rotation();
        Matrix D = Eigen::DiagonalMatrix<double, 3>(3.1, 1.0, 1.3);
        D = D.cwiseSqrt();
        sensor.noise_matrix(R*D);
    }

    // create filters
    ParticleFilter particle_filter(transition, sensor);
    GaussianFilter gaussian_filter(transition, sensor);

    // create intial beliefs
    GaussianBelief gaussian_belief;
    {
        gaussian_belief.mean(State::Zero());
        gaussian_belief.covariance(Matrix::Identity());
    }
    ParticleBelief particle_belief;
    particle_belief.from_distribution(gaussian_belief, N_particles);

    // run prediction
    for(size_t i = 0; i < N_steps; i++)
    {
        Observation observation(0.5, 0.5, 0.5);

        particle_filter.update(observation, particle_belief, particle_belief);
        gaussian_filter.update(observation, gaussian_belief, gaussian_belief);

        EXPECT_TRUE(moments_are_similar(
                        particle_belief.mean(), particle_belief.covariance(),
                        gaussian_belief.mean(), gaussian_belief.covariance()));
    }

}

TEST(particle_filter, predict_and_update)
{
    typedef Eigen::Matrix<double, 3, 1> State;
    typedef Eigen::Matrix<double, 3, 1> Observation;
    typedef Eigen::Matrix<double, 3, 1> Input;

    typedef Eigen::Matrix<double, 3, 3> Matrix;

    typedef fl::LinearGaussianProcessModel<State, Input> ProcessModel;
    typedef fl::LinearObservationModel<Observation, State> ObservationModel;

    // particle filter
    typedef fl::ParticleFilter<ProcessModel, ObservationModel> ParticleFilter;
    typedef ParticleFilter::Belief ParticleBelief;

    // gaussian filter
    typedef fl::GaussianFilter<ProcessModel, ObservationModel> GaussianFilter;
    typedef GaussianFilter::Belief GaussianBelief;

    srand(0);
    size_t N_particles = 10000;
    size_t N_steps = 10;
    size_t delta_time = 1;

    // create process model
    ProcessModel transition;
    {
        transition.A(some_rotation());
        Matrix R = some_rotation();
        Matrix D = Eigen::DiagonalMatrix<double, 3>(1, 3.5, 1.2);
        transition.covariance(R*D*R.transpose());
    }

    // create observation model
    /// \todo this is a hack because the GF does not currently work with the new
    /// observation model interface
    ObservationModel sensor;
    {
        sensor.sensor_matrix(some_rotation());
        Matrix R = some_rotation();
        Matrix D = Eigen::DiagonalMatrix<double, 3>(3.1, 1.0, 1.3);
        D = D.cwiseSqrt();
        sensor.noise_matrix(R*D);
    }

    // create filters
    ParticleFilter particle_filter(transition, sensor);
    GaussianFilter gaussian_filter(transition, sensor);

    // create intial beliefs
    GaussianBelief gaussian_belief;
    {
        gaussian_belief.mean(State::Zero());
        gaussian_belief.covariance(Matrix::Identity());
    }
    ParticleBelief particle_belief;
    particle_belief.from_distribution(gaussian_belief, N_particles);

    fl::StandardGaussian<State> standard_gaussian;
    State state = gaussian_belief.sample();
    // run prediction
    for(size_t i = 0; i < N_steps; i++)
    {
        // simulate system
        state = transition.predict_state(delta_time,
                                            state,
                                            standard_gaussian.sample(),
                                            State::Zero());
        Observation observation =
                sensor.observation(state, standard_gaussian.sample());

        // predict
        particle_filter.predict(delta_time, State::Zero(),
                                particle_belief, particle_belief);
        gaussian_filter.predict(delta_time, State::Zero(),
                                gaussian_belief, gaussian_belief);

        // update
        particle_filter.update(observation, particle_belief, particle_belief);
        gaussian_filter.update(observation, gaussian_belief, gaussian_belief);
    }

    State delta = particle_belief.mean() - gaussian_belief.mean();
    double mh_distance = delta.transpose() * gaussian_belief.precision() * delta;

    // make sure that the estimate of the pf is within one std dev
    EXPECT_TRUE(std::sqrt(mh_distance) <= 1.0);
}


/*
 * math.cpp
 *
 *  Created on: Apr 30, 2021
 *      Author: jelavice
 */

#include "icp_localization/common/math.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace icp_loco {

Eigen::Quaterniond fromRPY(const double roll, const double pitch, const double yaw)
{

  const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
  return yaw_angle * pitch_angle * roll_angle;
}

Eigen::Vector3d toRPY(const Eigen::Quaterniond &_q)
{
  Eigen::Quaterniond q(_q);
  q.normalize();
  const double r = getRollFromQuat(q.w(), q.x(), q.y(), q.z());
  const double p = getPitchFromQuat(q.w(), q.x(), q.y(), q.z());
  const double y = getYawFromQuat(q.w(), q.x(), q.y(), q.z());
  return Eigen::Vector3d(r,p,y);
}

Eigen::Quaterniond fromRPY(const Eigen::Vector3d &rpy){
	return fromRPY(rpy.x(), rpy.y(), rpy.z());
}

}  // namespace icp_loco


#include <gtest/gtest.h>

#include <Eigen/Eigen>
#include <boost/filesystem.hpp>

#include <CppADCodeGenEigenPy/ADModel.h>
#include <CppADCodeGenEigenPy/CompiledModel.h>
#include <CppADCodeGenEigenPy/Util.h>

#include "testing/models/MathFunctionsTestModel.h"

namespace CppADCodeGenEigenPy {
namespace MathFunctionsModelTest {

class MathFunctionsTestModelFixture : public ::testing::Test {
   protected:
    using Vector = CompiledModel<Scalar>::Vector;
    using Matrix = CompiledModel<Scalar>::Matrix;

    static void SetUpTestSuite() {
        // Compile and load our model
        boost::filesystem::create_directories(DIRECTORY_PATH);
        ad_model_ptr_.reset(new MathFunctionsTestModel<Scalar>());
        ad_model_ptr_->compile(MODEL_NAME, DIRECTORY_PATH,
                               DerivativeOrder::Second);
        compiled_model_ptr_.reset(
            new CompiledModel<Scalar>(MODEL_NAME, LIB_GENERIC_PATH));
    }

    static void TearDownTestSuite() {
        // Delete the compiled shared object.
        boost::filesystem::remove_all(DIRECTORY_PATH);
    }

    static std::unique_ptr<ADModel<Scalar>> ad_model_ptr_;
    static std::unique_ptr<CompiledModel<Scalar>> compiled_model_ptr_;
};

std::unique_ptr<ADModel<Scalar>> MathFunctionsTestModelFixture::ad_model_ptr_ =
    nullptr;
std::unique_ptr<CompiledModel<Scalar>>
    MathFunctionsTestModelFixture::compiled_model_ptr_ = nullptr;

TEST_F(MathFunctionsTestModelFixture, Evaluation) {
    Vector input = Vector::Ones(NUM_INPUT);

    Vector output_expected = evaluate<Scalar>(input);
    Vector output_actual = compiled_model_ptr_->evaluate(input);

    EXPECT_TRUE(output_actual.isApprox(output_expected))
        << "Function evaluation is incorrect.";
}

TEST_F(MathFunctionsTestModelFixture, Jacobian) {
    Vector input = Vector::Ones(NUM_INPUT);

    // clang-format off
    Matrix J_expected(NUM_OUTPUT, NUM_INPUT);
    J_expected << cos(input(0)) * cos(input(1)), -sin(input(0)) * sin(input(1)), 0,
                  0, 0, 0.5 / sqrt(input(2)),
                  2 * input.transpose();
    // clang-format on
    Matrix J_actual = compiled_model_ptr_->jacobian(input);

    EXPECT_TRUE(J_actual.isApprox(J_expected)) << "Jacobian is incorrect.";
}

TEST_F(MathFunctionsTestModelFixture, Hessian) {
    Vector input = Vector::Ones(NUM_INPUT);

    // clang-format off
    Matrix H0_expected(NUM_INPUT, NUM_INPUT);
    H0_expected <<
        -sin(input(0)) * cos(input(1)), -cos(input(0)) * sin(input(1)), 0,
        -cos(input(0)) * sin(input(1)), -sin(input(0)) * cos(input(1)), 0,
        0, 0, 0;
    // clang-format on

    Matrix H1_expected = Matrix::Zero(NUM_INPUT, NUM_INPUT);
    H1_expected(2, 2) = -0.25 * pow(input(2), -1.5);

    Matrix H2_expected = 2 * Matrix::Identity(NUM_INPUT, NUM_INPUT);

    Matrix H0_actual = compiled_model_ptr_->hessian(input, 0);
    Matrix H1_actual = compiled_model_ptr_->hessian(input, 1);
    Matrix H2_actual = compiled_model_ptr_->hessian(input, 2);

    EXPECT_TRUE(H0_actual.isApprox(H0_expected))
        << "Hessian for dim 0 is incorrect.";
    EXPECT_TRUE(H1_actual.isApprox(H1_expected))
        << "Hessian for dim 1 is incorrect.";
    EXPECT_TRUE(H2_actual.isApprox(H2_expected))
        << "Hessian for dim 2 is incorrect.";
}

}  // namespace MathFunctionsModelTest
}  // namespace CppADCodeGenEigenPy


#include <boost/cstdint.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/simd/sdk/memory/is_power_of_2.hpp>

using boost::simd::meta::is_power_of_2_c;

int main()
{
  BOOST_MPL_ASSERT(( is_power_of_2_c<2>::type ));
  BOOST_MPL_ASSERT(( is_power_of_2_c<4>::type ));
  BOOST_MPL_ASSERT(( is_power_of_2_c<8>::type ));
  BOOST_MPL_ASSERT_NOT(( is_power_of_2_c<0>::type ));
  BOOST_MPL_ASSERT_NOT(( is_power_of_2_c<10>::type ));
}


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

// Graph Type with nested interior edge properties for Cost Flow Algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, traits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, long>>>>>
    graph; // new! weightmap corresponds to costs
typedef boost::graph_traits<graph>::edge_descriptor edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator out_edge_it; // Iterator

using namespace std;

// Custom edge adder class
class edge_adder
{
    graph &G;

public:
    explicit edge_adder(graph &G) : G(G) {}
    void add_edge(int from, int to, long capacity, long cost)
    {
        auto c_map = boost::get(boost::edge_capacity, G);
        auto r_map = boost::get(boost::edge_reverse, G);
        auto w_map = boost::get(boost::edge_weight, G); // new!
        const edge_desc e = boost::add_edge(from, to, G).first;
        const edge_desc rev_e = boost::add_edge(to, from, G).first;
        c_map[e] = capacity;
        c_map[rev_e] = 0; // reverse edge has no capacity!
        r_map[e] = rev_e;
        r_map[rev_e] = e;
        w_map[e] = cost;      // new assign cost
        w_map[rev_e] = -cost; // new negative cost
    }
};

void solve() {
    int b, s, p;
    cin >> b >> s >> p;

    graph G(b + s);
    edge_adder adder(G);

    auto source = boost::add_vertex(G);
    auto target = boost::add_vertex(G);

    int maxValue = 50;

    for (int i = 0; i < b; ++i) {
      adder.add_edge(source, i, 1, 0);
      if (b <= s) {
        adder.add_edge(i, target, 1, maxValue);
      }
    }
    for (int j = 0; j < s; ++j) {
      adder.add_edge(b + j, target, 1, 0);
      if (b > s) {
        adder.add_edge(source, b + j, 1, maxValue);
      }
    }
    int bi, si, ci;
    for (int i = 0; i < p; ++i) {
      cin >> bi >> si >> ci;
      adder.add_edge(bi, b + si, 1, maxValue - ci);
    }

    boost::successive_shortest_path_nonnegative_weights(G, source, target);
    auto cost = boost::find_flow_cost(G);
    cost = min(b, s) * maxValue - cost;
    cout << cost << endl;
}

int main() {
    ios_base::sync_with_stdio(false);
    int t; cin >> t;
    for (int i = 0; i < t; ++i) {
        solve();
    }
    return 0;
}


#include <iostream>
#include <fstream>
#include <cmath>
#include <ctime>
#include <vector>
#include <string>

#include <sys/resource.h> /* give extra space to stack `rlimit` */

#include <Eigen/Eigen> /* Sparse Matrices */

#include <omp.h> /* Parallel */

using namespace std;

int get_grid_size(int b, int level, int c) {
    return (c+1) * pow(b, level) + 1;
}

void get_grid_layout(int b, int l, int level, int c, long** grid_layout, int grid_size) {
    // Checks if the parameters given are valid
    if ((l!=0 && b%2 != l%2) || (b==l)) {
        cout << "Invalid Input!" << endl;
    } else {
        // Fill Matrix Initially
        for (int y = 0; y<grid_size; y++) {
            for (int x = 0; x<grid_size; x++) {
                if (x%(c+1) == 0 && y%(c+1) == 0) {
                    grid_layout[y][x] = -1;
                } else {
                    grid_layout[y][x] = -2;
                }
            }
        }

        // Fills in the holes in the grid
        for (int current_level = 1; current_level<=level; current_level++) {
            int current_grid_size = get_grid_size(b, current_level, c);
            int prev_grid_size = get_grid_size(b, current_level-1, c);
            int hole_size = l*(prev_grid_size-1)-1;
            //#pragma omp parallel collapse(4)
            for (int j = (b-l)/2*(prev_grid_size-1)+1; j<grid_size; j+=(current_grid_size-1)) {
                for (int i = (b-l)/2*(prev_grid_size-1)+1; i<grid_size; i+=(current_grid_size-1)) {
                    for (int y = 0; y<hole_size; y++) {
                        for (int x = 0; x<hole_size; x++) {
                            grid_layout[j+y][i+x] = -1;
                        }
                    }
                }
            }
        }
    }
}

void display_grid_layout(long** grid_layout, int grid_size) {
    for (int y = 0; y<grid_size; y++) {
        for (int x = 0; x<grid_size; x++) {
            if (grid_layout[y][x] != -1) {
                cout <<  "\u2588 ";
            } else {
                cout << "  ";
            }

        }
        cout << endl;
    }
}

struct point {
    int y;
    int x;
};

void print_point(point p) {
    cout << "{" << p.y << ", " << p.x << "}";
}

// -3 is the offset (starts -3, -4, -5 and converts to 0, 1, 2, ...)
// Needs to be confirmed
void get_adj_list(long** grid_layout, int grid_size, int crosswires, vector< vector<long>> & adjacency_list, vector<point> & coordinates) {
    long next_available_index = -3;

    // Left Boundary Assignment
    for (int y = 0; y<grid_size; y++) {
        if (grid_layout[y][0] == -2) {
            grid_layout[y][0] = next_available_index;
            coordinates.push_back({y, 0});
            next_available_index--;
        }
    }

    // Right Boundary Assignment
    for (int y = 0; y<grid_size; y++) {
        if (grid_layout[y][grid_size-1] == -2) {
            grid_layout[y][grid_size-1] = next_available_index;
            coordinates.push_back({y, grid_size-1});
            next_available_index--;
        }
    }

    // Beginning of Flood Fill Algorithm
    vector<point> stack;
    stack.push_back({1, 0});

    while (stack.size() > 0) {

        // Fetch Last element in stack
        point p = stack[stack.size()-1];
        stack.pop_back();
        if (grid_layout[p.y][p.x] < 0) {
            // Marks as visited
            grid_layout[p.y][p.x] = -grid_layout[p.y][p.x]-3;
            vector<long> adj_row;

            // Checks Left
            if (p.x > 0 && p.y % (crosswires+1) != 0 && grid_layout[p.y][p.x-1] != -1) {
                if (grid_layout[p.y][p.x-1] >= 0) {
                    adj_row.push_back(grid_layout[p.y][p.x-1]);
                } else {
                    if (grid_layout[p.y][p.x-1] == -2) {
                        grid_layout[p.y][p.x-1] = next_available_index;
                        coordinates.push_back({p.y, p.x-1});
                        next_available_index--;
                    }
                    adj_row.push_back(-grid_layout[p.y][p.x-1]-3);
                    stack.push_back({p.y, p.x-1});
                }
            }

            // Checks Right
            if (p.x < grid_size-1 && p.y % (crosswires+1) != 0 && grid_layout[p.y][p.x+1] != -1) {
                if (grid_layout[p.y][p.x+1] >= 0) {
                    adj_row.push_back(grid_layout[p.y][p.x+1]);
                } else {
                    if (grid_layout[p.y][p.x+1] == -2) {
                        grid_layout[p.y][p.x+1] = next_available_index;
                        coordinates.push_back({p.y, p.x+1});
                        next_available_index--;
                    }
                    adj_row.push_back(-grid_layout[p.y][p.x+1]-3);
                    stack.push_back({p.y, p.x+1});
                }
            }

            // Checks Up
            if (p.y > 0 && p.x % (crosswires+1) != 0 && grid_layout[p.y-1][p.x] != -1) {
                if (grid_layout[p.y-1][p.x] >= 0) {
                    adj_row.push_back(grid_layout[p.y-1][p.x]);
                } else {
                    if (grid_layout[p.y-1][p.x] == -2) {
                        grid_layout[p.y-1][p.x] = next_available_index;
                        coordinates.push_back({p.y-1, p.x});
                        next_available_index--;
                    }
                    adj_row.push_back(-grid_layout[p.y-1][p.x]-3);
                    stack.push_back({p.y-1, p.x});
                }
            }

            // Checks Down
            if (p.y < grid_size-1 && p.x % (crosswires+1) != 0 && grid_layout[p.y+1][p.x] != -1) {
                if (grid_layout[p.y+1][p.x] >= 0) {
                    adj_row.push_back(grid_layout[p.y+1][p.x]);
                } else {
                    if (grid_layout[p.y+1][p.x] == -2) {
                        grid_layout[p.y+1][p.x] = next_available_index;
                        coordinates.push_back({p.y+1, p.x});
                        next_available_index--;
                    }
                    adj_row.push_back(-grid_layout[p.y+1][p.x]-3);
                    stack.push_back({p.y+1, p.x});
                }
            }

            while (adjacency_list.size() <= grid_layout[p.y][p.x]) {
                vector<long> blank_row;
                adjacency_list.push_back(blank_row);
            }
            adjacency_list[grid_layout[p.y][p.x]] = adj_row;
        }
    }
}

void get_laplacian(Eigen::SparseMatrix<short> & laplacian, vector< vector<long>> & adjacency_list) {
    for (int i = 0; i<adjacency_list.size(); i++) {
        vector<long> row = adjacency_list[i];
        for (int j = 0; j<row.size(); j++) {
            laplacian.coeffRef(i, row[j]) = 1;
        }
        laplacian.coeffRef(i, i) = -row.size();
    }
}

void print_laplacian(Eigen::SparseMatrix<short> & laplacian) {
    cout << "Laplacian Matrix: " << endl;
    long num_coordinates = laplacian.cols();
    for (int i = 0; i<num_coordinates; i++) {
        for (int j = 0; j<num_coordinates; j++) {
            cout << laplacian.coeff(i, j) << " ";
        }
        cout << endl;
    }
}

void print_mtrx(Eigen::SparseMatrix<double> & mtrx) {
    cout << "Matrix: " << endl;
    long num_coordinates = mtrx.cols();
    for (int i = 0; i<mtrx.rows(); i++) {
        for (int j = 0; j<mtrx.cols(); j++) {
            cout << mtrx.coeff(i, j) << " ";
        }
        cout << endl;
    }
}

template<typename Derived>
void find_potentials(Eigen::MatrixBase<Derived>& potentials, vector< vector<long>> & adjacency_list) {
    /*struct rlimit lim;
    getrlimit(RLIMIT_STACK, &lim);
    cout << lim.rlim_cur << endl;*/

    long num_coordinates = adjacency_list.size();
    int num_computed_points = potentials.rows();
    int num_boundary_points = num_coordinates-num_computed_points;
    cout << "Total Boundary Points: " << num_boundary_points << endl;
    cout << "Computed Points: " << num_computed_points << endl;

    cout << "1) Initailizing Matrices ..." << endl;
    Eigen::SparseMatrix<double> r(num_computed_points, num_boundary_points);
    Eigen::SparseMatrix<double> a(num_computed_points, num_computed_points);

    cout << "2) Reserving Space ..." << endl;
    r.reserve(Eigen::VectorXi::Constant(num_boundary_points, 5));
    a.reserve(Eigen::VectorXi::Constant(num_computed_points, 5));

    cout << "3) Inserting Elements ..." << endl;
    for (int i = 0; i<adjacency_list.size(); i++) {
        if (i >= num_boundary_points) {
            vector<long> row = adjacency_list[i];

            // Adds elements not on diagonal
            for (int j = 0; j<row.size(); j++) {
                if (row[j] < num_boundary_points) {
                    r.insert(i-num_boundary_points, row[j]) = 1;
                } else {
                    a.insert(i-num_boundary_points, row[j]-num_boundary_points) = 1;
                }
            }

            // Add elements on the diagonal
            if (i < num_boundary_points) {
                r.insert(i-num_boundary_points, i) = -(double)row.size();
            } else {
                a.insert(i-num_boundary_points, i-num_boundary_points) = -(double)row.size();
            }
        }
    }
    //print_mtrx(a);
    //print_mtrx(r);

    cout << "4) Setting Boundary Conditions ..." << endl;
    Eigen::SparseMatrix<double> dirichlet(num_boundary_points, 1);
    for (int i = 0; i<num_boundary_points; i++) {
        if (i < num_boundary_points / 2) {
            dirichlet.coeffRef(i, 0) = 0;
        } else {
            dirichlet.coeffRef(i, 0) = 1;
        }
    }

    cout << "5) Solving ..." << endl;
    Eigen::VectorXd b = -r*dirichlet;
    //Eigen::SparseLU<Eigen::SparseMatrix<double> > solver;
    //solver.analyzePattern(a);
    //solver.factorize(a);
    //potentials = solver.solve(b);

    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::Lower|Eigen::Upper> cg;

    cg.compute(a);
    cg.setMaxIterations(1);
    potentials = cg.solve(b);

    //Eigen::Matrix<double> potentials(2, 1);

    for (int i = 0; i<500; i++) {
        potentials = cg.solveWithGuess(b, potentials);
        cout << 1*(i+1) << endl;
        cout << cg.error() << endl;
    }

    //print_mtrx(potentials);
}

// TO BE CLEANED UP
template<typename Derived>
double max_difference(vector< vector<long>> & adjacency_list, Eigen::MatrixBase<Derived>& potentials) {
    long num_coordinates = adjacency_list.size();
    int num_computed_points = potentials.rows();
    int num_boundary_points = num_coordinates-num_computed_points;

    double max_diff = 0;

    for (int i = 0; i<adjacency_list.size(); i++) {
        // figure out potential
        double potential_a = 1;
        if (i < num_boundary_points/2) {
            potential_a = 0;
        } else if (i >= num_boundary_points) {
            potential_a = potentials.coeff(i-num_boundary_points, 0);
        }

        vector<long> row = adjacency_list[i];
        for (int j = 0; j<row.size(); j++) {
            long index = row[j];

            // figure out potential
            double potential_b = 1;
            if (index < num_boundary_points/2) {
                potential_b = 0;
            } else if (index >= num_boundary_points) {
                potential_b = potentials.coeff(index-num_boundary_points, 0);
            }

            double diff = abs(potential_a-potential_b);

            if (diff-max_diff > 0.0000001) {
                //cout << diff << endl;
                max_diff = diff;
            }
        }
    }

    return max_diff;
}
// THIS SECTION ABOVE

int harmonic_function(int b, int l, int level, int crosswires, string filename) {
    struct rlimit lim;
    getrlimit(RLIMIT_STACK, &lim);
    cout << lim.rlim_cur << endl;

    const rlimit stack_size = {10*1024*1024, 10*1024*1024};
    if (setrlimit(RLIMIT_STACK, &stack_size) == -1) {
        return 1;
    }
    // Parameters
    /*const int b = 3;
    const int l = 1;
    const int level = 1;
    const int crosswires = 1;*/

    // Making Grid Layout
    cout << "Making Grid Layout ..." << endl;
    int grid_size = get_grid_size(b, level, crosswires);
    cout << "Grid Size: " << grid_size << endl;
    long ** grid_layout = new long*[grid_size];
    for (int i = 0; i<grid_size; i++) {
        grid_layout[i] = new long[grid_size];
    }
    get_grid_layout(b, l, level, crosswires, grid_layout, grid_size);

    // View Grid Layout in Terminal
    //display_grid_layout(grid_layout, grid_size);

    // Adjacency List Calculation
    cout << "Making Adjacency List ..." << endl;
    vector< vector<long>> adjacency_list;
    vector<point> coordinates;
    get_adj_list(grid_layout, grid_size, crosswires, adjacency_list, coordinates);

    // FREE GRID LAYOUT
    for(int i =0 ; i<grid_size; i++) {
        delete[] grid_layout[i];
    }
    delete[] grid_layout;

    // Computes Laplacian
    long num_coordinates = adjacency_list.size();
    cout << "num_coordinates: " << num_coordinates << endl;
    //Eigen::SparseMatrix<short> laplacian(num_coordinates, num_coordinates);
    //get_laplacian(laplacian, adjacency_list);

    // Computes Potentials
    cout << "Compute Potentials ..." << endl;
    int num_boundary_points = 2*crosswires*pow(b,level);
    int num_computed_points = num_coordinates-num_boundary_points;
    Eigen::VectorXd potentials(num_computed_points);
    find_potentials(potentials, adjacency_list);

    // To be fixed up
    double max_diff = max_difference(adjacency_list, potentials);
    cout << max_diff << endl;

    // Below is the Printing code (needs to)
    // Output to file
    /*ofstream fout(filename);

    time_t now = time(0);
    string dt = ctime(&now);
    fout << "Produced by \"The Resistance\", " << dt;
    fout << "---------------------------------------------------------" << endl;
    fout << "Parameters: b = " << b << ", l = " << l << ", level = " << level << ", crosswires = " << crosswires << endl;
    fout << "---------------------------------------------------------" << endl;
    fout << "y\tx\tpotential" << endl;
    for (int i = 0; i<num_coordinates; i++) {
        if (i < num_boundary_points) {
            if (i < num_boundary_points/2) {
                fout << coordinates[i].y << "\t" << coordinates[i].x << "\t" << 0 << endl;
            } else {
                fout << coordinates[i].y << "\t" << coordinates[i].x << "\t" << 1 << endl;
            }
        } else {
            fout << coordinates[i].y << "\t" << coordinates[i].x << "\t" << potentials.coeff(i-num_boundary_points, 0) << endl;
        }
    }
    fout.close();*/
    return 0;
}

int main() {
    int b = 3;
    int l = 1;
    int level = 8;
    int crosswires = 1;
    omp_set_num_threads(4);
    /*for (int level = 0; level<8; level++) {
        cout << "----------------------\n";
        string filename = "../data/plus/plusdata_"+to_string(b)+"_"+to_string(l)+"_"+to_string(crosswires)+"_level"+to_string(level)+".dat";
        //cout << filename;
        harmonic_function(b, l, level, crosswires, filename);
    }*/

    harmonic_function(b, l, level, crosswires, "asdf");

}


#include "catch.hpp"

#include "libirc/linalg.h"

#ifdef HAVE_ARMA
#include <armadillo>
using vec3 = arma::vec3;
using vec = arma::vec;
using mat = arma::mat;
#elif HAVE_EIGEN3
#include <Eigen/Dense>
using vec3 = Eigen::Vector3d;
using vec = Eigen::VectorXd;
using mat = Eigen::MatrixXd;
#else
#error
#endif

using namespace irc;

TEST_CASE("Size", "[size]") {
  SECTION("Vector") {
    vec v = {1, 2, 3, 4, 5};

    CHECK(linalg::size(v) == 5);
  }

  SECTION("Matrix") {
    mat m = {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}};

    CHECK(linalg::size(m) == 9);
  }
}

TEST_CASE("Number of rows", "[nrows]") {
  SECTION("Vector") {
    vec v = {1, 2, 3, 4, 5};

    CHECK(linalg::n_rows(v) == 5);
  }

  SECTION("Matrix") {
    mat m = {{1, 2, 3}, {4, 5, 6}};

    CHECK(linalg::n_rows(m) == 2);
  }
}

TEST_CASE("Number of columns", "[ncols]") {
  SECTION("Vector") {
    vec v = {1, 2, 3, 4, 5};

    CHECK(linalg::n_cols(v) == 1);
  }

  SECTION("Matrix") {
    mat m = {{1, 2, 3}, {4, 5, 6}};

    CHECK(linalg::n_cols(m) == 3);
  }
}

TEST_CASE("Norm and normalization", "[norm]") {
  SECTION("Vector") {
    vec v = {1, 2, 3, 4};

    Approx t(std::sqrt(1 * 1 + 2 * 2 + 3 * 3 + 4 * 4));
    CHECK(linalg::norm(v) == t);

    v = linalg::normalize(v);

    CHECK(linalg::norm(v) == Approx(1.0));
  }

  SECTION("Matrix") {
    mat m = {{1, 2, 3}, {4, 5, 6}};

    Approx t(std::sqrt(91));
    CHECK(linalg::norm(m) == t);
  }
}

TEST_CASE("Dot product", "[dot]") {
  vec v1 = {1, 2, 3, 4};
  vec v2 = {4, 3, 2, 1};

  CHECK(linalg::dot(v1, v1) == Approx(std::pow(linalg::norm(v1), 2)));

  CHECK(linalg::dot(v2, v2) == Approx(std::pow(linalg::norm(v2), 2)));

  CHECK(linalg::dot(v1, v2) == Approx(1 * 4 + 2 * 3 + 3 * 2 + 4 * 1));

  CHECK(linalg::dot(v1, v2) == Approx(linalg::dot(v2, v1)));
}

TEST_CASE("Cross product", "[cross]") {
  vec3 v1 = {1, 2, 3};
  vec3 v2 = {3, 2, 1};

  vec3 v = linalg::cross(v1, v2);

  CHECK(v(0) == Approx(-4));
  CHECK(v(1) == Approx(8));
  CHECK(v(2) == Approx(-4));

  v = linalg::cross(v1, v1);

  CHECK(v(0) == Approx(0));
  CHECK(v(1) == Approx(0));
  CHECK(v(2) == Approx(0));

  v = linalg::cross(v2, v2);

  CHECK(v(0) == Approx(0));
  CHECK(v(1) == Approx(0));
  CHECK(v(2) == Approx(0));
}

TEST_CASE("Zeros", "[zeros]") {
  SECTION("Vector") {
    std::size_t n{100};

    vec v = linalg::zeros<vec>(n);

    REQUIRE(linalg::size(v) == n);
    for (std::size_t i{0}; i < n; i++) {
      CHECK(v(i) == Approx(0.));
    }
  }

  SECTION("Matrix") {
    std::size_t n_r{10};
    std::size_t n_c{20};

    std::size_t n{n_r * n_c};

    mat m = linalg::zeros<mat>(n_r, n_c);

    REQUIRE(linalg::size(m) == n);
    for (std::size_t i{0}; i < n; i++) {
      CHECK(m(i) == Approx(0.));
    }
  }
}

TEST_CASE("Ones", "[ones]") {
  SECTION("Matrix") {
    std::size_t n_r{10};
    std::size_t n_c{20};

    std::size_t n{n_r * n_c};

    mat m = linalg::ones<mat>(n_r, n_c);

    REQUIRE(linalg::size(m) == n);
    for (std::size_t i{0}; i < n; i++) {
      CHECK(m(i) == Approx(1.));
    }
  }
}

TEST_CASE("Identity", "[identity]") {
  SECTION("Matrix") {

    std::size_t n{10};

    mat m = linalg::identity<mat>(n);

    REQUIRE(linalg::size(m) == n * n);
    for (std::size_t i{0}; i < n; i++) {
      for (std::size_t j{0}; j < n; j++) {

        double m_ij = m(i, j);

        if (i == j) {
          CHECK(m_ij == Approx(1.));
        } else {
          CHECK(m_ij == Approx(0.));
        }
      }
    }
  }
}

TEST_CASE("Transpose", "[transpose]") {
  SECTION("Matrix") {

    mat m = {{1, 2, 3}, {4, 5, 6}};

    mat mt = linalg::transpose(m);

    std::size_t n_r{linalg::n_rows(m)};
    std::size_t n_c{linalg::n_cols(m)};

    REQUIRE(linalg::size(mt) == linalg::size(m));
    REQUIRE(linalg::n_rows(mt) == n_c);
    REQUIRE(linalg::n_cols(mt) == n_r);

    for (std::size_t i{0}; i < n_r; i++) {
      for (std::size_t j{0}; j < n_c; j++) {
        CHECK(mt(j, i) == Approx(m(i, j)));
      }
    }
  }
}

TEST_CASE("Inverse", "[inv]") {
  SECTION("diagonal") {

    std::size_t n{10};

    mat m = linalg::zeros<mat>(n, n);

    for (std::size_t i{0}; i < n; i++) {
      m(i, i) = i + 1;
    }

    mat inv = linalg::inv(m);

    for (std::size_t i{0}; i < n; i++) {
      for (std::size_t j{0}; j < n; j++) {
        if (i == j) {
          CHECK(inv(i, j) == Approx(1. / m(i, j)));
        } else {
          CHECK(inv(i, j) == Approx(0.));
        }
      }
    }
  }

  SECTION("2x2") {

    double a{1}, b{2}, c{3}, d{4};
    double det{a * d - b * c};

    mat m = {{1, 2}, {3, 4}};

    mat mi = {{d, -b}, {-c, a}};
    mi *= 1. / det;

    mat inv = linalg::inv(m);

    std::size_t n{linalg::size(m)};
    REQUIRE(linalg::size(inv) == n);

    for (std::size_t i{0}; i < n; i++) {
      CHECK(inv(i) == Approx(mi(i)));
    }
  }
}

TEST_CASE("Pseudo Inverse", "[pinv]") {
  mat m = {{1, 2, 3}, {4, 5, 6}};

  std::size_t n_r{linalg::n_rows(m)};
  std::size_t n_c{linalg::n_cols(m)};

  // WolframAlpha
  mat p = {{-17, 8}, {-2, 2}, {13, -4}};
  p /= 18.;

  mat pinv = linalg::pseudo_inverse(m);

  REQUIRE(linalg::n_rows(pinv) == n_c);
  REQUIRE(linalg::n_cols(pinv) == n_r);

  std::size_t n{n_r * n_c};

  for (std::size_t i{0}; i < n; i++) {
    CHECK(pinv(i) == Approx(p(i)));
  }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include "adjointvariategeneratorstest.hpp"

#include <boost/random.hpp>
#include "adjointtestutilities.hpp"

using namespace boost::unit_test_framework;
using namespace QuantLib;

namespace
{
    // Function getProperties(Distribution distribution)
    // Returns estimate of properties of variate generator with specefied Distribution.
    // Parameter: specified distribution.
    // Returns:
    //  properties[0] - Estimate of mean.
    //  properties[1] - Estimate of variance.
    template<typename Distribution>
    std::vector<Real> getProperties(Distribution distribution)
    {
        boost::mt19937 engine(42);
        boost::variate_generator<boost::mt19937&, Distribution> variateGenerator(engine, distribution);

        std::vector<Real> moments(2, 0.0);
        Size sampleSize = 100000;
        for (Size i = 0; i < sampleSize; i++)
        {
            // Variate.
            Real X = variateGenerator();
            // Estimate of first initial moment.
            moments[0] += X;
            // Estimate of second initial moment.
            moments[1] += std::pow(X, 2.0);
        }
        std::vector<Real> properties(2, 0.0);
        // Estimate of mean.
        properties[0] = moments[0] / sampleSize;
        // Estimate of variance.
        properties[1] = (moments[1] - std::pow(moments[0], 2.0) / sampleSize) / (sampleSize - 1);

        return properties;
    }

    // Checking consistency of vectors elements.
    bool checkWithAnalyticalDeriv(
        const std::vector<double>& adjointDeriv,
        const std::vector<Real>& analyticalDeriv,
        Real relativeTol,
        Real absoluteTol)
    {
        bool result = true;
        if (adjointDeriv.size() != analyticalDeriv.size())
        {
            result = false;
            BOOST_ERROR("\nAn adjoint derivatives vector and a analytical "
                        "derivatives vector have different sizes.");
        }
        for (size_t i = 0, n = adjointDeriv.size(); i < n; ++i)
        {
            Real err = std::abs(adjointDeriv[i] - analyticalDeriv[i]);
            Real toler = std::max(absoluteTol,
                                  relativeTol * std::max(std::abs(adjointDeriv[i]), std::abs(analyticalDeriv[i])));
            if (err > toler)
            {
                result = false;
                BOOST_ERROR("\nAdjoint derivative and analytical derivative at position " << i << " mismatch."
                            << "\n  adjoint: " << adjointDeriv[i]
                            << "\n  analytical: " << analyticalDeriv[i]
                            << "\n  tolerance: " << toler);
            }
        }
        return result;
    }
}

// Testing boost variate generator with different distributions with RealType = QuantLib::Real
// Properties of disributions (mean estimate and variance estimate) calculated by sample and then
// differentiated with respect to input value(s).
// Each test method returns true if adjoint derivatives are close enough to analytical derivatives,
// calculated using finite differences method; otherwise, it returns false.

// Method testExponentialVariate()
bool AdjointVariateGeneratorsTest::testExponentialVariate()
{
    BOOST_MESSAGE("Testing boost exponential distribution with Real...");

    std::vector<Real> lambda = { 4.0 };

    // Start of tape recording.
    // Mark \lambda as independent variable.
    cl::Independent(lambda);

    boost::random::exponential_distribution<Real> distribution(lambda[0]);

    std::vector<Real> properties = getProperties(distribution);

    // End of tape recording.
    // Differentiaion will be held with respect to the independent variables vector.
    cl::tape_function<double> f(lambda, properties);

    int propertiesNumber = properties.size();

    // Adjoint differentiation in Forward mode.
    std::vector<double> adjoint_derivatives(propertiesNumber);
    gradForward(f, adjoint_derivatives, false, false);

    // Calculation of derivatives using finite differences method.
    std::vector<Real> analytic_derivatives(properties.size());

    double h = 1e-8;

    lambda[0] += h;
    boost::random::exponential_distribution<Real> distributionfd(lambda[0]);
    std::vector<Real> propertiesfd = getProperties(distributionfd);
    lambda[0] -= h;

    for (int j = 0; j < properties.size(); j++)
    {
        analytic_derivatives[j] = (propertiesfd[j] - properties[j]) / h;
    }

    return checkWithAnalyticalDeriv(adjoint_derivatives, analytic_derivatives, 1e-4, 1e-10);
}

// Method testGammaVariate()
bool AdjointVariateGeneratorsTest::testGammaVariate()
{
    BOOST_MESSAGE("Testing boost gamma distribution with Real...");

    Real k = 10.0;
    Real theta = 3.0;
    std::vector<Real> parameters = { k, theta };

    // Start of tape recording.
    // Mark parameters as independent variables.
    cl::Independent(parameters);

    boost::random::gamma_distribution<Real> distribution(parameters[0], parameters[1]);

    std::vector<Real> properties = getProperties(distribution);

    // End of tape recording.
    // Differentiaion will be held with respect to the independent variables vector.
    cl::tape_function<double> f(parameters, properties);

    // Adjoint differentiation in Reverse mode.
    std::vector<double> adjoint_derivatives(properties.size() * parameters.size());
    gradReverse(f, adjoint_derivatives, false, false);

    // Calculation of derivatives using finite differences method.
    std::vector<Real> analytic_derivatives(properties.size() * parameters.size());

    double h = 1e-8;

    for (int i = 0; i < parameters.size(); i++)
    {
        parameters[i] += h;
        boost::random::gamma_distribution<Real> distributionfd(parameters[0], parameters[1]);
        std::vector<Real> propertiesfd = getProperties(distributionfd);
        parameters[i] -= h;

        for (int j = 0; j < properties.size(); j++)
        {
            analytic_derivatives[2 * j + i] = (propertiesfd[j] - properties[j]) / h;
        }
    }

    return checkWithAnalyticalDeriv(adjoint_derivatives, analytic_derivatives, 1e-3, 1e-10);
}

// Method testNormalVariate()
bool AdjointVariateGeneratorsTest::testNormalVariate()
{
    BOOST_MESSAGE("Testing boost normal distribution with Real...");

    Real mu = 5.0;
    Real sigma = 3.0;
    std::vector<Real> parameters = { mu, sigma };

    // Start of tape recording.
    // Mark parameters as independent variables.
    cl::Independent(parameters);

    boost::random::normal_distribution<Real> distribution(parameters[0], parameters[1]);

    std::vector<Real> properties = getProperties(distribution);

    // End of tape recording.
    // Differentiaion will be held with respect to the independent variables vector.
    cl::tape_function<double> f(parameters, properties);

    // Adjoint differentiation in Reverse mode.
    std::vector<double> adjoint_derivatives(properties.size() * parameters.size());
    gradReverse(f, adjoint_derivatives, false, false);

    // Calculation of derivatives using finite differences method.
    std::vector<Real> analytic_derivatives(properties.size() * parameters.size());

    double h = 1e-8;

    for (int i = 0; i < parameters.size(); i++)
    {
        parameters[i] += h;
        boost::random::normal_distribution<Real> distributionfd(parameters[0], parameters[1]);
        std::vector<Real> propertiesfd = getProperties(distributionfd);
        parameters[i] -= h;

        for (int j = 0; j < properties.size(); j++)
        {
            analytic_derivatives[2 * j + i] = (propertiesfd[j] - properties[j]) / h;
        }
    }

    return checkWithAnalyticalDeriv(adjoint_derivatives, analytic_derivatives, 1e-2, 1e-2);
}

// Method testLogNormalVariate()
bool AdjointVariateGeneratorsTest::testLogNormalVariate()
{
    BOOST_MESSAGE("Testing boost log-normal distribution with Real...");

    Real mu = 5.0;
    Real sigma = 3.0;
    std::vector<Real> parameters = { mu, sigma };

    // Start of tape recording.
    // Mark parameters as independent variables.
    cl::Independent(parameters);

    boost::random::lognormal_distribution<Real> distribution(parameters[0], parameters[1]);

    std::vector<Real> properties = getProperties(distribution);

    // End of tape recording.
    // Differentiaion will be held with respect to the independent variables vector.
    cl::tape_function<double> f(parameters, properties);

    // Adjoint differentiation in Reverse mode.
    std::vector<double> adjoint_derivatives(properties.size() * parameters.size());
    gradReverse(f, adjoint_derivatives, false, false);

    // Calculation of derivatives using finite differences method.
    std::vector<Real> analytic_derivatives(properties.size() * parameters.size());

    double h = 1e-8;

    for (int i = 0; i < parameters.size(); i++)
    {
        parameters[i] += h;
        boost::random::lognormal_distribution<Real> distributionfd(parameters[0], parameters[1]);
        std::vector<Real> propertiesfd = getProperties(distributionfd);
        parameters[i] -= h;

        for (int j = 0; j < properties.size(); j++)
        {
            analytic_derivatives[2 * j + i] = (propertiesfd[j] - properties[j]) / h;
        }
    }

    return checkWithAnalyticalDeriv(adjoint_derivatives, analytic_derivatives, 1e-4, 1e-10);
}

test_suite* AdjointVariateGeneratorsTest::suite()
{
    test_suite* suite = BOOST_TEST_SUITE("AdjointVariateGenerators test");
    suite->add(QUANTLIB_TEST_CASE(&AdjointVariateGeneratorsTest::testExponentialVariate));
    suite->add(QUANTLIB_TEST_CASE(&AdjointVariateGeneratorsTest::testGammaVariate));
    suite->add(QUANTLIB_TEST_CASE(&AdjointVariateGeneratorsTest::testNormalVariate));
    suite->add(QUANTLIB_TEST_CASE(&AdjointVariateGeneratorsTest::testLogNormalVariate));
    return suite;
}

#ifdef CL_ENABLE_BOOST_TEST_ADAPTER

BOOST_AUTO_TEST_SUITE(ad_variate_generator)

BOOST_AUTO_TEST_CASE(testExponentialVariate)
{
    BOOST_CHECK(AdjointVariateGeneratorsTest::testExponentialVariate());
}

BOOST_AUTO_TEST_CASE(testNormalVariate)
{
    BOOST_CHECK(AdjointVariateGeneratorsTest::testNormalVariate());
}

BOOST_AUTO_TEST_CASE(testGammaVariate)
{
    BOOST_CHECK(AdjointVariateGeneratorsTest::testGammaVariate());
}

BOOST_AUTO_TEST_CASE(testLogNormalVariate)
{
    BOOST_CHECK(AdjointVariateGeneratorsTest::testLogNormalVariate());
}

BOOST_AUTO_TEST_SUITE_END()

#endif