// test_maximum.cpp
// (c) Tivole

#include <boost/test/unit_test.hpp>
#include "../src/numerary.hpp"

namespace numerary
{

    BOOST_AUTO_TEST_SUITE(TestMaximum)

    BOOST_AUTO_TEST_CASE(test_maximum_bisection)
    {
        const double eps = 1.e-9;
        double answer;
        double expected_answer;
        short int result;
        short int expected_result;

        // Testing sin function
        expected_result = 1;
        expected_answer = M_PI_2;
        result = Numerary::maximum(sin, -3, 3, &answer, "bisection", eps);
        BOOST_CHECK_EQUAL(result, expected_result);
        BOOST_CHECK(fabs(answer - expected_answer) < 1.e-7);

	    // Testing log function
        expected_result = 1;
        expected_answer = 4.0;
        result = Numerary::maximum(log, 0.5, 4, &answer, "bisection", eps);
        BOOST_CHECK_EQUAL(result, expected_result);
        BOOST_CHECK(fabs(answer - expected_answer) < 1.e-7);
    }

    BOOST_AUTO_TEST_CASE(test_maximum_golden_ratio)
    {
        const double eps = 1.e-9;
        double answer;
        double expected_answer;
        short int result;
        short int expected_result;

        // Testing sin function
        expected_result = 1;
        expected_answer = M_PI_2;
        result = Numerary::maximum(sin, -3, 3, &answer, "golden_ratio", eps);
        BOOST_CHECK_EQUAL(result, expected_result);
        BOOST_CHECK(fabs(answer - expected_answer) < 1.e-7);

	    // Testing cos function
        expected_result = 1;
        expected_answer = 4.0;
        result = Numerary::maximum(log, 0.5, 4, &answer, "golden_ratio", eps);
        BOOST_CHECK_EQUAL(result, expected_result);
        BOOST_CHECK(fabs(answer - expected_answer) < 1.e-7);
    }

    BOOST_AUTO_TEST_SUITE_END()
}


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html [^]
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE. See the above copyright notices for more information.
//
//------------------------------------------------------------------------

// this file defines the rttbCoreTests for the test driver
// and all it expects is that you have a function called RegisterTests

#include <boost/shared_ptr.hpp>

#include "litCheckMacros.h"
#include "rttbBioModel.h"
#include "rttbDVH.h"
#include "rttbTCPLQModel.h"
#include "rttbNTCPLKBModel.h"
#include "rttbNTCPRSModel.h"
#include "rttbBioModelScatterPlots.h"
#include "rttbBioModelCurve.h"
#include "rttbDvhBasedModels.h"
#include "rttbDoseIteratorInterface.h"
#include "rttbDVHXMLFileReader.h"

namespace rttb
{
	namespace testing
	{

		/*! @brief RTBioModelTest.
		TCP calculated using a DVH PTV and LQ Model.
		NTCP tested using 3 Normal Tissue DVHs and LKB/RS Model.

		Test if calculation in new architecture returns similar results to the
		original implementation.

		WARNING: The values for comparison need to be adjusted if the input files are changed!
		*/
		int RTBioModelExampleTest(int argc, char* argv[])
		{
			PREPARE_DEFAULT_TEST_REPORTING;

			typedef rttb::models::CurveDataType CurveDataType;
			typedef std::multimap<double , std::pair<double, double> > ScatterPlotType;
			typedef core::DVH::Pointer DVHPointer;

			//increased accuracy requires double values in the calculation (rttbBaseType.h)
			double toleranceEUD = 1e-5;
			double tolerance = 1e-7;

			//ARGUMENTS: 1: ptv dvh file name
			//           2: normal tissue 1 dvh file name
			//           3: normal tissue 2 dvh file name
			//           4: normal tissue 3 dvh file name
			//...........5: Virtuos MPM_LR_ah dvh lung file name
			//...........6: Virtuos MPM_LR_ah dvh target file name

			std::string DVH_FILENAME_PTV;
			std::string DVH_FILENAME_NT1;
			std::string DVH_FILENAME_NT2;
			std::string DVH_FILENAME_NT3;
			std::string DVH_FILENAME_TV_TEST;
			std::string DVH_Virtuos_Target;
			std::string DVH_Virtuos_Lung;

			if (argc > 1)
			{
				DVH_FILENAME_PTV = argv[1];
			}

			if (argc > 2)
			{
				DVH_FILENAME_NT1 = argv[2];
			}

			if (argc > 3)
			{
				DVH_FILENAME_NT2 = argv[3];
			}

			if (argc > 4)
			{
				DVH_FILENAME_NT3 = argv[4];
			}

			if (argc > 5)
			{
				DVH_FILENAME_TV_TEST = argv[5];
			}

			if (argc > 6)
			{
				DVH_Virtuos_Lung = argv[6];
			}

			if (argc > 7)
			{
				DVH_Virtuos_Target = argv[7];
			}

			//DVH PTV
			rttb::io::other::DVHXMLFileReader dvhReader = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_PTV);
			DVHPointer dvhPtr = dvhReader.generateDVH();

			CHECK_CLOSE(6.04759613161786830000e+001, models::getEUD(dvhPtr, 10), toleranceEUD);

			rttb::io::other::DVHXMLFileReader dvhReader_test_tv = rttb::io::other::DVHXMLFileReader(
			            DVH_FILENAME_TV_TEST);
			DVHPointer dvh_test_tv = dvhReader_test_tv.generateDVH();

			//test TCP LQ Model
			models::BioModelParamType alpha = 0.35;
			models::BioModelParamType beta = 0.023333333333333;
			models::BioModelParamType roh = 10000000;
			int numFractions = 2;

			DoseTypeGy normalizationDose = 68;

			rttb::models::TCPLQModel tcplq = rttb::models::TCPLQModel(dvhPtr, alpha, beta, roh, numFractions);
			CHECK_EQUAL(alpha, tcplq.getAlphaMean());
			CHECK_EQUAL(alpha / beta, tcplq.getAlphaBeta());
			CHECK_EQUAL(roh, tcplq.getRho());

			CHECK_NO_THROW(tcplq.init());

			if (tcplq.init())
			{
				CHECK_CLOSE(1.00497232941856940000e-127, tcplq.getValue(), tolerance);
			}

			CurveDataType curve = models::getCurveDoseVSBioModel(tcplq, normalizationDose);
			CurveDataType::iterator it;

			for (it = curve.begin(); it != curve.end(); ++it)
			{
				if ((*it).first < 72)
				{
					CHECK_EQUAL(0, (*it).second);
				}
				else if ((*it).first > 150)
				{
					CHECK((*it).second > 0.9);
				}
			}

			models::BioModelParamType alphaBeta = 10;
			tcplq.setParameters(alpha, alphaBeta, roh, 0.08);
			CHECK_EQUAL(alpha, tcplq.getAlphaMean());
			CHECK_EQUAL(alphaBeta, tcplq.getAlphaBeta());
			CHECK_EQUAL(roh, tcplq.getRho());

			if (tcplq.init())
			{
				CHECK_CLOSE(1.84e-005, tcplq.getValue(), tolerance);
			}

			normalizationDose = 40;
			curve = models::getCurveDoseVSBioModel(tcplq, normalizationDose);

			alpha = 1;
			alphaBeta = 14.5;
			tcplq.setAlpha(alpha);
			tcplq.setAlphaBeta(alphaBeta);
			tcplq.setRho(roh);
			CHECK_EQUAL(alpha, tcplq.getAlphaMean());
			CHECK_EQUAL(alphaBeta, tcplq.getAlphaBeta());
			CHECK_EQUAL(roh, tcplq.getRho());

			if (tcplq.init())
			{
				CHECK_CLOSE(0.954885, tcplq.getValue(), toleranceEUD);
			}

			alpha = 0.9;
			alphaBeta = 1;
			tcplq.setAlpha(alpha);
			tcplq.setAlphaBeta(alphaBeta);
			tcplq.setRho(roh);
			CHECK_EQUAL(alpha, tcplq.getAlphaMean());
			CHECK_EQUAL(alphaBeta, tcplq.getAlphaBeta());
			CHECK_EQUAL(roh, tcplq.getRho());

			if (tcplq.init())
			{
				CHECK_EQUAL(1, tcplq.getValue());
			}

			//TCP LQ Test
			alpha = 0.3;
			beta = 0.03;
			roh = 10000000;
			numFractions = 20;
			rttb::models::TCPLQModel tcplq_test = rttb::models::TCPLQModel(dvh_test_tv, alpha, beta, roh,
			                                      numFractions);
			CHECK_EQUAL(alpha, tcplq_test.getAlphaMean());
			CHECK_EQUAL(alpha / beta, tcplq_test.getAlphaBeta());
			CHECK_EQUAL(roh, tcplq_test.getRho());
			CHECK_NO_THROW(tcplq_test.init());

			if (tcplq_test.init())
			{
				CHECK_CLOSE(9.79050278878883180000e-001, tcplq_test.getValue(), tolerance);
			}

			normalizationDose = 60;
			curve = models::getCurveDoseVSBioModel(tcplq_test, normalizationDose);

			//DVH HT 1
			rttb::io::other::DVHXMLFileReader dvhReader2 = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_NT1);
			DVHPointer dvhPtr2 = dvhReader2.generateDVH();

			CHECK_CLOSE(1.07920836034015810000e+001, models::getEUD(dvhPtr2, 10), toleranceEUD);

			//test RTNTCPLKBModel
			rttb::models::NTCPLKBModel lkb = rttb::models::NTCPLKBModel();
			models::BioModelParamType aVal = 10;
			models::BioModelParamType mVal = 0.16;
			models::BioModelParamType d50Val = 55;
			CHECK_EQUAL(0, lkb.getA());
			CHECK_EQUAL(0, lkb.getM());
			CHECK_EQUAL(0, lkb.getD50());
			lkb.setDVH(dvhPtr2);
			CHECK_EQUAL(dvhPtr2, lkb.getDVH());
			lkb.setA(aVal);
			CHECK_EQUAL(aVal, lkb.getA());
			lkb.setM(mVal);
			CHECK_EQUAL(mVal, lkb.getM());
			lkb.setD50(d50Val);
			CHECK_EQUAL(d50Val, lkb.getD50());
			CHECK_NO_THROW(lkb.init());

			if (lkb.init())
			{
				CHECK_CLOSE(2.53523522831366570000e-007, lkb.getValue(), tolerance);
			}

			//test RTNTCPRSModel
			rttb::models::NTCPRSModel rs = rttb::models::NTCPRSModel();
			models::BioModelParamType gammaVal = 1.7;
			models::BioModelParamType sVal = 1;
			CHECK_EQUAL(0, rs.getGamma());
			CHECK_EQUAL(0, rs.getS());
			CHECK_EQUAL(0, rs.getD50());
			rs.setDVH(dvhPtr2);
			CHECK_EQUAL(dvhPtr2, rs.getDVH());
			rs.setD50(d50Val);
			CHECK_EQUAL(d50Val, rs.getD50());
			rs.setGamma(gammaVal);
			CHECK_EQUAL(gammaVal, rs.getGamma());
			rs.setS(sVal);
			CHECK_EQUAL(sVal, rs.getS());
			CHECK_NO_THROW(rs.init());

			if (rs.init())
			{
				CHECK_CLOSE(3.70385888626145740000e-009, rs.getValue(), tolerance);
			}

			//DVH HT 2
			rttb::io::other::DVHXMLFileReader dvhReader3 = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_NT2);
			DVHPointer dvhPtr3 = dvhReader3.generateDVH();
			CHECK_CLOSE(1.26287047025885110000e+001, models::getEUD(dvhPtr3, 10), toleranceEUD);

			//test RTNTCPLKBModel
			aVal = 10;
			mVal = 0.16;
			d50Val = 55;

			lkb.setDVH(dvhPtr3);
			CHECK_EQUAL(dvhPtr3, lkb.getDVH());
			lkb.setA(aVal);
			CHECK_EQUAL(aVal, lkb.getA());
			lkb.setM(mVal);
			CHECK_EQUAL(mVal, lkb.getM());
			lkb.setD50(d50Val);
			CHECK_EQUAL(d50Val, lkb.getD50());

			if (lkb.init())
			{
				CHECK_CLOSE(7.36294657754956700000e-007, lkb.getValue(), tolerance);
			}

			//test RTNTCPRSModel
			rs = rttb::models::NTCPRSModel();
			gammaVal = 1.7;
			sVal = 1;
			CHECK_EQUAL(0, rs.getGamma());
			CHECK_EQUAL(0, rs.getS());
			CHECK_EQUAL(0, rs.getD50());
			rs.setDVH(dvhPtr3);
			CHECK_EQUAL(dvhPtr3, rs.getDVH());
			rs.setD50(d50Val);
			CHECK_EQUAL(d50Val, rs.getD50());
			rs.setGamma(gammaVal);
			CHECK_EQUAL(gammaVal, rs.getGamma());
			rs.setS(sVal);
			CHECK_EQUAL(sVal, rs.getS());

			if (rs.init())
			{
				CHECK_CLOSE(1.76778795490939440000e-007, rs.getValue(), tolerance);
			}

			//DVH HT 3
			rttb::io::other::DVHXMLFileReader dvhReader4 = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_NT3);
			DVHPointer dvhPtr4 = dvhReader4.generateDVH();
			CHECK_CLOSE(2.18212982041056310000e+001, models::getEUD(dvhPtr4, 10), toleranceEUD);

			//test RTNTCPLKBModel
			aVal = 10;
			mVal = 0.16;
			d50Val = 55;
			lkb.setDVH(dvhPtr4);
			CHECK_EQUAL(dvhPtr4, lkb.getDVH());
			lkb.setA(aVal);
			CHECK_EQUAL(aVal, lkb.getA());
			lkb.setM(mVal);
			CHECK_EQUAL(mVal, lkb.getM());
			lkb.setD50(d50Val);
			CHECK_EQUAL(d50Val, lkb.getD50());

			if (lkb.init())
			{
				CHECK_CLOSE(8.15234192641929420000e-005, lkb.getValue(), tolerance);
			}

			//test RTNTCPRSModel
			rs = rttb::models::NTCPRSModel();
			gammaVal = 1.7;
			sVal = 1;
			CHECK_EQUAL(0, rs.getGamma());
			CHECK_EQUAL(0, rs.getS());
			CHECK_EQUAL(0, rs.getD50());
			rs.setDVH(dvhPtr4);
			CHECK_EQUAL(dvhPtr4, rs.getDVH());
			rs.setD50(d50Val);
			CHECK_EQUAL(d50Val, rs.getD50());
			rs.setGamma(gammaVal);
			CHECK_EQUAL(gammaVal, rs.getGamma());
			rs.setS(sVal);
			CHECK_EQUAL(sVal, rs.getS());

			if (rs.init())
			{
				CHECK_CLOSE(2.02607985020919480000e-004, rs.getValue(), tolerance);
			}

			//test using Virtuos Pleuramesotheliom MPM_LR_ah
			//DVH PTV

			rttb::io::other::DVHXMLFileReader dR_Target = rttb::io::other::DVHXMLFileReader(DVH_Virtuos_Target);
			DVHPointer dvhPtrTarget = dR_Target.generateDVH();

			rttb::io::other::DVHXMLFileReader dR_Lung = rttb::io::other::DVHXMLFileReader(DVH_Virtuos_Lung);
			DVHPointer dvhPtrLung = dR_Lung.generateDVH();

			//test TCP LQ Model
			models::BioModelParamType alphaMean = 0.34;
			models::BioModelParamType alphaVarianz = 0.02;
			models::BioModelParamType alpha_beta = 28;
			models::BioModelParamType rho = 1200;

			int numFractionsVirtuos = 27;

			rttb::models::TCPLQModel tcplqVirtuos = rttb::models::TCPLQModel(dvhPtrTarget, rho,
			                                        numFractionsVirtuos, alpha_beta,
			                                        alphaMean, alphaVarianz);

			if (tcplqVirtuos.init())
			{
				CHECK_CLOSE(0.8894, tcplqVirtuos.getValue(), 1e-4);
			}

			models::BioModelParamType d50Mean = 20;
			models::BioModelParamType m = 0.36;
			models::BioModelParamType a = 1.06;

			rttb::models::NTCPLKBModel lkbVirtuos = rttb::models::NTCPLKBModel(dvhPtrLung, d50Mean, m, a);

			if (lkbVirtuos.init())
			{
				CHECK_CLOSE(0.0397, lkbVirtuos.getValue(), 1e-4);
			}

			RETURN_AND_REPORT_TEST_SUCCESS;

		}

	}//testing
}//rttb


#include "ExplicitEuler.h"
#include <Eigen/LU>
#include <Eigen/Core>

bool ExplicitEuler::stepScene( TwoDScene& scene, scalar dt )
{
    // Your code goes here!

    // Some tips on getting data from TwoDScene:
    // A vector containing all of the system's position DoFs. x0, y0, x1, y1, ...
    //VectorXs& x = scene.getX();
    // A vector containing all of the system's velocity DoFs. v0, v0, v1, v1, ...
    //VectorXs& v = scene.getV();
    // A vector containing the masses associated to each DoF. m0, m0, m1, m1, ...
    //const VectorXs& m = scene.getM();
    // Determine if the ith particle is fixed
    // if( scene.isFixed(i) )

    VectorXs& qn = scene.getX();
    VectorXs& qn1 = qn;
    VectorXs& qdotn = scene.getV();
    VectorXs& qdotn1 = qdotn;

    const VectorXs& m = scene.getM();
    MatrixXs M = m.asDiagonal();
    MatrixXs MMinus1 = M.inverse();

    VectorXs f(qn.size());
    scene.accumulateGradU(f);

    //(n,1) = (n,n)*(n,1)
    VectorXs Pn =  M*qdotn;
    //(n,1) = (n,1) + (1,1)*(n,n)*(n,1)
    qn1 = qn + dt*MMinus1*Pn;
    qdotn1 = qdotn + dt*f;

    for(int i = 0; i < scene.getNumParticles(); ++i)
    {
        if(scene.isFixed(i))
        {
            scene.setVelocity(i, {0,0});
        }
    }

    return true;
}


#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl; using namespace mtl::mat;

    const unsigned                xd= 2, yd= 5, n= xd * yd;
    dense2D<double>               A(n, n);
    laplacian_setup(A, xd, yd);
    dense_vector<double>          v(n), w(n, 7.0);

    // Scale A with 4 and multiply the scaled view with w
    v= 4 * A * w;
    std::cout << "v is " << v << "\n";

    // Scale w with 4 and multiply the scaled view with A
    v= A * (4 * w);
    std::cout << "v is " << v << "\n";

    // Scale both with 2 before multiplying
    v= 2 * A * (2 * w);
    std::cout << "v is " << v << "\n";

    // Scale v after the MVP
    v= A * w;
    v*= 4;
    std::cout << "v is " << v << "\n";

    return 0;
}


//
// Created by Alex Beccaro on 09/01/18.
//

#include "problem56.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include <generics.hpp>

using boost::multiprecision::uint1024_t;
using generics::digits;

namespace problems {
    uint32_t problem56::solve(uint32_t base_ub, uint32_t exp_ub) {
        uint32_t result = 0;

        for (uint32_t a = 1; a < base_ub; a++) {
            uint1024_t n = 1;
            for (uint32_t b = 1; b < exp_ub; b++) {
                n *= a;
                auto digs = digits(n);

                uint32_t sum = 0;
                for (const auto &d : digs)
                    sum += d;

                if (sum > result)
                    result = sum;
            }
        }

        return result;
    }
}


// ===========================================================================
// Imagine++ Libraries
// Copyright (C) Imagine
// For detailed information: http://imagine.enpc.fr/software
// ===========================================================================

#include "Imagine/LinAlg/MyEigen.h"
#include <Eigen/Dense>

using namespace Eigen;

//N'existe pas dans Eigen Blas
//corriger
//info=0 si succes, -i si ieme parametre illegal, i si bdsdc ne converge pas
//info important car utilise dans Matrix.h
// Singular values decomposition
/* jobz est non usite... et est dans la liste des arguments de Imagine::svd */
template <typename T>
void subSingularValuesDecomposition(const char *jobz,int *m,int *n, T *a, T *s, T *u, T *vt, int *info)
{
    assert((*jobz == 'A') || (*jobz == 'S')); // ?
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (a, *m, *n, OuterStride<>(*m));
    Eigen::JacobiSVD<Matrix<T, Dynamic, Dynamic> > svd (AMap, ComputeFullU | ComputeFullV);
    Matrix<T, Dynamic, Dynamic> u1 = svd.matrixU();
    Matrix<T, Dynamic, Dynamic> v1 = svd.matrixV();
    Matrix<T, Dynamic, Dynamic> sv = svd.singularValues();
    Map<Matrix<T, Dynamic, 1> > sMap (s, sv.rows());
    sMap = sv;
    Map<Matrix<T, Dynamic, Dynamic> > uMap (u, u1.rows(), u1.cols());
    uMap = u1;
    Map<Matrix<T, Dynamic, Dynamic> > vtMap (vt, v1.rows(), v1.cols());
    vtMap = v1.transpose();
    Matrix<T, Dynamic, Dynamic> sigma = Matrix<T, Dynamic, Dynamic>::Zero(*m,*n);
    for (int i = 0 ; i < sMap.rows() ; i++){
        sigma(i, i) = sMap(i);
    };
    *info = 1 - AMap.isApprox(uMap * sigma * vtMap);
}

void singularValuesDecomposition(const char *jobz,int *m,int *n, double *a, double *s, double *u, double *vt, int *info) { subSingularValuesDecomposition<double>(jobz, m, n, a, s, u, vt, info); }
void singularValuesDecomposition(const char *jobz,int *m,int *n, float *a, float *s, float *u, float *vt, int *info) { subSingularValuesDecomposition<float>(jobz, m, n, a, s, u, vt, info); }

// Returns Eigen values (replaces xgeev_)
template <typename T>
void subEigenValues(int *n, T *a, T *wr, T *wi, T *vr, int *info)
{
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (a, *n, *n, OuterStride<>(*n));

    EigenSolver<Matrix<T, Dynamic, Dynamic> > ces(AMap);
    *info = 1 - int(ces.info());
    Matrix<std::complex<T>, Dynamic, 1> w1 = ces.eigenvalues();
    Map<Matrix<T, Dynamic, 1> > wrMap (wr, w1.rows());
    Map<Matrix<T, Dynamic, 1> > wiMap (wi, w1.rows());
    Matrix<std::complex<T>, Dynamic, Dynamic> Vr = ces.eigenvectors();
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > VrMap (vr, *n, Vr.cols(), OuterStride<>(1));
    wrMap = w1.real();
    wiMap = w1.imag();
    int j = 0;
    while (j < Vr.cols())
    {
        Matrix<T, Dynamic, 1> Xr = Vr.col(j).real();
        Matrix<T, Dynamic, 1> Xc = Vr.col(j).imag();
        if (Xc.isZero())
        {
            VrMap.col(j) = Xr;
            j++;
        }
        else
        {
            VrMap.col(j) = Xr;
            VrMap.col(j + 1) = Xc;
            j += 2;
        };
    }
}

void eigenValues(int *n, double *a, double *wr, double *wi, double *vr, int *info) { subEigenValues<double>(n, a, wr, wi, vr, info); }
void eigenValues(int *n, float *a, float *wr, float *wi, float *vr, int *info) { subEigenValues<float>(n, a, wr, wi, vr, info); }


#include <vector>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <tf/transform_datatypes.h>

tf::Quaternion getAverageQuaternion(
  const std::vector<tf::Quaternion>& quaternions,
  const std::vector<double>& weights)
{
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, quaternions.size());
  Eigen::Vector3d vec;
  for (size_t i = 0; i < quaternions.size(); ++i)
  {
    // Weigh the quaternions according to their associated weight
    tf::Quaternion quat = quaternions[i] * weights[i];
    // Append the weighted Quaternion to a matrix Q.
    Q(0,i) = quat.x();
    Q(1,i) = quat.y();
    Q(2,i) = quat.z();
    Q(3,i) = quat.w();
  }

  // Creat a solver for finding the eigenvectors and eigenvalues
  Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());

  // Find index of maximum (real) Eigenvalue.
  auto eigenvalues = es.eigenvalues();
  size_t max_idx = 0;
  double max_value = eigenvalues[max_idx].real();
  for (size_t i = 1; i < 4; ++i)
  {
    double real = eigenvalues[i].real();
    if (real > max_value)
    {
      max_value = real;
      max_idx = i;
    }
  }

  // Get corresponding Eigenvector, normalize it and return it as the average quat
  auto eigenvector = es.eigenvectors().col(max_idx).normalized();

  tf::Quaternion mean_orientation(
    eigenvector[0].real(),
    eigenvector[1].real(),
    eigenvector[2].real(),
    eigenvector[3].real()
  );

  return mean_orientation;
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <numeric>
#include <boost/algorithm/string/find.hpp>

class Calculator
{
    public:
        Calculator() = default;

        Calculator(std::string file_name)
        {
            std::ifstream file(file_name);
            std::string expression;
            if(file.is_open())
            {
                while(std::getline(file, expression))
                {
                    expressions.push_back(expression);
                }
            }
        }

        std::vector<std::string> expressions;

        long long calculate(bool addition_over_multiplication = false)
        {
            long long result = std::accumulate(std::begin(expressions), std::end(expressions),
            0LL, [&](const long long previous, std::string expression)
            {
                expression.erase(std::remove_if(expression.begin(), expression.end(), isspace), expression.end());
                return previous + (addition_over_multiplication == true ? calculate(expression, true) : calculate(expression));
            });
            return result;
        }

    private:
        std::string extract(const std::string expression, int start) const
        {
            std::string part = expression.substr(start + 1);
            int opened_parts = 1;
            int end = 0;
            while(opened_parts != 0 && end < part.size())
            {
                if(part[end] == '(') opened_parts++;
                else if(part[end] == ')') opened_parts--;
                end++;
            }
            return part.substr(0, end - 1);
        }

        enum class Mode
        {
            sum = 1, multiply = 2
        };

        long long do_operation(long long first, long long second, Mode mode) const
        {
            return ((mode == Mode::sum) ? first + second : first * second);
        }

        long long calculate(const std::string expression, bool addition_over_multiplication = false) const
        {
            long long first_element = -1LL;
            long long second_element = -1LL;
            Mode mode = Mode::sum;
            for(int i = 0; i < expression.size(); i++)
            {
                if(expression[i] == ')') continue;
                if(expression[i] == '(')
                {
                    std::string part = extract(expression, i);
                    second_element = first_element == -1LL ? second_element : calculate(part, addition_over_multiplication);
                    first_element = first_element == -1LL ? calculate(part, addition_over_multiplication) : first_element;
                    i += part.size();
                }
                else if(expression[i] == '*' || expression[i] == '+')
                {
                    mode = expression[i] == '*' ? Mode::multiply : Mode::sum;
                    if(addition_over_multiplication && mode == Mode::multiply)
                    {
                        second_element = calculate(expression.substr(i + 1), addition_over_multiplication);
                        first_element *= second_element;
                        break;
                    }
                }
                else // number
                {
                    long long num = (long long)(expression[i] - '0');
                    second_element = first_element == -1LL ? second_element : num;
                    first_element = first_element == -1LL ? num : first_element;
                }
                if(first_element != -1LL && second_element != -1LL)
                {
                    first_element = do_operation(first_element, second_element, mode);
                    second_element = -1LL;
                }
            }
            return first_element;
        }
};

void part1(const std::string& file_name)
{
    std::cout << "======\nPart 1\n======\n";
    Calculator calculator(file_name);
    std::cout << "Sum of the resulting values = " << calculator.calculate() << '\n';
}

void part2(const std::string& file_name)
{
    std::cout << "======\nPart 2\n======\n";
    Calculator calculator(file_name);
    std::cout << "Sum of the resulting values = " << calculator.calculate(true) << '\n';
}

int main()
{
    const std::string file_name = "/home/daria/Documents/AoC2020/input/day18.txt";
    part1(file_name);
    std::cout << '\n';
    part2(file_name);
    std::cout << '\n';
}


//
//  cross_product.cpp
//  BGV-Adder
//
//  Created by Hindrik Stegenga on 26/10/2020.
//  Copyright © 2020 RUG. All rights reserved.
//

#include "cross_product.hpp"
#include <helib/FHE.h>
#include <NTL/ZZX.h>
#include <NTL/tools.h>

using helib::Ctxt;

array<long, CV_SIZE> compute_cross_product(array<i16, CV_SIZE> lhs, array<i16, CV_SIZE> rhs) {
    long k = 128; // Security parameter
    long L = 16; // Number of levels in the modulus default is 16
    long c = 3; // Nr of columns in key switch matrix.
    long w = 64; // secret key hamming weight

    // Change to 65537 for noise warning. (Always fun times)
    // Compensate for that by setting L = 128. (Increase modchain basically)
    // Set P to 65537 and L to 32 to showcase decryption failures.

    long p = 1021; // plaintext base default = 1021
    long d = 0; // Degree of field extension
    long r = 1; // hensel lifting

    // Determine a value for m
    auto m = helib::FindM(k, L, c, p, d, 0, 0);
    // Setup context
    auto context = helib::Context(m, p, r);
    // Build mod chain
    helib::buildModChain(context, L, c);

    // Generating secret key and public key
    NTL::ZZX encryption_polynomial = context.alMod.getFactorsOverZZ()[0];
    auto secretKey = helib::SecKey(context);
    secretKey.GenSecKey();
    const helib::PubKey& publicKey = secretKey;

    // Initialize ciphertexts
    array<Ctxt, CV_SIZE> lhs_ciphertext {
        Ctxt(publicKey),
        Ctxt(publicKey),
        Ctxt(publicKey)
    }, rhs_ciphertext {
        Ctxt(publicKey),
        Ctxt(publicKey),
        Ctxt(publicKey)
    };

    // Plaintext must be encrypted as a polynomial using zzx api.
    for (size_t i = 0; i < CV_SIZE; ++i) {
        publicKey.Encrypt(lhs_ciphertext[i], NTL::ZZX(lhs[i]));
        publicKey.Encrypt(rhs_ciphertext[i], NTL::ZZX(rhs[i]));
    }

    // Apply operations on the ciphertexts.
    // Cross product is vec a x b.
    // 0 => ay * bz - az * by
    // 1 => az * bx - ax * bz
    // 2 => ax * by - ay * bx

    // Left hand side of equations
    array<Ctxt, CV_SIZE> lhs_a = {
        lhs_ciphertext[1], //ay
        lhs_ciphertext[2], //az
        lhs_ciphertext[0], //ax
    };
    array<Ctxt, CV_SIZE> lhs_b = {
        rhs_ciphertext[2], //bz
        rhs_ciphertext[0], //bx
        rhs_ciphertext[1], //by
    };

    // Compute first set of products
    for (size_t i = 0; i < CV_SIZE; ++i) {
        lhs_a[i] *= lhs_b[i];
    }

    // Right hand side of equations
    array<Ctxt, CV_SIZE> rhs_a = {
        lhs_ciphertext[2], //az
        lhs_ciphertext[0], //ax
        lhs_ciphertext[1], //ay
    };
    array<Ctxt, CV_SIZE> rhs_b = {
        rhs_ciphertext[1], //by
        rhs_ciphertext[2], //bz
        rhs_ciphertext[0], //bx
    };

    // Compute second set of products
    for (size_t i = 0; i < CV_SIZE; ++i) {
        rhs_a[i] *= rhs_b[i];
    }

    // Compute negation of products
    for (size_t i = 0; i < CV_SIZE; ++i) {
        lhs_a[i] -= rhs_a[i];
    }

    array<Ctxt, CV_SIZE> cipher_results = lhs_a;

    // Decrypt the results using secret key and convert back from
    // polynomial representation to numeric.
    array<long, CV_SIZE> return_values = {0,0,0};
    array<NTL::ZZX, CV_SIZE> plaintext_results;
    for (size_t i = 0; i < 3; ++i) {
        NTL::ZZX zzx;
        secretKey.Decrypt(zzx, cipher_results[i]);
        conv(return_values[i], zzx[0]);
        // Compensate for negative numbers by checking if it's larger than p/2,
        // In such case it wrapped around due to negative numbers
        if (return_values[i] > p / 2) {
            return_values[i] += (-1 * p);
        }
    }

    return return_values;
}


#include "ShapeEvaluation.h"
#include "EvaluationUtil.h"

#include <iostream>
#include <Eigen/Core>
#include <Eigen/SVD>

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMajorMatrix;

namespace shapeworks {

  //---------------------------------------------------------------------------
double ShapeEvaluation::ComputeCompactness(const ParticleSystem &particleSystem, const int nModes,
                                                       const std::string &saveTo)
{
  const int N = particleSystem.N();
  if (nModes > N-1){
    throw std::invalid_argument("Invalid mode of variation specified");
  }
  Eigen::VectorXd cumsum = ShapeEvaluation::ComputeFullCompactness(particleSystem);

  if (!saveTo.empty()) {
    std::ofstream of(saveTo);
    of << cumsum;
    of.close();
  }

  return cumsum(nModes - 1);
}

//---------------------------------------------------------------------------
Eigen::VectorXd ShapeEvaluation::ComputeFullCompactness(const ParticleSystem &particleSystem, std::function<void(float)> progress_callback)
{
  const int N = particleSystem.N();
  const int D = particleSystem.D();
  const int num_modes = N-1; // the number of modes is one less than the number of samples

  if (num_modes < 1) {
    return Eigen::VectorXd();
  }
  Eigen::MatrixXd Y = particleSystem.Particles();
  const Eigen::VectorXd mu = Y.rowwise().mean();
  Y.colwise() -= mu;

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y);
  const auto S = svd.singularValues().array().pow(2) / (N * D);

  // Compute cumulative sum
  Eigen::VectorXd cumsum(num_modes);
  cumsum(0) = S(0);
  for (int i = 1; i < num_modes; i++) {
    if (progress_callback) {
      progress_callback(static_cast<float>(i) / static_cast<float>(N));
    }
    cumsum(i) = cumsum(i-1) + S(i);
  }
  cumsum /= S.sum();
  return cumsum;
}

//---------------------------------------------------------------------------
double ShapeEvaluation::ComputeGeneralization(const ParticleSystem &particleSystem, const int nModes,
                                                          const std::string &saveTo)
{
  const int N = particleSystem.N();
  const int D = particleSystem.D();
  const Eigen::MatrixXd &P = particleSystem.Particles();

  if (nModes > N-1){
    throw std::invalid_argument("Invalid mode of variation specified");
  }
  // Keep track of the reconstructions so we can visualize them later
  std::vector<Reconstruction> reconstructions;

  double totalDist = 0.0;
  for (int leave = 0; leave < N; leave++) {

    Eigen::MatrixXd Y(D, N - 1);
    Y.leftCols(leave) = P.leftCols(leave);
    Y.rightCols(N - leave - 1) = P.rightCols(N - leave - 1);

    const Eigen::VectorXd mu = Y.rowwise().mean();
    Y.colwise() -= mu;
    const Eigen::VectorXd Ytest = P.col(leave);

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y, Eigen::ComputeFullU);
    const auto epsi = svd.matrixU().block(0, 0, D, nModes);
    const auto betas = epsi.transpose() * (Ytest - mu);
    const Eigen::VectorXd rec = epsi * betas + mu;

    const int numParticles = D / VDimension;
    const Eigen::Map<const RowMajorMatrix> Ytest_reshaped(Ytest.data(), numParticles, VDimension);
    const Eigen::Map<const RowMajorMatrix> rec_reshaped(rec.data(), numParticles, VDimension);
    const double dist = (rec_reshaped - Ytest_reshaped).rowwise().norm().sum() / numParticles;
    totalDist += dist;

    reconstructions.push_back({dist, leave, rec_reshaped});
  }
  const double generalization = totalDist / N;

  // Save the reconstructions if needed. Generates XML files that can be opened in
  // ShapeWorksView2
  if (!saveTo.empty()) {
    SaveReconstructions(reconstructions, particleSystem.Paths(), saveTo);
  }

  return generalization;
}

Eigen::VectorXd ShapeEvaluation::ComputeFullGeneralization(const ParticleSystem &particleSystem, std::function<void(float)> progress_callback)
{
  const int N = particleSystem.N();
  const int D = particleSystem.D();
  const Eigen::MatrixXd &P = particleSystem.Particles();

  if (N <= 1) {
    return Eigen::VectorXd();
  }

  Eigen::VectorXd generalizations(N-1);

  Eigen::VectorXd totalDists = Eigen::VectorXd::Zero(N-1);

  for (int leave = 0; leave < N; leave++) {
    if (progress_callback) {
      progress_callback(static_cast<float>(leave) / static_cast<float>(N));
    }
    Eigen::MatrixXd Y(D, N - 1);
    Y.leftCols(leave) = P.leftCols(leave);
    Y.rightCols(N - leave - 1) = P.rightCols(N - leave - 1);

    const Eigen::VectorXd mu = Y.rowwise().mean();
    Y.colwise() -= mu;
    const Eigen::VectorXd Ytest = P.col(leave);

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y, Eigen::ComputeFullU);

    for (int mode = 1; mode < N; mode++) {

      const auto epsi = svd.matrixU().block(0, 0, D, mode);
      const auto betas = epsi.transpose() * (Ytest - mu);
      const Eigen::VectorXd rec = epsi * betas + mu;

      const int numParticles = D / VDimension;
      const Eigen::Map<const RowMajorMatrix> Ytest_reshaped(Ytest.data(), numParticles, VDimension);
      const Eigen::Map<const RowMajorMatrix> rec_reshaped(rec.data(), numParticles, VDimension);
      const double dist = (rec_reshaped - Ytest_reshaped).rowwise().norm().sum() / numParticles;
      totalDists(mode-1) += dist;
    }
  }

  generalizations = totalDists / N;

  return generalizations;
}

//---------------------------------------------------------------------------
double ShapeEvaluation::ComputeSpecificity(const ParticleSystem &particleSystem, const int nModes,
                                                       const std::string &saveTo)
{

  const int N = particleSystem.N();
  const int D = particleSystem.D();

  if (nModes > N-1){
    throw std::invalid_argument("Invalid mode of variation specified");
  }
  const int nSamples = 1000;

  // Keep track of the reconstructions so we can visualize them later
  std::vector<Reconstruction> reconstructions;

  Eigen::VectorXd meanSpecificity(nModes);
  Eigen::VectorXd stdSpecificity(nModes);
  Eigen::MatrixXd spec_store(nModes, 4);

  // PCA calculations
  const Eigen::MatrixXd &ptsModels = particleSystem.Particles();
  const Eigen::VectorXd mu = ptsModels.rowwise().mean();
  Eigen::MatrixXd Y = ptsModels;

  Y.colwise() -= mu;

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y, Eigen::ComputeFullU);
  const auto epsi = svd.matrixU().block(0, 0, D, nModes);
  const auto allEigenValues = svd.singularValues();
  const auto eigenValues = allEigenValues.head(nModes);

  Eigen::MatrixXd samplingBetas(nModes, nSamples);
  MultiVariateNormalRandom sampling{eigenValues.asDiagonal()};
  for (int modeNumber = 0; modeNumber < nModes; modeNumber++) {
    for (int i = 0; i < nSamples; i++) {
      samplingBetas.col(i) = sampling();
    }

    Eigen::MatrixXd samplingPoints = (epsi * samplingBetas).colwise() + mu;

    const int numParticles = D / VDimension;
    const int nTrain = ptsModels.cols();

    Eigen::VectorXd distanceToClosestTrainingSample(nSamples);

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

      Eigen::VectorXd pts_m = samplingPoints.col(i);
      Eigen::MatrixXd ptsDistance_vec = ptsModels.colwise() - pts_m;
      Eigen::MatrixXd ptsDistance(Eigen::MatrixXd::Constant(1, nTrain, 0.0));

      for (int j = 0; j < nTrain; j++) {
        Eigen::Map<const RowMajorMatrix> ptsDistance_vec_reshaped(ptsDistance_vec.col(j).data(), numParticles,
                                                                  VDimension);
        ptsDistance(j) = (ptsDistance_vec_reshaped).rowwise().norm().sum();
      }

      int closestIdx, _r;
      distanceToClosestTrainingSample(i) = ptsDistance.minCoeff(&_r, &closestIdx);

      Eigen::Map<const RowMajorMatrix> pts_m_reshaped(pts_m.data(), numParticles, VDimension);
      reconstructions.push_back(Reconstruction{
              distanceToClosestTrainingSample(i),
              (int) closestIdx,
              pts_m_reshaped,
      });
    }

    meanSpecificity(modeNumber) = distanceToClosestTrainingSample.mean();
  }

  if (!saveTo.empty()) {
    SaveReconstructions(reconstructions, particleSystem.Paths(), saveTo);
  }

  const int numParticles = D / VDimension;
  const double specificity = meanSpecificity(nModes - 1) / numParticles;

  return specificity;
}

//---------------------------------------------------------------------------
Eigen::VectorXd ShapeEvaluation::ComputeFullSpecificity(const ParticleSystem &particleSystem, std::function<void(float)> progress_callback)
{
  const int N = particleSystem.N();
  const int D = particleSystem.D();
  const int numParticles = D / VDimension;

  Eigen::VectorXd specificities(N-1);

  // PCA calculations
  const Eigen::MatrixXd &ptsModels = particleSystem.Particles();
  const int nTrain = ptsModels.cols();

  const Eigen::VectorXd mu = ptsModels.rowwise().mean();
  Eigen::MatrixXd Y = ptsModels;
  Y.colwise() -= mu;
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y, Eigen::ComputeFullU);
  const auto allEigenValues = svd.singularValues();

  for (int nModes=1;nModes<N;nModes++) {
    if (progress_callback) {
      progress_callback(static_cast<float>(nModes) / static_cast<float>(N));
    }

    const int nSamples = 1000;

    Eigen::VectorXd stdSpecificity(nModes);
    Eigen::MatrixXd spec_store(nModes, 4);
    const auto eigenValues = allEigenValues.head(nModes);
    const auto epsi = svd.matrixU().block(0, 0, D, nModes);

    Eigen::MatrixXd samplingBetas(nModes, nSamples);
    MultiVariateNormalRandom sampling{eigenValues.asDiagonal()};
    for (int i = 0; i < nSamples; i++) {
      samplingBetas.col(i) = sampling();
    }

    Eigen::MatrixXd samplingPoints = (epsi * samplingBetas).colwise() + mu;
    Eigen::VectorXd distanceToClosestTrainingSample(nSamples);

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

      Eigen::VectorXd pts_m = samplingPoints.col(i);
      Eigen::MatrixXd ptsDistance_vec = ptsModels.colwise() - pts_m;
      Eigen::MatrixXd ptsDistance(Eigen::MatrixXd::Constant(1, nTrain, 0.0));

      for (int j = 0; j < nTrain; j++) {
        Eigen::Map<const RowMajorMatrix> ptsDistance_vec_reshaped(ptsDistance_vec.col(j).data(), numParticles,
                                                                  VDimension);
        ptsDistance(j) = (ptsDistance_vec_reshaped).rowwise().norm().sum();
      }

      int closestIdx, _r;
      distanceToClosestTrainingSample(i) = ptsDistance.minCoeff(&_r, &closestIdx);
    }

    double meanSpecificity = distanceToClosestTrainingSample.mean();
    const double specificity = meanSpecificity / numParticles;
    specificities(nModes-1) = specificity;
  }
  return specificities;
}

} // shapeworks


#ifndef CANNON_ML_PIECEWISE_LSTD_H
#define CANNON_ML_PIECEWISE_LSTD_H

/*!
 * \file cannon/ml/piecewise_lstd.hpp
 * \brief File containing PiecewiseLSTDFilter class definition.
 */

#include <Eigen/Core>

using namespace Eigen;

namespace cannon {
  namespace ml {

    /*!
     * \brief Class representing a Least-Squares Temporal Difference (LSTD)
     * approximator using a feature set that produces a piecewise-affine value
     * function approximation. This is for use in reinforcement learning algorithms.
     */
    class PiecewiseLSTDFilter {
      public:
        PiecewiseLSTDFilter() = delete;

        /*!
         * \brief Constructor taking state space dimension, number of affine
         * regions, and discount factor.
         */
        PiecewiseLSTDFilter(unsigned int in_dim, unsigned int num_refs,
                            double discount_factor, double alpha = 1.0)
            : in_dim_(in_dim + 1), param_dim_(in_dim_ * num_refs),
              num_refs_(num_refs), discount_factor_(discount_factor),
              alpha_(alpha),
              a_inv_(MatrixXd::Identity(param_dim_, param_dim_) * alpha_),
              b_(VectorXd::Zero(param_dim_)),
              theta_(VectorXd::Zero(param_dim_)) {}

        /*!
         * \brief Update this approximation in light of a single data point,
         * which is a transition from one state to another with associated
         * reward.
         *
         * \param in_vec Feature vector for first state.
         * \param next_in_vec Feature vector for next state.
         * \param idx Region index for first state.
         * \param next_idx Region index for next state.
         * \param reward Reward associated with this state transition.
         */
        void process_datum(const VectorXd &in_vec, const VectorXd &next_in_vec,
                           unsigned int idx, unsigned int next_idx,
                           double reward);

        /*!
         * \brief Get the matrix representing the linear portion of the local,
         * affine approximation in the region with for the input index.
         *
         * \param idx Region index
         *
         * \returns Estimated local linear approximation parameter matrix.
         */
        VectorXd get_mat(unsigned int idx);

        /*!
         * \brief Predict the value of the input state using the estimated
         * piecewise-affine value function.
         *
         * \param in_vec Feature vector for the state.
         * \param idx Region index of the state.
         *
         * \returns Value function prediction.
         */
        double predict(const VectorXd& in_vec, unsigned int idx);

        /*!
         * \brief Reset this value function approximation.
         */
        void reset();

      private:
        /*!
         * \brief Make internal feature vector for the input state in the
         * region with the input index.
         *
         * \param in_vec Input features for state.
         * \param idx Region index for the state.
         *
         * \returns Internal feature representation leading to piecewise-affine
         * function.
         */
        RowVectorXd make_feature_vec_(VectorXd in_vec, unsigned int idx) const;

        /*!
         * \brief Update approximation of the linear portion of the LSTD filter
         * given a particular state transition.
         *
         * \param feat Internal feature vector of the first state.
         * \param next_feat Internal feature vector of the next state.
         */
        void update_a_inv_(const RowVectorXd& feat, const RowVectorXd& next_feat);

        /*!
         * \brief Update approximation of the vector portion of the LSTD filter.
         *
         * \param feat Internal feature vector of the first state.
         * \param next_feat Internal feature vector of the next state.
         */
        void update_b_(const RowVectorXd& feat, double reward);

        /*!
         * \brief Update parameter vector theta, if necessary. This lazy
         * evaluation can save time if multiple value function updates occur
         * between predictions.
         */
        void check_theta_();

        // Parameters
        unsigned int in_dim_; //!< Dimension of input
        unsigned int param_dim_; //!< Dimension of internal feature space
        unsigned int num_refs_; //!< Number of linear regions
        double discount_factor_; //!< Discount factor for value function
        double alpha_; //!< L2 regularization parameter

        // Matrices
        MatrixXd a_inv_; //!< Linear portion of LSTD filter
        VectorXd b_; //!< Vector portion of LSTD filter
        VectorXd theta_; //!< Parameters of value function approximation
        bool theta_updated_ = false; //!< Whether theta has been updated since last A, b update
    };

  } // namespace ml
} // namespace cannon
#endif /* ifndef CANNON_ML_PIECEWISE_LSTD_H */


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

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/sqrt.hpp>
#include <boost/core/lightweight_test.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>

template <typename Real>
void test_mp_sqrt()
{
    constexpr Real tol = 2*std::numeric_limits<Real>::epsilon();

    // Sqrt(2)
    constexpr Real test_val = boost::math::ccmath::sqrt(Real(2));
    constexpr Real sqrt2 = Real(1.4142135623730950488016887242096980785696718753769480731766797379Q);
    constexpr Real abs_test_error = (test_val - sqrt2) > 0 ? (test_val - sqrt2) : (sqrt2 - test_val);
    static_assert(abs_test_error < tol, "Out of tolerance");

    // inf
    constexpr Real test_inf = boost::math::ccmath::sqrt(std::numeric_limits<Real>::infinity());
    static_assert(test_inf == std::numeric_limits<Real>::infinity(), "Not infinity");

    // NAN
    constexpr Real test_nan = boost::math::ccmath::sqrt(std::numeric_limits<Real>::quiet_NaN());
    static_assert(test_nan, "Not a NAN");

    // 100'000'000
    constexpr Real test_100m = boost::math::ccmath::sqrt(100000000);
    static_assert(test_100m == 10000, "Incorrect");
}

#endif

template <typename Real>
void test_float_sqrt()
{
    using std::abs;

    constexpr Real tol = 2*std::numeric_limits<Real>::epsilon();

    constexpr Real test_val = boost::math::ccmath::sqrt(Real(2));
    constexpr Real sqrt2 = Real(1.4142135623730950488016887l);
    constexpr Real abs_test_error = (test_val - sqrt2) > 0 ? (test_val - sqrt2) : (sqrt2 - test_val);
    static_assert(abs_test_error < tol, "Out of tolerance");

    Real known_val = std::sqrt(Real(2));
    BOOST_TEST(abs(test_val - known_val) < tol);

    // 1000 eps
    constexpr Real test_1000 = boost::math::ccmath::sqrt(1000*std::numeric_limits<Real>::epsilon());
    Real known_1000 = std::sqrt(1000*std::numeric_limits<Real>::epsilon());
    BOOST_TEST(abs(test_1000 - known_1000) < tol);

    // inf
    constexpr Real test_inf = boost::math::ccmath::sqrt(std::numeric_limits<Real>::infinity());
    static_assert(test_inf == std::numeric_limits<Real>::infinity(), "Not infinity");

    // NAN
    constexpr Real test_nan = boost::math::ccmath::sqrt(std::numeric_limits<Real>::quiet_NaN());
    static_assert(test_nan, "Not a NAN");

    // 100'000'000
    constexpr Real test_100m = boost::math::ccmath::sqrt(100000000);
    static_assert(test_100m == 10000, "Incorrect");

    // MAX / 2
    // Only tests float since double and long double will exceed maximum template depth
    if constexpr (std::is_same_v<float, Real>)
    {
        constexpr Real test_max = boost::math::ccmath::sqrt((std::numeric_limits<Real>::max)() / 2);
        Real known_max = std::sqrt((std::numeric_limits<Real>::max)() / 2);
        BOOST_TEST(abs(test_max - known_max) < tol);
    }
}

template <typename Z>
void test_int_sqrt()
{
    using std::abs;

    constexpr double tol = 2*std::numeric_limits<double>::epsilon();

    constexpr double test_val = boost::math::ccmath::sqrt(Z(2));
    constexpr double dummy = 1;
    static_assert(test_val > dummy, "Not constexpr");

    double known_val = std::sqrt(2.0);

    BOOST_TEST(abs(test_val - known_val) < tol);
}

// Only test on platforms that provide BOOST_MATH_IS_CONSTANT_EVALUATED
#ifndef BOOST_MATH_NO_CONSTEXPR_DETECTION
int main()
{
    test_float_sqrt<float>();
    test_float_sqrt<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_float_sqrt<long double>();
    #endif

    #if defined(BOOST_HAS_FLOAT128) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
    test_mp_sqrt<boost::multiprecision::float128>();
    #endif

    test_int_sqrt<int>();
    test_int_sqrt<unsigned>();
    test_int_sqrt<long>();
    test_int_sqrt<std::int32_t>();
    test_int_sqrt<std::int64_t>();
    test_int_sqrt<std::uint32_t>();

    return boost::report_errors();
}
#else
int main()
{
    return 0;
}
#endif


/*! \file mesh_generator.hpp
  \brief Set of functions to generate points.
  \author Elad Steinberg
*/
#ifndef MESHGENERATOR_HPP
#define MESHGENERATOR_HPP 1

#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif // _MSC_VER
#include <vector>
#include <cmath>
#include "../tessellation/geometry.hpp"
#include <algorithm>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

/*! \brief Generates a cartesian mesh
  \param nx Number of points along the x axis
  \param ny Number of points along the y axis
  \param lower_left Lower left point
  \param upper_right Upper right point
  \return Set of two dimensional points
*/
std::vector<Vector2D> cartesian_mesh(int nx, int ny,
				Vector2D const& lower_left,
				Vector2D const& upper_right);

/*!
  \brief Generates a round grid with constant point density
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \param xmin Left edge of confining rectangle
  \param xmax Right edge of confining rectangle
  \param ymax Upper edge of confining rectangle
  \param ymin Lower edge of confining rectangle
  \return List of two dimensional points
*/
std::vector<Vector2D> CirclePointsRmax(int PointNum,double Rmin,double Rmax,
	double xmax, double ymax, double xmin, double ymin, double xc = 0, double yc = 0);

/*!
  \brief Generates a round grid with 1/r^2 point density confined to a rectangle given by xmin,xmax,ymin and ymax.
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \param xmin Left edge of confining rectangle
  \param xmax Right edge of confining rectangle
  \param ymax Upper edge of confining rectangle
  \param ymin Lower edge of confining rectangle
  \return List of two dimensional points
*/
std::vector<Vector2D> CirclePointsRmax_2(int PointNum,double Rmin,double Rmax,
				    double xc=0,double yc=0,double xmax=1,double ymax=1,double xmin=-1,
				    double ymin=1);
/*!
  \brief Generates a round grid with 1/r point density confined to a rectangle given by xmin,xmax,ymin and ymax.
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \param xmin Left edge of confining rectangle
  \param xmax Right edge of confining rectangle
  \param ymax Upper edge of confining rectangle
  \param ymin Lower edge of confining rectangle
  \return List of two dimensional points
*/
std::vector<Vector2D> CirclePointsRmax_1(int PointNum,double Rmin,double Rmax,
				    double xc=0,double yc=0,double xmax=1,double ymax=1,double xmin=-1,double ymin=-1);

/*!
  \brief Creates a circle of evenly spaced points
  \param point_number Number of points along the circumference
  \param radius Radius of the circle
  \param center Position of the center of the circle
  \return List of two dimensional points
*/
std::vector<Vector2D> circle_circumference(size_t point_number,
					   double radius,
					   Vector2D const& center);

/*!
  \brief Creates a line of evenly spaced points y=slope*x+b
  \param PointNum The number of points
  \param xmin The minimum x of the line
  \param xmax The maximum x of the line
  \param ymin The minimum y of the line
  \param ymax The maximum y of the line
  \return List of two dimensional points
*/
std::vector<Vector2D> Line(int PointNum,double xmin,double xmax,double ymin,double ymax);

/*!
  \brief Generates a round grid with r^alpha point density confined to a rectangle given by xmin,xmax,ymin and ymax.
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \param xmin Left edge of confining rectangle
  \param xmax Right edge of confining rectangle
  \param ymax Upper edge of confining rectangle
  \param ymin Lower edge of confining rectangle
  \param alpha The point density, should not be -1 or -2
  \return List of two dimensional points
*/
std::vector<Vector2D> CirclePointsRmax_a(int PointNum,double Rmin,double Rmax,
				    double xc,double yc,double xmax,double ymax,double xmin,double ymin,
				    double alpha);

/*!
  \brief Generates a rectangular grid with random 1/r point density
  \param PointNum The number of points.
  \param xl The left boundary
  \param xr The right boundary
  \param yd The lower boundary
  \param yu The upper boundary
  \param minR The inner radius in which there are no points
  \param xc The X of center of the circle
  \param yc The Y of center of the circle
  \return List of two dimensional points
*/
std::vector<Vector2D> RandPointsR(int PointNum,double xl=-0.5,double xr=0.5,
			     double yd=-0.5,double yu=0.5,double minR=0,double xc=0,
				 double yc=0);

/*!
  \brief Generates a random rectangular grid with uniform point density and a constant seed
  \param PointNum The number of points.
  \param lowerleft The lower left point of the domain
  \param upperright The upper right point of the domain
  \return List of two dimensional points
*/
std::vector<Vector2D> RandSquare(int PointNum,Vector2D const& lowerleft, Vector2D const& upperright);

/*!
\brief Generates a random rectangular grid with uniform point density and a constant seed
\param PointNum The number of points.
\param xl The left boundary
\param xr The right boundary
\param yd The lower boundary
\param yu The upper boundary
\return List of two dimensional points
*/
std::vector<Vector2D> RandSquare(int PointNum, double xl = -0.5, double xr = 0.5,
	double yd = -0.5, double yu = 0.5);

/*!
  \brief Generates a random rectangular grid with uniform point density. This is when reseting the seed between calls isn't wanted
  \param PointNum The number of points.
  \param eng The random number generator
  \param xl The left boundary
  \param xr The right boundary
  \param yd The lower boundary
  \param yu The upper boundary
  \return List of two dimensional points
*/

std::vector<Vector2D> RandSquare(int PointNum,boost::random::mt19937 &eng,
	double xl=-0.5,double xr=0.5,double yd=-0.5,double yu=0.5);

/*!
  \brief Generates a random round grid with r^(1-a) point density
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param alpha The radial density of the points, shouldn't be 1
  \param lowerleft The lowerleft corner of the domain
  \param upperright The upperright corner of the domain
  \param center The center of the coordinates system
  \return List of two dimensional points
*/

std::vector<Vector2D> RandPointsRa(int PointNum,double Rmin,double Rmax,double alpha,
	Vector2D const& lowerleft,Vector2D const& upperright,Vector2D const& center=Vector2D(0,0));

/*!
  \brief Generates a random round grid with 1/r point density
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \return List of two dimensional points
*/
std::vector<Vector2D> RandPointsRmax(int PointNum,double Rmin,double Rmax,
				double xc=0,double yc=0);

/*!
\brief Generates a random round grid with 1/y point density where y is the cylindrical r coordinate
\param PointNum The number of points.
\param ll The lower left point of the domain. Should be ll.y>0
\param ur The upper right point of the domain
\return List of two dimensional points
*/
std::vector<Vector2D> RandPointsCylinder(int PointNum, Vector2D const& ll,Vector2D const& ur);

#endif //MESHGENERATOR_HPP


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example iterative-mtl4.cpp
*
*   The following tutorial shows how to use the iterative solvers in ViennaCL with objects from the MTL4 Library directly.
*
*   \note MTL4 provides iterative solvers through the ITK library. You might also want to check these.
*
*   We begin with including the necessary headers:
**/

// include necessary system headers
#include <iostream>

// MTL4 headers
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

// Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on Eigen objects
#define VIENNACL_WITH_MTL4 1

// ViennaCL includes
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "vector-io.hpp"

/**
*  In the following we run the CG method, the BiCGStab method, and the GMRES method with MTL4 types directly.
*  First, the matrices are set up, then the respective solvers are called.
**/
int main(int, char *[])
{
  typedef double    ScalarType;

  mtl::compressed2D<ScalarType> mtl4_matrix;
  mtl4_matrix.change_dim(65025, 65025);
  set_to_zero(mtl4_matrix);

  mtl::dense_vector<ScalarType> mtl4_rhs(65025, 1.0);
  mtl::dense_vector<ScalarType> mtl4_result(65025, 0.0);
  mtl::dense_vector<ScalarType> mtl4_residual(65025, 0.0);

  /**
  * Read system from file
  **/

  mtl::io::matrix_market_istream("../examples/testdata/mat65k.mtx") >> mtl4_matrix;

  /**
  *  Conjugate Gradient (CG) solver:
  **/
  std::cout << "----- Running CG -----" << std::endl;
  mtl4_result = viennacl::linalg::solve(mtl4_matrix, mtl4_rhs, viennacl::linalg::cg_tag());

  mtl4_residual = mtl4_matrix * mtl4_result - mtl4_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(mtl4_residual) / viennacl::linalg::norm_2(mtl4_rhs) << std::endl;

  /**
  *  Stabilized Bi-Conjugate Gradient (BiCGStab) solver:
  **/
  std::cout << "----- Running BiCGStab -----" << std::endl;
  mtl4_result = viennacl::linalg::solve(mtl4_matrix, mtl4_rhs, viennacl::linalg::bicgstab_tag());

  mtl4_residual = mtl4_matrix * mtl4_result - mtl4_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(mtl4_residual) / viennacl::linalg::norm_2(mtl4_rhs) << std::endl;

  /**
  *  Generalized Minimum Residual (GMRES) solver:
  **/
  std::cout << "----- Running GMRES -----" << std::endl;
  mtl4_result = viennacl::linalg::solve(mtl4_matrix, mtl4_rhs, viennacl::linalg::gmres_tag());

  mtl4_residual = mtl4_matrix * mtl4_result - mtl4_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(mtl4_residual) / viennacl::linalg::norm_2(mtl4_rhs) << std::endl;

  /**
  *   That's it. Print a success message and exit.
  **/
  std::cout << std::endl;
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  std::cout << std::endl;
}


#define BOOST_TEST_MODULE "test_potential_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/local/CosinePotential.hpp>
#include <mjolnir/math/constants.hpp>

BOOST_AUTO_TEST_CASE(CosinePotential_double)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();
    const real_type    k = 2.0;
    const std::int32_t n = 2;
    const real_type   v0 = 3.0;

    mjolnir::CosinePotential<real_type> potential(k, n, v0);

    const real_type x_min = -pi;
    const real_type x_max =  pi;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + dx * i;
        const real_type pot1 = potential.potential(x + h);
        const real_type pot2 = potential.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = potential.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE(CosinePotential_float)
{
    using real_type = float;
    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();
    const real_type    k = 2.0;
    const std::int32_t n = 2;
    const real_type   v0 = 3.0;

    mjolnir::CosinePotential<real_type> potential(k, n, v0);

    const real_type x_min = -2 * pi;
    const real_type x_max =  2 * pi;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + dx * i;
        const real_type pot1 = potential.potential(x + h);
        const real_type pot2 = potential.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = potential.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}


#include <iostream>
#include <sstream>
#include <assert.h>
#include <math.h>
#include <cmath>
#include <iomanip>
#include "../flt.hpp"
#include <boost/algorithm/string.hpp>

using namespace epilog::pow;

static void header( const std::string &str )
{
    std::cout << "\n";
    std::cout << "--- [" + str + "] " + std::string(60 - str.length(), '-') << "\n";
    std::cout << "\n";
}

static bool tol_check(double d, flt1648 v, double tol=0.00001)
{
    double vd = v.to_double();
    assert((d < 0) == (vd < 0));
    if (vd == d) {
	return true;
    }
    double rel = fabs(0.5 - fabs(vd) / (fabs(d)+fabs(vd)));
    assert(rel < tol);
    return true;
}

const size_t N = 10;
const double db[N] = { 2.3e-7, 2.134e+10, -5.91352e+5, 2.12345e-3, 2e+256,
		       123.456e-127, 42e-42, -99e+42, -5e-17, -4.321e+19 };
flt1648 fl[N];

class next_count {
public:
    friend std::ostream & operator << (std::ostream &out, const next_count &nc);
};

static int cnt = 0;

inline std::ostream & operator << (std::ostream &out, const next_count &nc) {
    cnt++;
    out << "[" << cnt << "]: ";
    return out;
}

static void test_flt_init()
{
    std::cout.precision(17);

    for (size_t i = 0; i < N; i++) {
	int exp = 0;
	double frac = frexp(db[i], &exp);
	fl[i] = flt1648( exp, fxp1648(frac));
	std::cout << next_count() << "db[" << i << "]=" << db[i] << std::endl;
	std::cout << next_count() << "fl[" << i << "]=" << fl[i] << std::endl;
	tol_check(db[i], fl[i]);
    }
}

static void test_flt_add()
{
    header("test_flt_add");

    for (size_t i = 0; i < N; i++) {
	for (size_t j = 0; j < N; j++) {
	    double ad = db[i], bd = db[j];
	    flt1648 af = fl[i], bf = fl[j];
	    std::cout << next_count() << ad << "+" << bd << "=" << (ad+bd) << std::endl;
	    std::cout << next_count() << af << "+" << bf << "=" << (af+bf) << std::endl;
	    tol_check((ad+bd), (af+bf));
	}
    }
}

static void test_flt_sub()
{
    header("test_flt_sub");

    for (size_t i = 0; i < N; i++) {
	for (size_t j = 0; j < N; j++) {
	    double ad = db[i], bd = db[j];
	    flt1648 af = fl[i], bf = fl[j];
	    std::cout << next_count() << ad << "-" << bd << "=" << (ad-bd) << std::endl;
	    std::cout << next_count() << af << "-" << bf << "=" << (af-bf) << std::endl;
	    tol_check((ad-bd), (af-bf));
	}
    }
}

static void test_flt_mul()
{
    header("test_flt_mul");

    for (size_t i = 0; i < N; i++) {
	for (size_t j = 0; j < N; j++) {
	    double ad = db[i], bd = db[j];
	    flt1648 af = fl[i], bf = fl[j];
	    std::cout << next_count() << ad << "*" << bd << "=" << (ad*bd) << std::endl;
	    std::cout << next_count() << af << "*" << bf << "=" << (af*bf) << std::endl;
	    tol_check((ad*bd), (af*bf));
	}
    }
}

static void test_flt_div()
{
    header("test_flt_div");

    for (size_t i = 0; i < N; i++) {
	for (size_t j = 0; j < N; j++) {
	    double ad = db[i], bd = db[j];
	    flt1648 af = fl[i], bf = fl[j];
	    std::cout << next_count() << ad << "/" << bd << "=" << (ad/bd) << std::endl;
	    std::cout << next_count() << af << "/" << bf << "=" << (af/bf) << std::endl;
	    tol_check((ad/bd), (af/bf));
	}
    }
}

static void test_flt_reciprocal()
{
    header("test_flt_reciprocal");

    for (size_t i = 0; i < N; i++) {
	double ad = db[i];
	flt1648 af = fl[i];
	double ad_reciprocal = 1.0 / ad;
	flt1648 af_reciprocal = af.reciprocal();
	std::cout << next_count() << ad << " reciprocal=" << ad_reciprocal << std::endl;
	std::cout << next_count() << af << " reciprocal=" << af_reciprocal << std::endl;
	tol_check(ad_reciprocal, af_reciprocal);
    }
}

static void test_flt_values()
{
    header("test_flt_values");

    auto v = flt1648::from(12, 1234567);

    std::cout << "VALUE: " << v.to_double() << std::endl;

    tol_check(12.1234567, v);
}

static void test_bitcoin_difficulty()
{
    header("test_bitcoin_difficulty");

    auto base_value = flt1648(0x0404cb);
    auto mult_value = flt1648(1) << (8*(0x1b - 3));
    auto target_value = base_value * mult_value;
    std::string expect = "0x00000000000404CB000000000000000000000000000000000000000000000000";
    auto target = "0x" + boost::to_upper_copy(target_value.to_integer_string(32));
    std::cout << "TARGET: " << target << std::endl;
    std::cout << "EXPECT: " << expect << std::endl;
    assert( expect == target );

    auto max_value = flt1648(0x00ffff) * (flt1648(1) << (8*(0x1d - 3)));
    auto difficulty = max_value / target_value;
    std::cout << "Difficulty: " << difficulty.to_double() << std::endl;
    std::cout << "Expect    : 16307.42..." << std::endl;

    assert(abs(difficulty.to_double() - 16307.42) < 0.01);
}

static void test_our_difficulty()
{
    header("test_out_difficulty");

    auto base_value = flt1648(16307) << 32;
    auto target_value = flt1648(1) / base_value;
    auto difficulty = flt1648(1) - target_value;

    std::cout << "DIFFICULTY: " << difficulty.to_double() << std::endl;

    auto rel_target = flt1648(1) - difficulty;

    std::cout << "REL TARGET: " << rel_target.to_double() << std::endl;

    auto max_target = flt1648(1) << 256;
    auto target = rel_target * max_target;

    std::cout << "TARGET    : " << target.to_double() << std::endl;

    std::cout << "TARGET INT: 0x" << target.to_integer_string(32) << std::endl;
}

int main(int argc, char *argv[])
{
    test_flt_init();
    test_flt_add();
    test_flt_sub();
    test_flt_mul();
    test_flt_div();
    test_flt_reciprocal();
    test_flt_values();
    test_bitcoin_difficulty();
    test_our_difficulty();

    return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Array3d v(M_PI, M_PI/2, M_PI/3);
cout << v.cos() << endl;

  return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/utility.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/operation/svd.hpp>

using namespace std;
int main(int, char**)
{
    using namespace mtl;
    unsigned size=3, row= size, col=size+1;

    double normA(0), tol(0.0000001);
    dense_vector<double>                    vec(size), vec1(size);
    dense2D<double>                         A(row, col), A_t(row, col), S(row, row), V(row, col), D(col,col), norm(row, col),
					    AT(col,row), A_tT(col,row), ST(col, col), VT(col,row), DT(row,row), normT(col, row);
    A= 0;

    A[0][0]=1;    A[0][1]=1;    A[0][2]=1;
    A[1][0]=1;    A[1][1]=2;    A[1][2]=2;
    A[2][0]=9;    A[2][1]=3;    A[2][2]=2;
    A[2][3]=4;    A[0][3]=4;    A[1][3]=3;
    std::cout<<"A=\n"<< A <<"\n";
    AT= trans(A);
    std::cout<<"START--------------\n";

    boost::tie(S, V, D)= svd(A, tol);
    std::cout<<"MAtrix  S=\n"<< S <<"\n";
    std::cout<<"MAtrix  V=\n"<< V <<"\n";
    std::cout<<"MAtrix  D=\n"<< D <<"\n";
    A_t= S*V*trans(D);
    std::cout<<"MAtrix  A=S*V*D'=\n"<< A_t <<"\n";
    std::cout<<"Original A==\n"<< A <<"\n";
    norm= A_t - A;
    normA= one_norm(norm);
    std::cout<< "norm(SVD-A)=" << normA << "\n";
//     if (normA > size*size*tol) throw mtl::logic_error("wrong SVD decomposition of matrix A");
    std::cout<<"START--------------\n";
#if 1
    boost::tie(ST, VT, DT)= svd(AT, tol);
    std::cout<<"MAtrix  ST=\n"<< ST <<"\n";
    std::cout<<"MAtrix  VT=\n"<< VT <<"\n";
    std::cout<<"MAtrix  DT=\n"<< DT <<"\n";
    A_tT= ST*VT*trans(DT);
    std::cout<<"MAtrix  AT=S*V*D'=\n"<< A_tT <<"\n";
    std::cout<<"Original A==\n"<< AT <<"\n";
    normT= A_tT - AT;
    normA= one_norm(normT);
    std::cout<< "norm(SVD-A)=" << normA << "\n";
#endif
    return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace std;

    double aa[2][2]= {{1., 2.},
		      {3., 4.}};
    mtl::dense2D<double> A(aa), B(2,2);
    B = A*A;

    cout << (A*A) << endl << B << endl;

    MTL_THROW_IF(B(0, 1) != (A*A)(0,1), mtl::runtime_error("Wrong value in matrix product expression!\n"));

    return 0;
}


/* Boost test/pi.cpp
 * test if the pi constant is correctly defined
 *
 * Copyright Guillaume Melquiond, Sylvain Pion 2002-2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors nor Polytechnic University make any
 * representation about the suitability of this software for any
 * purpose. It is provided "as is" without express or implied warranty.
 *
 * $Id: pi.cpp,v 1.3 2003/02/05 17:34:36 gmelquio Exp $
 */

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

#define PI 3.14159265358979323846

typedef boost::numeric::interval<int>         I_i;
typedef boost::numeric::interval<float>       I_f;
typedef boost::numeric::interval<double>      I_d;
typedef boost::numeric::interval<long double> I_ld;

using boost::numeric::interval_lib::pi;
using boost::numeric::interval_lib::pi_half;
using boost::numeric::interval_lib::pi_twice;

int test_main(int, char *[]) {
  I_i  pi_i  = pi<I_i>();
  I_f  pi_f  = pi<I_f>();
  I_d  pi_d  = pi<I_d>();
  I_ld pi_ld = pi<I_ld>();

  BOOST_TEST(in((int)   PI, pi_i));
  BOOST_TEST(in((float) PI, pi_f));
  BOOST_TEST(in((double)PI, pi_d));
  BOOST_TEST(subset(pi_i, widen(I_i((int)   PI), 1)));
  BOOST_TEST(subset(pi_f, widen(I_f((float) PI), std::numeric_limits<float> ::min())));
  BOOST_TEST(subset(pi_d, widen(I_d((double)PI), std::numeric_limits<double>::min())));

  // We can't test the following equalities for interval<int>.
  I_f pi_f_half = pi_half<I_f>();
  I_f pi_f_twice = pi_twice<I_f>();

  I_d pi_d_half = pi_half<I_d>();
  I_d pi_d_twice = pi_twice<I_d>();

  I_ld pi_ld_half = pi_half<I_ld>();
  I_ld pi_ld_twice = pi_twice<I_ld>();

  BOOST_TEST(equal(2.0f * pi_f_half, pi_f));
  BOOST_TEST(equal(2.0  * pi_d_half, pi_d));
  BOOST_TEST(equal(2.0l * pi_ld_half, pi_ld));

  BOOST_TEST(equal(2.0f * pi_f, pi_f_twice));
  BOOST_TEST(equal(2.0  * pi_d, pi_d_twice));
  BOOST_TEST(equal(2.0l * pi_ld, pi_ld_twice));

  return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define _USE_MATH_DEFINES
#include <algorithm>
#include <iostream>
#include <vector>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>

#include "perf.hpp"

namespace compute = boost::compute;

using compute::float2_;

float rand_float()
{
    return (float(rand()) / float(RAND_MAX)) * 1000.f;
}

void serial_cartesian_to_polar(const float *input, size_t n, float *output)
{
    for(size_t i = 0; i < n; i++){
        float x = input[i*2+0];
        float y = input[i*2+1];

        float magnitude = std::sqrt(x*x + y*y);
        float angle = std::atan2(y, x) * 180.f / M_PI;

        output[i*2+0] = magnitude;
        output[i*2+1] = angle;
    }
}

void serial_polar_to_cartesian(const float *input, size_t n, float *output)
{
    for(size_t i = 0; i < n; i++){
        float magnitude = input[i*2+0];
        float angle = input[i*2+1];

        float x = magnitude * cos(angle);
        float y = magnitude * sin(angle);

        output[i*2+0] = x;
        output[i*2+1] = y;
    }
}

// converts from cartesian coordinates (x, y) to polar coordinates (magnitude, angle)
BOOST_COMPUTE_FUNCTION(float2_, cartesian_to_polar, (float2_ p),
{
    float x = p.x;
    float y = p.y;

    float magnitude = sqrt(x*x + y*y);
    float angle = atan2(y, x) * 180.f / M_PI;

    return (float2)(magnitude, angle);
});

// converts from polar coordinates (magnitude, angle) to cartesian coordinates (x, y)
BOOST_COMPUTE_FUNCTION(float2_, polar_to_cartesian, (float2_ p),
{
    float magnitude = p.x;
    float angle = p.y;

    float x = magnitude * cos(angle);
    float y = magnitude * sin(angle);

    return (float2)(x, y)
});

int main(int argc, char *argv[])
{
    perf_parse_args(argc, argv);

    std::cout << "size: " << PERF_N << std::endl;

    // setup context and queue for the default device
    compute::device device = compute::system::default_device();
    compute::context context(device);
    compute::command_queue queue(context, device);
    std::cout << "device: " << device.name() << std::endl;

    // create vector of random numbers on the host
    std::vector<float> host_vector(PERF_N*2);
    std::generate(host_vector.begin(), host_vector.end(), rand_float);

    // create vector on the device and copy the data
    compute::vector<float2_> device_vector(PERF_N, context);
    compute::copy_n(
        reinterpret_cast<float2_ *>(&host_vector[0]),
        PERF_N,
        device_vector.begin(),
        queue
    );

    perf_timer t;
    for(size_t trial = 0; trial < PERF_TRIALS; trial++){
        t.start();
        compute::transform(
            device_vector.begin(),
            device_vector.end(),
            device_vector.begin(),
            cartesian_to_polar,
            queue
        );
        queue.finish();
        t.stop();
    }
    std::cout << "time: " << t.min_time() / 1e6 << " ms" << std::endl;

    // perform saxpy on host
    t.clear();
    for(size_t trial = 0; trial < PERF_TRIALS; trial++){
        t.start();
        serial_cartesian_to_polar(&host_vector[0], PERF_N, &host_vector[0]);
        t.stop();
    }
    std::cout << "host time: " << t.min_time() / 1e6 << " ms" << std::endl;

    std::vector<float> device_data(PERF_N*2);
    compute::copy(
        device_vector.begin(),
        device_vector.end(),
        reinterpret_cast<float2_ *>(&device_data[0]),
        queue
    );

    for(size_t i = 0; i < PERF_N; i++){
        float host_value = host_vector[i];
        float device_value = device_data[i];

        if(std::abs(device_value - host_value) > 1e-3){
            std::cout << "ERROR: "
                      << "value at " << i << " "
                      << "device_value (" << device_value << ") "
                      << "!= "
                      << "host_value (" << host_value << ")"
                      << std::endl;
            return -1;
        }
    }

    return 0;
}


#include <map>
#include <string>
#include <vector>
#include <iostream>
#include <Eigen/Dense>
#include "../include/two_layer_net.h"
#include "../datasets/include/mnist.h"
#include "../matplotlibcpp.h"

using namespace Eigen;
namespace plt = matplotlibcpp;

int main()
{
    using std::cout;
    using std::endl;
    using std::map;
    using std::vector;
    using std::string;
    using namespace MyDL;

    // ハイパーパラメータ
    int num_iters = 3000;
    double learning_rate = 0.05;
    int batch_size = 100;
    int input_size = 28 * 28;
    int hidden_size = 100;
    int output_size = 10;

    // MNISTデータローダ
    MnistEigenDataset mnist(batch_size);

    // 各種変数初期化
    MatrixXd train_X = MatrixXd::Zero(batch_size, input_size);
    MatrixXd train_y = MatrixXd::Zero(batch_size, output_size);
    MatrixXd test_X = MatrixXd::Zero(batch_size, input_size);
    MatrixXd test_y = MatrixXd::Zero(batch_size, output_size);
    bool one_hot_label = true;

    // ネットワーク生成
    TwoLayerNet net(input_size, hidden_size, output_size, 0.01);

    // 最適化用
    map<string, MatrixXd> grads;
    double loss;
    double accuracy;

    // 学習経過プロット用
    vector<double> loss_history(num_iters);
    vector<int> plot_counter(num_iters);
    vector<double> accuracy_history(num_iters / 10);
    vector<int> accuracy_counter(num_iters / 10);

    // 最適化実行
    for (int i = 0; i < num_iters; i++){
        // 次のミニバッチ取得
        mnist.next_train(train_X, train_y, one_hot_label);

        // 勾配計算
        grads = net.gradient(train_X, train_y); // 内部で

        // 勾配更新
        for (auto i = grads.begin(); i != grads.end(); i++){
            net.params[i->first] -= learning_rate * grads[i->first];
        }

        // 損失プロット用
        loss = net.loss(train_X, train_y);
        loss_history[i] = loss;
        plot_counter[i] = i;

        cout << "iteration" << i << " loss: " << loss << endl;

        // 10step毎にaccuracy計測
        if (i % 10 == 0){
            mnist.next_test(test_X, test_y, one_hot_label);
            accuracy = net.accuracy(test_X, test_y);

            cout << "accuracy: " << accuracy << endl;

            accuracy_history[i/10] = accuracy;
            accuracy_counter[i/10] = i / 10;
        }
    }

    // visualize
    plt::title("Loss History");
    plt::plot(plot_counter, loss_history, "b");
    plt::grid(true);
    plt::save("mnist_learning_curve.png");

    plt::cla();
    plt::title("Accuracy History");
    plt::plot(accuracy_counter, accuracy_history, "r");
    plt::grid(true);
    plt::save("mnist_accuracy_plot.png");

    return 0;
}


#include "drake/math/autodiff.h"

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

#include "drake/common/autodiff.h"
#include "drake/common/eigen_types.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/common/test_utilities/expect_no_throw.h"
#include "drake/math/autodiff_gradient.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::AutoDiffScalar;

namespace drake {
namespace math {
namespace {

class AutodiffTest : public ::testing::Test {
 protected:
  typedef Eigen::AutoDiffScalar<VectorXd> Scalar;

  void SetUp() override {
    vec_.resize(2);

    // Set up to evaluate the derivatives at the values v0 and v1.
    vec_[0].value() = v0_;
    vec_[1].value() = v1_;

    // Provide enough room for differentiation with respect to both variables.
    vec_[0].derivatives().resize(2);
    vec_[1].derivatives().resize(2);

    // Herein, the shorthand notation is used: v0 = vec_[0] and v1 = vec_[1].
    // Set partial of v0 with respect to v0 (itself) to 1 (∂v0/∂v0 = 1).
    // Set partial of v0 with respect to v1 to 0 (∂v0/∂v1 = 0).
    vec_[0].derivatives()(0) = 1.0;
    vec_[0].derivatives()(1) = 0.0;

    // Set partial of v1 with respect to v0 to 0 (∂v1/∂v0 = 0).
    // Set partial of v1 with respect to v1 (itself) to 1 (∂v1/∂v1 = 1).
    vec_[1].derivatives()(0) = 0.0;
    vec_[1].derivatives()(1) = 1.0;

    // Do a calculation that is a function of variables v0 and v1.
    output_calculation_ = DoMath(vec_);
  }

  // Do a calculation involving real functions of two real variables. These
  // functions were chosen due to ease of differentiation.
  static VectorX<Scalar> DoMath(const VectorX<Scalar>& v) {
    VectorX<Scalar> output(3);
    // Shorthand notation: Denote v0 = v[0], v1 = v[1].
    // Function 0: y0 = cos(v0) + sin(v0) * cos(v0) / v1
    // Function 1: y1 = sin(v0) + v1.
    // Function 2: y2 = v0^2 + v1^3.
    output[0] = cos(v[0]) + sin(v[0]) * cos(v[0]) / v[1];
    output[1] = sin(v[0]) + v[1];
    output[2] = v[0] * v[0] + v[1] * v[1] * v[1];
    return output;
  }

  VectorX<Scalar> vec_;                 // Array of variables.
  VectorX<Scalar> output_calculation_;  // Functions that depend on variables.

  // Arbitrary values 7 and 9 will be used as test data.
  const double v0_ = 7.0;
  const double v1_ = 9.0;
};

// Tests that ToValueMatrix extracts the values from the autodiff.
TEST_F(AutodiffTest, ToValueMatrix) {
  const VectorXd values = autoDiffToValueMatrix(output_calculation_);
  VectorXd expected(3);
  expected[0] = cos(v0_) + sin(v0_) * cos(v0_) / v1_;
  expected[1] = sin(v0_) + v1_;
  expected[2] = v0_ * v0_ + v1_ * v1_ * v1_;
  EXPECT_TRUE(
      CompareMatrices(expected, values, 1e-10, MatrixCompareType::absolute))
      << values;
}

// Tests that ToGradientMatrix extracts the gradients from the autodiff.
TEST_F(AutodiffTest, ToGradientMatrix) {
  MatrixXd gradients = autoDiffToGradientMatrix(output_calculation_);

  MatrixXd expected(3, 2);
  // Shorthand notation: Denote v0 = vec_[0], v1 = vec_[1].
  // Function 0: y0 = cos(v0) + sin(v0) * cos(v0) / v1
  // Function 1: y1 = sin(v0) + v1.
  // Function 2: y2 = v0^2 + v1^3.
  // Calculate partial derivatives of y0, y1, y2 with respect to v0, v1.
  // ∂y0/∂v0 = -sin(v0) + (cos(v0)^2 - sin(v0)^2) / v1
  expected(0, 0) =
      -sin(v0_) + (cos(v0_) * cos(v0_) - sin(v0_) * sin(v0_)) / v1_;
  // ∂y0/∂v1 = -sin(v0) * cos(v0) / v1^2
  expected(0, 1) = -sin(v0_) * cos(v0_) / (v1_ * v1_);
  // ∂y1/∂v0 = cos(v0).
  expected(1, 0) = cos(v0_);
  // ∂y1/∂v1 = 1.
  expected(1, 1) = 1.0;
  // ∂y2/∂v0 = 2 * v0.
  expected(2, 0) = 2 * v0_;
  // ∂y2/∂v1 = 3 * v1^2.
  expected(2, 1) = 3 * v1_ * v1_;

  EXPECT_TRUE(
      CompareMatrices(expected, gradients, 1e-10, MatrixCompareType::absolute))
      << gradients;
}

GTEST_TEST(AdditionalAutodiffTest, DiscardGradient) {
  // Test the double case:
  Eigen::Matrix2d test = Eigen::Matrix2d::Identity();
  EXPECT_TRUE(CompareMatrices(DiscardGradient(test), test));

  Eigen::MatrixXd test2 = Eigen::Vector3d{1., 2., 3.};
  EXPECT_TRUE(CompareMatrices(DiscardGradient(test2), test2));

  // Test the AutoDiff case
  Vector3<AutoDiffXd> test3 = test2;
  // Note:  Neither of these would compile:
  //   Eigen::Vector3d test3out = test3;
  //   Eigen::Vector3d test3out = test3.cast<double>();
  // (so even compiling is a success).
  Eigen::Vector3d test3out = DiscardGradient(test3);
  EXPECT_TRUE(CompareMatrices(test3out, test2));

  Eigen::Isometry3d test5 = Eigen::Isometry3d::Identity();
  EXPECT_TRUE(CompareMatrices(DiscardGradient(test5).linear(), test5.linear()));
  EXPECT_TRUE(CompareMatrices(DiscardGradient(test5).translation(),
                              test5.translation()));

  Isometry3<AutoDiffXd> test6 = Isometry3<AutoDiffXd>::Identity();
  test6.translate(Vector3<AutoDiffXd>{3., 2., 1.});
  Eigen::Isometry3d test6b = DiscardGradient(test6);
  EXPECT_TRUE(CompareMatrices(test6b.linear(), Eigen::Matrix3d::Identity()));
  EXPECT_TRUE(
      CompareMatrices(test6b.translation(), Eigen::Vector3d{3., 2., 1.}));
}

GTEST_TEST(AdditionalAutodiffTest, DiscardZeroGradient) {
  // Test the double case:
  Eigen::Matrix2d test = Eigen::Matrix2d::Identity();
  DRAKE_EXPECT_NO_THROW(DiscardZeroGradient(test));
  EXPECT_TRUE(CompareMatrices(DiscardZeroGradient(test), test));

  Eigen::MatrixXd test2 = Eigen::Vector3d{1., 2., 3.};
  DRAKE_EXPECT_NO_THROW(DiscardZeroGradient(test2));
  EXPECT_TRUE(CompareMatrices(DiscardZeroGradient(test2), test2));
  // Check that the returned value is a reference to the original data.
  EXPECT_EQ(&DiscardZeroGradient(test2), &test2);

  // Test the AutoDiff case
  Eigen::Matrix<AutoDiffXd, 3, 1> test3 = test2;
  DRAKE_EXPECT_NO_THROW(DiscardZeroGradient(test3));
  // Note:  Neither of these would compile:
  //   Eigen::Vector3d test3out = test3;
  //   Eigen::Vector3d test3out = test3.cast<double>();
  // (so even compiling is a success).
  Eigen::Vector3d test3out = DiscardZeroGradient(test3);
  EXPECT_TRUE(CompareMatrices(test3out, test2));
  test3 =
      initializeAutoDiffGivenGradientMatrix(test2, Eigen::MatrixXd::Zero(3, 2));
  EXPECT_TRUE(CompareMatrices(DiscardZeroGradient(test3), test2));
  test3 =
      initializeAutoDiffGivenGradientMatrix(test2, Eigen::MatrixXd::Ones(3, 2));
  EXPECT_THROW(DiscardZeroGradient(test3), std::runtime_error);
  DRAKE_EXPECT_NO_THROW(DiscardZeroGradient(test3, 2.));

  Eigen::Isometry3d test5 = Eigen::Isometry3d::Identity();
  DRAKE_EXPECT_NO_THROW(DiscardZeroGradient(test5));
  EXPECT_TRUE(
      CompareMatrices(DiscardZeroGradient(test5).linear(), test5.linear()));
  EXPECT_TRUE(CompareMatrices(DiscardZeroGradient(test5).translation(),
                              test5.translation()));
  // Check that the returned value is a reference to the original data.
  EXPECT_EQ(&DiscardZeroGradient(test5), &test5);

  Isometry3<AutoDiffXd> test6 = Isometry3<AutoDiffXd>::Identity();
  test6.translate(Vector3<AutoDiffXd>{3., 2., 1.});
  DRAKE_EXPECT_NO_THROW(DiscardZeroGradient(test5));
  Eigen::Isometry3d test6b = DiscardZeroGradient(test6);
  EXPECT_TRUE(CompareMatrices(test6b.linear(), Eigen::Matrix3d::Identity()));
  EXPECT_TRUE(
      CompareMatrices(test6b.translation(), Eigen::Vector3d{3., 2., 1.}));
  test6.linear()(0, 0).derivatives() = Vector3d{1., 2., 3.};

  EXPECT_THROW(DiscardZeroGradient(test6), std::runtime_error);
}

// Make sure that casting to autodiff always results in zero gradients.
GTEST_TEST(AdditionalAutodiffTest, CastToAutoDiff) {
  Vector2<AutoDiffXd> dynamic = Vector2d::Ones().cast<AutoDiffXd>();
  const auto dynamic_gradients = autoDiffToGradientMatrix(dynamic);
  EXPECT_EQ(dynamic_gradients.rows(), 2);
  EXPECT_EQ(dynamic_gradients.cols(), 0);

  using VectorUpTo16d = Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 16, 1>;
  using AutoDiffUpTo16d = Eigen::AutoDiffScalar<VectorUpTo16d>;
  Vector2<AutoDiffUpTo16d> dynamic_max =
      Vector2d::Ones().cast<AutoDiffUpTo16d>();
  const auto dynamic_max_gradients = autoDiffToGradientMatrix(dynamic_max);
  EXPECT_EQ(dynamic_max_gradients.rows(), 2);
  EXPECT_EQ(dynamic_max_gradients.cols(), 0);

  Vector2<AutoDiffScalar<Vector3d>> fixed =
      Vector2d::Ones().cast<AutoDiffScalar<Vector3d>>();
  const auto fixed_gradients = autoDiffToGradientMatrix(fixed);
  EXPECT_EQ(fixed_gradients.rows(), 2);
  EXPECT_EQ(fixed_gradients.cols(), 3);
  EXPECT_TRUE(fixed_gradients.isZero(0.));
}

}  // namespace
}  // namespace math
}  // namespace drake


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template <typename Matrix>
void setup(Matrix& A)
{
    const int n= int(num_rows(A));
    A= 1.0;
    mtl::mat::inserter<Matrix, mtl::update_plus<double> > ins(A);

    for (int i= 0; i < 2 * n; i++) {
	int r= rand()%n, c= rand()%n;
	ins[r][c] << -1;
	ins[r][r] << 1;
    }
}

template <typename At, typename Lt, typename Ut>
void dense_ilu_0(const At& As, const Lt& Ls, const Ut& Us)
{
    mtl::dense2D<double> LU(As);

    const int n= int(num_rows(LU));
    for (int i= 1; i < n; i++)
	for (int k= 0; k < i; k++) {
	    LU[i][k]/= LU[k][k];
	    for (int j= k + 1; j < n; j++)
		if (LU[i][j] != 0)
		    LU[i][j]-= LU[i][k] * LU[k][j];
	}
    std::cout << "Factorizing A = \n" << As << "-> LU = \n" << LU;
    // std::cout << "L = \n" << Ls << "\nU = \n" << Us;

    MTL_THROW_IF(std::abs(LU[2][1] - Ls[2][1]) > 0.001, mtl::runtime_error("Wrong value in L for sparse ILU(0) factorization"));

    MTL_THROW_IF(std::abs(LU[2][2] - 1. / Us[2][2]) > 0.001, mtl::runtime_error("Wrong value in U for sparse ILU(0) factorization"));
}

int main(int, char**)
{
    // For a more realistic example set sz to 1000 or larger
    const int N = 3;

    typedef mtl::compressed2D<double>  matrix_type;
    typedef mtl::dense_vector<double>  vector_type;
    mtl::compressed2D<double>          A(N, N);
    setup(A);

    itl::pc::ilu_0<matrix_type>        P(A);

    if(N < 11)
	dense_ilu_0(A, P.get_L(), P.get_U());

    mtl::dense_vector<double> x(N, 3.0), x2(N), Px(N), x3(N), x4(N), x5(N);

    matrix_type L(P.get_L()), U(P.get_U()), UT(trans(U));

    std::cout << "L is\n" << L << "U is \n" << U;

    x2= strict_upper(U) * x;
    for (int i= 0; i < N; i++)
	x2[i]+= 1. / U[i][i] * x[i];
    std::cout << "U*x = " << x2 << "\n";

    Px= L * x2 + x2;
    std::cout << "P*x = (L+I)*U*x = " << Px << "\n";

    x4= unit_lower_trisolve(L, Px);
    std::cout << "L^{-1} * Px = " << x4 << "\n";

    MTL_THROW_IF(two_norm(vector_type(x4 - x2)) > 0.01, mtl::runtime_error("Error in unit_lower_trisolve."));

    x5= inverse_upper_trisolve(U, x4);
    std::cout << "U^{-1} * L^{-1} * Px = " << x5 << "\n";

    MTL_THROW_IF(two_norm(vector_type(x5 - x)) > 0.01, mtl::runtime_error("Error in inverse_upper_trisolve."));

    x3= solve(P, Px);
    std::cout << "solve(P, Px) = " << x3 << "\n";
    MTL_THROW_IF(two_norm(vector_type(x3 - x)) > 0.01, mtl::runtime_error("Error in solve."));

    // Now test adjoint solve
    x2= trans(L) * x + x;
    std::cout << "\n\nNow test adjoint solve\n(L+I)^T*x = " << x2 << "\n";

    //Px= trans(strict_upper(U)) * x2;
    Px= strict_lower(UT) * x2;
    for (int i= 0; i < N; i++)
	Px[i]+= 1. / U[i][i] * x2[i];
    std::cout << "P^T*x = ((L+I)*U)^T*x = " << Px << "\n";

    x4= inverse_lower_trisolve(adjoint(U), Px);
    std::cout << "U^{-T} * Px = " << x4 << "\n";

    MTL_THROW_IF(two_norm(vector_type(x4 - x2)) > 0.01, mtl::runtime_error("Error in inverse_lower_trisolve."));

    x5= unit_upper_trisolve(adjoint(L), x4);
    std::cout << "L^{-T} * U^{-T} * Px = " << x5 << "\n";
    MTL_THROW_IF(two_norm(vector_type(x5 - x)) > 0.01, mtl::runtime_error("Error in unit_upper_trisolve."));

    x3= adjoint_solve(P, Px);
    std::cout << "adjoint_solve(P, Px) = " << x3 << "\n";
    MTL_THROW_IF(two_norm(vector_type(x3 - x)) > 0.01, mtl::runtime_error("Error in adjoint_solve."));

#if 0
    mtl::compressed2D<double>          A2;
    laplacian_setup(A2, 3, 3);

    itl::pc::ilu_0<matrix_type, float>  P2(A2);
    vector_type  xiota(9), y, yc(9);
    iota(xiota);

    std::cout << "Halloooooo" << xiota << '\n';
    y= solve(P, xiota);
    std::cout << y << '\n';
#endif

    return 0;
}


#include <iterator>
#include <string>
#include <ctime>
#include <vector>
#include <iostream>
#include <fstream>
#include <boost/assign/std/vector.hpp>
#include <boost/random.hpp>
#include <boost/assert.hpp>
#include <boost/numeric/ublas/io.hpp>
#include<iostream>
#include "expm.hpp"
using namespace boost::numeric::ublas;
using namespace std;
int main(void)
{
	matrix<complex<double> > mat(3,3);
	matrix<complex<double> > gen(3,3);  // Generator of rotaion around z aix in group theory
	complex<double> img = std::complex<double>(0,1);
	gen(0,0) = 0  ; gen(0,1) = -img; gen(0,2) = 0;
	gen(1,0) = img; gen(1,1) = 0   ; gen(1,2) = 0;
	gen(2,0) = 0  ; gen(2,1) = 0   ; gen(2,2) = 0;

	double theta = 1.5;
	mat = img * theta * gen;
	cout<< "Rotation Matrix : "<< expm_pad(gen) <<"\n\n";
	return 0;
}