/*******************************************************************************
*
*  Filename    : KeysPlot.cc
*  Description : Implementation for Keys Comparison plots
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/Common/interface/PlotStyle.hpp"
#include "TstarAnalysis/MassRecoCompare/interface/Common.hpp"

#include "ManagerUtils/Maths/interface/RooFitExt.hpp"
#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/PlotUtils/interface/RooFitUtils.hpp"

#include <boost/format.hpp>

using namespace std;

void
MakeKeysPlots( KeysCompMgr* mgr )
{
  TCanvas* c     = mgr::NewCanvas();
  RooPlot* frame = KeysCompMgr::x().frame();

  RooAbsData* dataset = mgr->DataSet( "" );

  TGraph* setplot = mgr::PlotOn(
    frame, dataset,
    RooFit::DrawOption( PGS_DATA ),
    RooFit::Binning( 40, KeysCompMgr::x().getMin(), KeysCompMgr::x().getMax() )// Reducing to 40 bin
    );
  tstar::RemoveDataXBar( (TGraphAsymmErrors*)setplot );

  vector<TGraph*> keyplotlist;
  vector<RooAbsPdf*> pdflist;

  for( const auto& pdfname : mgr->PdfNameList() ){
    if( pdfname.find( "rho" ) != string::npos ){
      pdflist.push_back( mgr->Pdf( pdfname ) );
      keyplotlist.push_back( mgr::PlotOn( frame, mgr->Pdf( pdfname ) ) );
    }
  }

  frame->Draw();

  // Styling
  mgr::SetFrame( frame );
  mgr::DrawCMSLabel( SIMULATION );

  for( size_t i = 0; i < keyplotlist.size(); ++i ){
    keyplotlist.at( i )->SetLineColor( Color_Sequence[i] );
  }

  // Making legend
  TLegend* leg = mgr::NewLegend( 0.55, 0.6 );

  boost::format setfmt( "M_{t*}=%dGeV/c^{2}" );
  const string setentry = boost::str( setfmt % reconamer.GetInput<int>( "mass" ) );
  leg->AddEntry( setplot, setentry.c_str(), "pe" );

  boost::format pdffmt( "Kernel approx (%s)" );

  for( size_t i = 0; i < keyplotlist.size(); ++i ){
    const TGraph* pdfplot = keyplotlist.at( i );
    const RooAbsPdf* pdf  = pdflist.at( i );
    const string pdfentry = str( pdffmt % pdf->GetTitle() );
    leg->AddEntry( pdfplot, pdfentry.c_str(), "l" );
  }

  leg->Draw();

  // Adding additional label
  mgr::LatexMgr latex;
  latex.SetOrigin( PLOT_X_MIN, PLOT_Y_MAX + TEXT_MARGIN/2, BOTTOM_LEFT)
  .WriteLine( reconamer.GetExt<string>( "channel", "Root Name" ) )
  .SetOrigin( PLOT_X_TEXT_MAX, 0.6-TEXT_MARGIN, TOP_RIGHT )
  .WriteLine( mgr->LatexName() );

  // Y axis range settings
  keyplotlist.push_back( setplot );
  const double ymax = mgr::GetYmax( keyplotlist );

  // for Linear scale
  frame->SetMinimum( 0 );
  frame->SetMaximum( ymax * 1.2 );
  mgr::SaveToPDF( c, reconamer.PlotFileName( "keyscomp" ) );

  // for Log scale
  frame->SetMinimum( 0.3 );
  frame->SetMaximum( pow( 10, ( log10( ymax )+0.5 )*1.2 ) );
  c->SetLogy( kTRUE );
  mgr::SaveToPDF( c, reconamer.PlotFileName( "keyscomp", "log" ) );

  delete c;
  delete frame;
}

/******************************************************************************/

void
MakeGaussPlots( KeysCompMgr* mgr )
{
  TCanvas* c     = mgr::NewCanvas();
  RooPlot* frame = KeysCompMgr::x().frame();

  TGraph* setplot = mgr::PlotOn(
    frame,
    mgr->DataSet( "" ),
    RooFit::DrawOption( PGS_DATA ),
    RooFit::Binning( 40, KeysCompMgr::x().getMin(), KeysCompMgr::x().getMax() )// Reducing to 40 bin
    );

  TGraph* keysplot = mgr::PlotOn(
    frame,
    mgr->Pdf( "rho1.36" )
    );

  TGraph* gaussplot = mgr::PlotOn(
    frame,
    mgr->Pdf( "gauss" )
    );

  TGraph* doublegauss = mgr::PlotOn(
    frame,
    mgr->Pdf( "doublegauss" )
    );

  tstar::RemoveDataXBar( (TGraphAsymmErrors*)setplot );

  frame->Draw();

  // Styling
  mgr::SetFrame( frame );
  mgr::DrawCMSLabelOuter( SIMULATION );

  keysplot->SetLineColor( Color_Sequence[0] );
  gaussplot->SetLineColor( Color_Sequence[1] );
  doublegauss->SetLineColor( Color_Sequence[2] );

  const double kskeys   = KSTest( *( mgr->DataSet( "" ) ), *( mgr->Pdf( "rho1.36" ) ), KeysCompMgr::x() );
  const double ksgauss  = KSTest( *( mgr->DataSet( "" ) ), *( mgr->Pdf( "gauss" ) ), KeysCompMgr::x() );
  const double ksdgauss = KSTest( *( mgr->DataSet( "" ) ), *( mgr->Pdf( "doublegauss" ) ), KeysCompMgr::x() );

  // Making legend
  TLegend* leg = mgr::NewLegend( 0.45, 0.6 );
  boost::format setfmt( "Signal (%dGeV/c^{2})" );
  const string setentry = boost::str( setfmt % reconamer.GetInput<int>( "mass" ) );

  boost::format pdffmt( "%s (K=%.3lf)" );
  leg->AddEntry( setplot,     setentry.c_str(),                                               "pe" );
  leg->AddEntry( keysplot,    boost::str( pdffmt % "Kernel approximation" % kskeys ).c_str(), "l" );
  leg->AddEntry( gaussplot,   boost::str( pdffmt % "Gaussian" % ksgauss ).c_str(),            "l" );
  leg->AddEntry( doublegauss, boost::str( pdffmt % "Double Gaussian" % ksdgauss ).c_str(),    "l" );
  leg->Draw();

  // Adding additional label
  mgr::LatexMgr latex;
  latex.SetOrigin( PLOT_X_TEXT_MAX, 0.6-TEXT_MARGIN, TOP_RIGHT )
  .WriteLine( mgr->LatexName() );

  // Y axis range settings
  const double ymax = mgr::GetYmax( setplot, keysplot, gaussplot, doublegauss );

  // for Linear scale
  frame->SetMinimum( 0 );
  frame->SetMaximum( ymax * 1.6 );
  mgr::SaveToPDF( c, reconamer.PlotFileName( "gausscomp" ) );

  // for Log scale
  frame->SetMinimum( 0.3 );
  frame->SetMaximum( pow( 10, ( log10( ymax )+0.5 )*1.2 ) );
  c->SetLogy( kTRUE );
  mgr::SaveToPDF( c, reconamer.PlotFileName( "guasscomp", "log" ) );

  delete c;
  delete frame;

}

/******************************************************************************/

void
MakeConvPlots( KeysCompMgr* mgr )
{
  TCanvas* c     = mgr::NewCanvas();
  RooPlot* frame = KeysCompMgr::x().frame();

  TGraph* setplot = mgr::PlotOn(
    frame,
    mgr->DataSet( "" ),
    RooFit::DrawOption( PGS_DATA ),
    RooFit::Binning( 40, KeysCompMgr::x().getMin(), KeysCompMgr::x().getMax() )// Reducing to 40 bin
    );

  TGraph* keysplot = mgr::PlotOn(
    frame,
    mgr->Pdf( "rho1.36" )
    );

  TGraph* convplot = mgr::PlotOn(
    frame,
    mgr->Pdf( "conv" )
    );

  TGraph* conv2plot = mgr::PlotOn(
    frame,
    mgr->Pdf( "convsum2" )
    );

  frame->Draw();

  // Styling
  mgr::SetFrame( frame );
  mgr::DrawCMSLabel( SIMULATION );

  keysplot->SetLineColor( Color_Sequence[0] );
  convplot->SetLineColor( Color_Sequence[1] );
  conv2plot->SetLineColor( Color_Sequence[2] );

  const double kskeys  = KSTest( *( mgr->DataSet( "" ) ), *( mgr->Pdf( "rho1.36" ) ), KeysCompMgr::x() );
  const double ksconv  = KSTest( *( mgr->DataSet( "" ) ), *( mgr->Pdf( "conv" ) ), KeysCompMgr::x() );
  const double ksconv2 = KSTest( *( mgr->DataSet( "" ) ), *( mgr->Pdf( "convsum2" ) ), KeysCompMgr::x() );

  // Making legend
  TLegend* leg = mgr::NewLegend( PLOT_X_TEXT_MIN, 0.65 );
  boost::format setfmt( "Signal (%dGeV/c^{2})" );
  const string setentry = boost::str( setfmt % reconamer.GetInput<int>( "mass" ) );

  boost::format pdffmt( "%s (K=%.3lf)" );
  leg->AddEntry( setplot,   setentry.c_str(),                                                               "lp" );
  leg->AddEntry( keysplot,  boost::str( pdffmt % "Kernel approximation" % kskeys ).c_str(),                 "l" );
  leg->AddEntry( convplot,  boost::str( pdffmt % "Bif.Gaus #otimes Gauss" % ksconv ).c_str(),               "l" );
  leg->AddEntry( conv2plot, boost::str( pdffmt % "(Bif.Gaus #otimes Gauss)+(Bif.Gauss #otimes Gauss)" % ksconv2 ).c_str(), "l" );
  leg->Draw();

  // Adding additional label
  mgr::LatexMgr latex;
  latex.SetOrigin( PLOT_X_TEXT_MAX, 0.6-TEXT_MARGIN, TOP_RIGHT )
  .WriteLine( mgr->LatexName() );

  // Y axis range settings
  const double ymax = mgr::GetYmax( setplot, keysplot, convplot, conv2plot );

  // for Linear scale
  frame->SetMinimum( 0 );
  frame->SetMaximum( ymax * 1.6 );
  mgr::SaveToPDF( c, reconamer.PlotFileName( "convcomp" ) );

  // for Log scale
  frame->SetMinimum( 0.3 );
  frame->SetMaximum( pow( 10, ( log10( ymax )+0.5 )*1.2 ) );
  c->SetLogy( kTRUE );
  mgr::SaveToPDF( c, reconamer.PlotFileName( "convcomp", "log" ) );

  delete c;
  delete frame;

}


#include <atomic>
#include <chrono>
#include <string>
#include "QPanda.h"
#include <algorithm>
#include "gtest/gtest.h"

#include "ThirdParty/rapidjson/writer.h"
#include "ThirdParty/rapidjson/document.h"
#include "ThirdParty/rapidjson/stringbuffer.h"

#include "ThirdParty/Eigen/Dense"
#include "ThirdParty/Eigen/Sparse"
#include <EigenUnsupported/Eigen/KroneckerProduct>

#define CHECK_RUN_TIME_BEGIN                  auto start_time = chrono::system_clock::now()\

#define CHECK_RUN_TIME_END_AND_COUT_SECONDS(argv)   auto final_time = chrono::system_clock::now();\
                                                    auto duration = chrono::duration_cast<chrono::microseconds>(final_time - start_time);\
                                                    std::cout << argv << " -> run_time counts :" << double(duration.count()) * chrono::microseconds::period::num / chrono::microseconds::period::den << std::endl\

using namespace std;
using namespace rapidjson;

constexpr size_t SHOTS = 100;

QCircuit qft_prog(const QVec& qvec)
{
    QCircuit qft = CreateEmptyCircuit();
    for (auto i = 0; i < qvec.size(); i++)
    {
        qft << H(qvec[qvec.size() - 1 - i]);
        for (auto j = i + 1; j < qvec.size(); j++)
        {
            qft << CR(qvec[qvec.size() - 1 - j],
                qvec[qvec.size() - 1 - i], 2 * PI / (1 << (j - i + 1)));
        }
    }
    return qft;
}

QProg ghz_prog(const QVec& q)
{
    auto prog = QProg();
    prog << H(q[0]);

    for (auto i = 0; i < q.size() - 1; ++i)
    {
        prog << CNOT(q[i], q[i + 1]);
    }

    return prog;
}

static std::string build_chain_typed_quantum_chip_config_data(size_t qubit_num)
{
    rapidjson::Document arch_doc;
    arch_doc.SetObject();

    rapidjson::Document::AllocatorType &arch_alloc = arch_doc.GetAllocator();

    rapidjson::Value adj_object(rapidjson::kObjectType);
    for (SizeType qubit_addr = 0; qubit_addr < qubit_num; qubit_addr++)
    {
        rapidjson::Value qubit_array(rapidjson::kArrayType);

        if (qubit_addr != 0)
        {
            rapidjson::Value qubit_value(rapidjson::kObjectType);

            qubit_value.AddMember("v", qubit_addr - 1, arch_alloc);
            qubit_value.AddMember("w", 1.0, arch_alloc);

            qubit_array.PushBack(qubit_value, arch_alloc);
        }

        if (qubit_addr != (qubit_num - 1))
        {
            rapidjson::Value qubit_value(rapidjson::kObjectType);

            qubit_value.AddMember("v", qubit_addr + 1, arch_alloc);
            qubit_value.AddMember("w", 1.0, arch_alloc);

            qubit_array.PushBack(qubit_value, arch_alloc);
        }

        std::string qubit_addr_str = to_string(qubit_addr);

        rapidjson::Value qubit_value(kStringType);
        qubit_value.SetString(qubit_addr_str.c_str(), (rapidjson::SizeType)qubit_addr_str.size(), arch_alloc);

        adj_object.AddMember(qubit_value, qubit_array, arch_alloc);
    }

    arch_doc.AddMember("adj", adj_object, arch_alloc);
    arch_doc.AddMember("QubitCount", (SizeType)qubit_num, arch_alloc);

    //construct final json
    rapidjson::Document doc;
    doc.SetObject();

    rapidjson::Document::AllocatorType &alloc = doc.GetAllocator();

    doc.AddMember("QuantumChipArch", arch_doc, alloc);

    rapidjson::StringBuffer buffer;
    rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
    doc.Accept(writer);

    return buffer.GetString();
}

bool test_matrix_decompose_mps()
{
    auto qvm = initQuantumMachine(QMachineType::CPU);
    auto q = qvm->allocateQubits(2);
    auto c = qvm->allocateCBits(2);

    QCircuit test_cir;
    test_cir << H(q[0]) << H(q[1]) << CNOT(q[0], q[1]) << T(q[1]);
    //test_cir << CNOT(q[0], q[1]);
    QStat stat_0;
    /*{
        auto prog = QProg();
        prog << test_cir;
        qvm->directlyRun(prog);
        stat_0 = qvm->getQState();
        std::cout << "stat_0:\n" << stat << endl;
    }*/
    const QStat target_matrix = getCircuitMatrix(test_cir);
    std::cout << "target_matrix:\n" << target_matrix << endl;

    auto cir = matrix_decompose_qr({ q[0], q[1] }, target_matrix);
    //auto cir = matrix_decompose_householder({ q[0], q[1] }, target_matrix);
    std::cout << "decomposed circuit:" << cir << endl;
    const auto mat_2 = getCircuitMatrix(cir);
    std::cout << "mat_2:\n" << mat_2 << endl;

    auto prog = QProg();
    prog << cir;
    qvm->directlyRun(prog);
    auto stat = qvm->getQState();

    destroyQuantumMachine(qvm);

    if (0 == mat_compare(target_matrix, mat_2, MAX_COMPARE_PRECISION)) {
        return true;
    }

    return false;
}

TEST(MPS, test)
{
    auto machine = initQuantumMachine(CPU);
    machine->setConfigure({ 64,64 });

    auto qv = machine->allocateQubits(36);
    auto cv = machine->allocateCBits(36);

    auto _grover_prog = ghz_prog(qv);
    _grover_prog << MeasureAll(qv, cv);

    cout << convert_qprog_to_originir(_grover_prog, machine);
    getchar();

	test_matrix_decompose_mps();
    return;
#if 1
    try
    {
        QCloudMachine QCM;
        QCM.init("C60FBD87EF084DBA820945D052218AA8", true);
        //QCM.init();

        //QVec qv;
        //std::vector<ClassicalCondition> cv;
        //QProg prog1 = convert_originir_to_qprog("E:\\N4.txt", &QCM, qv, cv);

        QCM.set_qcloud_api("http://10.10.10.197:8060");
        QCM.set_real_chip_api("http://10.10.10.197:8060");

        QVec qv;
        vector<ClassicalCondition> cv;
        auto ir = convert_originir_to_qprog("E:\\N4.txt", &QCM, qv, cv);

        TASK_STATUS status;
        auto result = QCM.full_amplitude_measure(ir,1000);
        cout << 123 << endl;
        /* TASK_STATUS status;
         auto taskid = QCM.get_expectation_commit(prog, hamiltonian, q, status);
         if (status == TASK_STATUS::FAILED)
         {
             cout << QCM.get_last_error() << endl;
         }
         else
         {
             auto result = QCM.get_expectation_exec(taskid, status);
             if (status == TASK_STATUS::FAILED)
             {
                 cout << QCM.get_last_error() << endl;
             }
             else
             {
                 auto result = QCM.get_expectation_exec(taskid, status);
                 std::cout << result << std::endl;
             }
         }*/

        QCM.finalize();
    }
    catch (const std::exception e)
    {
        cout << e.what() << endl;
    }
    catch (...)
    {
        cout << 123 << endl;
    }

    //QProg measure_prog;
    //measure_prog << HadamardQCircuit(q) << MeasureAll(q, c);
    //auto result2 = QCM.get_state_tomography_density(measure_prog, 1000, REAL_CHIP_TYPE::ORIGIN_WUYUAN_D4);
    //for (auto val : result2)
    //{
    //    cout << val << endl;
    //}

    //auto result3 = QCM.get_state_fidelity(measure_prog, 1000, REAL_CHIP_TYPE::ORIGIN_WUYUAN_D4);
    //cout << result3 << endl;

    return;
#endif
}


/**
 * DD cut generation for CPLEX
 */

#include <cassert>
#include <queue>
#include <stack>
#include <boost/unordered_set.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/any.hpp>
#include "cut_cplex.hpp"
#include "flow_decomp.hpp"
#include "../bdd/bdd.hpp"
#include "../util/graph.hpp"
#include "../util/stats.hpp"

using boost::any_cast;
using boost::unordered_set;

/**
 * Generates a cut for the set of points represented by the given BDD towards the direction of the given point.
 * All input must be in layer space. Store additional information in cut_info unless it is NULL (default).
 */
Inequality* generate_bdd_inequality(BDD* bdd, const vector<double>& x, const vector<double>& interior_point, Options* options,
                                    CutInfo* cut_info /* = NULL */)
{
	Inequality* facet;
	int nvars = bdd->layers.size() - 1;

	Stats stats;
	stats.register_name("cut-time");
	stats.start_timer(0);
	try {

		IloEnv env;
		IloModel model(env);

		IloNumVarArray u(env, nvars);
		IloArray<IloNumVarArray> v(env, nvars+1);

		// Create variables
		for (int i = 0; i < nvars; ++i) {
			u[i] = IloNumVar(env, -IloInfinity, IloInfinity, IloNumVar::Float, ("u" + to_string(i)).c_str());
		}

		for (int i = 0; i < nvars + 1; ++i) {
			int size = bdd->layers[i].size();
			v[i] = IloNumVarArray(env, size);
			for (int j = 0; j < size; ++j) {
				v[i][j] = IloNumVar(env, -IloInfinity, IloInfinity, IloNumVar::Float,
				                    ("v" + to_string(i) + "," + to_string(j)).c_str());
			}
		}

		// Define objective
		IloObjective obj = IloAdd(model, IloMaximize(env));
		for (int i = 0; i < nvars; ++i) {
			obj.setLinearCoef(u[i], x[i] - interior_point[i]);
		}

		// Initialize constraint arrays
		IloArray<IloRangeArray> zero_arc_constrs(env, nvars + 1);
		IloArray<IloRangeArray> one_arc_constrs(env, nvars + 1);
		for (int i = 0; i < nvars + 1; ++i) {
			int size = bdd->layers[i].size();
			zero_arc_constrs[i] = IloRangeArray(env, size);
			one_arc_constrs[i] = IloRangeArray(env, size);
		}

		// Add constraints
		for (int i = 0; i < nvars; ++i) {
			int size = bdd->layers[i].size();
			for (int j = 0; j < size; ++j) {
				Node* zero_node = bdd->layers[i][j]->zero_arc;
				Node* one_node = bdd->layers[i][j]->one_arc;

				if (zero_node != NULL) {
					// cout << i << "," << j << " / " << zero_node->layer << "," << zero_node->id << endl;
					// model.add( v[zero_node->layer][zero_node->id] <= v[i][j] );
					zero_arc_constrs[i][j] = IloRange(env, v[zero_node->layer][zero_node->id] - v[i][j], 0,
					                                  ("a0_" + to_string(i) + "," + to_string(j)).c_str());
					model.add(zero_arc_constrs[i][j]);
				}
				if (one_node != NULL) {
					// cout << i << "," << j << " / " << one_node->layer << "," << one_node->id << endl;
					// model.add( v[one_node->layer][one_node->id] <= v[i][j] - u[i] );
					one_arc_constrs[i][j] = IloRange(env, v[one_node->layer][one_node->id] - v[i][j] + u[i], 0,
					                                 ("a1_" + to_string(i) + "," + to_string(j)).c_str());
					model.add(one_arc_constrs[i][j]);
				}
			}
		}

		// v_s = 1 + u^T interior_point
		IloExpr vs(env);
		vs += 1;
		for (int i = 0; i < nvars; ++i) {
			vs += u[i] * interior_point[i];
		}
		assert(bdd->layers[0].size() == 1);
		model.add(v[0][0] == vs);

		// v_t = 0
		assert(bdd->layers[nvars].size() == 1);
		model.add(v[nvars][0] == 0);

		// Create CPLEX object
		IloCplex cplex(model);
		cplex.setParam(IloCplex::Threads, 1);
		cplex.setParam(IloCplex::AggInd, 100); // Important parameter for efficiency; there is often a lot to aggregate
		// cplex.setOut(env.getNullStream()); // Suppress output

		stats.end_timer(0);
		cout << "Time to construct LP model: " << stats.get_time(0) << endl;
		cout << cplex.getNrows() << " rows and " << cplex.getNcols() << " columns" << endl;
		// cplex.exportModel("cutlp.lp");

		// Solve cut LP
		stats.start_timer(0);
		cplex.solve();
		stats.end_timer(0);

		cout << "Time to solve LP: " << stats.get_time(0) << endl;

		double bound = cplex.getObjValue();
		cout << "Polar opt obj: " << bound << endl;

		// // Debugging info, if unbounded
		// cout << "Status: " << cplex.getStatus() << endl;
		// IloNumArray ray_vals(env);
		// IloNumVarArray ray_vars(env);
		// cplex.getRay(ray_vals, ray_vars);
		// cout << "Ray: ";
		// for (int i = 0; i < ray_vars.getSize(); ++i) {
		//   cout << ray_vars[i] << " " << ray_vals[i] << endl;
		// }
		// cout << endl;

		// Restrict to optimal face and perturb to obtain extreme point of the polar
		if (options->cut_perturbation_iterative) {
			perturb_bdd_cut_iterative(env, cplex, model, u, obj, nvars, x, interior_point);
		} else if (options->cut_perturbation_random) {
			perturb_bdd_cut_random(env, cplex, model, u, obj, nvars, x, interior_point);
		}

		vector<double> coeffs(nvars);
		double rhs;

		// Retrieve left-hand side coefficients: u
		for (int i = 0; i < nvars; ++i) {
			if (cplex.isExtracted(u[i])) {
				coeffs[i] = cplex.getValue(u[i]);
			} else {
				// If u[i] not extracted, this means it was not added to model: objective is zero and not added to constraints
				// Could be a bug, but it also may be the valid scenario of the variable being fixed to zero in the DD
				coeffs[i] = 0;
			}
		}

		// Retrieve right-hand side: 1 + u^T interior
		rhs = 1;
		for (int i = 0; i < nvars; ++i) {
			rhs += coeffs[i] * interior_point[i];
		}

		// Inequality u^T x <= 1 + u^T interior
		facet = new Inequality();
		facet->coeffs = coeffs;
		facet->rhs = rhs;

		// cout << "Relaxation facet generated (normalized), BDD order: ";
		// for( int i = 0; i < nvars; ++i ) {
		//   cout << facet->coeffs[i] / facet->rhs << " ";
		// }
		// cout << "<= 1" << endl;
		// cout << "Relaxation facet generated, BDD order: ";
		// for( int i = 0; i < nvars; ++i ) {
		//   cout << facet->coeffs[i] << " ";
		// }
		// cout << "<= " << facet->rhs << endl;

		double lhs = 0;
		for (int i = 0; i < nvars; ++i) {
			lhs += facet->coeffs[i] * x[i];
			// cout << "coeff = " << facet->coeffs[i] << ", x = " << x[i] << endl;
		}
		cout << "LHS = " << lhs << " / Violation: " << lhs - facet->rhs << endl;

		cout << "Distance = " << get_distance_hyperplane_point(facet, x) << endl;

		// // Debugging info: Assert complementary slackness
		// int n_tight = 0;
		// int n_nzdual = 0;
		// for (int i = 0; i < nvars; ++i) {
		//  	int size = bdd->layers[i].size();
		//  	// cout << "u[" << i << "] = " << cplex.getValue(u[i]) << endl;
		//  	for (int j = 0; j < size; ++j) {
		//  		Node* zero_node = bdd->layers[i][j]->zero_arc;
		//  		Node* one_node = bdd->layers[i][j]->one_arc;
		//  		if (zero_node != NULL) {
		//  			cout << "Dual (" << i << "," << j << ") -0-> (" << zero_node->layer << "," << zero_node->id << "): " << cplex.getDual(zero_arc_constrs[i][j]);
		//  			cout << " / slack: " << cplex.getValue(v[zero_node->layer][zero_node->id]) - cplex.getValue(v[i][j]) << endl;
		//  			assert(DBL_EQ(cplex.getDual(zero_arc_constrs[i][j]), 0) || DBL_EQ(cplex.getValue(v[zero_node->layer][zero_node->id]), cplex.getValue(v[i][j])));
		//  			if (DBL_EQ(cplex.getValue(v[zero_node->layer][zero_node->id]), cplex.getValue(v[i][j])))
		//  				n_tight++;
		//  			if (!DBL_EQ(cplex.getDual(zero_arc_constrs[i][j]), 0))
		//  				n_nzdual++;
		//  		}
		//  		if (one_node != NULL) {
		//  			cout << "Dual (" << i << "," << j << ") -1-> (" << one_node->layer << "," << one_node->id << "): " << cplex.getDual(one_arc_constrs[i][j]);
		//  			cout << " / slack: " << cplex.getValue(v[one_node->layer][one_node->id]) - (cplex.getValue(v[i][j]) - cplex.getValue(u[i])) << endl;
		//  			assert(DBL_EQ(cplex.getDual(one_arc_constrs[i][j]), 0) || DBL_EQ(cplex.getValue(v[one_node->layer][one_node->id]), cplex.getValue(v[i][j]) - cplex.getValue(u[i])));
		//  			if (DBL_EQ(cplex.getValue(v[one_node->layer][one_node->id]), cplex.getValue(v[i][j]) - cplex.getValue(u[i])))
		//  				n_tight++;
		//  			if (!DBL_EQ(cplex.getDual(one_arc_constrs[i][j]), 0))
		//  				n_nzdual++;
		//  		}
		//  		// cout << "v[" << i << "][" << j << "] = " << cplex.getValue(v[i][j]) << endl;
		//  	}
		// }
		// // cout << "Tight arcs = " << n_tight << " / Nonzero duals = " << n_nzdual << endl;

		// Store additional information in cut_info
		if (cut_info != NULL) {
			int bdd_size = bdd->layers.size();
			cut_info->zero_arc_flow.clear();
			cut_info->zero_arc_flow.resize(bdd_size);
			cut_info->one_arc_flow.clear();
			cut_info->one_arc_flow.resize(bdd_size);
			for (int layer = 0; layer < bdd_size; ++layer) {
				int size = bdd->layers[layer].size();
				for (int k = 0; k < size; ++k) {
					Node* node = bdd->layers[layer][k];
					double zero_flow = 0;
					if (node->zero_arc != NULL) {
						zero_flow = cplex.getDual(zero_arc_constrs[layer][k]);
					}
					double one_flow = 0;
					if (node->one_arc != NULL) {
						one_flow = cplex.getDual(one_arc_constrs[layer][k]);
					}
					cut_info->zero_arc_flow[layer].push_back(zero_flow);
					cut_info->one_arc_flow[layer].push_back(one_flow);
				}
			}

			// // Uncomment for debugging purposes
			// print_all_paths_in_flow(bdd, cut_info->zero_arc_flow, cut_info->one_arc_flow);
		}

		obj.end();
		zero_arc_constrs.end();
		one_arc_constrs.end();
		env.end();

	} catch (IloException& ex) {
		cout << "error: " << ex << endl;
		exit(1);
	}

	return facet;
}

void perturb_bdd_cut_iterative(IloEnv env, IloCplex cplex, IloModel model, const IloNumVarArray u, IloObjective obj, int nvars,
	const vector<double>& x, const vector<double>& interior_point)
{
	// cplex.setOut(env.getNullStream()); // Suppress output
	cout << "Before perturbation: ";
	for (int i = 0; i < nvars; ++i) {
		cout << cplex.getValue(u[i]) << " ";
	}
	cout << endl;

	IloExpr obj_expr(env);
	for (int i = 0; i < nvars; ++i) {
		obj_expr += u[i] * (x[i] - interior_point[i]);
	}
	double bound = cplex.getObjValue();
	// model.add( obj_expr == bound );
	model.add(obj_expr <= bound + 1e-5);
	model.add(obj_expr >= bound - 1e-5);
	obj_expr.end();

	for (int k = 0; k < nvars; ++k) {
		for (int i = 0; i < nvars; ++i) {
			obj.setLinearCoef(u[i], 0);
		}
		obj.setLinearCoef(u[k], 1);
		cplex.solve();
		if (k != nvars - 1) {
			cout << "u[" << k << "] = " << cplex.getObjValue() << endl;
			if (DBL_EQ(cplex.getObjValue(), 0)) {
				u[k].setBounds(0, 0);
			} else {
				// u[k].setBounds(cplex.getObjValue(), cplex.getObjValue());
				u[k].setBounds(cplex.getObjValue() - 1e-5, cplex.getObjValue() + 1e-5);
			}
		}
	}

	cout << "After perturbation: ";
	for (int i = 0; i < nvars; ++i) {
		cout << cplex.getValue(u[i]) << " ";
	}
	cout << endl;
}

void perturb_bdd_cut_random(IloEnv env, IloCplex cplex, IloModel model, const IloNumVarArray u, IloObjective obj, int nvars,
	const vector<double>& x, const vector<double>& interior_point)
{
	// Perturb the objective slightly
	cout << "Before perturbation: ";
	for (int i = 0; i < nvars; ++i) {
		cout << cplex.getValue(u[i]) << " ";
	}
	cout << endl;

	IloExpr obj_expr(env);
	for (int i = 0; i < nvars; ++i) {
		obj_expr += u[i] * (x[i] - interior_point[i]);
	}
	double bound = cplex.getObjValue();
	model.add(obj_expr == bound);
	// model.add( obj_expr <= bound + 1e-5 );
	// model.add( obj_expr >= bound - 1e-5 );
	obj_expr.end();

	for (int i = 0; i < nvars; ++i) {
		double pert = ((double) rand() / (double) RAND_MAX - 0.5) * 2 * 1e-4;
		double new_coeff = x[i] - interior_point[i] + pert;
		if (new_coeff < 0) {
			new_coeff = x[i] - interior_point[i] - pert;
		}
		obj.setLinearCoef(u[i], new_coeff);
	}

	cplex.solve();

	cout << "After perturbation: ";
	for (int i = 0; i < nvars; ++i) {
		cout << cplex.getValue(u[i]) << " ";
	}
	cout << endl;
}


#ifndef EXPSUM_REDUCTION_CONEIG_SYM_RRD_HPP
#define EXPSUM_REDUCTION_CONEIG_SYM_RRD_HPP

#include <cassert>

#include <armadillo>

#include "arma/lapack_extra.hpp"

namespace expsum
{

//
// Compute accurate con-eigenvalue decomposition of the matrix $A=XD^{2}X^{H}$
//
// This class computes the con-eigenvalue decomposition
//
// ``` math
//   A U = \Lambda \overline{U},
// ```
//
// of a real symmetric (or complex Hermitian) and positive-definite matrix $A$
// having a rank-revealing decomposition of the form, $A = X D^{2} X^{H}$.
//
// @X  A matrix of type `T` of dimension $m \times n$.
//     On entry, the rank-revealing factor $X$.
//     On exit, orthonormal con-eigenvectors of the matrix $A$.
// @d  A vector of real values with size $n$.
//     On entry, the diagonal elements of rank-revealing factor $D$. The
//     diagonal of $D$ must be all positive and decreasing.
//     On exit, con-eigenvalues of the matrix $A$.
// @threshold threshold value for con-eigenvalues.
//

template <typename T>
class coneig_sym_rrd
{
public:
    // Scalar types
    using size_type    = arma::uword;
    using value_type   = T;
    using real_type    = typename arma::get_pod_type<T>::result;
    using complex_type = std::complex<real_type>;

    // Matrix/Vector types
    using vector_type         = arma::Col<value_type>;
    using matrix_type         = arma::Mat<value_type>;
    using index_vector_type   = arma::uvec;
    using real_vector_type    = arma::Col<real_type>;
    using real_matrix_type    = arma::Mat<real_type>;
    using complex_vector_type = arma::Col<complex_type>;
    using complex_matrix_type = arma::Mat<complex_type>;

    static size_type run(matrix_type& X, real_vector_type& d,
                         real_type threshold, value_type* work,
                         real_type* rwork);

private:
    //
    // Compute QR factorization of matrix G = Q * R.
    //
    static void qr_factorization(matrix_type& G, value_type* work)
    {
        auto n          = static_cast<arma::blas_int>(G.n_rows);
        auto lwork      = n * n;
        value_type* tau = work + n * n;
        arma::blas_int info;
        arma::lapack::geqrf(&n, &n, G.memptr(), &n, tau, work, &lwork, &info);
        if (info)
        {
            std::ostringstream msg;
            msg << "(coneig_sym_rrd) xGEQRF failed with info " << info;
            throw std::runtime_error(msg.str());
        }
    }
    //
    // Solve R * Y = X, where R is upper triangular matrix.
    //
    static void tri_solve(matrix_type& R, matrix_type& X)
    {
        char uplo  = 'U';
        char trans = 'N';
        char diag  = 'N';
        auto m     = static_cast<arma::blas_int>(R.n_rows);
        auto nrhs  = static_cast<arma::blas_int>(X.n_cols);
        arma::blas_int info;
        arma::lapack::trtrs(&uplo, &trans, &diag, &m, &nrhs, R.memptr(), &m,
                            X.memptr(), &m, &info);
        if (info)
        {
            std::ostringstream msg;
            msg << "(coneig_sym_rrd) xTRTRS failed with info " << info;
            throw std::runtime_error(msg.str());
        }
    }
    //
    // Compute singular values and corresponding left singular vectors of upper
    // triangular matrix R using one-sided Jacobi method.
    //
    // --- for real matrix
    static void jacobi_svd(matrix_type& R, real_vector_type& sigma,
                           real_type* work, real_type* /*dummy*/)
    {
        assert(R.n_rows == R.n_cols);
        assert(sigma.n_elem == R.n_cols);

        char joba = 'U'; // Input matrix R is upper triangular matrix
        char jobu = 'U'; // Compute left singular vectors
        char jobv = 'N'; // Do not compute right singular vectors
        auto n    = static_cast<arma::blas_int>(R.n_cols);
        auto mv   = arma::blas_int();
        auto ldv  = arma::blas_int(2);

        real_type dummy_v[2];
        auto lwork = 2 * n;

        arma::blas_int info;

        arma::lapack::gesvj(&joba, &jobu, &jobv, &n, &n, R.memptr(), &n,
                            sigma.memptr(), &mv, &dummy_v[0], &ldv, work,
                            &lwork, &info);

        if (info < arma::blas_int())
        {
            std::ostringstream msg;
            msg << "[s/d]GESVJ error: " << -info
                << " th argument had an illegal value";
            throw std::logic_error(msg.str());
        }

        if (info > arma::blas_int())
        {
            std::ostringstream msg;
            msg << "[s/d]GESVJ did not converge in the maximal allowed number "
                << info << " of sweeps";
            throw std::runtime_error(msg.str());
        }
    }
    // --- for complex matrix
    static void jacobi_svd(matrix_type& R, real_vector_type& sigma,
                           complex_type* work, real_type* rwork)
    {
        assert(R.n_rows == R.n_cols);
        assert(sigma.n_elem == R.n_cols);

        char joba = 'U'; // Input matrix R is upper triangular matrix
        char jobu = 'U'; // Compute left singular vectors
        char jobv = 'N'; // Do not compute right singular vectors
        auto n    = static_cast<arma::blas_int>(R.n_cols);
        auto mv   = arma::blas_int();
        auto ldv  = arma::blas_int(2);

        complex_type dummy_v[2];
        auto lwork  = 2 * n;
        auto lrwork = std::max(arma::blas_int(6), 2 * n);

        arma::blas_int info;

        arma::lapack::gesvj(&joba, &jobu, &jobv, &n, &n, R.memptr(), &n,
                            sigma.memptr(), &mv, &dummy_v[0], &ldv, work,
                            &lwork, rwork, &lrwork, &info);

        if (info < arma::blas_int())
        {
            std::ostringstream msg;
            msg << "[c/z]GESVJ error: " << -info
                << " th argument had an illegal value";
            throw std::logic_error(msg.str());
        }

        if (info > arma::blas_int())
        {
            std::ostringstream msg;
            msg << "[c/z]GESVJ did not converge in the maximal allowed number "
                << info << " of sweeps";
            throw std::runtime_error(msg.str());
        }
    }
};

//
// Required memory for workspace
//
// work size:
//    for matrix G : n * n
//    for matrix Y : n * n
//    for workspace: 2 * n
//    ------------------------------
//    Total        : 2 * n * (n + 1)
//
// rwork size:
//    if `T` is real type   : n
//    if `T` is complex type: 3 * n
//
template <typename T>
typename coneig_sym_rrd<T>::size_type
coneig_sym_rrd<T>::run(matrix_type& X, real_vector_type& d, real_type threshold,
                       value_type* work, real_type* rwork)
{
    // const size_type m = X.n_rows;
    const size_type n = X.n_cols;

    value_type* ptr1 = work;
    value_type* ptr2 = work + n * n;
    value_type* ptr3 = work + 2 * n * n;

    real_vector_type dinv(rwork, n, false, true);
    dinv = real_type(1) / d;

    //
    // Form G = D * (X.st() * X) * D
    //
    matrix_type G(ptr1, n, n, false, true);
    G = X.st() * X;
    for (size_type j = 0; j < n; ++j)
    {
        for (size_type i = 0; i < n; ++i)
        {
            G(i, j) *= d(i) * d(j);
        }
    }
    //
    // Compute G = Q * R by Householder QR factorization
    //
    qr_factorization(G, ptr2);
    matrix_type& R = G; // R = trimatu(G), on exit
    //
    // Compute SVD of R = U * S * V.t() using one-sided Jacobi method.
    // We need singular values and left singular vectors here.
    //
    real_vector_type& sigma = d;
    matrix_type U(ptr2, n, n, false, true);
    U = arma::trimatu(R); // make a copy of matrix R
    jacobi_svd(U, sigma, ptr3, rwork + n);

    //-------------------------------------------------------------------------
    // Truncation: discard con-eigenvalues if negligibly small
    //-------------------------------------------------------------------------
    auto sum_d     = real_type();
    size_type nvec = n;
    while (nvec)
    {
        sum_d += d(nvec - 1);
        if (2 * sum_d > threshold)
        {
            break;
        }
        --nvec;
    }

    if (nvec == size_type(0))
    {
        return size_type();
    }

    //-------------------------------------------------------------------------
    //
    // The eigenvectors of A * conj(A) are given as
    //
    //   conj(X') = X * D * V * S^{-1/2}.
    //
    // However, direct evaluation of eigenvectors with this formula might be
    // inaccurate since D is ill-conditioned. The following formula are used
    // instead.
    //
    //   conj(X') = X * D * R^(-1) * U * S^{1/2}
    //            = X * (D.inv() * R * D.inv())^(-1) * (D.inv() * U * S^{1/2})
    //            = X * R1.inv() * X1
    //
    //-------------------------------------------------------------------------
    //
    // Compute X1 = D^(-1) * U * S^{1/2} [in-place]
    //
    matrix_type U_trunc(ptr2, n, nvec, false, true);
    for (size_type j = 0; j < nvec; ++j)
    {
        const auto sj = std::sqrt(sigma(j));
        for (size_type i = 0; i < n; ++i)
        {
            U_trunc(i, j) *= sj * dinv(i);
        }
    }
    //
    // Compute R1 = D^(-1) * R * D^(-1) [in-place]
    //
    for (size_type j = 0; j < n; ++j)
    {
        for (size_type i = 0; i <= j; ++i)
        {
            R(i, j) *= dinv(i) * dinv(j);
        }
    }
    //
    // Solve R1 * Y1 = X1 in-place
    //
    tri_solve(R, U_trunc);
    //
    // Compute con-eigenvectors U = conj(X) * conj(Y).
    //
    X.head_cols(nvec) = arma::conj(X) * arma::conj(U_trunc);
    //
    // Adjust phase factor of each con-eigenvectors, so that U^{T} * U = I
    //
    if (arma::is_complex<T>::value)
    {
        for (size_type j = 0; j < nvec; ++j)
        {
            auto xj          = X.col(j);
            const auto t     = arma::dot(xj, xj);
            const auto phase = t / std::abs(t);
            const auto scale = std::sqrt(arma::access::alt_conj(phase));
            xj *= scale;
        }
    }

    return nvec;
}

} // namespace: expsum

#endif /* EXPSUM_REDUCTION_CONEIG_SYM_RRD_HPP */


/*
Copyright 2013-2015 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#ifndef MATH_MAX_SEMIRING_HPP_INCLUDED
#define MATH_MAX_SEMIRING_HPP_INCLUDED

#include <limits>
#include <iosfwd>
#include <type_traits>

#include <boost/mpl/bool.hpp>

#include <boost/functional/hash_fwd.hpp>

#include "magma.hpp"

#include "detail/is_close.hpp"

namespace math {

/**
Semiring whose \ref plus (and \ref choose) operation picks the maximum of the
two values.
\ref times performs multiplication on the underlying value.

The underlying value must always be non-negative, so that the additive identity
has value 0.
The multiplicative identity has value 1.

Division is only implemented for non-integer types.

This type supports Boost.Hash, if \c boost/functional/hash.hpp is included.

\tparam Type
    Underlying type that represents the value.
*/
template <class Type> class max_semiring;

template <class Type> struct max_semiring_tag;

template <class Type> struct decayed_magma_tag <max_semiring <Type>>
{ typedef max_semiring_tag <Type> type; };

template <class Type> class max_semiring {
private:
    Type value_;
public:
    /**
    Initialise with 0.
    */
    max_semiring() : value_()
    { assert (value_ >= Type (0)); }

    /**
    Initialise with \a value as the value.
    */
    explicit max_semiring (Type const & value) : value_ (value) {}

    /**
    Return the underlying value.
    */
    Type const & value() const { return value_; }
};

namespace detail {

    template <class Type> struct is_max_semiring_tag : boost::mpl::false_ {};
    template <class Type> struct is_max_semiring_tag <max_semiring_tag <Type>>
    : boost::mpl::true_ {};

} // namespace detail

MATH_MAGMA_GENERATE_OPERATORS (detail::is_max_semiring_tag)

namespace operation {

    /* Queries. */

    template <class Type>
        struct is_member <max_semiring_tag <Type>, typename std::enable_if <
            std::numeric_limits <Type>::has_quiet_NaN>::type>
    {
        // Detect NaN.
        bool operator() (max_semiring <Type> const & v) const { return v == v; }
    };

    template <class Type> struct equal <max_semiring_tag <Type>> {
        bool operator() (
            max_semiring <Type> const & left, max_semiring <Type> const & right)
            const
        { return left.value() == right.value(); }
    };

    template <class Type> struct approximately_equal <max_semiring_tag <Type>,
        typename std::enable_if <!std::numeric_limits <Type>::is_exact>::type>
    {
        bool operator() (
            max_semiring <Type> const & left, max_semiring <Type> const & right)
            const
        {
            static Type const tolerance = 1e-5;
            return math::detail::is_close (left.value(), right.value(),
                tolerance);
        }
    };

    // compare always orders smaller values first.
    template <class Type> struct compare <max_semiring_tag <Type>> {
        bool operator() (
            max_semiring <Type> const & left, max_semiring <Type> const & right)
            const
        { return left.value() < right.value(); }
    };

    // "choose" and "plus" both select the maximal element.
    template <class Type>
    struct order <max_semiring_tag <Type>, callable::choose>
    : reverse_order <compare <max_semiring_tag <Type>>> {};

    template <class Type>
    struct order <max_semiring_tag <Type>, callable::plus>
    : reverse_order <compare <max_semiring_tag <Type>>> {};

    /* Produce. */

    // Return the not-a-number value, not-a-number, if it is available.
    template <class Type> struct non_member <max_semiring_tag <Type>, typename
        std::enable_if <std::numeric_limits <Type>::has_quiet_NaN>::type>
    {
        max_semiring <Type> operator() () const {
            return max_semiring <Type> (
                std::numeric_limits <Type>::quiet_NaN());
        }
    };

    template <class Type>
        struct identity <max_semiring_tag <Type>, callable::times>
    {
        max_semiring <Type> operator() () const
        { return max_semiring <Type> (1); }
    };

    template <class Type>
        struct identity <max_semiring_tag <Type>, callable::choose>
    {
        max_semiring <Type> operator() () const
        { return max_semiring <Type> (0); }
    };

    // Additive identity: same as "choose".
    template <class Type>
        struct identity <max_semiring_tag <Type>, callable::plus>
    : identity <max_semiring_tag <Type>, callable::choose> {};

    // Multiplicative annihilator: forward to additive identity.
    template <class Type>
        struct annihilator <max_semiring_tag <Type>, callable::times>
    : identity <max_semiring_tag <Type>, callable::plus> {};

    /* Operations. */

    // "choose" and "plus" are implemented automatically, since "order" is
    // implemented for them.

    template <class Type> struct times <max_semiring_tag <Type>>
    : associative, commutative, approximate_if <
        boost::mpl::bool_ <!std::numeric_limits <Type>::is_exact>>
    {
        max_semiring <Type> operator() (max_semiring <Type> const & left,
            max_semiring <Type> const & right) const
        { return max_semiring <Type> (left.value() * right.value()); }
    };

    // Semiring in both directions.
    template <class Type> struct is_semiring <
        max_semiring_tag <Type>, either, callable::times, callable::choose>
    : rime::true_type {};

    template <class Type> struct is_semiring <
        max_semiring_tag <Type>, either, callable::times, callable::plus>
    : rime::true_type {};

    // Division is only implemented for non-integer types.
    template <class Type> struct divide <max_semiring_tag <Type>, either,
        typename std::enable_if <!std::numeric_limits <Type>::is_integer>::type>
    : approximate_if <boost::mpl::bool_ <!std::numeric_limits <Type>::is_exact>>
    {
        max_semiring <Type> operator() (max_semiring <Type> const & left,
            max_semiring <Type> const & right) const
        { return max_semiring <Type> (left.value() / right.value()); }
    };

    // Inversion is only implemented for non-integer types.
    template <class Type>
        struct invert <max_semiring_tag <Type>, either, callable::times,
        typename std::enable_if <!std::numeric_limits <Type>::is_integer>::type>
    {
        max_semiring <Type> operator() (max_semiring <Type> const & c) const
        { return max_semiring <Type> (1 / c.value()); }
    };

    template <class Type> struct print <max_semiring_tag <Type>> {
        template <class Stream>
            void operator() (Stream & stream, max_semiring <Type> const & c)
            const
        { stream << c.value(); }
    };

} // namespace operation

template <class Type>
    inline std::size_t hash_value (max_semiring <Type> const & m)
{ return boost::hash <Type>() (m.value()); }

} // namespace math

#endif // MATH_MAX_SEMIRING_HPP_INCLUDED


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

#include "data.hpp"
#include "../helper.hpp"

#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"

using namespace std;

typedef g2o::BlockSolver< g2o::BlockSolverTraits<1, 1> >  ScaleBlockSolver;
typedef g2o::LinearSolverCSparse<ScaleBlockSolver::PoseMatrixType> ScaleLinearSolver;

class VertexScale : public g2o::BaseVertex<1, double>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexScale()
    {
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual void setToOriginImpl()
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    virtual void oplusImpl(const double* update)
    {
      // cout<<_estimate<<" + "<<*update<<endl;
      _estimate += *update;
    }
};

class EdgeScaleDirect : public g2o::BaseUnaryEdge<1, Eigen::VectorXd, VertexScale>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeScaleDirect(const Eigen::Vector3d& point3d, const Eigen::Matrix3d& K, double tx, const cv::Mat& _img1)
    : img1(_img1)
    {
      Eigen::Vector3d KX = K * point3d;

      u_base = KX(0)/KX(2);
      v = KX(1)/KX(2);
      u_inc = tx*K(0,0)/point3d(2);
      au_as << u_inc, 0;
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }
    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    void computeError();
    void linearizeOplus();

  private:

    double u_base;
    double v;
    double u_inc;
    Eigen::Matrix<double,2,1> au_as;
    const cv::Mat& img1;
};


//
// Created by sam on 04/12/17.
//

#include <boost/numeric/ublas/io.hpp>
#include "PerceptronMonoLayer.hpp"

PerceptronMonoLayer::PerceptronMonoLayer(const std::vector<Neuron> &neurons) : neurons(neurons) {}

void PerceptronMonoLayer::learn(std::vector<ItemNN<double>> const &items) {
    size_t cycles = 25;
    this->learn(items, 25);
}

/**
 * Method to begin learning with the perceptron
 * @param numCycles number of times that the perceptron loop on the set of learning
 */
void PerceptronMonoLayer::learn(std::vector<ItemNN<double>> const &items, size_t numCycles) {
    size_t sizeSet = items.size();
    size_t j = 0;
    size_t correct, incorrect;
    std::vector<double> succes_rates;
    succes_rates.push_back(0.);
    std::vector<double> outputs;
    do {
        correct = 0;
        incorrect = 0;
        for (ItemNN<double> const &item:items) {
            for (auto &entry: item.getVecEntries()) {
                for (auto &neuron : neurons) {
                    double output = neuron.getOutput(entry);
                    outputs.push_back(output);
                }
                auto vec_expected = intToVec(item.getExpected());
                double outputInt = getIdMax(outputs);
                auto vec_result = intToVec(outputInt);
                if (vec_result == vec_expected) {
                    correct++;
                } else {
                    for (int i = 0; i < vec_expected.size(); i++) {
                        neurons[i].correctWeitghs(entry, vec_expected[i], outputs[i]);
                    }
                    incorrect++;
                }
                outputs.clear();
            }
        }
        double succes_rate = (correct * 1.0) / (items.size() * items[0].getVecEntries().size()) * 100;
        succes_rates.push_back(succes_rate);
        std::cout << "success rate:\n" << succes_rate << std::endl;
    } while (succes_rates[++j] != succes_rates[j - 1] && j < numCycles);
}

void PerceptronMonoLayer::test(std::vector<ItemNN<double>> const &e) {
    size_t i = 0;
    size_t correct = 0, incorrect = 0;
    for (ItemNN<double> const &item:e) {
        if (test(item))
            correct++;
        else
            incorrect++;

        ++i;
    }
    std::cout << "success rate test: " << (correct * 1.0) / e.size() * 100 << std::endl;
}

bool PerceptronMonoLayer::test(ItemNN<double> const &e) {
    std::vector<double> outputs;
    for (auto &neuronne : neurons) {
        double output = neuronne.getOutput(e.getEntry());
        outputs.push_back(output);
    }
    return (intToVec(getIdMax(outputs)) == intToVec(e.getExpected()));
}

std::vector<double> intToVec(size_t x) {
    std::vector<double> res = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    res[x] = 1;
    return res;
}

std::vector<double> intToVec(double x) {
    std::vector<double> res = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    res[x] = 1;
    return res;
}

size_t getIdMax(std::vector<double> output) {
    size_t res = 0;
    double buff = -DBL_MAX;
    for (size_t i = 0; i < output.size(); ++i) {
        if (output[i] > buff) {
            buff = output[i];
            res = i;
        }
    }
    return res;
}


//
// Copyright 2013 Christian Henning
// Copyright 2020 Mateusz Loskot <mateusz@loskot.net>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#include <boost/gil.hpp>
#include <boost/gil/extension/toolbox/color_spaces/hsv.hpp>

#include <boost/core/lightweight_test.hpp>

#include <iostream>

#include "test_utility_output_stream.hpp"

namespace gil = boost::gil;

void test_rgb_to_hsv()
{
    gil::rgb8_pixel_t p{128, 0, 128};
    gil::hsv32f_pixel_t h;
    gil::color_convert(p, h);

    BOOST_TEST_GT(gil::get_color(h, gil::hsv_color_space::hue_t()), 0.80);       // 0.83333331
    BOOST_TEST_LT(gil::get_color(h, gil::hsv_color_space::hue_t()), 0.85);
    BOOST_TEST_GE(gil::get_color(h, gil::hsv_color_space::saturation_t()), 1.0); // 1.00000000
    BOOST_TEST_LT(gil::get_color(h, gil::hsv_color_space::saturation_t()), 1.1);
    BOOST_TEST_GE(gil::get_color(h, gil::hsv_color_space::value_t()), 0.50);     // 0.50196081
    BOOST_TEST_LT(gil::get_color(h, gil::hsv_color_space::value_t()), 0.51);
}

void test_hsv_to_rgb()
{
    gil::rgb8_pixel_t p(128, 0, 128);
    gil::hsv32f_pixel_t h;
    gil::color_convert(p, h);

    gil::rgb8_pixel_t b;
    gil::color_convert(h, b);
    BOOST_TEST_EQ(b, gil::rgb8_pixel_t(128, 0, 128));
}

void test_image_assign_hsv()
{
    std::ptrdiff_t const w = 320;
    std::ptrdiff_t const h = 240;
    gil::hsv32f_image_t hsv_img(w, h);

    for (std::ptrdiff_t y = 0; y < h; y++)
    {
        gil::hsv32f_view_t::x_iterator hsv_x_it = view(hsv_img).row_begin(y);
        float v = static_cast<float>(h - y) / h;
        for (std::ptrdiff_t x = 0; x < w; x++)
        {
            float const hue = (x + 1.f) / w;
            gil::hsv32f_pixel_t const p(hue, 1.0, v);
            hsv_x_it[x] = p;
            BOOST_TEST_EQ(gil::view(hsv_img)(x, y), p);
        }
    }
}

void test_copy_pixels_rgb_to_hsv()
{
    gil::rgb8_image_t rgb_img(320, 240);
    gil::rgb8_pixel_t rgb_pix(64, 32, 64);
    gil::fill_pixels(view(rgb_img), rgb_pix);
    gil::hsv32f_image_t hsv_img(view(rgb_img).dimensions());
    gil::copy_pixels(gil::color_converted_view<gil::hsv32f_pixel_t>(view(rgb_img)), view(hsv_img));

    auto view = gil::view(hsv_img);
    for (auto it = view.begin(), end = view.end(); it != end; ++it)
    {
        auto h = *it;
        BOOST_TEST_GT(gil::get_color(h, gil::hsv_color_space::hue_t()), 0.80);       // 0.8333333
        BOOST_TEST_LT(gil::get_color(h, gil::hsv_color_space::hue_t()), 0.85);
        BOOST_TEST_GE(gil::get_color(h, gil::hsv_color_space::saturation_t()), 0.5); // 0.5000000
        BOOST_TEST_LT(gil::get_color(h, gil::hsv_color_space::saturation_t()),  0.51);
        BOOST_TEST_GT(gil::get_color(h, gil::hsv_color_space::value_t()), 0.25);     // 0.25
        BOOST_TEST_LT(gil::get_color(h, gil::hsv_color_space::value_t()), 0.26);
    }
}

int main()
{
    test_rgb_to_hsv();
    test_hsv_to_rgb();
    test_image_assign_hsv();
    test_copy_pixels_rgb_to_hsv();

    return ::boost::report_errors();
}


#ifndef NETWORKS_HPP
#define NETWORKS_HPP

#include <vector>
// For External Library
#include <torch/torch.h>
#include <boost/program_options.hpp>

// Define Namespace
namespace nn = torch::nn;
namespace po = boost::program_options;

// Function Prototype
void weights_init(nn::Module &m);
void DownSampling(nn::Sequential &sq, const size_t in_nc, const size_t out_nc, const bool BN, const bool ReLU, const bool bias=false);
void UpSampling(nn::Sequential &sq, const size_t in_nc, const size_t out_nc, const bool BN, const bool ReLU, const bool bias=false);

// -------------------------------------------------
// struct{VariationalAutoEncoderImpl}(nn::Module)
// -------------------------------------------------
struct VariationalAutoEncoderImpl : nn::Module{
private:
    torch::Tensor mean_keep, var_keep, z_keep;
    nn::Sequential encoder, encoder_mean, encoder_var;
    nn::Sequential decoder;
    torch::Tensor sampling(torch::Tensor &mean, torch::Tensor &var);
public:
    VariationalAutoEncoderImpl(){}
    VariationalAutoEncoderImpl(po::variables_map &vm);
    torch::Tensor forward(torch::Tensor x);
    torch::Tensor forward_z(torch::Tensor z);
    torch::Tensor kld_just_before();
    std::vector<long int> get_z_shape(const std::vector<long int> x_shape, torch::Device &device);
};

TORCH_MODULE(VariationalAutoEncoder);

#endif


/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <mc_rtc/logging.h>
#include <mc_trajectory/SequenceInterpolator.h>
#include <boost/test/unit_test.hpp>
#include <Eigen/Core>

BOOST_AUTO_TEST_CASE(TestSequenceInterpolator)
{
  constexpr double epsilon = 1e-10;
  using namespace mc_trajectory;
  LinearInterpolation<double> interpolator;
  using SequenceInterpolator = SequenceInterpolator<double, LinearInterpolation<double>>;

  using values_t = std::vector<std::pair<double, double>>;
  auto values = values_t{{0., 0.}, {1., 1.}, {2., 2.}};
  constexpr double dt = 0.005;

  BOOST_REQUIRE_THROW(SequenceInterpolator{}.compute(0), std::runtime_error);
  BOOST_REQUIRE_THROW(SequenceInterpolator{{}}.compute(0), std::runtime_error);
  BOOST_REQUIRE(!SequenceInterpolator{}.hasValues());
  BOOST_REQUIRE_NO_THROW(SequenceInterpolator{values});
  SequenceInterpolator interpolation{values};
  // Check interpolation in first interval
  int i = 0;
  double time = 0;
  for(; i < 1 / dt; i++)
  {
    BOOST_REQUIRE_CLOSE(interpolation.compute(time), interpolator(0, 1, time), epsilon);
    time += dt;
  }
  // Check interpolation in second interval
  for(; i < 2 / dt; i++)
  {
    BOOST_REQUIRE_CLOSE(interpolation.compute(time), interpolator(1, 2, time - 1), epsilon);
    time += dt;
  }
  // Check access out of bounds (returns last value)
  for(; i < 3 / dt; i++)
  {
    BOOST_REQUIRE_CLOSE(interpolation.compute(time), 2., epsilon);
    time += dt;
  }

  BOOST_REQUIRE_THROW(interpolation.values(values_t{{2, 0}, {1.999, 0}}),
                      std::runtime_error); // values not correctly ordered
  BOOST_REQUIRE_THROW(interpolation.values(values_t{{2, 0}, {1.5, 0}}),
                      std::runtime_error); // values not correctly ordered

  // Change values
  values = values_t{{0.5, 0.5}, {1., 1.}, {2., 2.}};
  BOOST_REQUIRE_NO_THROW(interpolation.values(values));
  BOOST_REQUIRE_THROW(interpolation.values({{3.5, 0.5}, {1., 1.}, {2., 2.}}), std::runtime_error);
  time = 0.5;
  BOOST_REQUIRE_CLOSE(interpolation.compute(time), 0.5, epsilon);

  BOOST_REQUIRE_NO_THROW(interpolation.values({{1.2, 2.3}}));
  time = 1.2;
  BOOST_REQUIRE_CLOSE(interpolation.compute(time), 2.3, epsilon);

  // test starting at second interval
  values = values_t{{0.0, 0.0}, {1., 1.}, {2., 2.}};
  BOOST_REQUIRE_NO_THROW(interpolation.values(values));
  BOOST_REQUIRE_CLOSE(interpolation.compute(1.2), interpolator(1, 2, 0.2), epsilon);

  // test starting at any time
  values = values_t{{0.0, 0.0}, {1., 1.}, {2., 2.}};
  BOOST_REQUIRE_NO_THROW(interpolation.values(values));
  BOOST_REQUIRE_CLOSE(interpolation.compute(1.2), interpolator(1, 2, 0.2), epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(1.3), interpolator(1, 2, 0.3), epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(0.5), interpolator(0, 1, 0.5), epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(2.00001), 2.0, epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(-2.00001), 0.0, epsilon);

  // single value
  interpolation.values({{1., 1.}});
  BOOST_REQUIRE_CLOSE(interpolation.compute(0), 1., epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(1), 1., epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(2), 1., epsilon);

  // negative value
  BOOST_REQUIRE_NO_THROW(interpolation.values({{-1., -1.}, {0., 0.}, {1., 1.}}));
  BOOST_REQUIRE_CLOSE(interpolation.compute(-0.2), -0.2, epsilon);

  // Realistic case: trajectory gains
  interpolation.values({{0, 20}, {3., 20}, {3.8, 50}, {4., 200}});
  BOOST_REQUIRE_CLOSE(interpolation.compute(0.), 20, epsilon);

  // Random access
  values = {{0, 0}, {3., 20}, {3.8, 50}, {4., 200}};
  interpolation.values(values);
  BOOST_REQUIRE_CLOSE(interpolation.compute(6), 200, epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(-1), 0, epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(3), 20, epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(3.8), 50, epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(2), 20 / 3. * 2, epsilon);
  BOOST_REQUIRE_CLOSE(interpolation.compute(3.9), interpolator(50, 200, 0.5), epsilon);
}


/* test_ranlux3.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/ranlux.hpp>

#define BOOST_RANDOM_URNG boost::random::ranlux3

#define BOOST_RANDOM_SEED_WORDS 24

// principal operation validated with CLHEP, values by experiment
#define BOOST_RANDOM_VALIDATION_VALUE 5957620U
#define BOOST_RANDOM_SEED_SEQ_VALIDATION_VALUE 11848780U
#define BOOST_RANDOM_ITERATOR_VALIDATION_VALUE 11620328U

#define BOOST_RANDOM_GENERATE_VALUES { 0x55E57B2CU, 0xF2DEF915U, 0x6D1A0CD9U, 0xCA0109F9U }

#include "test_generator.ipp"


/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#pragma once

#include "math.hpp"
#include "vector2.hpp"

#include <boost/optional.hpp>

namespace eXl
{
  template <class Real>
  class AABB2D
  {
  public:
    inline AABB2D(){}
    inline AABB2D(Real iMinX, Real iMinY, Real iMaxX, Real iMaxY)
    {
      m_Data[0] = Vector2<Real>(iMinX,iMinY);
      m_Data[1] = Vector2<Real>(iMaxX,iMaxY);
    }

    static inline AABB2D FromCenterAndSize(Vector2<Real> iCenter,Vector2<Real> iSize)
    {
      AABB2D ret;
      ret.m_Data[0] = iCenter - iSize / 2.0;
      ret.m_Data[1] = iCenter + iSize / 2.0;
      return ret;
    }

    inline AABB2D(Vector2<Real> iMin,Vector2<Real> iSize)
    {
      m_Data[0] = iMin;
      m_Data[1] = iMin + iSize;
    }

    template <class OtherReal>
    explicit inline AABB2D(AABB2D<OtherReal> const& iOther)
    {
      m_Data[0] = Vector2<Real>(iOther.m_Data[0]);
      m_Data[1] = Vector2<Real>(iOther.m_Data[1]);
    }

    inline bool Empty() const
    {
      return m_Data[0].X() == m_Data[1].X() || m_Data[0].Y() == m_Data[1].Y();
    }

    bool CircleTest(Vector2f const& iPos, Real iRadius)
    {
      if(Contains(iPos))
      {
        return true;
      }
      else
      {
        //Test segment X == X0
        Real dist = m_Data[0].X() - iPos.X();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real y = Math<Real>::Sqrt(dist) + iPos.Y();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].Y() - y && y - m_Data[0].Y() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }
        //Test segment X == X1
        dist = m_Data[1].X() - iPos.X();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real y = Math<Real>::Sqrt(dist) + iPos.Y();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].Y() - y && y - m_Data[0].Y() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }

        //TestSegment Y == Y0
        dist = m_Data[0].Y() - iPos.Y();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real x = Math<Real>::Sqrt(dist) + iPos.X();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].X() - x && x - m_Data[0].X() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }
        //Test segment Y == Y1
        dist = m_Data[1].Y() - iPos.Y();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real x = Math<Real>::Sqrt(dist) + iPos.X();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].X() - x && x - m_Data[0].X() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }
      }
      return false;
    }

    inline void Rotate(Real iAngle)
    {
      Vector2<Real> center = GetCenter();
      Vector2<Real> size = GetSize();
      Vector2<Real> points[4];
      points[0] = size * -0.5;
      points[1] = Vector2<Real>(size.X() * -0.5, size.Y() *  0.5) ;
      points[2] = Vector2<Real>(size.X() *  0.5, size.Y() * -0.5) ;
      points[3] = size * 0.5;
      points[0] = Vector2<Real>(points[0].X() * Math<Real>::Cos(iAngle) + points[0].Y() * Math<Real>::Sin(iAngle)
                         , points[0].Y() * Math<Real>::Cos(iAngle) - points[0].X() * Math<Real>::Sin(iAngle));
      points[1] = Vector2<Real>(points[1].X() * Math<Real>::Cos(iAngle) + points[1].Y() * Math<Real>::Sin(iAngle)
                         , points[1].Y() * Math<Real>::Cos(iAngle) - points[1].X() * Math<Real>::Sin(iAngle));
      points[2] = Vector2<Real>(points[2].X() * Math<Real>::Cos(iAngle) + points[2].Y() * Math<Real>::Sin(iAngle)
                         , points[2].Y() * Math<Real>::Cos(iAngle) - points[2].X() * Math<Real>::Sin(iAngle));
      points[3] = Vector2<Real>(points[3].X() * Math<Real>::Cos(iAngle) + points[3].Y() * Math<Real>::Sin(iAngle)
                         , points[3].Y() * Math<Real>::Cos(iAngle) - points[3].X() * Math<Real>::Sin(iAngle));

      m_Data[0] = m_Data[1] = points[0];
      for (unsigned int i = 1; i < 4; ++i)
      {
        m_Data[0].X() = Math<Real>::Min(m_Data[0].X(), points[i].X());
        m_Data[0].Y() = Math<Real>::Min(m_Data[0].Y(), points[i].Y());
        m_Data[1].X() = Math<Real>::Max(m_Data[1].X(), points[i].X());
        m_Data[1].Y() = Math<Real>::Max(m_Data[1].Y(), points[i].Y());
      }
      m_Data[0] += center;
      m_Data[1] += center;
    }

    inline Vector2<Real> GetCenter()const
    {
      return (m_Data[0] + m_Data[1])/2;
    }

    inline Vector2<Real> GetSize()const
    {
      return m_Data[1] - m_Data[0];
    }

    inline void Inflate(Real iCoeff)
    {
      Vector2<Real> center = (m_Data[1] + m_Data[0]) / 2;
      Vector2<Real> size = m_Data[1] - m_Data[0];
      size = size * iCoeff;
      m_Data[0] = center - size / 2.0;
      m_Data[1] = center + size / 2.0;
    }

    inline Real MinX()const{return m_Data[0].X();}
    inline Real MinY()const{return m_Data[0].Y();}
    inline Real MaxX()const{return m_Data[1].X();}
    inline Real MaxY()const{return m_Data[1].Y();}

    inline Real& MinX(){return m_Data[0].X();}
    inline Real& MinY(){return m_Data[0].Y();}
    inline Real& MaxX(){return m_Data[1].X();}
    inline Real& MaxY(){return m_Data[1].Y();}

    inline bool operator ==(AABB2D const& iOther) const
    {
      return m_Data[0]==iOther.m_Data[0] && m_Data[1] == iOther.m_Data[1];
    }

    inline bool operator !=(AABB2D const& iOther) const
    {
      return !((*this) == iOther);
    }

	  inline bool Empty()
	  {
		  return m_Data[0].X() == m_Data[1].X() || m_Data[0].Y() == m_Data[1].Y();
	  }

    inline void SetCommonBox(AABB2D<Real> const& iBox1,AABB2D<Real> const& iBox2)
    {
      m_Data[0].X() = Math<Real>::Max(iBox1.m_Data[0].X(),iBox2.m_Data[0].X());
      m_Data[0].Y() = Math<Real>::Max(iBox1.m_Data[0].Y(),iBox2.m_Data[0].Y());
      m_Data[1].X() = Math<Real>::Max(Math<Real>::Min(iBox1.m_Data[1].X(),iBox2.m_Data[1].X()),m_Data[0].X());
      m_Data[1].Y() = Math<Real>::Max(Math<Real>::Min(iBox1.m_Data[1].Y(),iBox2.m_Data[1].Y()),m_Data[0].Y());
    }

    inline void Absorb(Vector2<Real> const& iPoint)
    {
      m_Data[0].X() = Math<Real>::Min(m_Data[0].X(), iPoint.X());
      m_Data[0].Y() = Math<Real>::Min(m_Data[0].Y(), iPoint.Y());
      m_Data[1].X() = Math<Real>::Max(m_Data[1].X(), iPoint.X());
      m_Data[1].Y() = Math<Real>::Max(m_Data[1].Y(), iPoint.Y());
    }

    inline void Absorb(AABB2D<Real> const& iBox)
    {
      m_Data[0].X() = Math<Real>::Min(m_Data[0].X(),iBox.m_Data[0].X());
      m_Data[0].Y() = Math<Real>::Min(m_Data[0].Y(),iBox.m_Data[0].Y());
      m_Data[1].X() = Math<Real>::Max(m_Data[1].X(),iBox.m_Data[1].X());
      m_Data[1].Y() = Math<Real>::Max(m_Data[1].Y(),iBox.m_Data[1].Y());
    }

    inline bool IsInside(AABB2D const& iOther, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      return (m_Data[0].X() - iOther.m_Data[0].X() >= iEpsilon && m_Data[0].Y() - iOther.m_Data[0].Y() >= iEpsilon
           && iOther.m_Data[1].X() - m_Data[1].X() >= iEpsilon && iOther.m_Data[1].Y() - m_Data[1].Y() >= iEpsilon );
    }

    inline bool Intersect(AABB2D const& iOther, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      return iOther.m_Data[1].X() - m_Data[0].X() >= iEpsilon && m_Data[1].X() - iOther.m_Data[0].X() >= iEpsilon
          && iOther.m_Data[1].Y() - m_Data[0].Y() >= iEpsilon && m_Data[1].Y() - iOther.m_Data[0].Y() >= iEpsilon;
    }

    //0 No, 1 Left, 2 Right, 3 Down, 4 Up
    inline int Touch(AABB2D const& iOther, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      if(iOther.m_Data[1].Y() - m_Data[0].Y() >= iEpsilon && m_Data[1].Y() - iOther.m_Data[0].Y() >= iEpsilon)
      {
        Real val = m_Data[0].X() - iOther.m_Data[1].X();
        if(val <= iEpsilon && val >= -iEpsilon)
          return 1;

        val = m_Data[1].X() - iOther.m_Data[0].X();
        if(val <= iEpsilon && val >= -iEpsilon)
          return 2;
      }
      if(iOther.m_Data[1].X() - m_Data[0].X() >= iEpsilon && m_Data[1].X() - iOther.m_Data[0].X() >= iEpsilon)
      {
        Real val = m_Data[0].Y() - iOther.m_Data[1].Y();

        if(val <= iEpsilon && val >= -iEpsilon)
          return 3;

        val = m_Data[1].Y() - iOther.m_Data[0].Y();
        if(val <= iEpsilon && val >= -iEpsilon)
          return 4;
      }
      return 0;
    }

    boost::optional<Vector2<Real>> SegmentTest(Vector2<Real> const& iOrigin, Vector2<Real> const& iDir, Real iEpsilon = Math<Real>::ZERO_TOLERANCE)
    {
      Real const scaleX = 1.0 / iDir.X();
      Real const scaleY = 1.0 / iDir.Y();
      Real const signX = Math<Real>::Sign(scaleX);
      Real const signY = Math<Real>::Sign(scaleY);

      Real const nearTimeX = ((signX > 0 ? m_Data[0].X() : m_Data[1].X()) - iOrigin.X()) * scaleX;
      Real const nearTimeY = ((signY > 0 ? m_Data[0].Y() : m_Data[1].Y()) - iOrigin.Y()) * scaleY;
      Real const farTimeX = ((signX > 0 ? m_Data[1].X() : m_Data[0].X()) - iOrigin.X()) * scaleX;
      Real const farTimeY = ((signY > 0 ? m_Data[1].Y() : m_Data[0].Y()) - iOrigin.Y()) * scaleY;

      if (nearTimeX > farTimeY || nearTimeY > farTimeX)
      {
        return boost::none;
      }

      Real const nearTime = nearTimeX > nearTimeY ? nearTimeX : nearTimeY;
      Real const farTime = farTimeX < farTimeY ? farTimeX : farTimeY;

      if (nearTime > 1 - iEpsilon || farTime < -iEpsilon)
      {
        return boost::none;
      }

      return iOrigin + iDir * nearTime;
    }

    inline bool Contains(Vector2<Real>const& iPoint, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      return iPoint.X() - m_Data[0].X() >= iEpsilon && m_Data[1].X() - iPoint.X() >= iEpsilon
          && iPoint.Y() - m_Data[0].Y() >= iEpsilon && m_Data[1].Y() - iPoint.Y() >= iEpsilon;
    }

    // Neg value -> distance to min, Pos value -> distance to max, 0, inside
    Vector2f Classify(Vector2<Real>const& iPoint, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      Vector2f res(iPoint.X() - m_Data[0].X(), iPoint.Y() - m_Data[0].Y());
      for(uint32_t axis = 0; axis < 2; ++axis)
      {
        if(!(res.m_Data[axis] < -iEpsilon))
        {
          res.m_Data[axis] = m_Data[1].m_Data[axis] - iPoint.m_Data[axis];
          if(res.m_Data[axis] < -iEpsilon)
          {
            res.m_Data[axis] *= -1;
          }
          else
          {
            res.m_Data[axis] = 0;
          }
        }
      }

      return res;
    }

    inline Vector2<Real> GetDistX(AABB2D const& iOther) const
    {
      return Vector2<Real>(iOther.m_Data[0].X() - m_Data[0].X(),m_Data[1].X() - iOther.m_Data[1].X());
    }

    inline Vector2<Real> GetDistY(AABB2D const& iOther) const
    {
      return Vector2<Real>(iOther.m_Data[0].Y() - m_Data[0].Y(),m_Data[1].Y() - iOther.m_Data[1].Y());
    }

    Vector2<Real> m_Data[2];
  };

  typedef AABB2D<int> AABB2Di;
  typedef AABB2D<float> AABB2Df;
  typedef AABB2D<double> AABB2Dd;
}


#ifndef SKYLARK_GEMV_HPP
#define SKYLARK_GEMV_HPP

#include <boost/mpi.hpp>
#include "exception.hpp"

// Defines a generic Gemv function that recieves a wider set of matrices

namespace skylark { namespace base {

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::Matrix<T>& A, const elem::Matrix<T>& x,
    T beta, elem::Matrix<T>& y) {
    elem::Gemv(oA, alpha, A, x, beta, y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::Matrix<T>& A, const elem::Matrix<T>& x,
    elem::Matrix<T>& y) {
    elem::Gemv(oA, alpha, A, x, y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T>& A, const elem::DistMatrix<T>& x,
    T beta, elem::DistMatrix<T>& y) {
    elem::Gemv(oA, alpha, A, x, beta, y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T>& A, const elem::DistMatrix<T>& x,
    elem::DistMatrix<T>& y) {
    elem::Gemv(oA, alpha, A, x, y);
}

/**
 * The following combinations is not offered by Elemental, but is useful for us.
 * We implement it partially.
 */

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VC, elem::STAR>& x,
    T beta, elem::DistMatrix<T, elem::STAR, elem::STAR>& y) {
    // TODO verify sizes etc.
    // TODO verify matching grids.

    if (oA == elem::TRANSPOSE) {
        boost::mpi::communicator comm(y.Grid().Comm(), boost::mpi::comm_attach);
        elem::Matrix<T> ylocal(y.Matrix());
        elem::Gemv(elem::TRANSPOSE,
            alpha, A.LockedMatrix(), x.LockedMatrix(),
            beta / T(comm.size()), ylocal);
        boost::mpi::all_reduce(comm,
            ylocal.Buffer(), ylocal.MemorySize(), y.Buffer(),
            std::plus<T>());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VC, elem::STAR>& x,
    elem::DistMatrix<T, elem::STAR, elem::STAR>& y) {

    int y_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    elem::Zeros(y, y_height, 1);
    base::Gemv(oA, alpha, A, x, T(0), y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& x,
    T beta, elem::DistMatrix<T, elem::VC, elem::STAR>& y) {
    // TODO verify sizes etc.

    if (oA == elem::NORMAL) {
        elem::Gemv(elem::NORMAL,
            alpha, A.LockedMatrix(), x.LockedMatrix(),
            beta, y.Matrix());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& x,
    elem::DistMatrix<T, elem::VC, elem::STAR>& y) {

    int y_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    elem::Zeros(y, y_height, 1);
    base::Gemv(oA, alpha, A, x, T(0), y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const sparse_matrix_t<T>& A, const elem::Matrix<T>& x,
    T beta, elem::Matrix<T>& y) {
    // TODO verify sizes etc.

    const int* indptr = A.indptr();
    const int* indices = A.indices();
    const double *values = A.locked_values();
    double *yd = y.Buffer();
    const double *xd = x.LockedBuffer();

    int n = A.width();

    if (oA == elem::NORMAL) {
        elem::Scal(beta, y);

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for
#       endif
        for(int col = 0; col < n; col++) {
            T xv = alpha * xd[col];
            for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                     int row = indices[j];
                     T val = values[j];
                     yd[row] += val * xv;
                 }
        }

    } else {

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for
#       endif
        for(int col = 0; col < n; col++) {
            double yv = beta * yd[col];
            for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                     int row = indices[j];
                     T val = values[j];
                     yv += alpha * val * xd[row];
                 }
            yd[col] = yv;
        }

    }
}

} } // namespace skylark::base

#endif // SKYLARK_GEMV_HPP


#include <mlpack/core.hpp>
#include <mlpack/methods/amf/amf.hpp>
#include <mlpack/methods/amf/update_rules/svd_batch_learning.hpp>
#include <mlpack/methods/amf/init_rules/random_init.hpp>
#include <mlpack/methods/amf/init_rules/average_init.hpp>
#include <mlpack/methods/amf/termination_policies/validation_RMSE_termination.hpp>
#include <mlpack/methods/amf/termination_policies/simple_tolerance_termination.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

BOOST_AUTO_TEST_SUITE(SVDBatchTest);

using namespace std;
using namespace mlpack;
using namespace mlpack::amf;
using namespace arma;

/**
 * Make sure the SVD Batch lerning is converging.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
BOOST_AUTO_TEST_CASE(SVDBatchConvergenceElementTest)
{
  sp_mat data;
  data.sprandn(1000, 1000, 0.2);
  AMF<SimpleToleranceTermination<sp_mat>,
      AverageInitialization,
      SVDBatchLearning> amf;
  mat m1, m2;
  amf.Apply(data, 2, m1, m2);

  BOOST_REQUIRE_NE(amf.TerminationPolicy().Iteration(),
                   amf.TerminationPolicy().MaxIterations());
}

//! This is used to ensure we start from the same initial point.
class SpecificRandomInitialization
{
 public:
  SpecificRandomInitialization(const size_t n, const size_t r, const size_t m) :
      W(arma::randu<arma::mat>(n, r)),
      H(arma::randu<arma::mat>(r, m)) { }

  template<typename MatType>
  inline void Initialize(const MatType& /* V */,
                         const size_t /* r */,
                         arma::mat& W,
                         arma::mat& H)
  {
    W = this->W;
    H = this->H;
  }

 private:
  arma::mat W;
  arma::mat H;
};

/**
 * Make sure the momentum is working okay.
 */
BOOST_AUTO_TEST_CASE(SVDBatchMomentumTest)
{
  mat dataset;
  data::Load("GroupLens100k.csv", dataset);

  // Generate list of locations for batch insert constructor for sparse
  // matrices.
  arma::umat locations(2, dataset.n_cols);
  arma::vec values(dataset.n_cols);
  for (size_t i = 0; i < dataset.n_cols; ++i)
  {
    // We have to transpose it because items are rows, and users are columns.
    locations(0, i) = ((arma::uword) dataset(0, i));
    locations(1, i) = ((arma::uword) dataset(1, i));
    values(i) = dataset(2, i);
  }

  // Find maximum user and item IDs.
  const size_t maxUserID = (size_t) max(locations.row(0)) + 1;
  const size_t maxItemID = (size_t) max(locations.row(1)) + 1;

  // Fill sparse matrix.
  sp_mat cleanedData = arma::sp_mat(locations, values, maxUserID, maxItemID);

  // Create the initial matrices.
  SpecificRandomInitialization sri(cleanedData.n_rows, 2, cleanedData.n_cols);

  ValidationRMSETermination<sp_mat> vrt(cleanedData, 500);
  AMF<ValidationRMSETermination<sp_mat>,
      SpecificRandomInitialization,
      SVDBatchLearning> amf1(vrt, sri, SVDBatchLearning(0.0009, 0, 0, 0));

  mat m1, m2;
  const double regularRMSE = amf1.Apply(cleanedData, 2, m1, m2);

  AMF<ValidationRMSETermination<sp_mat>,
      SpecificRandomInitialization,
      SVDBatchLearning> amf2(vrt, sri, SVDBatchLearning(0.0009, 0, 0, 0.8));

  const double momentumRMSE = amf2.Apply(cleanedData, 2, m1, m2);

  BOOST_REQUIRE_LE(momentumRMSE, regularRMSE + 0.05);
}

/**
 * Make sure the regularization is working okay.
 */
BOOST_AUTO_TEST_CASE(SVDBatchRegularizationTest)
{
  mat dataset;
  data::Load("GroupLens100k.csv", dataset);

  // Generate list of locations for batch insert constructor for sparse
  // matrices.
  arma::umat locations(2, dataset.n_cols);
  arma::vec values(dataset.n_cols);
  for (size_t i = 0; i < dataset.n_cols; ++i)
  {
    // We have to transpose it because items are rows, and users are columns.
    locations(0, i) = ((arma::uword) dataset(0, i));
    locations(1, i) = ((arma::uword) dataset(1, i));
    values(i) = dataset(2, i);
  }

  // Find maximum user and item IDs.
  const size_t maxUserID = (size_t) max(locations.row(0)) + 1;
  const size_t maxItemID = (size_t) max(locations.row(1)) + 1;

  // Fill sparse matrix.
  sp_mat cleanedData = arma::sp_mat(locations, values, maxUserID, maxItemID);

  // Create the initial matrices.
  SpecificRandomInitialization sri(cleanedData.n_rows, 2, cleanedData.n_cols);

  ValidationRMSETermination<sp_mat> vrt(cleanedData, 2000);
  AMF<ValidationRMSETermination<sp_mat>,
      SpecificRandomInitialization,
      SVDBatchLearning> amf1(vrt, sri, SVDBatchLearning(0.0009, 0, 0, 0));

  mat m1, m2;
  double regularRMSE = amf1.Apply(cleanedData, 2, m1, m2);

  AMF<ValidationRMSETermination<sp_mat>,
      SpecificRandomInitialization,
      SVDBatchLearning> amf2(vrt, sri, SVDBatchLearning(0.0009, 0.5, 0.5, 0.8));

  double momentumRMSE = amf2.Apply(cleanedData, 2, m1, m2);

  BOOST_REQUIRE_LE(momentumRMSE, regularRMSE + 0.05);
}

/**
 * Make sure the SVD can factorize matrices with negative entries.
 */
BOOST_AUTO_TEST_CASE(SVDBatchNegativeElementTest)
{
  // Create two 5x3 matrices that we should be able to recover.
  mat testLeft;
  testLeft.randu(5, 3);
  testLeft -= 0.5; // Shift so elements are negative.

  mat testRight;
  testRight.randu(3, 5);
  testRight -= 0.5; // Shift so elements are negative.

  // Assemble a rank-3 matrix that is 5x5.
  mat test = testLeft * testRight;

  AMF<SimpleToleranceTermination<mat>,
      RandomInitialization,
      SVDBatchLearning> amf(SimpleToleranceTermination<mat>(),
                            RandomInitialization(),
                            SVDBatchLearning(0.1, 0.001, 0.001, 0));
  mat m1, m2;
  amf.Apply(test, 3, m1, m2);

  arma::mat result = m1 * m2;

  // 5% tolerance on the norm.
  BOOST_REQUIRE_CLOSE(arma::norm(test, "fro"), arma::norm(result, "fro"), 5.0);
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Copyright 2018, Gepetto team, LAAS-CNRS
 *
 * This file is part of sot-talos-balance.
 * sot-talos-balance is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-talos-balance is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "sot/talos_balance/quat-to-euler.hh"

#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <sot/core/stop-watch.hh>

#include <Eigen/Core>

namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace dg = ::dynamicgraph;
using namespace dg;
using namespace dg::command;

// Size to be aligned                       "-------------------------------------------------------"
#define PROFILE_QUATTOEULER_COMPUTATION "QuatToEuler computation                                "

#define INPUT_SIGNALS m_quaternionSIN

#define OUTPUT_SIGNALS m_eulerSOUT

/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef QuatToEuler EntityClassName;

/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(QuatToEuler, "QuatToEuler");

/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
QuatToEuler::QuatToEuler(const std::string& name)
    : Entity(name),
      CONSTRUCT_SIGNAL_IN(quaternion, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_OUT(euler, dynamicgraph::Vector, INPUT_SIGNALS) {
  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);

  /* Commands. */
  addCommand("init", makeCommandVoid0(*this, &QuatToEuler::init, docCommandVoid0("Initialize the entity.")));
}

/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */

DEFINE_SIGNAL_OUT_FUNCTION(euler, dynamicgraph::Vector) {
  const dynamicgraph::Vector& input = m_quaternionSIN(iter);
  const size_t sz = input.size();
  if ((size_t)(s.size()) != (sz - 1)) s.resize(sz - 1);

  getProfiler().start(PROFILE_QUATTOEULER_COMPUTATION);

  const Eigen::Map<const Eigen::Quaterniond> quat(input.segment<4>(3).data());

  s.head<3>() = input.head<3>();

  Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
  s[3] = euler[2];
  s[4] = euler[1];
  s[5] = euler[0];

  if (sz > 7) s.tail(sz - 7) = input.tail(sz - 7);

  getProfiler().stop(PROFILE_QUATTOEULER_COMPUTATION);

  return s;
}

/* --- COMMANDS ---------------------------------------------------------- */

/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */

void QuatToEuler::display(std::ostream& os) const {
  os << "QuatToEuler " << getName();
  try {
    getProfiler().report_all(3, os);
  } catch (ExceptionSignal e) {
  }
}

}  // namespace talos_balance
}  // namespace sot
}  // namespace dynamicgraph


///////////////////////////////////////////////////////////////////////////////
// statistics::model::functional::include.hpp                                //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_MODEL_FUNCTIONAL_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_FUNCTIONAL_INCLUDE_HPP_ER_2009

#include <boost/statistics/model/functional/log_likelihood_accumulator.hpp>
#include <boost/statistics/model/functional/log_likelihood_evaluator.hpp>
#include <boost/statistics/model/functional/log_posterior_evaluator.hpp>
#include <boost/statistics/model/functional/log_prior_evaluator.hpp>

#endif


#include <catch2/catch.hpp>
#include <Euclid/Render/Rasterizer.h>

#include <random>
#include <string>

#include <CGAL/Simple_cartesian.h>
#include <Euclid/IO/OffIO.h>
#include <Euclid/IO/PlyIO.h>
#include <Euclid/BoundingVolume/AABB.h>
#include <Euclid/Math/Vector.h>
#include <Eigen/Geometry>

#include <stb_image_write.h>

#include <config.h>

using Kernel = CGAL::Simple_cartesian<float>;

TEST_CASE("Render, Rasterizer", "[render][rasterizer]")
{
    const uint32_t width = 800;
    const uint32_t height = 600;

    std::string filename(DATA_DIR);
    filename.append("bunny_vn.ply");
    std::vector<float> positions;
    std::vector<float> normals;
    std::vector<unsigned> indices;
    Euclid::read_ply<3>(
        filename, positions, &normals, nullptr, &indices, nullptr);

    // fit model into unit box
    Euclid::AABB<Kernel> aabb(positions);
    Eigen::Vector3f translation;
    Euclid::cgal_to_eigen(aabb.center(), translation);
    float scale = std::max(aabb.xlen(), aabb.ylen());
    scale = std::max(scale, aabb.zlen());
    Eigen::Transform<float, 3, Eigen::Affine> transform;
    transform.setIdentity();
    transform.translate(-translation);
    transform.scale(1.0f / scale);

    Eigen::Vector3f view(0.0f, 1.0f, 2.0f);
    Eigen::Vector3f center(0.0f, 0.0f, 0.0f);
    Eigen::Vector3f up(0.0f, 1.0f, 0.0f);
    Euclid::PerspRasCamera persp(
        view, center, up, 60.0f, width, height, 0.5f, 5.0f);

    Euclid::Rasterizer rasterizer(
        width, height, Euclid::Rasterizer::SAMPLE_COUNT_1);
    rasterizer.attach_position_buffer(positions.data(), positions.size());
    rasterizer.attach_normal_buffer(normals.data(), normals.size());
    rasterizer.attach_index_buffer(indices.data(), indices.size());

    Euclid::Light l;
    l.position = view + Eigen::Vector3f(0.1f, 0.1f, 0.1f);
    l.color = { 1.0f, 1.0f, 1.0f };
    l.intensity = 0.5f;
    rasterizer.set_light(l);

    SECTION("perspective camera")
    {
        std::vector<uint8_t> pixels(3 * width * height);
        rasterizer.render_shaded(transform.matrix(), persp, pixels);

        std::string outfile(TMP_DIR);
        outfile.append("rasterizer_persp.png");
        stbi_write_png(
            outfile.c_str(), width, height, 3, pixels.data(), width * 3);
    }

    SECTION("orthographic camera")
    {
        const float xextent = 2.0f;
        const float yextent = xextent * height / (width + 1e-6f);
        Euclid::OrthoRasCamera ortho(
            view, center, up, xextent, yextent, 0.1f, 5.0f);

        std::vector<uint8_t> pixels(3 * width * height);
        rasterizer.render_shaded(transform.matrix(), ortho, pixels);

        std::string outfile(TMP_DIR);
        outfile.append("rasterizer_ortho.png");
        stbi_write_png(
            outfile.c_str(), width, height, 3, pixels.data(), width * 3);
    }

    SECTION("multisampling")
    {
        rasterizer.set_image(width, height, Euclid::Rasterizer::SAMPLE_COUNT_8);

        std::vector<uint8_t> pixels(3 * width * height);
        rasterizer.render_shaded(transform.matrix(), persp, pixels);

        std::string outfile(TMP_DIR);
        outfile.append("rasterizer_multisample.png");
        stbi_write_png(
            outfile.c_str(), width, height, 3, pixels.data(), width * 3);
    }

    SECTION("change material")
    {
        std::vector<uint8_t> pixels(3 * width * height);
        Euclid::Material material;
        material.ambient << 0.2f, 0.0f, 0.0f;
        material.diffuse << 0.7f, 0.0f, 0.0f;
        rasterizer.set_material(material);
        rasterizer.render_shaded(transform.matrix(), persp, pixels);

        std::string outfile(TMP_DIR);
        outfile.append("rasterizer_material.png");
        stbi_write_png(
            outfile.c_str(), width, height, 3, pixels.data(), width * 3);
    }

    SECTION("change background")
    {
        std::vector<uint8_t> pixels(3 * width * height);
        rasterizer.set_background(0.0f, 0.3f, 0.4f);
        rasterizer.render_shaded(transform.matrix(), persp, pixels);

        std::string outfile(TMP_DIR);
        outfile.append("rasterizer_background.png");
        stbi_write_png(
            outfile.c_str(), width, height, 3, pixels.data(), width * 3);
    }

    SECTION("random vertex color")
    {
        std::random_device rd;
        std::minstd_rand rd_gen(rd());
        std::uniform_real_distribution rd_number(0.0, 1.0);
        std::vector<float> rd_colors(positions.size());
        for (auto& color : rd_colors) {
            color = rd_number(rd_gen);
        }
        rasterizer.attach_color_buffer(rd_colors.data(), rd_colors.size());
        std::vector<uint8_t> pixels(3 * width * height);
        rasterizer.render_shaded(transform.matrix(), persp, pixels);

        std::string outfile(TMP_DIR);
        outfile.append("rasterizer_vertex_color.png");
        stbi_write_png(
            outfile.c_str(), width, height, 3, pixels.data(), width * 3);
    }

    SECTION("unlit")
    {
        std::vector<uint8_t> pixels(3 * width * height);
        rasterizer.render_unlit(transform.matrix(), persp, pixels);

        std::string outfile(TMP_DIR);
        outfile.append("rasterizer_unlit.png");
        stbi_write_png(
            outfile.c_str(), width, height, 3, pixels.data(), width * 3);
    }

    SECTION("unlit vcolor")
    {
        std::random_device rd;
        std::minstd_rand rd_gen(rd());
        std::uniform_real_distribution rd_number(0.0, 1.0);
        std::vector<float> rd_colors(positions.size());
        for (auto& color : rd_colors) {
            color = rd_number(rd_gen);
        }
        rasterizer.attach_color_buffer(rd_colors.data(), rd_colors.size());
        std::vector<uint8_t> pixels(3 * width * height);
        rasterizer.render_unlit(transform.matrix(), persp, pixels);

        std::string outfile(TMP_DIR);
        outfile.append("rasterizer_unlit_vcolor.png");
        stbi_write_png(
            outfile.c_str(), width, height, 3, pixels.data(), width * 3);
    }

    SECTION("depth")
    {
        std::vector<uint8_t> pixels(3 * width * height);
        rasterizer.render_depth(transform.matrix(), persp, pixels);

        std::string outfile1(TMP_DIR);
        outfile1.append("rasterizer_depth_linear.png");
        stbi_write_png(
            outfile1.c_str(), width, height, 3, pixels.data(), width * 3);

        rasterizer.render_depth(transform.matrix(), persp, pixels, true, false);
        std::string outfile2(TMP_DIR);
        outfile2.append("rasterizer_depth.png");
        stbi_write_png(
            outfile2.c_str(), width, height, 3, pixels.data(), width * 3);

        std::vector<float> values(3 * width * height);
        rasterizer.render_depth(transform.matrix(), persp, values);
    }
}