//////////////////////////////////////////////////////////////////////////////////////////////
/// \file NaiveSE3Tests.cpp
/// \brief Unit tests for the naive implementation of the SE3 Lie Group math.
/// \details Unit tests for the various Lie Group functions will test both
/// special cases,
///          and randomly generated cases.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

#include <gtest/gtest.h>

#include <math.h>
#include <iomanip>
#include <ios>
#include <iostream>

#include <Eigen/Dense>
#include <lgmath/CommonMath.hpp>
#include <lgmath/se3/Operations.hpp>
#include <lgmath/so3/Operations.hpp>

/////////////////////////////////////////////////////////////////////////////////////////////
///
/// UNIT TESTS OF SE(3) MATH
///
/////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of SE(3) hat function
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, Test4x4HatFunction) {
  // Number of random tests
  const unsigned numTests = 20;

  // Add vectors to be tested - random
  std::vector<Eigen::Matrix<double, 6, 1> > trueVecs;
  for (unsigned i = 0; i < numTests; i++) {
    trueVecs.push_back(Eigen::Matrix<double, 6, 1>::Random());
  }

  // Setup truth matrices
  std::vector<Eigen::Matrix<double, 4, 4> > trueMats;
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 4, 4> mat;
    mat << 0.0, -trueVecs.at(i)[5], trueVecs.at(i)[4], trueVecs.at(i)[0],
        trueVecs.at(i)[5], 0.0, -trueVecs.at(i)[3], trueVecs.at(i)[1],
        -trueVecs.at(i)[4], trueVecs.at(i)[3], 0.0, trueVecs.at(i)[2], 0.0, 0.0,
        0.0, 0.0;
    trueMats.push_back(mat);
  }

  // Test the function
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 4, 4> testMat = lgmath::se3::hat(trueVecs.at(i));
    std::cout << "true: " << trueMats.at(i) << std::endl;
    std::cout << "func: " << testMat << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(trueMats.at(i), testMat, 1e-6));
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of SE(3) curlyhat function
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, TestCurlyHatFunction) {
  // Number of random tests
  const unsigned numTests = 20;

  // Add vectors to be tested - random
  std::vector<Eigen::Matrix<double, 6, 1> > trueVecs;
  for (unsigned i = 0; i < numTests; i++) {
    trueVecs.push_back(Eigen::Matrix<double, 6, 1>::Random());
  }

  // Setup truth matrices
  std::vector<Eigen::Matrix<double, 6, 6> > trueMats;
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 6, 6> mat;
    mat << 0.0, -trueVecs.at(i)[5], trueVecs.at(i)[4], 0.0, -trueVecs.at(i)[2],
        trueVecs.at(i)[1], trueVecs.at(i)[5], 0.0, -trueVecs.at(i)[3],
        trueVecs.at(i)[2], 0.0, -trueVecs.at(i)[0], -trueVecs.at(i)[4],
        trueVecs.at(i)[3], 0.0, -trueVecs.at(i)[1], trueVecs.at(i)[0], 0.0, 0.0,
        0.0, 0.0, 0.0, -trueVecs.at(i)[5], trueVecs.at(i)[4], 0.0, 0.0, 0.0,
        trueVecs.at(i)[5], 0.0, -trueVecs.at(i)[3], 0.0, 0.0, 0.0,
        -trueVecs.at(i)[4], trueVecs.at(i)[3], 0.0;
    trueMats.push_back(mat);
  }

  // Test the function
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 6, 6> testMat = lgmath::se3::curlyhat(trueVecs.at(i));
    std::cout << "true: " << trueMats.at(i) << std::endl;
    std::cout << "func: " << testMat << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(trueMats.at(i), testMat, 1e-6));
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of homogeneous point to 4x6 matrix function
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, TestPointTo4x6MatrixFunction) {
  // Number of random tests
  const unsigned numTests = 20;

  // Add vectors to be tested - random
  std::vector<Eigen::Matrix<double, 4, 1> > trueVecs;
  for (unsigned i = 0; i < numTests; i++) {
    trueVecs.push_back(Eigen::Matrix<double, 4, 1>::Random());
  }

  // Setup truth matrices
  std::vector<Eigen::Matrix<double, 4, 6> > trueMats;
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 4, 6> mat;
    mat << trueVecs.at(i)[3], 0.0, 0.0, 0.0, trueVecs.at(i)[2],
        -trueVecs.at(i)[1], 0.0, trueVecs.at(i)[3], 0.0, -trueVecs.at(i)[2],
        0.0, trueVecs.at(i)[0], 0.0, 0.0, trueVecs.at(i)[3], trueVecs.at(i)[1],
        -trueVecs.at(i)[0], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
    trueMats.push_back(mat);
  }

  // Test the 3x1 function with scaling param
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 4, 6> testMat =
        lgmath::se3::point2fs(trueVecs.at(i).head<3>(), trueVecs.at(i)[3]);
    std::cout << "true: " << trueMats.at(i) << std::endl;
    std::cout << "func: " << testMat << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(trueMats.at(i), testMat, 1e-6));
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of homogeneous point to 6x4 matrix function
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, TestPointTo6x4MatrixFunction) {
  // Number of random tests
  const unsigned numTests = 20;

  // Add vectors to be tested - random
  std::vector<Eigen::Matrix<double, 4, 1> > trueVecs;
  for (unsigned i = 0; i < numTests; i++) {
    trueVecs.push_back(Eigen::Matrix<double, 4, 1>::Random());
  }

  // Setup truth matrices
  std::vector<Eigen::Matrix<double, 6, 4> > trueMats;
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 6, 4> mat;
    mat << 0.0, 0.0, 0.0, trueVecs.at(i)[0], 0.0, 0.0, 0.0, trueVecs.at(i)[1],
        0.0, 0.0, 0.0, trueVecs.at(i)[2], 0.0, trueVecs.at(i)[2],
        -trueVecs.at(i)[1], 0.0, -trueVecs.at(i)[2], 0.0, trueVecs.at(i)[0],
        0.0, trueVecs.at(i)[1], -trueVecs.at(i)[0], 0.0, 0.0;
    trueMats.push_back(mat);
  }

  // Test the 3x1 function with scaling param
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 6, 4> testMat =
        lgmath::se3::point2sf(trueVecs.at(i).head<3>(), trueVecs.at(i)[3]);
    std::cout << "true: " << trueMats.at(i) << std::endl;
    std::cout << "func: " << testMat << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(trueMats.at(i), testMat, 1e-6));
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of exponential functions: vec2tran and tran2vec
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, CompareAnalyticalAndNumericVec2Tran) {
  // Add vectors to be tested
  std::vector<Eigen::Matrix<double, 6, 1> > trueVecs;
  Eigen::Matrix<double, 6, 1> temp;
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI;
  trueVecs.push_back(temp);
  const unsigned numRand = 20;
  for (unsigned i = 0; i < numRand; i++) {
    trueVecs.push_back(Eigen::Matrix<double, 6, 1>::Random());
  }

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Calc matrices
  std::vector<Eigen::Matrix<double, 4, 4> > analyticTrans;
  for (unsigned i = 0; i < numTests; i++) {
    analyticTrans.push_back(lgmath::se3::vec2tran(trueVecs.at(i)));
  }

  // Compare analytical and numeric result
  {
    for (unsigned i = 0; i < numTests; i++) {
      Eigen::Matrix<double, 4, 4> numericTran =
          lgmath::se3::vec2tran(trueVecs.at(i), 20);
      std::cout << "ana: " << analyticTrans.at(i) << std::endl;
      std::cout << "num: " << numericTran << std::endl;
      EXPECT_TRUE(
          lgmath::common::nearEqual(analyticTrans.at(i), numericTran, 1e-6));
    }
  }

  // Test rot2vec
  {
    for (unsigned i = 0; i < numTests; i++) {
      Eigen::Matrix<double, 6, 1> testVec =
          lgmath::se3::tran2vec(analyticTrans.at(i));
      std::cout << "true: " << trueVecs.at(i) << std::endl;
      std::cout << "func: " << testVec << std::endl;
      EXPECT_TRUE(
          lgmath::common::nearEqualLieAlg(trueVecs.at(i), testVec, 1e-6));
    }
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of exponential jacobians: vec2jac and vec2jacinv
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, CompareAnalyticalJacobInvAndNumericCounterpartsInSE3) {
  // Add vectors to be tested
  std::vector<Eigen::Matrix<double, 6, 1> > trueVecs;
  Eigen::Matrix<double, 6, 1> temp;
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI;
  trueVecs.push_back(temp);
  const unsigned numRand = 20;
  for (unsigned i = 0; i < numRand; i++) {
    trueVecs.push_back(Eigen::Matrix<double, 6, 1>::Random());
  }

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Calc analytical matrices
  std::vector<Eigen::Matrix<double, 6, 6> > analyticJacs;
  std::vector<Eigen::Matrix<double, 6, 6> > analyticJacInvs;
  for (unsigned i = 0; i < numTests; i++) {
    analyticJacs.push_back(lgmath::se3::vec2jac(trueVecs.at(i)));
    analyticJacInvs.push_back(lgmath::se3::vec2jacinv(trueVecs.at(i)));
  }

  // Compare inversed analytical and analytical inverse
  for (unsigned i = 0; i < numTests; i++) {
    std::cout << "ana: " << analyticJacs.at(i) << std::endl;
    std::cout << "num: " << analyticJacInvs.at(i) << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(analyticJacs.at(i).inverse(),
                                          analyticJacInvs.at(i), 1e-6));
  }

  // Compare analytical and 'numerical' jacobian
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 6, 6> numericJac =
        lgmath::se3::vec2jac(trueVecs.at(i), 20);
    std::cout << "ana: " << analyticJacs.at(i) << std::endl;
    std::cout << "num: " << numericJac << std::endl;
    EXPECT_TRUE(
        lgmath::common::nearEqual(analyticJacs.at(i), numericJac, 1e-6));
  }

  // Compare analytical and 'numerical' jacobian inverses
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 6, 6> numericJac =
        lgmath::se3::vec2jacinv(trueVecs.at(i), 20);
    std::cout << "ana: " << analyticJacInvs.at(i) << std::endl;
    std::cout << "num: " << numericJac << std::endl;
    EXPECT_TRUE(
        lgmath::common::nearEqual(analyticJacInvs.at(i), numericJac, 1e-6));
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of adjoint tranformation identity, Ad(T(v)) = I +
/// curlyhat(v)*J(v)
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, TestIdentityAdTvEqualIPlusCurlyHatvTimesJv) {
  // Add vectors to be tested
  std::vector<Eigen::Matrix<double, 6, 1> > trueVecs;
  Eigen::Matrix<double, 6, 1> temp;
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI;
  trueVecs.push_back(temp);
  const unsigned numRand = 20;
  for (unsigned i = 0; i < numRand; i++) {
    trueVecs.push_back(Eigen::Matrix<double, 6, 1>::Random());
  }

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Test Identity
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double, 6, 6> lhs =
        lgmath::se3::tranAd(lgmath::se3::vec2tran(trueVecs.at(i)));
    Eigen::Matrix<double, 6, 6> rhs = Eigen::Matrix<double, 6, 6>::Identity() +
                                      lgmath::se3::curlyhat(trueVecs.at(i)) *
                                          lgmath::se3::vec2jac(trueVecs.at(i));
    std::cout << "lhs: " << lhs << std::endl;
    std::cout << "rhs: " << rhs << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(lhs, rhs, 1e-6));
  }
}

int main(int argc, char** argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// 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 "Geomlib_TriMeshMeanCurvatureFlow.h"

#pragma warning(push, 0)
#include <igl/is_vertex_manifold.h>
#include <igl/is_edge_manifold.h>
#include <igl/cotmatrix.h>
#include <igl/massmatrix.h>
#include <igl/doublearea.h>
#include <igl/barycenter.h>
#pragma warning(pop)

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

#include "TriMesh.h"
#include "ConversionUtilities.h"

namespace {

bool IglMeanCurvatureStep(
	const Eigen::MatrixXf& V,
	const Eigen::MatrixXi& F,
	Eigen::MatrixXf& NV,
	Eigen::MatrixXi& NF
)
{
	Eigen::MatrixXf U = V;

	Eigen::SparseMatrix<float> L;
	igl::cotmatrix(V, F, L);

	Eigen::SparseMatrix<float> M;
	igl::massmatrix(U, F, igl::MASSMATRIX_TYPE_BARYCENTRIC, M);

	const auto & S = (M - 0.001f * L);
	Eigen::SimplicialLLT<Eigen::SparseMatrix<float> > solver(S);
	if (solver.info() != Eigen::Success) {
		return false;
	}
	U = solver.solve(M * U).eval();

	// Compute centroid and subtract (also important for numerics)
	Eigen::VectorXf doubleArea;
	igl::doublearea(U, F, doubleArea);
	float area = 0.5f * doubleArea.sum();
	if (!(area > 0.0f)) { // we divide by it later
		return false;
	}

	Eigen::MatrixXf BC;
	igl::barycenter(U, F, doubleArea);
	Eigen::RowVector3f centroid(0, 0, 0);
	for (int i = 0; i < BC.rows(); ++i) {
		centroid += (0.5f * doubleArea(i) / area) * BC.row(i);
	}
	U.rowwise() -= centroid;

	// Normalize to unit surface area (important for numerics)
	if (!(sqrt(area) > 0.0f)) { // do you get 0.0f if you sqrt a really small float?
		return false;
	}
	U.array() /= sqrt(area);

	NV = U;
	NF = F;

	return true;
}

} //

using Urho3D::Variant;

Urho3D::Variant Geomlib::TriMesh_MeanCurvatureFlowStep(
	const Urho3D::Variant& tri_mesh
)
{
	if (!TriMesh_Verify(tri_mesh)) {
		return Variant();
	}

	Eigen::MatrixXf V;
	Eigen::MatrixXi F;
	IglMeshToMatrices(tri_mesh, V, F);

	/*
	if (!igl::is_edge_manifold(V, F)) {
		return Variant();
	}
	Eigen::VectorXi B;
	if (!igl::is_vertex_manifold(F, B)) {
		return Variant();
	}
	*/

	// V, F are ok
	Eigen::MatrixXf NV;
	Eigen::MatrixXi NF;
	bool success = IglMeanCurvatureStep(V, F, NV, NF);
	if (!success) {
		return Variant();
	}

	return TriMesh_Make(NV, NF);
}

bool Geomlib::TriMesh_MeanCurvatureFlow(
	const Urho3D::Variant& tri_mesh,
	int num_steps,
	Urho3D::Variant& tri_mesh_out
)
{
	if (!TriMesh_Verify(tri_mesh)) {
		return false;
	}
	if (num_steps <= 0) {
		return false;
	}

	Eigen::MatrixXf V;
	Eigen::MatrixXi F;
	IglMeshToMatrices(tri_mesh, V, F);

	// V, F are ok here

	for (int i = 0; i < num_steps; ++i) {

		Eigen::MatrixXf VV;
		Eigen::MatrixXi FF;
		bool success = IglMeanCurvatureStep(V, F, VV, FF);
		if (!success) {
			return false;
		}
		V.setZero(VV.rows(), VV.cols());
		V = VV;
		F.setZero(FF.rows(), FF.cols());
		F = FF;
	}

	tri_mesh_out = TriMesh_Make(V, F);
	if (!TriMesh_Verify(tri_mesh_out)) {
		return false;
	}
	return true;
}


/**
 * Copyright (c) 2018, The Akatsuki(Jacob.lsx). All rights reserved.
 */

#include <iostream>

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

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

#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <chrono>

using namespace std;
using namespace cv;

/* find the two photo feature matches points */
void find_feature_matches(const cv::Mat&, const cv::Mat&, std::vector<cv::KeyPoint>&, std::vector<cv::KeyPoint>&, std::vector<cv::DMatch>&);

/* Converts the pixel coordinate system to the normalized imaging plane coordinate system */
cv::Point2d pixel2cam(const cv::Point2d&, const cv::Mat&);

void bundleAdjustment( const std::vector<cv::Point3f>, const std::vector<cv::Point2f>, const cv::Mat&, cv::Mat&, cv::Mat&);

int main(int argc, char** argv)
{
    google::InitGoogleLogging(argv[0]);
    if (argc != 5) {
        cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2." << endl;
        return 1;
    }

    //-- 1st, read photo
    cv::Mat img_1 = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(argv[2], CV_LOAD_IMAGE_COLOR);

    if (img_1.empty() || img_2.empty()) {
        cout << "img1 or img2 is no find." << endl;
        return 1;
    }

    //-- 2nd, find the two photo feature matches points
    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "feature matches points total is " << matches.size() << endl;

    //-- 3rd, create 3 dimensions point correspondences
    cv::Mat d1 = cv::imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);      // 深度图为16位无符号数，单通道图像
    cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    std::vector<cv::Point3f> pts_3d;
    std::vector<cv::Point2f> pts_2d;
    for (cv::DMatch m:matches) {
        ushort d = d1.ptr<unsigned short>(int (keypoints_1[m.queryIdx].pt.y))[int (keypoints_1[m.queryIdx].pt.x)];

        if (d == 0) {   /* bad depth */
            continue;
        }
        float dd = d/5000.0;
        cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        pts_3d.push_back(cv::Point3d (p1.x*dd, p1.y*dd, dd));
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);
    }
    cout << "3d-2d pairs: " << pts_3d.size() << endl;

    //-- 4th, use PnP or bundle adjustment compute camera pose.
    cv::Mat r, t;
    cv::solvePnP(pts_3d, pts_2d, K, cv::Mat(), r, t, false);    // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    cv::Mat R;
    cv::Rodrigues(r, R);    // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout << "R = " << endl << R << endl;
    cout << "t = " << endl << t << endl;

    cout << "\r\ncalling bundle adjustment" << endl;

    bundleAdjustment(pts_3d, pts_2d, K, r, t);

    return 0;
}

/**
 * find the two photo feature matches points
 */
void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2, std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2, std::vector<cv::DMatch>& matches)
{
    //-- Initialize
    cv::Mat descriptors_1, descriptors_2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    //-- 1st: detect Oriented FAST KeyPoint
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 2nd: Calculates BRIEF descriptors with KeyPoint's coordinate
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 3rd: 对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    std::vector<cv::DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 4th: 匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    /* 找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 */
    for (int i = 0; i < descriptors_1.rows; i++) {
        double dist = match[i].distance;
        if (dist < min_dist) {
            min_dist = dist;
        }
        if (dist > max_dist) {
            max_dist = dist;
        }
    }

    printf("-- Max distance : %f \r\n", max_dist);
    printf("-- Min distance : %f \r\n", min_dist);

    // 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_2.rows; i ++) {
        if (match[i].distance <= max(2*min_dist, 30.0)) {
            matches.push_back(match[i]);
        }
    }
}

/**
 * Converts the pixel coordinate system to the normalized imaging plane coordinate system
 */
cv::Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
    return cv::Point2d
        (
            (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
            (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
        );
}

class ReprojectionError
{
public:
    ReprojectionError(const cv::Point2f p_2d, const cv::Point3f p_3d) : p_2d_(p_2d), p_3d_(p_3d) {}

    template<typename T>
    bool operator()(const T* const T_, const T* const point_3d_,  T* residual_) const;

    // Factory to hide the construction of the CostFunction object from the client code.
    static ceres::CostFunction* create(const cv::Point2f &p_2d, const cv::Point3f &p_3d);

private:
    template<typename T>
    static inline void camProjectionWithoutDistortion(const T* const r, const T* const t, const T* const pt_3d, T* predicted);

    cv::Point2f p_2d_;
    cv::Point3f p_3d_;
    static float fx_, fy_, cx_, cy_;
};

float ReprojectionError::fx_ = 520.9;
float ReprojectionError::fy_ = 521.0;
float ReprojectionError::cx_ = 325.1;
float ReprojectionError::cy_ = 249.7;

template<typename T>
bool ReprojectionError::operator()(const T* const r_, const T* const t_,  T* residual_) const
{
    T p[3] = {(T)p_3d_.x, (T)p_3d_.y, (T)p_3d_.z};
    T predicted[2];

    // T_[0, 1, 2] are the angle-axis rotation
    // T_[3, 4, 5] are the translation
    camProjectionWithoutDistortion(r_, t_, p, predicted);

    // The error is the difference between the predicted and observed position
    residual_[0] = predicted[0] - T(p_2d_.x);
    residual_[1] = predicted[1] - T(p_2d_.y);

//     cout << residual_[1] << ", " << residual_[2] << endl;
}

template<typename T>
inline void ReprojectionError::camProjectionWithoutDistortion(const T* const r, const T* const t, const T* const pt_3d, T* predicted)
{
    T p[3];
    ceres::AngleAxisRotatePoint(r, pt_3d, p);

    p[0] += t[0];
    p[1] += t[1];
    p[2] += t[2];

    T px_normalized = p[0] / p[2];
    T py_normalized = p[1] / p[2];

    predicted[0] = (T)fx_ * px_normalized + (T)cx_;
    predicted[1] = (T)fy_ * py_normalized + (T)cy_;
}

ceres::CostFunction* ReprojectionError::create(const cv::Point2f &p_2d, const cv::Point3f &p_3d)
{
    return (new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3, 3>(
                new ReprojectionError(p_2d, p_3d)));
}

void bundleAdjustment(
    const std::vector<cv::Point3f> points_3d,
    const std::vector<cv::Point2f> points_2d,
    const cv::Mat& K,
    cv::Mat& r,
    cv::Mat& t )
{
    double rotation[3];
    double translation[3];
    for (int i = 0; i < 3; i++) {
        rotation[i] = r.at<double>(i, 0);
        translation[i] = t.at<double>(i, 0);
    }

    ceres::Problem problem;
    for (int i = 0; i < points_2d.size(); i ++) {
        ceres::CostFunction* cost_function = ReprojectionError::create(points_2d[i], points_3d[i]);
        problem.AddResidualBlock(cost_function, nullptr, rotation, translation);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    ceres::Solve(options, &problem, &summary);

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optimization costs time: " << time_used.count() << " seconds." << endl;

    cout << summary.BriefReport() << endl;

    cout << endl << "after optimization: " << endl;

    cv::Mat r_vec = (cv::Mat_<double>(3, 1) << rotation[0], rotation[1], rotation[2]);
    cv::Mat R;
    cv::Rodrigues(r_vec, R);

    cout << "R = \r\n" << R << endl;
    cout << "t = \r\n" << translation[0] << ", " << translation[1] << ", " << translation[2] << endl;

//     cout << "T = " << endl << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}


/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

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

#include <mitsuba/core/vmf.h>
#include <mitsuba/core/warp.h>
#include <mitsuba/core/brent.h>
#include <boost/bind.hpp>

MTS_NAMESPACE_BEGIN

Float VonMisesFisherDistr::eval(Float cosTheta) const {
	if (m_kappa == 0.0f)
		return INV_FOURPI;
#if 0
	return math::fastexp(cosTheta * m_kappa)
		* m_kappa / (4 * M_PI * std::sinh(m_kappa));
#else
	/* Numerically stable version */
	return math::fastexp(m_kappa * std::min((Float)0, cosTheta - 1))
		* m_kappa / (2 * M_PI * (1-math::fastexp(-2*m_kappa)));
#endif
}

Vector VonMisesFisherDistr::sample(const Point2 &sample) const {
	if (m_kappa == 0)
		return warp::squareToUniformSphere(sample);

#if 0
	Float cosTheta = math::fastlog(math::fastexp(-m_kappa) + 2 *
						sample.x * std::sinh(m_kappa)) / m_kappa;
#else
	/* Numerically stable version */
	Float cosTheta = 1 + (math::fastlog(sample.x +
		math::fastexp(-2 * m_kappa) * (1 - sample.x))) / m_kappa;
#endif

	Float sinTheta = math::safe_sqrt(1-cosTheta*cosTheta),
	      sinPhi, cosPhi;

	math::sincos(2*M_PI * sample.y, &sinPhi, &cosPhi);

	return Vector(cosPhi * sinTheta,
		sinPhi * sinTheta, cosTheta);
}

Float VonMisesFisherDistr::getMeanCosine() const {
	if (m_kappa == 0)
		return 0;
	Float coth = m_kappa > 6 ? 1 : ((std::exp(2*m_kappa)+1)/(std::exp(2*m_kappa)-1));
	return coth-1/m_kappa;
}

static Float A3(Float kappa) {
	return 1/ std::tanh(kappa) - 1 / kappa;
}

std::string VonMisesFisherDistr::toString() const {
	std::ostringstream oss;
	oss << "VonMisesFisherDistr[kappa=" << m_kappa << "]";
	return oss.str();
}

static Float dA3(Float kappa) {
	Float csch = 2.0f /
		(math::fastexp(kappa)-math::fastexp(-kappa));
	return 1/(kappa*kappa) - csch*csch;
}

static Float A3inv(Float y, Float guess) {
	Float x = guess;
	int it = 1;

	while (true) {
		Float residual = A3(x)-y,
			  deriv = dA3(x);
		x -= residual/deriv;

		if (++it > 20) {
			SLog(EWarn, "VanMisesFisherDistr::convolve(): Newton's method "
				" did not converge!");
			return guess;
		}

		if (std::abs(residual) < 1e-5f)
			break;
	}
	return x;
}

Float VonMisesFisherDistr::convolve(Float kappa1, Float kappa2) {
	return A3inv(A3(kappa1) * A3(kappa2), std::min(kappa1, kappa2));
}

Float VonMisesFisherDistr::forPeakValue(Float x) {
	if (x < INV_FOURPI) {
		return 0.0f;
	} else if (x > 0.795) {
		return 2 * M_PI * x;
	} else {
		return std::max((Float) 0.0f,
			(168.479f * x * x + 16.4585f * x - 2.39942f) /
			(-1.12718f * x * x + 29.1433f * x + 1.0f));
	}
}

static Float meanCosineFunctor(Float kappa, Float g) {
	return VonMisesFisherDistr(kappa).getMeanCosine()-g;
}

Float VonMisesFisherDistr::forMeanLength(Float l) {
	return (3*l - l*l*l) / (1-l*l);
}

Float VonMisesFisherDistr::forMeanCosine(Float g) {
	if (g == 0)
		return 0;
	else if (g < 0)
		SLog(EError, "Error: vMF distribution cannot be created for g<0.");

	BrentSolver brentSolver(100, 1e-6f);
	BrentSolver::Result result = brentSolver.solve(
		boost::bind(&meanCosineFunctor, _1, g), 0, 1000);
	SAssert(result.success);
	return result.x;
}

MTS_NAMESPACE_END


#include <iostream>
#include <fstream>
#include <sstream>
#include <cassert>
//#include <NTL/mat_ZZ_p.h>
#include <NTL/mat_ZZ.h>
//#include <NTL/ZZ_p.h>
#include <NTL/ZZ.h>
//#include <NTL/vec_ZZ_p.h>
#include <NTL/vec_ZZ.h>
#include <cmath>
#include <stack>

using namespace std;
using namespace NTL;

const ZZ w(1ll << 40ll);
ZZ aBound(1000), tBound(aBound), eBound(1000);
int l = 100;

const mat_ZZ hCat(const mat_ZZ& A, const mat_ZZ& B);
const mat_ZZ vCat(const mat_ZZ& A, const mat_ZZ& B);

const vec_ZZ decrypt(const mat_ZZ& S, const vec_ZZ& c);

// returns c*
const vec_ZZ getBitVector(const vec_ZZ& c);

// returns S*
const mat_ZZ getBitMatrix(const mat_ZZ& S);

// returns S
const mat_ZZ getSecretKey(const mat_ZZ& T);

// returns M
const mat_ZZ keySwitchMatrix(const mat_ZZ& S, const mat_ZZ& T);

// finds c* then returns Mc*
const vec_ZZ keySwitch(const mat_ZZ& M, const vec_ZZ& c);

// as described, treating I as the secret key and wx as ciphertext
const vec_ZZ encrypt(const mat_ZZ& T, const vec_ZZ& x);

const mat_ZZ getRandomMatrix(long row, long col, const ZZ& bound);

// server side addition with same secret key
const vec_ZZ addn(const vec_ZZ& c1, const vec_ZZ& c2);

// server side linear transformation,
// returns S(Gx) given c=Sx and M (key switch matrix from GS to S)
const vec_ZZ linearTransform(const mat_ZZ& M, const vec_ZZ& c);

// returns M, the key switch matrix from GS to S,
// to be sent to server
const mat_ZZ linearTransformClient(const mat_ZZ& T, const mat_ZZ& G);

// computes an inner product, given two ciphertexts and the keyswitch matrix
const vec_ZZ innerProd(const vec_ZZ& c1, const vec_ZZ& c2, const mat_ZZ& M);

// returns M, the key switch matrix from vec(S^t S) to S,
// to be sent to the server
const mat_ZZ innerProdClient(const mat_ZZ& T);

// returns a column vector
const mat_ZZ vectorize(const mat_ZZ& M);

const mat_ZZ copyRows(const mat_ZZ& row, long numrows);

// finds c* then returns Mc*
const vec_ZZ keySwitch(const mat_ZZ& M, const vec_ZZ& c) {
	vec_ZZ cstar = getBitVector(c);
	return M * cstar;
}

const mat_ZZ getRandomMatrix(long row, long col, const ZZ& bound){
	mat_ZZ A;
	A.SetDims(row, col);
	for (int i=0; i<row; ++i){
		for (int j=0; j<col; ++j){
			A[i][j] = RandomBnd(bound);
		}
	}
	return A;
}

// returns S*
const mat_ZZ getBitMatrix(const mat_ZZ& S) {
	mat_ZZ result;
	int rows = S.NumRows(), cols = S.NumCols();
	result.SetDims(rows, l * cols);

	vec_ZZ powers;
	powers.SetLength(l);
	powers[0] = 1;
	for(int i = 0; i < l - 1; ++i) {
		powers[i+1] = powers[i]*2;
	}

	for(int i = 0; i < rows; ++i) {
		for(int j = 0; j < cols; ++j) {
			for(int k = 0; k < l; ++k) {
				result[i][j*l + k] = S[i][j] * powers[k];
			}
		}
	}

	return result;
}

// returns c*
const vec_ZZ getBitVector(const vec_ZZ& c) {
	vec_ZZ result;
	int length = c.length();
	result.SetLength(length * l);
	for(int i = 0; i < length; ++i) {
		ZZ sign = (c[i] < ZZ(0)) ? ZZ(-1) : ZZ(1);
		ZZ value = c[i] * sign;
		for(int j = 0; j < l; ++j) {
			result[i * l + j] = sign*bit(value, j);
		}
	}
	return result;
}

// returns S
const mat_ZZ getSecretKey(const mat_ZZ& T) {
	mat_ZZ I;
	ident(I, T.NumRows());
	return hCat(I, T);
}

const mat_ZZ hCat(const mat_ZZ& A, const mat_ZZ& B) {
	assert(A.NumRows() == B.NumRows());

	int rows = A.NumRows(), colsA = A.NumCols(), colsB = B.NumCols();
	mat_ZZ result;
	result.SetDims(rows, colsA + colsB);

	// Copy A
	for(int i = 0; i < rows; ++i) {
		for(int j = 0; j < colsA; ++j) {
			result[i][j] = A[i][j];
		}
	}

	// Copy B
	for(int i = 0; i < rows; ++i) {
		for(int j = 0; j < colsB; ++j) {
			result[i][colsA + j] = B[i][j];
		}
	}

	return result;
}

const mat_ZZ vCat(const mat_ZZ& A, const mat_ZZ& B) {
	assert(A.NumCols() == B.NumCols());

	int cols = A.NumCols(), rowsA = A.NumRows(), rowsB = B.NumRows();
	mat_ZZ result;
	result.SetDims(rowsA + rowsB, cols);

	// Copy A
	for(int i = 0; i < rowsA; ++i) {
		for(int j = 0; j < cols; ++j) {
			result[i][j] = A[i][j];
		}
	}

	// Copy B
	for(int i = 0; i < rowsB; ++i) {
		for(int j = 0; j < cols; ++j) {
			result[i + rowsA][j] = B[i][j];
		}
	}

	return result;
}

inline const ZZ nearestInteger(const ZZ& x, const ZZ& w) {
	return (x + (w+1)/2) / w;
}

const vec_ZZ decrypt(const mat_ZZ& S, const vec_ZZ& c) {
	vec_ZZ Sc = S*c;
	vec_ZZ output;
	output.SetLength(Sc.length());
	for (int i=0; i<Sc.length(); i++) {
		output[i] = nearestInteger(Sc[i], w);
	}
	return output;
}

const mat_ZZ keySwitchMatrix(const mat_ZZ& S, const mat_ZZ& T) {
	mat_ZZ Sstar = getBitMatrix(S);
	mat_ZZ A = getRandomMatrix(T.NumCols(),Sstar.NumCols(),aBound);
	mat_ZZ E = getRandomMatrix(Sstar.NumRows(),Sstar.NumCols(),eBound);
	return vCat(Sstar + E - T*A, A);
}

const vec_ZZ encrypt(const mat_ZZ& T, const vec_ZZ& x) {
	mat_ZZ I;
	ident(I, x.length());
	return keySwitch(keySwitchMatrix(I, T), w * x);
}

const vec_ZZ addVectors(const vec_ZZ& c1, const vec_ZZ& c2){
	return c1 + c2;
}

const vec_ZZ linearTransform(const mat_ZZ& M, const vec_ZZ& c){
	return M * getBitVector(c);
}

const mat_ZZ linearTransformClient(const mat_ZZ& G, const mat_ZZ& S, const mat_ZZ& T){
	return keySwitchMatrix(G * S, T);
}

const vec_ZZ innerProd(const vec_ZZ& c1, const vec_ZZ& c2, const mat_ZZ& M){
	mat_ZZ cc1;
	mat_ZZ cc2;
	mat_ZZ cc;

	cc1.SetDims(c1.length(), 1);
	for (int i=0; i<c1.length(); ++i){
		cc1[i][0] = c1[i];
	}
	cc2.SetDims(1, c2.length());
	for (int i=0; i<c2.length(); ++i){
		cc2[0][i] = c2[i];
	}
	cc = vectorize(cc1 * cc2);

	vec_ZZ output;
	output.SetLength(cc.NumRows());
	for (int i=0; i<cc.NumRows(); i++) {
		output[i] = nearestInteger(cc[i][0], w);
	}
	return M * getBitVector(output);
}

const mat_ZZ innerProdClient(const mat_ZZ& T){
	mat_ZZ S = getSecretKey(T);
	mat_ZZ tvsts = transpose(vectorize(transpose(S) * S));
	mat_ZZ mvsts = copyRows(tvsts, T.NumRows());
	return keySwitchMatrix(mvsts, T);
}

const mat_ZZ copyRows(const mat_ZZ& row, long numrows){
	mat_ZZ ans;
	ans.SetDims(numrows, row.NumCols());
	for (int i=0; i<ans.NumRows(); ++i){
		for (int j=0; j<ans.NumCols(); ++j){
			ans[i][j] = row[0][j];
		}
	}
	return ans;
}

const mat_ZZ vectorize(const mat_ZZ& M){
	mat_ZZ ans;
	ans.SetDims(M.NumRows() * M.NumCols(), 1);
	for (int i=0; i<M.NumRows(); ++i){
		for (int j=0; j<M.NumCols(); ++j){
			ans[i*M.NumCols() + j][0] = M[i][j];
		}
	}
	return ans;
}

int main() {
	ifstream cin("vhe.in");

	cin.tie(NULL);
	ios_base::sync_with_stdio(false);

	stack<vec_ZZ*> vectors;
	stack<mat_ZZ*> matrices;

	string operation;
	while (cin >> operation) {
		//cerr << "Operation: " << operation << endl;

		if (operation == "vector") {
			vec_ZZ* v = new vec_ZZ();
			cin >> (*v);
			//cerr << "Vector (" << v.length() << ")" << endl;
			vectors.push(v);

		} else if (operation == "matrix") {
			mat_ZZ* m = new mat_ZZ();
			cin >> (*m);
			//cerr << "Matrix (" << m.NumRows() << ", " << m.NumCols() << ")" << endl;
			matrices.push(m);

		} else if (operation == "duplicate-vector") {
			vectors.push(vectors.top());

		} else if (operation == "duplicate-matrix") {
			matrices.push(matrices.top());

		} else if (operation == "add") {
			vec_ZZ& v1 = *vectors.top(); vectors.pop();
			vec_ZZ& v2 = *vectors.top(); vectors.pop();
			vec_ZZ* v = new vec_ZZ();
			(*v) = addVectors(v1, v2);
			vectors.push(v);

		} else if (operation == "scalar-multiply") {
			ZZ x;
			cin >> x;
			vec_ZZ& v = *vectors.top(); vectors.pop();
			vec_ZZ* v2 = new vec_ZZ();
			(*v2) = v * x;
			vectors.push(v2);

		} else if (operation == "linear-transform") {
			vec_ZZ& v = *vectors.top(); vectors.pop();
			mat_ZZ& m = *matrices.top(); matrices.pop();
			vec_ZZ* r = new vec_ZZ();
			(*r) = linearTransform(m, v);
			vectors.push(r);

		} else if (operation == "linear-transform-key-switch") {
			mat_ZZ& T = *matrices.top(); matrices.pop();
			mat_ZZ& S = *matrices.top(); matrices.pop();
			mat_ZZ& G = *matrices.top(); matrices.pop();
			mat_ZZ* m = new mat_ZZ();
			(*m) = linearTransformClient(G, S, T);
			matrices.push(m);

		} else if (operation == "inner-product") {
			vec_ZZ& v1 = *vectors.top(); vectors.pop();
			vec_ZZ& v2 = *vectors.top(); vectors.pop();
			mat_ZZ& m = *matrices.top(); matrices.pop();
			vec_ZZ *v = new vec_ZZ();
			(*v) = innerProd(v1, v2, m);
			vectors.push(v);

		} else if (operation == "inner-product-key-switch") {
			mat_ZZ& T = *matrices.top(); matrices.pop();
			mat_ZZ *m = new mat_ZZ();
			(*m) = innerProdClient(T);
			matrices.push(m);

		} else if (operation == "key-switch") {
			vec_ZZ& v = *vectors.top(); vectors.pop();
			mat_ZZ& m = *matrices.top(); matrices.pop();
			vec_ZZ *v2 = new vec_ZZ();
			(*v2) = keySwitch(m, v);
			vectors.push(v2);

		} else if (operation == "random-matrix") {
			int rows, cols;
			cin >> rows >> cols;
			mat_ZZ *m = new mat_ZZ();
			(*m) = getRandomMatrix(rows, cols, tBound);
			matrices.push(m);

		} else if (operation == "identity") {
			int rows;
			cin >> rows;
			mat_ZZ *I = new mat_ZZ();
			ident(*I, rows);
			matrices.push(I);

		} else if (operation == "key-switch-matrix") {
			mat_ZZ& T = *matrices.top(); matrices.pop();
			mat_ZZ& S = *matrices.top(); matrices.pop();
			mat_ZZ *m = new mat_ZZ();
			(*m) = keySwitchMatrix(S, T);
			matrices.push(m);

		} else if (operation == "get-secret-key") {
			mat_ZZ &T = *matrices.top(); matrices.pop();
			mat_ZZ *s = new mat_ZZ();
			(*s) = getSecretKey(T);
			matrices.push(s);

		} else if (operation == "encrypt") {
			mat_ZZ& T = *matrices.top(); matrices.pop();
			vec_ZZ& x = *vectors.top(); vectors.pop();
			vec_ZZ *v = new vec_ZZ();
			(*v) = encrypt(T, x);
			vectors.push(v);

		} else if (operation == "decrypt") {
			mat_ZZ& S = *matrices.top(); matrices.pop();
			vec_ZZ& c = *vectors.top(); vectors.pop();
			vec_ZZ *x = new vec_ZZ();
			(*x) = decrypt(S, c);
			vectors.push(x);

		} else {
			cerr << "Unknown command: " << operation << endl;
		}

	}

	stack<vec_ZZ> vectors2;
	while (vectors.size()) {
		vectors2.push(*vectors.top()); vectors.pop();
	}
	while (vectors2.size()) {
		cout << vectors2.top() << endl; vectors2.pop();
	}

	stack<mat_ZZ> matrices2;
	while (matrices.size()) {
		matrices2.push(*matrices.top()); matrices.pop();
	}
	while (matrices2.size()) {
		cout << matrices2.top() << endl; matrices2.pop();
	}

	// // Testing for the 3 fundamental operations:
	// const int N = 10;
	// vec_ZZ x1;
	// vec_ZZ x2;
	// x1.SetLength(N);
	// x2.SetLength(N);
	// for(int i = 0; i < N; ++i) {
	// 	x1[i] = RandomBnd(10000);
	// 	x2[i] = RandomBnd(10000);
	// }
	// cout << x1 << endl;
	// cout << x2 << endl;
	// mat_ZZ T = getRandomMatrix(N, N, tBound);
	// mat_ZZ S = getSecretKey(T);
	// vec_ZZ c1 = encrypt(T, x1);
	// vec_ZZ c2 = encrypt(T, x2);

	// // Testing for inner product no switch
	// vec_ZZ cc = innerProdNoSwitch(x1, c2);
	// vec_ZZ dxx = innerProdNoSwitchDecrypt(cc, S);
	// ZZ xx;
	// InnerProduct(xx, x1, x2);

	// cout << xx << endl;
	// cout << dxx[0] << endl;
	// cout << xx - dxx[0] << endl;

	// // Testing for inner product
	// mat_ZZ M;
	// vec_ZZ cc;
	// vec_ZZ dxx;
	// ZZ xx;

	// M = innerProdClient(T);
	// cc = innerProd(c1, c2, M);
	// dxx = decrypt(getSecretKey(T), cc);
	// InnerProduct(xx, x1, x2);

	// cout << xx << endl;
	// cout << dxx[0] << endl;
	// cout << xx - dxx[0] << endl;

	// // Testing for linear transform
	// mat_ZZ G;
	// mat_ZZ M;
	// vec_ZZ cc;
	// vec_ZZ dxx;
	// vec_ZZ xx;

	// G = getRandomMatrix(N, N, aBound);
	// M = linearTransformClient(T, G);
	// cc = linearTransform(M, c1);
	// dxx = decrypt(getSecretKey(T), cc);
	// xx = G * x1;

	// cout << G << endl;
	// cout << xx << endl;
	// cout << dxx << endl;
	// cout << xx - dxx << endl;

	// // Testing for addition:
	// vec_ZZ cplus;
	// vec_ZZ dxplus;
	// vec_ZZ xplus;

	// cplus = addn(c1, c2);
	// dxplus = decrypt(getSecretKey(T), cplus);
	// xplus = x1 + x2;

	// cout << xplus << endl;
	// cout << dxplus << endl;
	// cout << xplus - dxplus << endl;

}


#define _USE_MATH_DEFINES

#include <stdlib.h>
#include <time.h>
#include <math.h>

#include <climits>
#include <cfloat>
#include <cmath>

#include <boost/multiprecision/cpp_int.hpp>

#include "externals.h"
#include "typedValue.h"
#include "stack.h"
#include "word.h"
#include "context.h"
#include "mathMacro.h"

const BigInt kBigInt_FLT_MAX(FLT_MAX);
const BigInt kBigInt_Minus_FLT_MAX(-FLT_MAX);
const BigInt kBigInt_DBL_MAX(DBL_MAX);
const BigInt kBigInt_Minus_DBL_MAX(-DBL_MAX);

static double deg2rad(double inTheta) { return inTheta/180.0*M_PI; }
static BigFloat deg2rad(BigFloat inTheta) { return inTheta/180.0*M_PI; }
static double rad2deg(double inTheta) { return inTheta/M_PI*180.0; }
static BigFloat rad2deg(BigFloat inTheta) { return inTheta/M_PI*180.0; }

static int    maxOp(int     inA,int inB)     { return std::max(inA,inB); }
static long   maxOp(long    inA,long inB)    { return std::max(inA,inB); }
static float  maxOp(float   inA,float inB)   { return std::max(inA,inB); }
static double maxOp(double  inA,double inB)  { return std::max(inA,inB); }
static BigInt maxOp(BigInt inA,BigInt inB) {
	return inA>inB ? inA : inB;
}
static BigFloat maxOp(BigFloat inA,BigFloat inB) {
	return inA>inB ? inA : inB;
}

static int    minOp(int     inA,int inB)     { return std::min(inA,inB); }
static long   minOp(long    inA,long inB)    { return std::min(inA,inB); }
static float  minOp(float   inA,float inB)   { return std::min(inA,inB); }
static double minOp(double  inA,double inB)  { return std::min(inA,inB); }
static BigInt minOp(BigInt inA,BigInt inB) {
	return inA<inB ? inA : inB;
}
static BigFloat minOp(BigFloat inA,BigFloat inB) {
	return inA<inB ? inA : inB;
}

void InitDict_Math() {
	Install(new Word("true",WORD_FUNC {
		inContext.DS.emplace_back(true);
		NEXT;
	}));

	Install(new Word("false",WORD_FUNC {
		inContext.DS.emplace_back(false);
		NEXT;
	}));

	Install(new Word("+",WORD_FUNC { TwoOp(+); },LVOP::LVOpSupported2args| LVOP::ADD));
	Install(new Word("-",WORD_FUNC { TwoOp(-); },LVOP::LVOpSupported2args| LVOP::SUB));
	Install(new Word("*",WORD_FUNC { TwoOp(*); },LVOP::LVOpSupported2args| LVOP::MUL));
	Install(new Word("/",WORD_FUNC { TwoOp(/); },LVOP::LVOpSupported2args| LVOP::DIV));
	Install(new Word("%",WORD_FUNC {
		if(inContext.DS.size()<2) {
			return inContext.Error(NoParamErrorID::E_DS_AT_LEAST_2);
		}
		TypedValue tos=Pop(inContext.DS);
		CheckIntegerAndNonZero(tos);
		TypedValue& second=ReadTOS(inContext.DS);
		ModAssign(second,tos); 	// second %= tos;
		NEXT;
	}));

	Install(new Word("2*",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_AT_LEAST_2);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:		tos.intValue	   *= 2;	break;
			case DataType::kTypeLong:		tos.longValue	   *= 2L;	break;
			case DataType::kTypeBigInt:		*(tos.bigIntPtr)   *= 2;	break;
			case DataType::kTypeFloat:		tos.floatValue	   *= 2.0f;	break;
			case DataType::kTypeDouble:		tos.doubleValue	   *= 2.0;	break;
			case DataType::kTypeBigFloat:	*(tos.bigFloatPtr) *= 2;	break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		NEXT;
	},LVOP::LVOpSupported1args | LVOP::TWC));

	Install(new Word("@+",WORD_FUNC { RefTwoOp(inContext.DS,+); }));
	Install(new Word("@-",WORD_FUNC { RefTwoOp(inContext.DS,-); }));
	Install(new Word("@*",WORD_FUNC { RefTwoOp(inContext.DS,*); }));
	Install(new Word("@/",WORD_FUNC { RefTwoOp(inContext.DS,/); }));
	Install(new Word("@%",WORD_FUNC {
		if(inContext.DS.size()<2) {
			return inContext.Error(NoParamErrorID::E_DS_AT_LEAST_2);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		TypedValue& second=ReadSecond(inContext.DS);
		CheckIntegerAndNonZero(tos);
		RefMod(inContext.DS,second,tos);
		NEXT;
	}));

	Install(new Word("+@",WORD_FUNC { TwoOpAssignTOS(+); }));
	Install(new Word("-@",WORD_FUNC { TwoOpAssignTOS(-); }));
	Install(new Word("*@",WORD_FUNC { TwoOpAssignTOS(*); }));
	Install(new Word("/@",WORD_FUNC { TwoOpAssignTOS(/); }));
	Install(new Word("%@",WORD_FUNC {
		if(inContext.DS.size()<2) {
			return inContext.Error(NoParamErrorID::E_DS_AT_LEAST_2);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		TypedValue& second=ReadSecond(inContext.DS);
		CheckIntegerAndNonZero(tos);
		ModAssignTOS(inContext,second,tos);
		NEXT;
	}));

	Install(new Word("&",WORD_FUNC { BitOp(&); }));
	Install(new Word("|",WORD_FUNC { BitOp(|); }));
	Install(new Word("^",WORD_FUNC { BitOp(^); }));
	Install(new Word(">>",WORD_FUNC { BitShiftOp(>>); }));
	Install(new Word("<<",WORD_FUNC { BitShiftOp(<<); }));

	Install(new Word("@&",WORD_FUNC { RefBitOp(&); }));
	Install(new Word("@|",WORD_FUNC { RefBitOp(|); }));
	Install(new Word("@^",WORD_FUNC { RefBitOp(^); }));
	Install(new Word("@>>",WORD_FUNC { RefBitShiftOp(>>); }));
	Install(new Word("@<<",WORD_FUNC { RefBitShiftOp(<<); }));

	// bitwise NOT.
	Install(new Word("~",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_AT_LEAST_2);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt: 	tos.intValue=~tos.intValue;		break;
			case DataType::kTypeLong:	tos.longValue=~tos.longValue;	break;
			case DataType::kTypeBigInt:	*tos.bigIntPtr=~*tos.bigIntPtr;	break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_INT_OR_LONG_OR_BIGINT,tos);
		}
		NEXT;
	}));

	Install(new Word("@~",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_AT_LEAST_2);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:
				inContext.DS.emplace_back(~tos.intValue);
				break;
			case DataType::kTypeLong:
				inContext.DS.emplace_back(~tos.longValue);
				break;
			case DataType::kTypeBigInt:
				inContext.DS.emplace_back(~*tos.bigIntPtr);
				break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_INT_OR_LONG_OR_BIGINT,tos);
		}
		NEXT;
	}));

	Install(new Word(">",WORD_FUNC { CmpOp(>); }));
	Install(new Word("<",WORD_FUNC { CmpOp(<); }));
	Install(new Word(">=",WORD_FUNC { CmpOp(>=); }));
	Install(new Word("<=",WORD_FUNC { CmpOp(<=); }));
	Install(new Word("==",WORD_FUNC { CmpOp(==); }));
	Install(new Word("!=",WORD_FUNC { CmpOp(!=); }));

	Install(new Word("@>",WORD_FUNC { RefCmpOp(>); }));
	Install(new Word("@<",WORD_FUNC { RefCmpOp(<); }));
	Install(new Word("@>=",WORD_FUNC { RefCmpOp(>=); }));
	Install(new Word("@<=",WORD_FUNC { RefCmpOp(<=); }));
	Install(new Word("@==",WORD_FUNC { RefCmpOp(==); }));
	Install(new Word("@!=",WORD_FUNC { RefCmpOp(!=); }));

	Install(new Word("&&",WORD_FUNC { BoolOp(&&); }));
	Install(new Word("||",WORD_FUNC { BoolOp(||); }));
	Install(new Word("xor",WORD_FUNC { BoolOp(!=); }));
	Install(new Word("not",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType!=DataType::kTypeBool) {
			return inContext.Error(InvalidTypeErrorID::E_TOS_BOOL,tos);
		}
		tos.boolValue = tos.boolValue != true;
		NEXT;
	}));
	Install(new Word("not-true?",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue tos=Pop(inContext.DS);
		inContext.DS.emplace_back(!(tos.dataType==DataType::kTypeBool
									&& tos.boolValue==true));
		NEXT;
	}));

	Install(new Word("sqrt",WORD_FUNC { OneArgFloatingFunc(sqrt); }));

	Install(new Word("square",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:
				tos.intValue*=tos.intValue;
				break;
			case DataType::kTypeLong:
				tos.longValue*=tos.longValue;
				break;
			case DataType::kTypeBigInt:
				*(tos.bigIntPtr) *= *(tos.bigIntPtr);
				break;
			case DataType::kTypeFloat:
				tos.floatValue*=tos.floatValue;
				break;
			case DataType::kTypeDouble:
				tos.doubleValue*=tos.doubleValue;
				break;
			case DataType::kTypeBigFloat:
				*(tos.bigFloatPtr) *= *(tos.bigFloatPtr);
				break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word("exp", WORD_FUNC { OneParamFunc(exp);  }));
	Install(new Word("log", WORD_FUNC { OneParamFunc(log);  }));
	Install(new Word("log10", WORD_FUNC { OneParamFunc(log10);  }));

	Install(new Word("sin",WORD_FUNC { OneParamFunc(sin); }));
	Install(new Word("cos",WORD_FUNC { OneParamFunc(cos); }));
	Install(new Word("tan",WORD_FUNC { OneParamFunc(tan); }));

	Install(new Word("asin",WORD_FUNC { OneParamFunc(asin); }));
	Install(new Word("acos",WORD_FUNC { OneParamFunc(acos); }));
	Install(new Word("atan",WORD_FUNC { OneParamFunc(atan); }));

	Install(new Word("abs",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue tos=Pop(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:
				inContext.DS.emplace_back(abs(tos.intValue));
				break;
			case DataType::kTypeLong:
				inContext.DS.emplace_back(abs(tos.longValue));
				break;
			case DataType::kTypeBigInt:
				inContext.DS.emplace_back(abs(*(tos.bigIntPtr)));
				break;
			case DataType::kTypeFloat:
				inContext.DS.emplace_back(abs(tos.floatValue));
				break;
			case DataType::kTypeDouble:
				inContext.DS.emplace_back(abs(tos.doubleValue));
				break;
			case DataType::kTypeBigFloat:
				inContext.DS.emplace_back(abs(*(tos.bigIntPtr)));
				break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word("floor",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		FloorOrCeil(floor,tos);
		NEXT;
	}));

	Install(new Word("ceil",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		FloorOrCeil(ceil,tos);
		NEXT;
	}));

	Install(new Word("deg-to-rad",WORD_FUNC { OneParamFunc(deg2rad); }));
	Install(new Word("rad-to-deg",WORD_FUNC { OneParamFunc(rad2deg); }));

	Install(new Word("pow",WORD_FUNC {
		using namespace boost::multiprecision;

		if(inContext.DS.size()<2) {
			return inContext.Error(NoParamErrorID::E_DS_AT_LEAST_2);
		}

		TypedValue tos=Pop(inContext.DS);
		TypedValue& second=ReadTOS(inContext.DS);
		if(second.dataType==DataType::kTypeDouble) {
			switch(tos.dataType) {
				case DataType::kTypeInt: /* double x int -> double */
					second.doubleValue=pow(second.doubleValue,(double)tos.intValue);
					break;
				case DataType::kTypeLong: /* double x long -> double */
					second.doubleValue=pow(second.doubleValue,(double)tos.longValue);
					break;
				case DataType::kTypeFloat: /* double x float -> double */
					second.doubleValue=pow(second.doubleValue,(double)tos.floatValue);
					break;
				case DataType::kTypeDouble: /* double x double -> dobule */
					second.doubleValue=pow(second.doubleValue,tos.doubleValue);
					break;
				case DataType::kTypeBigInt: { /* double x bigInt -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.doubleValue),
									  BigFloat(*tos.bigIntPtr));
						second.bigFloatPtr=bigFloat;
						second.dataType=DataType::kTypeBigFloat;
					}
					break;
				case DataType::kTypeBigFloat: { /* double x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.doubleValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=DataType::kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==DataType::kTypeBigInt) {
			switch(tos.dataType) {
				case DataType::kTypeInt:	/* bigInt x int -> bigInt */
					*second.bigIntPtr=pow(*second.bigIntPtr,tos.intValue);
					break;
				case DataType::kTypeLong:	/* bigInt x long -> bigInt */
					*second.bigIntPtr=pow(*second.bigIntPtr,tos.longValue);
					break;
				case DataType::kTypeFloat: {	/* bigInt x float -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(*second.bigIntPtr),
									  BigFloat(tos.floatValue));
						delete(second.bigIntPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=DataType::kTypeBigFloat;
					}
					break;
				case DataType::kTypeDouble: {	/* bigInt x double -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(*second.bigIntPtr),
									  BigFloat(tos.doubleValue));
						delete(second.bigIntPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=DataType::kTypeBigFloat;
					}
					break;
				case DataType::kTypeBigInt: /* bigInt x bigInt -> bigInt */
					return inContext.Error(InvalidTypeTosSecondErrorID::E_OUT_OF_SUPPORT_TOS_SECOND,tos,second);
				case DataType::kTypeBigFloat: { /* bigInt x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(*second.bigIntPtr),*tos.bigFloatPtr);
						delete(second.bigIntPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=DataType::kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==DataType::kTypeBigFloat) {
			switch(tos.dataType) {
				case DataType::kTypeInt: /* bigFloat x int -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(tos.intValue));
					break;
				case DataType::kTypeLong: /* bigFloat x long -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											 BigFloat(tos.longValue));
					break;
				case DataType::kTypeFloat: /* bigFloat x float -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(tos.floatValue));
					break;
				case DataType::kTypeDouble: /* bigFloat x double -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(tos.doubleValue));
					break;
				case DataType::kTypeBigInt: /* bigFloat x bigInt -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,
											BigFloat(*tos.bigIntPtr));
					break;
				case DataType::kTypeBigFloat: /* bigFloat x bigFloat -> bigFloat */
					*second.bigFloatPtr=pow(*second.bigFloatPtr,*tos.bigFloatPtr);
					break;
				default: goto onError;
			}
		} else if(second.dataType==DataType::kTypeInt) {
			switch(tos.dataType) {
				case DataType::kTypeInt:	/* int x int -> int */
					second.intValue=(int)pow(second.intValue,tos.intValue);
					break;
				case DataType::kTypeLong:	/* int x long -> long */
					second.longValue=(long)pow((long)second.intValue,tos.longValue);
					second.dataType=DataType::kTypeLong;
					break;
				case DataType::kTypeFloat: /* int x float -> float */
					second.floatValue=(float)pow((float)second.intValue,
												 tos.floatValue);
					second.dataType=DataType::kTypeFloat;
					break;
				case DataType::kTypeDouble: /* int x double -> double */
					second.doubleValue=(double)pow((double)second.intValue,
												   tos.doubleValue);
					second.dataType=DataType::kTypeDouble;
					break;
				case DataType::kTypeBigInt: /* int x bigInt -> OutOfSupport */
					return inContext.Error(InvalidTypeTosSecondErrorID::E_OUT_OF_SUPPORT_TOS_SECOND,tos,second);
				case DataType::kTypeBigFloat: { /* int x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.intValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=DataType::kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==DataType::kTypeLong) {
			switch(tos.dataType) {
				case DataType::kTypeInt:	/* long x int -> long */
					second.longValue=(long)pow(second.longValue,(long)tos.intValue);
					break;
				case DataType::kTypeLong:	/* long x long -> long */
					second.longValue=(long)pow(second.longValue,tos.longValue);
					break;
				case DataType::kTypeFloat:	/* long x float -> float */
					second.floatValue=(float)pow((float)second.longValue,
												 tos.floatValue);
					second.dataType=DataType::kTypeFloat;
					break;
				case DataType::kTypeDouble:	/* long x double -> double */
					second.doubleValue=(double)pow((double)second.longValue,
												   tos.doubleValue);
					second.dataType=DataType::kTypeDouble;
					break;
				case DataType::kTypeBigInt: /* long x bigInt -> OutOfSupport */
					return inContext.Error(InvalidTypeTosSecondErrorID::E_OUT_OF_SUPPORT_TOS_SECOND,tos,second);
				case DataType::kTypeBigFloat: { /* long x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.longValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=DataType::kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else if(second.dataType==DataType::kTypeFloat) {
			switch(tos.dataType) {
				case DataType::kTypeInt: /* float x int -> float */
					second.floatValue=(float)pow((double)second.floatValue,
												 (double)tos.intValue);
					break;
				case DataType::kTypeLong: /* float x long -> float*/
					second.floatValue=(float)pow((double)second.floatValue,
												 (double)tos.longValue);
					break;
				case DataType::kTypeFloat: /* float x float -> float */
					second.floatValue=(float)pow((double)second.floatValue,
												 (double)tos.floatValue);
					break;
				case DataType::kTypeDouble: /* float x double -> dobule */
					second.doubleValue=pow((double)second.floatValue,tos.doubleValue);
					second.dataType=DataType::kTypeDouble;
					break;
				case DataType::kTypeBigInt: { /* float x bigInt -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.floatValue),
									  BigFloat(*tos.bigIntPtr));
						second.bigFloatPtr=bigFloat;
						second.dataType=DataType::kTypeBigFloat;
					}
					break;
				case DataType::kTypeBigFloat: { /* float x bigFloat -> bigFloat */
						BigFloat *bigFloat=new BigFloat();
						*bigFloat=pow(BigFloat(second.floatValue),*tos.bigFloatPtr);
						second.bigFloatPtr=bigFloat;
						second.dataType=DataType::kTypeBigFloat;
					}
					break;
				default: goto onError;
			}
		} else {
onError:
			return inContext.Error(InvalidTypeTosSecondErrorID::E_INVALID_DATA_TYPE_TOS_SECOND,tos,second);
		}
		NEXT;
	}));

	Install(new Word("max",WORD_FUNC {
		if(inContext.DS.size()<2) {
			return inContext.Error(NoParamErrorID::E_DS_AT_LEAST_2);
		}
		TypedValue tos=Pop(inContext.DS);
		TypedValue second=Pop(inContext.DS);
		PushFuncResult(inContext.DS,maxOp,second,tos);
		NEXT;
	}));

	Install(new Word("min",WORD_FUNC {
		if(inContext.DS.size()<2) {
			return inContext.Error(NoParamErrorID::E_DS_AT_LEAST_2);
		}
		TypedValue tos=Pop(inContext.DS);
		TypedValue second=Pop(inContext.DS);
		PushFuncResult(inContext.DS,minOp,second,tos);
		NEXT;
	}));

	Install(new Word("0?",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:
				tos.dataType=DataType::kTypeBool;
				tos.boolValue=tos.intValue==0;
				break;
			case DataType::kTypeLong:
				tos.dataType=DataType::kTypeBool;
				tos.boolValue=tos.longValue==0;
				break;
			case DataType::kTypeBigInt: {
					BigInt *biPtr=tos.bigIntPtr;
					tos.dataType=DataType::kTypeBool;
					tos.boolValue=*biPtr==0;
					delete(biPtr);
				}
				break;
			case DataType::kTypeFloat:
				tos.dataType=DataType::kTypeBool;
				tos.boolValue=tos.floatValue==0;
				break;
			case DataType::kTypeDouble:
				tos.dataType=DataType::kTypeBool;
				tos.boolValue=tos.doubleValue==0;
				break;
			case DataType::kTypeBigFloat: {
					BigFloat *bfPtr=tos.bigFloatPtr;
					tos.dataType=DataType::kTypeBool;
					tos.boolValue=*bfPtr==0;
					delete(bfPtr);
				}
				break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	// equivalent to '1 +'.
	Install(new Word("1+",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:		tos.intValue+=1;		break;
			case DataType::kTypeLong:		tos.longValue+=1;		break;
			case DataType::kTypeFloat:		tos.floatValue+=1;		break;
			case DataType::kTypeDouble:		tos.doubleValue+=1;		break;
			case DataType::kTypeBigInt:		*tos.bigIntPtr+=1;		break;
			case DataType::kTypeBigFloat:	*tos.bigFloatPtr+=1;	break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		NEXT;
	},LVOP::LVOpSupported1args | LVOP::INC));

	// equivalent to '1 -'.
	Install(new Word("1-",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:		tos.intValue-=1;	break;
			case DataType::kTypeLong:		tos.longValue-=1;	break;
			case DataType::kTypeFloat:		tos.floatValue-=1;	break;
			case DataType::kTypeDouble:		tos.doubleValue-=1;	break;
			case DataType::kTypeBigInt:		*tos.bigIntPtr-=1;	break;
			case DataType::kTypeBigFloat:	*tos.bigFloatPtr-=1;break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		NEXT;
	},LVOP::LVOpSupported1args | LVOP::DEC));

	Install(new Word("2/",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}

		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:		tos.intValue/=2;		break;
			case DataType::kTypeLong:		tos.longValue/=2;		break;
			case DataType::kTypeFloat:		tos.floatValue/=2.0f;	break;
			case DataType::kTypeDouble:		tos.doubleValue/=2.0;	break;
			case DataType::kTypeBigInt:		*tos.bigIntPtr/=2;		break;
			case DataType::kTypeBigFloat:	*tos.bigFloatPtr/=2.0;	break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word("even?",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:
				tos.boolValue = (tos.intValue & 0x01)==0;
				tos.dataType=DataType::kTypeBool;
				break;
			case DataType::kTypeLong:
				tos.boolValue = (tos.longValue & 0x01)==0;
				tos.dataType=DataType::kTypeBool;
				break;
				case DataType::kTypeBigInt: {
					bool result=(*tos.bigIntPtr & 0x01)==0;
					delete tos.bigIntPtr;
					tos.dataType=DataType::kTypeBool;
					tos.boolValue=result;
				}
				break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_INT_OR_LONG_OR_BIGINT,tos);
		}
		NEXT;
	}));

	Install(new Word("@even?",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		bool result=false;
		switch(tos.dataType) {
			case DataType::kTypeInt: 		result = (tos.intValue & 0x01)==0; 	break;
			case DataType::kTypeLong:		result = (tos.longValue & 0x01)==0; break;
			case DataType::kTypeBigInt: 	result = (*tos.bigIntPtr & 0x01)==0;break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_INT_OR_LONG_OR_BIGINT,tos);
		}
		inContext.DS.emplace_back(result);
		NEXT;
	}));

	Install(new Word("rand-max",WORD_FUNC {
		inContext.DS.emplace_back(RAND_MAX);
		NEXT;
	}));

	// I ---
	Install(new Word("set-random-seed",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue tos=Pop(inContext.DS);
		if(tos.dataType!=DataType::kTypeInt) {
			return inContext.Error(InvalidTypeErrorID::E_TOS_INT,tos);
		}
		srand((unsigned int)tos.intValue);
		NEXT;
	}));

	// [0,RAND_MAX]
	Install(new Word("rand",WORD_FUNC {
		inContext.DS.emplace_back(rand());
		NEXT;
	}));

	// [0,1]
	Install(new Word("random",WORD_FUNC {
		inContext.DS.emplace_back((float)rand()/(float)RAND_MAX);
		NEXT;
	}));

	Install(new Word("randomize",WORD_FUNC {
		srand((unsigned int)time(NULL));
		NEXT;
	}));

	// n1 --- n2
	// s.t. n2 is an integer where n2 in [0,n1).
	Install(new Word("rand-to",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}

		TypedValue tos=Pop(inContext.DS);
		if(tos.dataType!=DataType::kTypeInt) {
			return inContext.Error(InvalidTypeErrorID::E_TOS_INT,tos);
		}

		const int n=tos.intValue;
		if(n<0 || RAND_MAX<n) {
			return inContext.Error(ErrorIdWithInt::E_TOS_POSITIVE_INT,n);
		}
		int t=(int)(rand()/((float)RAND_MAX+1)*n);
		inContext.DS.emplace_back(t);
		NEXT;
	}));

	Install(new Word("pi",WORD_FUNC {
		inContext.DS.emplace_back(M_PI);
		NEXT;
	}));

	Install(new Word(">int",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}

		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType==DataType::kTypeFloat) {
			if(tos.floatValue<INT_MIN || INT_MAX<tos.floatValue) {
				return inContext.Error(NoParamErrorID::E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=(int)tos.floatValue;
		} else if(tos.dataType==DataType::kTypeDouble) {
			if(tos.doubleValue<INT_MIN || INT_MAX<tos.doubleValue) {
				return inContext.Error(NoParamErrorID::E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=(int)tos.doubleValue;
		} else if(tos.dataType==DataType::kTypeLong) {
			if(tos.longValue<(long)INT_MIN || (long)INT_MAX<tos.longValue) {
				return inContext.Error(NoParamErrorID::E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=(int)tos.longValue;
		} else if(tos.dataType==DataType::kTypeBigInt) {
			if(*tos.bigIntPtr<INT_MIN || INT_MAX<*tos.bigIntPtr) {
				return inContext.Error(NoParamErrorID::E_CAN_NOT_CONVERT_TO_INT_DUE_TO_OVERFLOW);
			}
			tos.intValue=static_cast<int>(*tos.bigIntPtr);
		} else if(tos.dataType==DataType::kTypeAddress) {
			tos.dataType=DataType::kTypeInt;
		} else if(tos.dataType!=DataType::kTypeInt) {
			return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		tos.dataType=DataType::kTypeInt;
		NEXT;
	}));

	Install(new Word(">long",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType==DataType::kTypeFloat) {
			if(tos.floatValue<LONG_MIN || LONG_MAX<tos.floatValue) {
				return inContext.Error(NoParamErrorID::E_CAN_NOT_CONVERT_TO_LONG_DUE_TO_OVERFLOW);
			}
			tos.longValue=(long)tos.floatValue;
		} else if(tos.dataType==DataType::kTypeDouble) {
			if(tos.doubleValue<LONG_MIN || LONG_MAX<tos.doubleValue) {
				return inContext.Error(NoParamErrorID::E_CAN_NOT_CONVERT_TO_LONG_DUE_TO_OVERFLOW);
			}
			tos.longValue=(long)tos.doubleValue;
		} else if(tos.dataType==DataType::kTypeInt) {
			tos.longValue=(long)tos.intValue;
		} else if(tos.dataType==DataType::kTypeBigInt) {
			if(*tos.bigIntPtr<LONG_MIN || LONG_MAX<*tos.bigIntPtr) {
				return inContext.Error(NoParamErrorID::E_CAN_NOT_CONVERT_TO_LONG_DUE_TO_OVERFLOW);
			}
			tos.longValue=static_cast<long>(*tos.bigIntPtr);
		} else if(tos.dataType!=DataType::kTypeLong) {
			return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		tos.dataType=DataType::kTypeLong;
		NEXT;
	}));

	Install(new Word(">INT",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue tos=Pop(inContext.DS);
		BigInt bigInt;
		switch(tos.dataType) {
			case DataType::kTypeInt:
				bigInt=tos.intValue;
				break;
			case DataType::kTypeLong:
				bigInt=tos.longValue;
				break;
			case DataType::kTypeFloat:
				bigInt=static_cast<BigInt>(tos.floatValue);
				break;
			case DataType::kTypeDouble:
				bigInt=static_cast<BigInt>(tos.doubleValue);
				break;
			case DataType::kTypeString:
				bigInt=BigInt(*tos.stringPtr);
				break;
			case DataType::kTypeBigInt:
				// do nothing
				break;
			case DataType::kTypeBigFloat:
				bigInt=static_cast<BigInt>(*tos.bigFloatPtr);
				break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER_OR_STRING,tos);
		}
		if(tos.dataType!=DataType::kTypeBigInt) {
			inContext.DS.emplace_back(bigInt);
		} else {
			inContext.DS.emplace_back(*tos.bigIntPtr);
		}
		NEXT;
	}));

	Install(new Word(">float",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:
				tos.floatValue=(float)tos.intValue;
				tos.dataType=DataType::kTypeFloat;
				break;
			case DataType::kTypeLong:
				tos.floatValue=(float)tos.longValue;
				tos.dataType=DataType::kTypeFloat;
				break;
				case DataType::kTypeBigInt: {
					if(*tos.bigIntPtr>kBigInt_FLT_MAX
					  || *tos.bigIntPtr<kBigInt_Minus_FLT_MAX) {
						return inContext.Error(
								NoParamErrorID::E_CAN_NOT_CONVERT_TO_FLOAT_DUE_TO_OVERFLOW);
					}
					float f=tos.ToFloat(inContext);
					delete tos.bigIntPtr;
					tos.floatValue=f;
					tos.dataType=DataType::kTypeFloat;
				}
				break;
			case DataType::kTypeFloat:
				// do nothing
				break;
			case DataType::kTypeDouble:
				if(tos.doubleValue>FLT_MAX || tos.doubleValue<-FLT_MAX) {
					return inContext.Error(NoParamErrorID::E_CAN_NOT_CONVERT_TO_FLOAT_DUE_TO_OVERFLOW);
				}
				tos.floatValue=(float)tos.doubleValue;
				tos.dataType=DataType::kTypeFloat;
				break;
			case DataType::kTypeBigFloat: {
					if(*tos.bigFloatPtr>FLT_MAX || *tos.bigFloatPtr<-FLT_MAX) {
						return inContext.Error(
								NoParamErrorID::E_CAN_NOT_CONVERT_TO_FLOAT_DUE_TO_OVERFLOW);
					}
					float f=tos.ToFloat(inContext);
					delete tos.bigFloatPtr;
					tos.floatValue=f;
					tos.dataType=DataType::kTypeFloat;
				}
				break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word(">double",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		switch(tos.dataType) {
			case DataType::kTypeInt:
				tos.doubleValue=(double)tos.intValue;
				tos.dataType=DataType::kTypeDouble;
				break;
			case DataType::kTypeLong:
				tos.doubleValue=(double)tos.longValue;
				tos.dataType=DataType::kTypeDouble;
				break;
			case DataType::kTypeBigInt: {
					if(*tos.bigIntPtr>kBigInt_DBL_MAX
					  || *tos.bigIntPtr<kBigInt_Minus_DBL_MAX) {
						return inContext.Error(
								NoParamErrorID::E_CAN_NOT_CONVERT_TO_DOUBLE_DUE_TO_OVERFLOW);
					}
					double t=tos.ToDouble(inContext);
					delete tos.bigIntPtr;
					tos.doubleValue=t;
					tos.dataType=DataType::kTypeDouble;
				}
				break;
			case DataType::kTypeFloat:
				tos.doubleValue=(double)tos.floatValue;
				tos.dataType=DataType::kTypeDouble;
				break;
			case DataType::kTypeDouble:
				// do nothing
				break;
			case DataType::kTypeBigFloat: {
					if(*tos.bigFloatPtr>DBL_MAX || *tos.bigFloatPtr<-DBL_MAX) {
						return inContext.Error(
								NoParamErrorID::E_CAN_NOT_CONVERT_TO_DOUBLE_DUE_TO_OVERFLOW);
					}
					double t=tos.ToDouble(inContext);
					delete tos.bigFloatPtr;
					tos.doubleValue=t;
					tos.dataType=DataType::kTypeDouble;
				}
				break;
			default:
				return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER,tos);
		}
		NEXT;
	}));

	Install(new Word(">FLOAT",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue tos=Pop(inContext.DS);
		BigFloat bigFloat;
		switch(tos.dataType) {
			case DataType::kTypeInt:
				bigFloat=tos.intValue;
				break;
			case DataType::kTypeLong:
				bigFloat=tos.longValue;
				break;
			case DataType::kTypeFloat:
				bigFloat=tos.floatValue;
				break;
			case DataType::kTypeDouble:
				bigFloat=tos.doubleValue;
				break;
			case DataType::kTypeString:
				bigFloat=BigFloat(*tos.stringPtr);
				break;
			case DataType::kTypeBigInt:
				bigFloat=static_cast<BigFloat>(*tos.bigIntPtr);
				break;
			case DataType::kTypeBigFloat:
			   	/* do nothing */
				break;
			default:
	  			return inContext.Error(InvalidTypeErrorID::E_TOS_NUMBER_OR_STRING,tos);
		}
		if(tos.dataType!=DataType::kTypeBigFloat) {
			inContext.DS.emplace_back(bigFloat);
		} else {
			inContext.DS.emplace_back(*tos.bigFloatPtr);
		}
		NEXT;
	}));

	Install(new Word(">address",WORD_FUNC {
		if(inContext.DS.size()<1) {
			return inContext.Error(NoParamErrorID::E_DS_IS_EMPTY);
		}
		TypedValue& tos=ReadTOS(inContext.DS);
		if(tos.dataType!=DataType::kTypeInt && tos.dataType!=DataType::kTypeAddress) {
			return inContext.Error(InvalidTypeErrorID::E_TOS_INT,tos);
		}
		tos.dataType=DataType::kTypeAddress;
		NEXT;
	}));
}


// test computeCost
#include <cassert>
#include <armadillo>
#include "../../ex1/computeCost.hpp"
#include "../../ex1/util.hpp"

using namespace std;
using namespace arma;

int main()
{
    fmat X;
    fvec y, theta;

    parse_mat(&X, "[ 2, 1, 3; 7, 1, 9; 1, 8, 1; 3, 7, 4 ]");
    parse_mat(&y, "[2 ; 5 ; 5 ; 6]");
    parse_mat(&theta, "[0.4 ; 0.6 ; 0.8]");

    auto J = computeCost(X, y, theta);
    cout << "J = " << J << endl;
    assert(J >= 5.2950f && J < 5.29501f);

    return 0;
}


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_PROJECTION_HPP
#define MCL_PROJECTION_HPP 1

#include <Eigen/Core>

namespace mcl
{

// Projection on Triangle
template <typename T>
static inline Eigen::Matrix<T,3,1> point_on_triangle(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &p1, const Eigen::Matrix<T,3,1> &p2, const Eigen::Matrix<T,3,1> &p3);

// Projection on Sphere
template <typename T>
static inline Eigen::Matrix<T,3,1> point_on_sphere(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &center, const T &rad);

// Projection on a Box
template <typename T>
static inline Eigen::Matrix<T,3,1> point_on_box(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &bmin, const Eigen::Matrix<T,3,1> &bmax);

// Project a point on to a plane
template <typename T>
static inline Eigen::Matrix<T,3,1> point_on_plane(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &plane_norm, const Eigen::Matrix<T,3,1> &plane_pt);

// Projection on an edge
template <typename T>
static inline Eigen::Matrix<T,2,1> point_on_edge(const Eigen::Matrix<T,2,1> &point, const Eigen::Matrix<T,2,1> &p1, const Eigen::Matrix<T,2,1> &p2);

// Projection on an edge (3D)
template <typename T>
static inline Eigen::Matrix<T,3,1> point_on_edge(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &p1, const Eigen::Matrix<T,3,1> &p2);

// Computes the shortest vector from p toward q, as well as
// the barycentric coordinates of the closest points.
// The distance between the segments is the norm of this vector.
// Source: https://github.com/evouga/collisiondetection, license: public domain.
template <typename T>
static inline Eigen::Matrix<T,3,1> edge_to_edge(
	const Eigen::Matrix<T,3,1> &p0,
	const Eigen::Matrix<T,3,1> &p1,
	const Eigen::Matrix<T,3,1> &q0,
	const Eigen::Matrix<T,3,1> &q1,
	Eigen::Matrix<T,4,1> &bary);

//
//	Implementation
//

// I do not know the source of the function.
// If anyone knows, please tell me!
template <typename T>
Eigen::Matrix<T,3,1> point_on_triangle(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &p1, const Eigen::Matrix<T,3,1> &p2, const Eigen::Matrix<T,3,1> &p3)
{
	auto clamp_zero_one = [](const T &val){ return val < 0 ? 0 : (val > 1 ? 1 : val); };

	Eigen::Matrix<T,3,1> edge0 = p2 - p1;
	Eigen::Matrix<T,3,1> edge1 = p3 - p1;
	Eigen::Matrix<T,3,1> v0 = p1 - point;

	T a = edge0.dot(edge0);
	T b = edge0.dot(edge1);
	T c = edge1.dot(edge1);
	T d = edge0.dot(v0);
	T e = edge1.dot(v0);
	T det = a*c - b*b;
	T s = b*e - c*d;
	T t = b*d - a*e;

	const T zero(0);
	const T one(1);

	if ( s + t < det ) {
		if ( s < zero ) {
		    if ( t < zero ) {
			if ( d < zero ) {
			    s = clamp_zero_one( -d/a );
			    t = zero;
			}
			else {
			    s = zero;
			    t = clamp_zero_one( -e/c );
			}
		    }
		    else {
			s = zero;
			t = clamp_zero_one( -e/c );
		    }
		}
		else if ( t < zero ) {
		    s = clamp_zero_one( -d/a );
		    t = zero;
		}
		else {
		    T invDet = one / det;
		    s *= invDet;
		    t *= invDet;
		}
	}
	else {
		if ( s < zero ) {
		    T tmp0 = b+d;
		    T tmp1 = c+e;
		    if ( tmp1 > tmp0 ) {
			T numer = tmp1 - tmp0;
			T denom = a-T(2)*b+c;
			s = clamp_zero_one( numer/denom );
			t = one-s;
		    }
		    else {
			t = clamp_zero_one( -e/c );
			s = zero;
		    }
		}
		else if ( t < zero ) {
		    if ( a+d > b+e ) {
			T numer = c+e-b-d;
			T denom = a-T(2)*b+c;
			s = clamp_zero_one( numer/denom );
			t = one-s;
		    }
		    else {
			s = clamp_zero_one( -e/c );
			t = zero;
		    }
		}
		else {
		    T numer = c+e-b-d;
		    T denom = a-T(2)*b+c;
		    s = clamp_zero_one( numer/denom );
		    t = one - s;
		}
	}

	return (p1 + edge0*s + edge1*t);

} // end project triangle

template <typename T>
Eigen::Matrix<T,3,1> point_on_sphere(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &center, const T &rad)
{
	Eigen::Matrix<T,3,1> dir = point-center;
	dir.normalize();
	return (center + dir*rad);
} // end project sphere

template <typename T>
Eigen::Matrix<T,3,1> point_on_box(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &bmin, const Eigen::Matrix<T,3,1> &bmax)
{
	// Loops through axes and moves point to nearest surface
	Eigen::Matrix<T,3,1> x = point;
	T dx = std::numeric_limits<T>::max();
	for (int i=0; i<3; ++i)
	{
		T dx_max = std::abs(bmax[i]-point[i]);
		T dx_min = std::abs(bmin[i]-point[i]);
		if(dx_max < dx)
		{
			x = point;
			x[i] = bmax[i];
			dx = dx_max;
		}
		if (dx_min < dx)
		{
			x = point;
			x[i] = bmin[i];
			dx = dx_min;
		}
	}
	return x;
} // end project box

template <typename T>
Eigen::Matrix<T,3,1> point_on_plane(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &plane_norm, const Eigen::Matrix<T,3,1> &plane_pt)
{
    T d = -1 * plane_norm.dot(point-plane_pt);
    Eigen::Matrix<T,3,1> t_vec = plane_norm * d;
    return point + t_vec;
}

template <typename T>
static Eigen::Matrix<T,2,1> point_on_edge(const Eigen::Matrix<T,2,1> &p, const Eigen::Matrix<T,2,1> &e0, const Eigen::Matrix<T,2,1> &e1)
{
	Eigen::Matrix<T,2,1> e = (e1-e0);
	T e_len2 = e.dot(e);
	if(e_len2 <= 0.0) { return e0; } // zero length edge
	Eigen::Matrix<T,2,1> pe0 = (p-e0);
	T t = pe0.dot(e)/e_len2;
	if (t < 0.0){ return e0; }
	else if (t > 1.0) { return e1; }
	return e0 + t * e;
}

// Ericson, Real-Time Collision Detection
template <typename T>
static Eigen::Matrix<T,3,1> point_on_edge(const Eigen::Matrix<T,3,1> &point, const Eigen::Matrix<T,3,1> &p1, const Eigen::Matrix<T,3,1> &p2)
{
	Eigen::Matrix<T,3,1> ab = p2-p1;
	T t = ab.dot(point-p1);
	Eigen::Matrix<T,3,1> d = point; // result

	// c projects outside the [a,b] interval, on the a side; clamp to a
	if (t <= 0)
	{
		t = 0;
		d = p1;
	}
	else
	{
		T denom = ab.dot(ab); // Always nonnegative since denom = ||ab|| ∧ 2
		// c projects outside the [a,b] interval, on the b side; clamp to b
		if (t >= denom)
		{
			t = 1;
			d = p2;
		}
		// c projects inside the [a,b] interval; must do deferred divide now
		else
		{
			t = t/denom;
			d = p1+t*ab;
		}
	}
	return d;
}

template <typename T>
static Eigen::Matrix<T,3,1> edge_to_edge(
    const Eigen::Matrix<T,3,1> &p0,
    const Eigen::Matrix<T,3,1> &p1,
    const Eigen::Matrix<T,3,1> &q0,
    const Eigen::Matrix<T,3,1> &q1,
    Eigen::Matrix<T,4,1> &bary)
{
	typedef Eigen::Matrix<T,3,1> eteVec3;
	auto clamp01 = [](T x)
	{
		return std::min(T(1), std::max(x, T(0)));
	};

	eteVec3 d1 = p1-p0;
	eteVec3 d2 = q1-q0;
	eteVec3 r = p0-q0;
	T a = d1.squaredNorm();
	T e = d2.squaredNorm();
	T f = d2.dot(r);
	T s = 0;
	T t = 0;
	T c = d1.dot(r);
	T b = d1.dot(d2);
	T denom = a*e-b*b;
	if (denom != T(0)) {
		s = clamp01((b*f-c*e)/denom);
	}
	else { // Parallel/degenerate edges
		s = 0;
	}
	T tnom = b*s + f;
	if(tnom < 0 || e == 0)
	{
		t = 0;
		if(a == 0) {
			s = 0;
		} else {
			s = clamp01(-c/a);
		}
	}
	else if(tnom > e)
	{
		t = 1.0;
		if(a == 0) {
			s = 0;
		} else {
			s = clamp01((b-c)/a);
		}
	}
	else {
		t = tnom/e;
	}

	eteVec3 c1 = p0 + s*d1;
	eteVec3 c2 = q0 + t*d2;
	bary[0] = 1-s;
	bary[1] = s;
	bary[2] = 1-t;
	bary[3] = t;
	return c2-c1;
}

} // end namespace mcl

#endif


#ifndef EXPSUM_KERNEL_FUNCTIONS_SPH_BESSEL_KERNEL_HPP
#define EXPSUM_KERNEL_FUNCTIONS_SPH_BESSEL_KERNEL_HPP

#include <armadillo>

#include "expsum/constants.hpp"
#include "expsum/kernel_functions/gamma.hpp"

namespace expsum
{

template <typename T>
struct sph_bessel_kernel
{
public:
    using size_type    = arma::uword;
    using real_type    = T;
    using complex_type = std::complex<T>;

    using vector_type = arma::Col<complex_type>;
    using matrix_type = arma::Mat<complex_type>;

private:
    vector_type exponent_;
    vector_type weight_;

public:
    void compute(size_type n, real_type band_limit, real_type eps);

    size_type size() const
    {
        return exponent_.size();
    }

    const vector_type& exponents() const
    {
        return exponent_;
    }

    const vector_type& weights() const
    {
        return weight_;
    }
};

template <typename T>
void sph_bessel_kernel<T>::compute(size_type n, real_type band_limit,
                                   real_type eps)
{
    constexpr const real_type one  = real_type(1);
    constexpr const real_type zero = real_type();
    // i^{n % 4}
    constexpr const complex_type phase_p[4] = {
        complex_type(one, zero), complex_type(zero, one),
        complex_type(-one, zero), complex_type(zero, -one)};
    // (-i)^{n % 4}
    constexpr const complex_type phase_m[4] = {
        complex_type(one, zero), complex_type(zero, -one),
        complex_type(-one, zero), complex_type(zero, one)};

    // k mod 4 = k & mask_mod4
    constexpr const size_type mask_mod4 = 3; // 0b11

    // static const auto huge = std::sqrt(std::numeric_limits<T>::max());
    // static const auto huge = std::numeric_limits<T>::max() / 10;

    const auto delta = 1 / band_limit;

    real_type coeff = real_type(1);
    real_type t_lower, t_upper, h = real_type(1);

    for (size_type k = 0; k <= n; ++k)
    {
        const auto beta    = static_cast<real_type>(k + 1);
        const auto log_eps = std::log(eps / coeff);
        // Lower bound
        t_lower = std::min((log_eps + std::lgamma(beta + 1)) / beta, t_lower);
        t_upper = std::max(t_upper, -std::log(delta) + std::log(-log_eps) +
                                        std::log(beta) + real_type(0.5));
        h = std::min(h, 2 * constant<T>::pi /
                            (std::log(T(3)) - beta * std::log(std::cos(T(1))) -
                             log_eps));
        std::cout << "  (" << n << ',' << k << "):\n"
                  << "    t_lower = " << t_lower << '\n'
                  << "    t_upper = " << t_upper << '\n'
                  << "    a(n, k) = " << coeff << '\n'
                  << "    h       = " << h << std::endl;
        coeff *= static_cast<real_type>((n - k) * (n + k + 1)) / (2 * k + 2);
    }

    const auto N = static_cast<size_type>(std::floor((t_upper - t_lower) / h));

    vector_type a(2 * N);
    vector_type w(2 * N);

    for (size_type i = 0; i < N; ++i)
    {
        const auto ai = std::exp(t_lower + i * h);
        a(2 * i + 0)  = complex_type(ai, real_type(-1));
        a(2 * i + 1)  = complex_type(ai, real_type(1));
    }
    // a(2 * N) = huge;

    for (size_type i = 0; i < N; ++i)
    {
        const auto ti = t_lower + i * h;
        coeff         = real_type(0.5);
        auto w0       = complex_type();
        auto w1       = complex_type();
        for (size_type k = 0; k <= n; ++k)
        {
            auto base_w = coeff * h * std::exp((k + 1) * ti);
            w0 += base_w * phase_m[(n - k + 1) & mask_mod4];
            w1 += base_w * phase_p[(n - k + 1) & mask_mod4];
            coeff *= static_cast<real_type>((n - k) * (n + k + 1)) /
                     (2 * (k + 1) * (k + 1));
        }
        w(2 * i + 0) = w0;
        w(2 * i + 1) = w1;
    }
    // w(2 * N) = -arma::sum(w.head(2 * N)) + (n == 0 ? one : zero);

    exponent_.swap(a);
    weight_.swap(w);
    return;
}

} // namespace: expsum

#endif /* EXPSUM_KERNEL_FUNCTIONS_SPH_BESSEL_KERNEL_HPP */


//
// Copyright 2019 Mateusz Loskot <mateusz at loskot dot net>
//
// 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
//
#define BOOST_TEST_MODULE gil/test/extension/numeric/channel_numeric_operations
#include "unit_test.hpp"

#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/channel_numeric_operations.hpp>

#include <tuple>
#include <type_traits>

#include "core/channel/test_fixture.hpp"

namespace gil = boost::gil;
namespace fixture = boost::gil::test::fixture;

BOOST_AUTO_TEST_SUITE(channel_plus_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(plus_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_plus_t<channel_t, channel_t, channel_t> f;
    BOOST_TEST(f(0, 0) == channel_t(0));
    BOOST_TEST(f(100, 27) == channel_t(127));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(plus_integer_mixed_types, channel_t, fixture::channel_integer_types)
{
    {
        using channel1_t = channel_t;
        using channel2_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
        gil::channel_plus_t<channel1_t, channel2_t, channel1_t> f;
        BOOST_TEST(f(0, 0) == channel1_t(0));
        BOOST_TEST(f(100, 27) == channel_t(127));
    }
    {
        using channel1_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
        using channel2_t = channel_t;
        gil::channel_plus_t<channel1_t, channel2_t, channel2_t> f;
        BOOST_TEST(f(0, 0) == channel2_t(0));
        BOOST_TEST(f(100, 27) == channel_t(127));
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(plus_integer_signed_types_with_overflow, channel_t, fixture::channel_integer_signed_types)
{
    // Signed integer overflow is UB, so just check addition does not yield mathematically
    // expected value but is constrained by the range of representable values for given type.

    auto const max_value = gil::channel_traits<channel_t>::max_value();
    gil::channel_plus_t<channel_t, channel_t, channel_t> f;
    BOOST_TEST(f(max_value, 1) != std::int64_t(max_value) + 1);
    BOOST_TEST(f(max_value, max_value) != std::int64_t(max_value) + max_value);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(plus_integer_unsigned_types_with_wraparound, channel_t, fixture::channel_integer_unsigned_types)
{
    // The C Standard, 6.2.5, paragraph 9 [ISO/IEC 9899:2011], states:
    // A computation involving unsigned operands can never overflow, because a result that
    // cannot be represented by the resulting unsigned integer type is reduced modulo the number
    // that is one greater than the largest value that can be represented by the resulting type.

    auto const max_value = gil::channel_traits<channel_t>::max_value();
    auto const min_value = gil::channel_traits<channel_t>::min_value();
    gil::channel_plus_t<channel_t, channel_t, channel_t> f;
    BOOST_TEST(f(max_value, 1) == min_value);
    BOOST_TEST(f(max_value, max_value) == max_value - 1);
}

BOOST_AUTO_TEST_SUITE_END() // channel_plus_t

BOOST_AUTO_TEST_SUITE(channel_minus_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(minus_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_minus_t<channel_t, channel_t, channel_t> f;
    BOOST_TEST(f(0, 0) == channel_t(0));
    BOOST_TEST(f(100, 27) == channel_t(73));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(minus_integer_mixed_types, channel_t, fixture::channel_integer_types)
{
    {
        using channel1_t = channel_t;
        using channel2_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
        gil::channel_minus_t<channel1_t, channel2_t, channel1_t> f;
        BOOST_TEST(f(0, 0) == channel1_t(0));
        BOOST_TEST(f(100, 27) == channel_t(73));
    }
    {
        using channel1_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
        using channel2_t = channel_t;
        gil::channel_minus_t<channel1_t, channel2_t, channel2_t> f;
        BOOST_TEST(f(0, 0) == channel2_t(0));
        BOOST_TEST(f(100, 27) == channel_t(73));
    }
}

BOOST_AUTO_TEST_SUITE_END() // channel_minus_t

BOOST_AUTO_TEST_SUITE(channel_multiplies_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(multiplies_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_multiplies_t<channel_t, channel_t, channel_t> f;
    BOOST_TEST(f(0, 0) == channel_t(0));
    BOOST_TEST(f(1, 1) == channel_t(1));
    BOOST_TEST(f(4, 2) == channel_t(8));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(multiplies_integer_mixed_types, channel_t, fixture::channel_integer_types)
{
    {
        using channel1_t = channel_t;
        using channel2_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
        gil::channel_multiplies_t<channel1_t, channel2_t, channel1_t> f;
        BOOST_TEST(f(0, 0) == channel1_t(0));
        BOOST_TEST(f(4, 2) == channel_t(8));
    }
    {
        using channel1_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
        using channel2_t = channel_t;
        gil::channel_multiplies_t<channel1_t, channel2_t, channel2_t> f;
        BOOST_TEST(f(0, 0) == channel2_t(0));
        BOOST_TEST(f(4, 2) == channel_t(8));
    }
}

BOOST_AUTO_TEST_SUITE_END() // channel_multiplies_t

BOOST_AUTO_TEST_SUITE(channel_divides_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(divides_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_divides_t<channel_t, channel_t, channel_t> f;
    BOOST_TEST(f(0, 1) == channel_t(0));
    BOOST_TEST(f(1, 1) == channel_t(1));
    BOOST_TEST(f(4, 2) == channel_t(2));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(divides_integer_mixed_types, channel_t, fixture::channel_integer_types)
{
    {
        using channel1_t = channel_t;
        using channel2_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
        gil::channel_divides_t<channel1_t, channel2_t, channel1_t> f;
        BOOST_TEST(f(0, 1) == channel1_t(0));
        BOOST_TEST(f(4, 2) == channel_t(2));
    }
    {
        using channel1_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
        using channel2_t = channel_t;
        gil::channel_divides_t<channel1_t, channel2_t, channel2_t> f;
        BOOST_TEST(f(0, 1) == channel2_t(0));
        BOOST_TEST(f(4, 2) == channel_t(2));
    }
}

BOOST_AUTO_TEST_SUITE_END() // channel_divides_t

BOOST_AUTO_TEST_SUITE(channel_plus_scalar_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(plus_scalar_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_plus_scalar_t<channel_t, int, channel_t> f;
    BOOST_TEST(f(0, 0) == channel_t(0));
    BOOST_TEST(f(100, 27) == channel_t(127));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(plus_scalar_integer_mixed_types, channel_t, fixture::channel_integer_types)
{
    using channel_result_t = std::uint8_t;
    gil::channel_plus_scalar_t<channel_t, int, channel_result_t> f;
    BOOST_TEST(f(0, 0) == channel_result_t(0));
    BOOST_TEST(f(100, 27) == channel_result_t(127));
}

BOOST_AUTO_TEST_SUITE_END() // channel_plus_scalar_t

BOOST_AUTO_TEST_SUITE(channel_minus_scalar_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(minus_scalar_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_minus_scalar_t<channel_t, int, channel_t> f;
    BOOST_TEST(f(0, 0) == channel_t(0));
    BOOST_TEST(f(100, 27) == channel_t(73));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(minus_scalar_integer_mixed_types, channel_t, fixture::channel_integer_types)
{
    using channel_result_t = std::uint8_t;
    gil::channel_minus_scalar_t<channel_t, int, std::uint8_t> f;
    BOOST_TEST(f(0, 0) == channel_result_t(0));
    BOOST_TEST(f(100, 27) == channel_result_t(73));
}

BOOST_AUTO_TEST_SUITE_END() // channel_minus_scalar_t

BOOST_AUTO_TEST_SUITE(channel_multiplies_scalar_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(multiplies_scalar_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_multiplies_scalar_t<channel_t, channel_t, channel_t> f;
    BOOST_TEST(f(0, 0) == channel_t(0));
    BOOST_TEST(f(1, 1) == channel_t(1));
    BOOST_TEST(f(4, 2) == channel_t(8));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(multiplies_scalar_integer_mixed_types, channel_t, fixture::channel_integer_types)
{
    using channel_result_t = std::uint8_t;
    gil::channel_multiplies_scalar_t<channel_t, int, channel_result_t> f;
    BOOST_TEST(f(0, 0) == channel_result_t(0));
    BOOST_TEST(f(4, 2) == channel_result_t(8));
}

BOOST_AUTO_TEST_SUITE_END() // channel_multiplies_scalar_t

BOOST_AUTO_TEST_SUITE(channel_divides_scalar_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(divides_scalar_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_divides_scalar_t<channel_t, channel_t, channel_t> f;
    BOOST_TEST(f(0, 1) == channel_t(0));
    BOOST_TEST(f(1, 1) == channel_t(1));
    BOOST_TEST(f(4, 2) == channel_t(2));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(divides_scalar_integer_mixed_types, channel_t, fixture::channel_integer_types)
{
    using channel_result_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
    gil::channel_divides_scalar_t<channel_t, int, channel_result_t> f;
    BOOST_TEST(f(0, 1) == channel_t(0));
    BOOST_TEST(f(4, 2) == channel_t(2));
}

BOOST_AUTO_TEST_SUITE_END() // channel_divides_scalar_t

BOOST_AUTO_TEST_SUITE(channel_halves_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(halves_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_halves_t<channel_t> f;
    {
        channel_t c(0);
        f(c);
        BOOST_TEST(c == channel_t(0));
    }
    {
        channel_t c(2);
        f(c);
        BOOST_TEST(c == channel_t(1));
    }
    {
        channel_t c(4);
        f(c);
        BOOST_TEST(c == channel_t(2));
    }
}

BOOST_AUTO_TEST_SUITE_END() // channel_halves_t

BOOST_AUTO_TEST_SUITE(channel_zeros_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(zeros_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_zeros_t<channel_t> f;
    {
        channel_t c(0);
        f(c);
        BOOST_TEST(c == channel_t(0));
    }
    {
        channel_t c(2);
        f(c);
        BOOST_TEST(c == channel_t(0));
    }
    {
        channel_t c(4);
        f(c);
        BOOST_TEST(c == channel_t(0));
    }
}

BOOST_AUTO_TEST_SUITE_END() // channel_zeros_t

BOOST_AUTO_TEST_SUITE(channel_assigns_t)

BOOST_AUTO_TEST_CASE_TEMPLATE(assigns_integer_same_types, channel_t, fixture::channel_integer_types)
{
    gil::channel_assigns_t<channel_t, channel_t> f;
    {
        channel_t c1(10);
        channel_t c2(20);
        f(c1, c2);
        BOOST_TEST(c2 == c1);
    }

}

BOOST_AUTO_TEST_SUITE_END() // channel_assigns_t


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

// Copyright (c) 2011-2012 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)

//[intersects_linestring
//` Check if two linestrings intersect each other

#include <iostream>

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

int main()
{
    // Calculate the intersects of a cartesian polygon
    typedef boost::geometry::model::d2::point_xy<double> P;
    boost::geometry::model::linestring<P> line1, line2;

    boost::geometry::read_wkt("linestring(1 1,2 2,3 3)", line1);
    boost::geometry::read_wkt("linestring(2 1,1 2,4 0)", line2);

    bool b = boost::geometry::intersects(line1, line2);

    std::cout << "Intersects: " << (b ? "YES" : "NO") << std::endl;

    return 0;
}

//]

//[intersects_linestring_output
/*`
Output:
[pre
Intersects: YES
]
*/
//]