/**
 *  sim_onlineca_multi.cpp
 *
 *  Simulate the LTL motion planning with collision avoidance
 *  with multiple moving obstacles.
 *
 *  Created by Yinan Li on Feb. 27, 2021.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <algorithm>
#include <utility>
#include <cmath>
#include <sys/stat.h>
#include <boost/numeric/odeint.hpp>
#include <cstdlib>
#include <ctime>

#include "src/grid.h"
#include "src/definitions.h"
#include "src/abstraction.hpp"
#include "src/DBAparser.h"
#include "src/bsolver.hpp"
#include "src/patcher.h"
#include "src/hdf5io.h"

#include "car.hpp"
#include "odes.hpp"

int main(int argc, char *argv[])
{
    std::string specfile, ctlrfile, cafile, graphfile;
    specfile = "dba1.txt";
    ctlrfile = "controller_dba1_0.2-0.2-0.2.h5";
    graphfile = "gwin.h5";
    const double eta[] = {0.2, 0.2, 0.2};

    cafile = "controller_safety_abst-0.8-0.9-0.1-0.1.h5";
    const double eta_r[] = {0.1, 0.1, 0.1};

    /* Input arguments:
     * ./simMulti dbafile ctlrfile cafile graphfile eta[0] eta_r[0]
     */
    if(argc!=1) {
	if(argc==7) {
	    specfile = std::string(argv[1]);
	    ctlrfile = std::string(argv[2]);
	    cafile = std::string(argv[3]);
	    graphfile = std::string(argv[4]);
	    eta[0] = std::atof(argv[5]);
	    eta[1] = std::atof(argv[5]);
	    eta[2] = std::atof(argv[5]);
	    eta_r[0] = std::atof(argv[6]);
	    eta_r[1] = std::atof(argv[6]);
	    eta_r[2] = std::atof(argv[6]);
	} else {
	    std::cout << "Improper number of arguments.\n";
	    std::exit(1);
	}
    }
    std::cout << "Simulate " << specfile << " with controller " << ctlrfile << '\n';

    /**
     * Setup the motion planning workspace
     **/
    const int x_dim = 3;
    const double theta = 3.5;
    const double xlb[] = {0, 0, -theta};
    const double xub[] = {10, 10, theta};
    /* set the control values */
    const int u_dim = 2;
    const double ulb[] = {-1.0, -1.0};
    double uub[] = {1.0, 1.0};
    /* discretization precision */
    double mu[] = {0.3, 0.3};
    /* generate grid */
    rocs::grid x_grid(x_dim,eta,xlb,xub);
    x_grid.gridding();
    rocs::grid u_grid(u_dim,mu,ulb,uub);
    std::cout << "Number of discrete states: " << x_grid._nv << "\n";
    std::cout << "Number of discrete inputs: " << u_grid._nv << "\n";

    clock_t tb, te;

    /**
     * Load specification
     **/
    std::cout << "\nReading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
	std::exit(1);
    boost::dynamic_bitset<> isacc(nNodes, false);
    for (rocs::UintSmall i = 0; i < acc.size(); ++i)
	isacc[acc[i]] = true;

    /**
     * Load global controller
     **/
    std::cout << "\nLoading global controller...\n";
    std::vector<long long> w_x0, encode3;
    std::vector<NODE_POST> nts_ctrlr;
    std::vector<CTRL> ctrl;
    std::vector<int> q_prime;
    rocs::h5FileHandler planRdr(ctlrfile, H5F_ACC_RDONLY);
    planRdr.read_discrete_controller(w_x0, encode3, nts_ctrlr, ctrl, q_prime);

    /**
     * Load local safety controller
     **/
    std::cout << "\nLoading local safety controller...\n";
    rocs::h5FileHandler reader(cafile, H5F_ACC_RDONLY);
    std::vector<size_t> optCtlr;
    std::vector<double> value;
    boost::dynamic_bitset<> safeCtlr;
    boost::dynamic_bitset<> win;
    size_t cdims[2];
    reader.read_discrete_controller(win, safeCtlr, cdims, optCtlr, value);
    double xrlb[] = {-3, -3, -theta};
    double xrub[] = {3, 3, theta};
    rocs::grid rel_grid(3, eta_r, xrlb, xrub);
    rel_grid.gridding();

    /**
     * Launch patcher
     **/
    rocs::Patcher local;
    struct stat buffer;
    if(stat(graphfile.c_str(), &buffer) == 0) {
    	/* Read from a file */
    	std::cout << "\nReading winning graph...\n";
    	rocs::h5FileHandler graphRdr(graphfile, H5F_ACC_RDONLY);
    	tb = clock();
    	graphRdr.read_winning_graph(local);
    	te = clock();
    	std::cout << "Time of reading graph: " << (float)(te - tb)/CLOCKS_PER_SEC << '\n';
    } else {
    	std::cout << "Graph file doesn't exist.\n";
    	return 1;
    }
    int horizon = 3;  //set forward propagation horizon

    /**
     * Simulation
     **/
    const double drange[] = {3.0, 3.0};
    /* set ode solver */
    const double tsim = 0.1; //simulation time
    const double dt = 0.001; //integration step size for odeint
    boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;
    boost::dynamic_bitset<> u_safe(cdims[1]);

    /* Initial condition of the ego robot */
    rocs::Rn x{1, 1, M_PI/3.0};
    rocs::Rn u{0, 0};
    long long x_index = x_grid.val_to_id(x);
    std::vector<long long>::iterator p1;
    p1 = std::lower_bound(w_x0.begin(), w_x0.end(), x_index);
    if(*p1 != x_index) {
	std::cout << x[0] <<  " "  << x[1] << " " << x[2];
	std::cout << " is not in the winning set, change a new initial state." << std::endl;
	return 1;
    }
    /* Initial condition of 3 obstacles */
    rocs::Rn uomax{0.8, 0.8};
    rocs::Rn uomin{-0.8, -0.8};
    double v;

    /* State in local relative coordinate */
    rocs::Rn xr1{0,0,0};
    rocs::Rn xr2{0,0,0};
    rocs::Rn xr3{0,0,0};
    long long xr_id1, xr_id2, xr_id3;

    /* obstacle 1 */
    rocs::Rn xo1{3.0, 10.0, -1.5};
    rocs::Rn uo1{0.1, 0.0};
    auto obstacle_behavior1 = [&uo1, tsim](int j) {
				  double t = tsim*j;
				  if(t >= 0) {
				      uo1[0] = 0.2;
				      uo1[1] = 0.0;
				  }
			      };

    /* obstacle 2 */
    rocs::Rn xo2{5.3, 0.0, 2.0};
    rocs::Rn uo2{0.5, 0.0};
    auto obstacle_behavior2 = [&uo2, tsim](int j) {
				  double t = tsim*j;
				  if(t >= 0) {
				      if(t < 14) {
					  uo2[0] = 0.5;
					  uo2[1] = 0.0;
				      } else {
					  uo2[0] = 0.5;
					  uo2[1] = 0.6;
				      }
				  }
			      };

    /* obstacle 3 */
    rocs::Rn xo3{10.0, 6.4, M_PI};
    rocs::Rn uo3{0.15, 0.0};
    auto obstacle_behavior3 = [&uo3, tsim](int j) {
				  double t = tsim*j;
				  if(t >= 0) {
				      if(t < 31) {
					  uo3[0] = 0.2;
					  uo3[1] = 0.0;
				      } else if(t < 35) {
					  uo3[0] = 0.2;
					  uo3[1] = 0.3;
				      } else {
					  uo3[0] = 0.2;
					  uo3[1] = 0.0;
				      }
				  }
			      };

    /* Open files for writing results and logs */
    std::string simfile = "traj_closedloop_ca_4.txt";
    std::ofstream ctlrWtr(simfile);
    if(!ctlrWtr.is_open())
	ctlrWtr.open(simfile, std::ios::out);
    std::string simother1 = "traj_other_robot_4_1.txt";
    std::string simother2 = "traj_other_robot_4_2.txt";
    std::string simother3 = "traj_other_robot_4_3.txt";
    std::ofstream otherWtr1(simother1);
    std::ofstream otherWtr2(simother2);
    std::ofstream otherWtr3(simother3);
    if(!otherWtr1.is_open())
    	otherWtr1.open(simother1, std::ios::out);
    if(!otherWtr2.is_open())
    	otherWtr2.open(simother2, std::ios::out);
    if(!otherWtr3.is_open())
    	otherWtr3.open(simother3, std::ios::out);
    /********** Logging **********/
    std::ofstream logger;
    logger.open("logs_4.txt", std::ios::out);
    /********** Logging **********/

    /* Simulation loop */
    srand(time(NULL));
    float tpat = 0;
    int max_num_achieve_acc=5, max_num_iteration=1500; //3000000;
    rocs::UintSmall q;
    int i, j;
    double t0, tc; //record the time when an obstacle detected
    bool o1, o2, o3;
    o1 = false;
    o2 = false;
    o3 = false;
    std::vector<CTRL>::iterator p7;
    NODE_POST p5;
    std::cout << "\nLaunching simulation...\n";
    for(q = q0, i = j = 0; i<max_num_achieve_acc && j<max_num_iteration; ++j) {
	/* choose obstacle trajectory */
	obstacle_behavior1(j);
	obstacle_behavior2(j);
	obstacle_behavior3(j);

	/* Determine the control u by the product state (x_index, q) */
	p5 = nts_ctrlr[encode3[x_index]];
	p7 = std::lower_bound(ctrl.begin()+p5.pos, ctrl.begin()+p5.pos+p5.num_a,
			      q, [](const CTRL &item, const int val) {
				     return item.q < val;});
	if(p7->q != q) {
	    // std::cout << j << "th iteration: " << "Reach accepting state " << i << " times.\n";
	    // std::cout << "Automaton state: " << q << '\n';
	    // std::cout << "Position in ctrl list: " << p5.pos << ", number of actions: " << p5.num_a << '\n';
	    std::cout << "Error in ctrl" << std::endl;
	    break;
	}
	u_grid.id_to_val(u, p7->u);

	/* Print to screen */
	std::cout << "# " << j << ":\n";
	std::cout << "x: [" << x[0] <<  ','  << x[1] << ',' << x[2] << "]\n";
	std::cout << "Nominal control: " << p7->u << '('
		  << '[' << u[0] << ',' << u[1] << "])\n";
	std::cout << "current dba state: " << q << "\n";

	tc = j*tsim; //record the current time

	/* Get the relative state xr */
	xr1[0] = xo1[0]-x[0];
	xr1[1] = xo1[1]-x[1];
	xr1[2] = xo1[2]-x[2];
	xr2[0] = xo2[0]-x[0];
	xr2[1] = xo2[1]-x[1];
	xr2[2] = xo2[2]-x[2];
	xr3[0] = xo3[0]-x[0];
	xr3[1] = xo3[1]-x[1];
	xr3[2] = xo3[2]-x[2];
	if(xr1[2] > M_PI) //convert angle into [-pi, pi]
	    xr1[2] -= 2*M_PI;
	if(xr1[2] < -M_PI)
	    xr1[2] += 2*M_PI;
	if(xr2[2] > M_PI) //convert angle into [-pi, pi]
	    xr2[2] -= 2*M_PI;
	if(xr2[2] < -M_PI)
	    xr2[2] += 2*M_PI;
	if(xr3[2] > M_PI) //convert angle into [-pi, pi]
	    xr3[2] -= 2*M_PI;
	if(xr3[2] < -M_PI)
	    xr3[2] += 2*M_PI;
	inertia_to_body(xr1, x); //convert from the inertial to body frame
	inertia_to_body(xr2, x);
	inertia_to_body(xr3, x);

	/* Deal with obstacles */
	if(std::fabs(xr1[0])*std::fabs(xr1[0]) +
	   std::fabs(xr1[1])*std::fabs(xr1[1]) < drange[0]*drange[1]) {
	    o1 = true;
	    std::cout << "Obstacle 1 detected.\n";
	    std::cout << "Relative coordinate: ";
	    if(rel_grid._bds.isout(xr1)) { // xr is the out-of-domain node
		std::cout << rel_grid._nv;
	    } else {
		xr_id1 = rel_grid.val_to_id(xr1); //uniform grid rel_grid
		std::cout << xr_id1;
	    }
	    std::cout << "([" << xr1[0] << ',' << xr1[1] << ',' << xr1[2] << "])\n";
	} else {
	    o1 = false;
	}//end if obstacle is within range

	if(std::fabs(xr2[0])*std::fabs(xr2[0]) +
	   std::fabs(xr2[1])*std::fabs(xr2[1]) < drange[0]*drange[1]) {
	    o2 = true;
	    std::cout << "Obstacle 2 detected.\n";
	    std::cout << "Relative coordinate: ";
	    if(rel_grid._bds.isout(xr2)) { // xr is the out-of-domain node
		std::cout << rel_grid._nv;
	    } else {
		xr_id2 = rel_grid.val_to_id(xr2); //uniform grid rel_grid
		std::cout << xr_id2;
	    }
	    std::cout << "([" << xr2[0] << ',' << xr2[1] << ',' << xr2[2] << "])\n";
	} else {
	    o2 = false;
	}//end if obstacle is within range

	if(std::fabs(xr3[0])*std::fabs(xr3[0]) +
	   std::fabs(xr3[1])*std::fabs(xr3[1]) < drange[0]*drange[1]) {
	    o3 = true;
	    std::cout << "Obstacle 3 detected.\n";
	    std::cout << "Relative coordinate: ";
	    if(rel_grid._bds.isout(xr3)) { // xr is the out-of-domain node
		std::cout << rel_grid._nv;
	    } else {
		xr_id3 = rel_grid.val_to_id(xr3); //uniform grid rel_grid
		std::cout << xr_id3;
	    }
	    std::cout << "([" << xr3[0] << ',' << xr3[1] << ',' << xr3[2] << "])\n";
	} else {
	    o3 = false;
	}//end if obstacle is within range

	/* Perform RH (receding horizon) */
	if(o1||o2||o3) {
	    /********** Logging **********/
	    logger << tc << '(' << j << "): "
	       << x_index << '(' << x[0] << ',' << x[1] << ',' << x[2] << "), ";
	    if(o1) {
		logger << "o1," << xr_id1 << '(' << xr1[0] << ',' << xr1[1] << ',' << xr1[2] << "), ";
		for(size_t k = 0; k < cdims[1]; ++k)
		    logger << safeCtlr[xr_id1*cdims[1]+k];
		logger << '\n';
	    }
	    if(o2) {
		logger << "o2," << xr_id2 << '(' << xr2[0] << ',' << xr2[1] << ',' << xr2[2]<< "), ";
		for(size_t k = 0; k < cdims[1]; ++k)
		    logger << safeCtlr[xr_id2*cdims[1]+k];
		logger << '\n';
	    }
	    if(o3) {
		logger << "o3," << xr_id3 << '(' << xr3[0] << ',' << xr3[1] << ',' << xr3[2] << ")";
		for(size_t k = 0; k < cdims[1]; ++k)
		    logger << safeCtlr[xr_id3*cdims[1]+k];
		logger << '\n';
	    }
	    std::cout << "o1:"<< o1 <<", o2:"<< o2 <<", o3:" << o3 <<'\n';
	    // std::cout << "Current state id of the robot:" << xp << ", key:" << key << '\n';
	    /********** Logging **********/

	    size_t xp = x_index*nNodes+q; // (idx,idq)=idx*nNodes+idq (buchi.c)
	    size_t uid;

	    /********** Logging **********/
	    size_t row, key = local._encode[xp];
	    row = local._idmap[key];
	    // std::cout << "id_n0n1: " << xp << ',' << " id_np: " << key << '\n';
	    for(size_t k = 0; k < u_grid._nv; ++k) {
		if(local._winfts._npost[row*local._na+k]) {//outdeg>0
		    logger << k << ' ';
		}
	    }
	    logger << ", ";
	    /********** Logging **********/

	    /* Determine safe inputs for RH module */
	    std::cout << "Use RH to find safe control inputs.\n ";
	    u_safe.set();
	    int nSafeAct = 0;
	    rocs::Rn uu(u_dim);
	    double dtemp, u_dist=(uub[0]-ulb[0])*(uub[0]-ulb[0])+(uub[1]-ulb[1])*(uub[1]-ulb[1]);
	    for(size_t k = 0; k < cdims[1]; ++k) {
		if(o1)
		    u_safe[k] = u_safe[k] & safeCtlr[xr_id1*cdims[1]+k];
		if(o2)
		    u_safe[k] = u_safe[k] & safeCtlr[xr_id2*cdims[1]+k];
		if(o3)
		    u_safe[k] = u_safe[k] & safeCtlr[xr_id3*cdims[1]+k];

		if(u_safe[k]) {
		    /************* Logging **************/
		    // std::cout << k << ',';
		    // std::cout << "Outdeg: "
		    // 	      << local._winfts._npost[row*local._na+k]
		    // 	      << '\n';
		    /************* Logging **************/
		    if(local._winfts._npost[row*local._na+k]) {//outdeg>0
			++nSafeAct; //count the # of safe valid controls
			/* take the closest to the original one */
			u_grid.id_to_val(uu, k);
			dtemp = (uu[0]-u[0])*(uu[0]-u[0])+(uu[1]-u[1])*(uu[1]-u[1]);
			if(dtemp < u_dist) {
			    uid = k;
			    u_dist= dtemp;
			}
			/************* Logging **************/
			// std::cout << "Post nodes: ";
			// size_t pidstart=local._winfts._ptrpost[row*local._na+k];
			// for(size_t pid=pidstart; pid<pidstart+local._winfts._npost[row*local._na+k]; ++pid)
			//     std::cout << local._winfts._idpost[pid] << ' ';
			// std::cout << '\n';
			/************* Logging **************/
		    }
		    // /************* Logging **************/
		    // logger << k << ' ';
		    // /************* Logging **************/
		}
		/********** Logging **********/
		logger << u_safe[k];
		/********** Logging **********/
	    }
	    /********** Logging **********/
	    u_grid.id_to_val(uu, uid);
	    logger << '\n'
		   << p7->u << ' ' << u[0] << ',' << u[1] << ';'
		   << uid << ' ' << uu[0] << ',' << uu[1] << ' ' << u_dist << ';';
	    /********** Logging **********/

	    /* Get a safe control from patcher */
	    if(u_safe[p7->u]) {
		std::cout << "The static strategy is safe.\n";
		// u_grid.id_to_val(u, uid);
		logger << p7->u << ' ';
	    } else if(nSafeAct > 1) {
		tb = clock();
		int suc = local.solve_local_reachability(xp, horizon, u_safe);
		te = clock();
		tpat += (float)(te - tb)/CLOCKS_PER_SEC;
		std::cout << "Average time for RH: "
			  << tpat/(j+1) << ".\n";
		if(!suc) {//if fails in re-planning, use the closest u
		    std::cout << "Apply the safe control input closest to the original one.\n";
		    u_grid.id_to_val(u, uid);
		    logger << uid << ' ';
		} else {
		    std::cout << "Apply controls returned by local reachability.\n";
		    u_grid.id_to_val(u, local._ctlr);
		    logger << local._ctlr << ' ';
		}
	    } else if(nSafeAct > 0) {
		std::cout << "Apply the only one safe control.\n";
		u_grid.id_to_val(u, uid);
		logger << uid << ' ';
	    } else {
		std::cout << "No safe controls. Freeze.\n";
		u[0] = 0;
		u[1] = 0;
		logger << p7->u << ' ';
	    }
	    /********** Logging **********/
	    logger << u[0] << ',' << u[1] << "\n\n";
	    /********** Logging **********/
	}//end RH

	/* Write to txt file */
	// ctlrWtr << j << ':';
	for(int d = 0; d < x_dim; ++d) {
	    /* the controlled state */
	    ctlrWtr << x[d];
	    /* the state of the other robot */
	    otherWtr1 << xo1[d];
	    otherWtr2 << xo2[d];
	    otherWtr3 << xo3[d];
	    if(d<x_dim-1) {
		ctlrWtr << ',';
		otherWtr1 << ',';
		otherWtr2 << ',';
		otherWtr3 << ',';
	    }
	}
	ctlrWtr << ';';
	otherWtr1 << ';';
	otherWtr2 << ';';
	otherWtr3 << ';';
	for(int d = 0; d < u_dim; ++d) {
	    ctlrWtr << u[d];
	    otherWtr1 << uo1[d];
	    otherWtr2 << uo2[d];
	    otherWtr3 << uo3[d];
	    if(d<u_dim-1) {
		ctlrWtr << ',';
		otherWtr1 << ',';
		otherWtr2 << ',';
		otherWtr3 << ',';
	    }
	}
	ctlrWtr << '\n';
	otherWtr1 << '\n';
	otherWtr2 << '\n';
	otherWtr3 << '\n';

	/* Integrate ego robot trajectory */
	boost::numeric::odeint::integrate_const(rk45, car_dynamics(u), x, 0.0, tsim, dt);
	if(x[2] > M_PI) //convert angle into [-pi, pi]
	    x[2] -= 2*M_PI;
	if(x[2] < -M_PI)
	    x[2] += 2*M_PI;

	/* integrate obstacle trajectory after it appears */
	boost::numeric::odeint::integrate_const(rk45, car_dynamics(uo1), xo1, 0.0, tsim, dt);
	if(xo1[2] > M_PI) //convert angle into [-pi, pi]
	    xo1[2] -= 2*M_PI;
	if(xo1[2] < -M_PI)
	    xo1[2] += 2*M_PI;
	boost::numeric::odeint::integrate_const(rk45, car_dynamics(uo2), xo2, 0.0, tsim, dt);
	if(xo2[2] > M_PI) //convert angle into [-pi, pi]
	    xo2[2] -= 2*M_PI;
	if(xo2[2] < -M_PI)
	    xo2[2] += 2*M_PI;
	boost::numeric::odeint::integrate_const(rk45, car_dynamics(uo3), xo3, 0.0, tsim, dt);
	if(xo3[2] > M_PI) //convert angle into [-pi, pi]
	    xo3[2] -= 2*M_PI;
	if(xo3[2] < -M_PI)
	    xo3[2] += 2*M_PI;

	/* Update automaton state q */
	x_index = x_grid.val_to_id(x);
	q = q_prime[p5.label*nNodes + q];
	if(isacc[q])
	    std::cout << "******** achieve acc \'" << q << "\' " << ++i << " time ********" << std::endl;
    }

    ctlrWtr.close();
    otherWtr1.close();
    otherWtr2.close();
    otherWtr3.close();
    logger.close();

    return 0;
}


#include <CUnit/Basic.h>
#include <iostream>
#include <stdlib.h>
#include "ddhot.hpp"
#include "ddhcry.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
#include "blake3.h"

/** Hashes in of size len into out using Blake3

    @param out Output array
    @param in Input array
    @param len Size of input array
*/
void blake3(uint8_t *out, const uint8_t *in, size_t len)
{
  blake3_hasher hasher;
  blake3_hasher_init(&hasher);
  blake3_hasher_update(&hasher, in, len);
  blake3_hasher_finalize(&hasher, out, BLAKE3_OUT_LEN);
}

/** Tests OT from DDH as in [PVW08] */
void ddhot_test()
{
  const size_t numtests = 1000;
  ec_params_t params(NID_X9_62_prime256v1);
  crs_t crs(params, 32);

  EC_POINT *g, *h, *u, *v, *u1, *v1;
  int b;
  BIGNUM *m, *m1, *m2;

  boost::mt19937 rng;
  boost::uniform_int<> dist(0,1);
  boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
      bit(rng, dist);

  g = EC_POINT_new(params.group);
  h = EC_POINT_new(params.group);
  u = EC_POINT_new(params.group);
  v = EC_POINT_new(params.group);
  u1 = EC_POINT_new(params.group);
  v1 = EC_POINT_new(params.group);
  m = BN_new();
  m1 = BN_new();
  m2 = BN_new();

  for (size_t i = 0; i < numtests; i++) {
      alice_ot_t alice(crs);
      bob_ot_t bob(crs);
      BN_rand(m, 32, BN_RAND_TOP_ANY, BN_RAND_BOTTOM_ANY);
      BN_rand(m1, 32, BN_RAND_TOP_ANY, BN_RAND_BOTTOM_ANY);

      b = bit();

      alice.msg1(g, h, b);
      bob.msg1(u, v,
              u1, v1,
              g, h,
              m, m1);
      alice.msg2(m2,
              u, v,
              u1, v1);

      BIGNUM *tmp = nullptr;
      if (b == 0) {
          tmp = m;
      } else {
          tmp = m1;
      }
      BN_sub(m2, m2, tmp);

      CU_ASSERT(BN_is_zero(m2));
  }

  EC_POINT_free(g);
  EC_POINT_free(h);
  EC_POINT_free(u);
  EC_POINT_free(v);
  EC_POINT_free(u1);
  EC_POINT_free(v1);
  BN_free(m);
  BN_free(m1);
  BN_free(m2);
}

/** Implements ROT from DDH OT in [PVW08] using
    black box transform */
void ddhrot_test()
{
  const size_t numtests = 1000;
  ec_params_t params(NID_X9_62_prime256v1);
  crs_t crs(params, 32);

  EC_POINT *g, *h, *u, *v, *u1, *v1;
  int b, c_sender;
  uint8_t r0_receiver[32], r1_receiver[32], r_sender[32];
  uint8_t r0_receiver_hash[32], r1_receiver_hash[32], c_pipe_r_sender_hash[32], c_pipe_r_receiver_hash[32], c_pipe_r_receiver_hash_generated[32];
  uint8_t r0_sender_hash[32], r1_sender_hash[32];
  BIGNUM *m, *m1, *m2, *check;

  boost::mt19937 rng;
  boost::uniform_int<> dist(0,1);
  boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
      bit(rng, dist);

  boost::uniform_int<> dist_char(0,255);
  boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
      byte(rng, dist);

  g = EC_POINT_new(params.group);
  h = EC_POINT_new(params.group);
  u = EC_POINT_new(params.group);
  v = EC_POINT_new(params.group);
  u1 = EC_POINT_new(params.group);
  v1 = EC_POINT_new(params.group);
  m = BN_new();
  m1 = BN_new();
  m2 = BN_new();
  check = BN_new();

  for (size_t i = 0; i < numtests; i++) {
      // Sender
      c_sender = bit();

      // Receiver
      for (size_t j = 0; j < 32; j++) {
          r0_receiver[j] = byte();
          r1_receiver[j] = byte();
      }
      blake3(r0_receiver_hash, r0_receiver, 32);
      blake3(r1_receiver_hash, r1_receiver, 32);

      uint8_t r0_sender_hash[32], r1_sender_hash[32];
      memcpy(r0_sender_hash, r0_receiver_hash, 32);
      memcpy(r1_sender_hash, r1_receiver_hash, 32);

      /*
       * START Standard OT
       */
      alice_ot_t alice(crs);
      bob_ot_t bob(crs);
      BN_rand(m, 32, BN_RAND_TOP_ANY, BN_RAND_BOTTOM_ANY);
      BN_rand(m1, 32, BN_RAND_TOP_ANY, BN_RAND_BOTTOM_ANY);

      b = bit();

      alice.msg1(g, h, b);
      bob.msg1(u, v,
              u1, v1,
              g, h,
              m, m1);
      alice.msg2(m2,
              u, v,
              u1, v1);

      BIGNUM *tmp = nullptr;
      if (b == 0) {
          tmp = m;
      } else {
          tmp = m1;
      }
      BN_sub(check, m2, tmp);

      CU_ASSERT(BN_is_zero(check));
      /*
       * END Standard OT
       */
      int m_bin_size = BN_num_bytes(m);
      int m1_bin_size = BN_num_bytes(m1);

      CU_ASSERT(m_bin_size <= 32);
      CU_ASSERT(m1_bin_size <= 32);

      // Sender
      // Receive r0 and r1
      uint8_t r0_sender[32], r1_sender[32];
      memcpy(r0_sender, r0_receiver, 32);
      memcpy(r1_sender, r1_receiver, 32);

      uint8_t rc_Mc_xor_result[32], other_rc_Mc_xor_result[32];
      uint8_t m_bin_sender[32], m1_bin_sender[32];

      blake3(r0_sender_hash, r0_sender, 32);
      blake3(r1_sender_hash, r1_sender, 32);

      bool r0_all_equal = true;
      bool r1_all_equal = true;
      for (size_t j = 0; j < 32; j++) {
          r0_all_equal &= (r0_sender_hash[j] == r0_receiver_hash[j]);
          r1_all_equal &= (r1_sender_hash[j] == r1_receiver_hash[j]);

          m_bin_sender[j] = 0;
          m1_bin_sender[j] = 0;
      }
      CU_ASSERT(r0_all_equal);
      CU_ASSERT(r1_all_equal);

      BN_bn2bin(m, m_bin_sender);
      BN_bn2bin(m1, m1_bin_sender);

      if (c_sender == 0) {
          for (size_t j = 0; j < 32; j++) {
              rc_Mc_xor_result[j] = r0_sender[j] ^ m_bin_sender[j];
              other_rc_Mc_xor_result[j] = r1_sender[j] ^ m1_bin_sender[j];
          }
      } else {
          for (size_t j = 0; j < 32; j++) {
              rc_Mc_xor_result[j] = r1_sender[j] ^ m1_bin_sender[j];
              other_rc_Mc_xor_result[j] = r0_sender[j] ^ m_bin_sender[j];
          }
      }

      // r_{c ^ 0} ^ M_{c ^ 0}
      // r_{c ^ 1} ^ M_{c ^ 1}

      // Receiver
      int c_xor_b_result;
      uint8_t rb_Mb_xor_result[32], m2_bin_receiver[32];

      int c_receiver;
      memcpy(&c_receiver, &c_sender, 4);

      memset(m2_bin_receiver, 0x00, 32);
      BN_bn2bin(m2, m2_bin_receiver);

      // c ^ b
      c_xor_b_result = c_receiver ^ b;
      // r_b ^ M_b
      if (b == 0) {
          for (size_t j = 0; j < 32; j++) {
              rb_Mb_xor_result[j] = r0_receiver[j] ^ m2_bin_receiver[j];
          }
      } else {
          for (size_t j = 0; j < 32; j++) {
              rb_Mb_xor_result[j] = r1_receiver[j] ^ m2_bin_receiver[j];
          }
      }

      // Final Check
      bool m_all_equal = true;
      if (c_xor_b_result == 0) {
          for (size_t j = 0; j < 32; j++) {
              m_all_equal &= (rb_Mb_xor_result[j] == rc_Mc_xor_result[j]);
          }
      } else {
          for (size_t j = 0; j < 32; j++) {
              m_all_equal &= (rb_Mb_xor_result[j] == other_rc_Mc_xor_result[j]);
          }
      }
      CU_ASSERT(m_all_equal);
  }

  EC_POINT_free(g);
  EC_POINT_free(h);
  EC_POINT_free(u);
  EC_POINT_free(v);
  EC_POINT_free(u1);
  EC_POINT_free(v1);
  BN_free(m);
  BN_free(m1);
  BN_free(m2);
  BN_free(check);
}

int main(int argc, char *argv[])
{
  if (CUE_SUCCESS != CU_initialize_registry())
    return CU_get_error();

#ifdef OT_TEST
  CU_pSuite suite1 = CU_add_suite("DDHOT", NULL, NULL);
  if (suite1 == NULL) abort();

  if ((NULL == CU_add_test(suite1, "ddhot", ddhot_test)))
    {
      abort();
    }
#else
  CU_pSuite suite2 = CU_add_suite("DDHROT", NULL, NULL);
  if (suite2 == NULL) abort();

  if ((NULL == CU_add_test(suite2, "ddhrot", ddhrot_test)))
    {
      abort();
    }
#endif

  CU_basic_set_mode(CU_BRM_VERBOSE);
  CU_basic_run_tests();
  CU_cleanup_registry();
  return CU_get_error();
}


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// 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_GIL_GIL_CHANNEL_ALGORITHM_HPP
#define BOOST_GIL_GIL_CHANNEL_ALGORITHM_HPP

#include <boost/gil/channel.hpp>
#include <boost/gil/promote_integral.hpp>
#include <boost/gil/typedefs.hpp>

#include <boost/mpl/less.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/type_traits.hpp>

#include <limits>

namespace boost { namespace gil {

namespace detail {

// some forward declarations
template <typename SrcChannelV, typename DstChannelV, bool SrcIsIntegral, bool DstIsIntegral> struct channel_converter_unsigned_impl;
template <typename SrcChannelV, typename DstChannelV, bool SrcIsGreater> struct channel_converter_unsigned_integral;
template <typename SrcChannelV, typename DstChannelV, bool SrcLessThanDst, bool SrcDivisible> struct channel_converter_unsigned_integral_impl;
template <typename SrcChannelV, typename DstChannelV, bool SrcLessThanDst, bool CannotFitInInteger> struct channel_converter_unsigned_integral_nondivisible;

//////////////////////////////////////
////  unsigned_integral_max_value - given an unsigned integral channel type, returns its maximum value as an MPL integral constant
//////////////////////////////////////

template <typename UnsignedIntegralChannel>
struct unsigned_integral_max_value : public mpl::integral_c<UnsignedIntegralChannel,std::numeric_limits<UnsignedIntegralChannel>::max()> {};

template <>
struct unsigned_integral_max_value<uint8_t> : public mpl::integral_c<uint32_t,0xFF> {};
template <>
struct unsigned_integral_max_value<uint16_t> : public mpl::integral_c<uint32_t,0xFFFF> {};
template <>
struct unsigned_integral_max_value<uint32_t> : public mpl::integral_c<uintmax_t,0xFFFFFFFF> {};

template <int K>
struct unsigned_integral_max_value<packed_channel_value<K> >
    : public mpl::integral_c<typename packed_channel_value<K>::integer_t, (uint64_t(1)<<K)-1> {};

//////////////////////////////////////
////  unsigned_integral_num_bits - given an unsigned integral channel type, returns the minimum number of bits needed to represent it
//////////////////////////////////////

template <typename UnsignedIntegralChannel>
struct unsigned_integral_num_bits : public mpl::int_<sizeof(UnsignedIntegralChannel)*8> {};

template <int K>
struct unsigned_integral_num_bits<packed_channel_value<K> >
    : public mpl::int_<K> {};

} // namespace detail

/**
\defgroup ChannelConvertAlgorithm channel_convert
\brief Converting from one channel type to another
\ingroup ChannelAlgorithm

Conversion is done as a simple linear mapping of one channel range to the other,
such that the minimum/maximum value of the source maps to the minimum/maximum value of the destination.
One implication of this is that the value 0 of signed channels may not be preserved!

When creating new channel models, it is often a good idea to provide specializations for the channel conversion algorithms, for
example, for performance optimizations. If the new model is an integral type that can be signed, it is easier to define the conversion
only for the unsigned type (\p channel_converter_unsigned) and provide specializations of \p detail::channel_convert_to_unsigned
and \p detail::channel_convert_from_unsigned to convert between the signed and unsigned type.

Example:
\code
// float32_t is a floating point channel with range [0.0f ... 1.0f]
float32_t src_channel = channel_traits<float32_t>::max_value();
assert(src_channel == 1);

// uint8_t is 8-bit unsigned integral channel (typedef-ed from unsigned char)
uint8_t dst_channel = channel_convert<uint8_t>(src_channel);
assert(dst_channel == 255);     // max value goes to max value
\endcode
*/

/**
\defgroup ChannelConvertUnsignedAlgorithm channel_converter_unsigned
\ingroup ChannelConvertAlgorithm
\brief Convert one unsigned/floating point channel to another. Converts both the channel type and range
 @{
 */

//////////////////////////////////////
////  channel_converter_unsigned
//////////////////////////////////////

template <typename SrcChannelV, typename DstChannelV>     // Model ChannelValueConcept
struct channel_converter_unsigned
    : public detail::channel_converter_unsigned_impl<SrcChannelV,DstChannelV,is_integral<SrcChannelV>::value,is_integral<DstChannelV>::value> {};

/// \brief Converting a channel to itself - identity operation
template <typename T> struct channel_converter_unsigned<T,T> : public detail::identity<T> {};

namespace detail {

//////////////////////////////////////
////  channel_converter_unsigned_impl
//////////////////////////////////////

/// \brief This is the default implementation. Performance specializatons are provided
template <typename SrcChannelV, typename DstChannelV, bool SrcIsIntegral, bool DstIsIntegral>
struct channel_converter_unsigned_impl {
    typedef SrcChannelV argument_type;
    typedef DstChannelV result_type;
    DstChannelV operator()(SrcChannelV src) const {
        return DstChannelV(channel_traits<DstChannelV>::min_value() +
            (src - channel_traits<SrcChannelV>::min_value()) / channel_range<SrcChannelV>() * channel_range<DstChannelV>());
    }
private:
    template <typename C>
    static double channel_range() {
        return double(channel_traits<C>::max_value()) - double(channel_traits<C>::min_value());
    }
};

// When both the source and the destination are integral channels, perform a faster conversion
template <typename SrcChannelV, typename DstChannelV>
struct channel_converter_unsigned_impl<SrcChannelV,DstChannelV,true,true>
    : public channel_converter_unsigned_integral<SrcChannelV,DstChannelV,
    mpl::less<unsigned_integral_max_value<SrcChannelV>,unsigned_integral_max_value<DstChannelV> >::value > {};

//////////////////////////////////////
////  channel_converter_unsigned_integral
//////////////////////////////////////

template <typename SrcChannelV, typename DstChannelV>
struct channel_converter_unsigned_integral<SrcChannelV,DstChannelV,true>
    : public channel_converter_unsigned_integral_impl<SrcChannelV,DstChannelV,true,
    !(unsigned_integral_max_value<DstChannelV>::value % unsigned_integral_max_value<SrcChannelV>::value) > {};

template <typename SrcChannelV, typename DstChannelV>
struct channel_converter_unsigned_integral<SrcChannelV,DstChannelV,false>
    : public channel_converter_unsigned_integral_impl<SrcChannelV,DstChannelV,false,
    !(unsigned_integral_max_value<SrcChannelV>::value % unsigned_integral_max_value<DstChannelV>::value) > {};

//////////////////////////////////////
////  channel_converter_unsigned_integral_impl
//////////////////////////////////////

// Both source and destination are unsigned integral channels,
// the src max value is less than the dst max value,
// and the dst max value is divisible by the src max value
template <typename SrcChannelV, typename DstChannelV>
struct channel_converter_unsigned_integral_impl<SrcChannelV,DstChannelV,true,true> {
    DstChannelV operator()(SrcChannelV src) const {
        typedef typename unsigned_integral_max_value<DstChannelV>::value_type integer_t;
        static const integer_t mul = unsigned_integral_max_value<DstChannelV>::value / unsigned_integral_max_value<SrcChannelV>::value;
        return DstChannelV(src * mul);
    }
};

// Both source and destination are unsigned integral channels,
// the dst max value is less than (or equal to) the src max value,
// and the src max value is divisible by the dst max value
template <typename SrcChannelV, typename DstChannelV>
struct channel_converter_unsigned_integral_impl<SrcChannelV,DstChannelV,false,true> {
    DstChannelV operator()(SrcChannelV src) const {
        typedef typename unsigned_integral_max_value<SrcChannelV>::value_type integer_t;
        static const integer_t div = unsigned_integral_max_value<SrcChannelV>::value / unsigned_integral_max_value<DstChannelV>::value;
        static const integer_t div2 = div/2;
        return DstChannelV((src + div2) / div);
    }
};

// Prevent overflow for the largest integral type
template <typename DstChannelV>
struct channel_converter_unsigned_integral_impl<uintmax_t,DstChannelV,false,true> {
    DstChannelV operator()(uintmax_t src) const {
        static const uintmax_t div = unsigned_integral_max_value<uint32_t>::value / unsigned_integral_max_value<DstChannelV>::value;
        static const uintmax_t div2 = div/2;
        if (src > unsigned_integral_max_value<uintmax_t>::value - div2)
            return unsigned_integral_max_value<DstChannelV>::value;
        return DstChannelV((src + div2) / div);
    }
};

// Both source and destination are unsigned integral channels,
// and the dst max value is not divisible by the src max value
// See if you can represent the expression (src * dst_max) / src_max in integral form
template <typename SrcChannelV, typename DstChannelV, bool SrcLessThanDst>
struct channel_converter_unsigned_integral_impl<SrcChannelV,DstChannelV,SrcLessThanDst,false>
    : public channel_converter_unsigned_integral_nondivisible<SrcChannelV,DstChannelV,SrcLessThanDst,
    mpl::greater<
        mpl::plus<unsigned_integral_num_bits<SrcChannelV>,unsigned_integral_num_bits<DstChannelV> >,
        unsigned_integral_num_bits<uintmax_t>
    >::value> {};

// Both source and destination are unsigned integral channels,
// the src max value is less than the dst max value,
// and the dst max value is not divisible by the src max value
// The expression (src * dst_max) / src_max fits in an integer
template <typename SrcChannelV, typename DstChannelV>
struct channel_converter_unsigned_integral_nondivisible<SrcChannelV,DstChannelV,true,false> {
    DstChannelV operator()(SrcChannelV src) const {
        typedef typename base_channel_type<DstChannelV>::type dest_t;
        return DstChannelV(static_cast<dest_t>( src * unsigned_integral_max_value<DstChannelV>::value) / unsigned_integral_max_value<SrcChannelV>::value);
    }
};

// Both source and destination are unsigned integral channels,
// the src max value is less than the dst max value,
// and the dst max value is not divisible by the src max value
// The expression (src * dst_max) / src_max cannot fit in an integer (overflows). Use a double
template <typename SrcChannelV, typename DstChannelV>
struct channel_converter_unsigned_integral_nondivisible<SrcChannelV,DstChannelV,true,true> {
    DstChannelV operator()(SrcChannelV src) const {
        static const double mul = unsigned_integral_max_value<DstChannelV>::value / double(unsigned_integral_max_value<SrcChannelV>::value);
        return DstChannelV(src * mul);
    }
};

// Both source and destination are unsigned integral channels,
// the dst max value is less than (or equal to) the src max value,
// and the src max value is not divisible by the dst max value
template <typename SrcChannelV, typename DstChannelV, bool CannotFit>
struct channel_converter_unsigned_integral_nondivisible<SrcChannelV,DstChannelV,false,CannotFit> {
    DstChannelV operator()(SrcChannelV src) const {

        typedef typename detail::unsigned_integral_max_value< SrcChannelV >::value_type src_integer_t;
        typedef typename detail::unsigned_integral_max_value< DstChannelV >::value_type dst_integer_t;

        static const double div = unsigned_integral_max_value<SrcChannelV>::value
                                / static_cast< double >( unsigned_integral_max_value<DstChannelV>::value );

        static const src_integer_t div2 = static_cast< src_integer_t >( div / 2.0 );

        return DstChannelV( static_cast< dst_integer_t >(( static_cast< double >( src + div2 ) / div )));
    }
};

} // namespace detail

/////////////////////////////////////////////////////
///  float32_t conversion
/////////////////////////////////////////////////////

template <typename DstChannelV> struct channel_converter_unsigned<float32_t,DstChannelV> {
    typedef float32_t argument_type;
    typedef DstChannelV result_type;
    DstChannelV operator()(float32_t x) const
    {
        typedef typename detail::unsigned_integral_max_value< DstChannelV >::value_type dst_integer_t;
        return DstChannelV( static_cast< dst_integer_t >(x*channel_traits<DstChannelV>::max_value()+0.5f ));
    }
};

template <typename SrcChannelV> struct channel_converter_unsigned<SrcChannelV,float32_t> {
    typedef float32_t argument_type;
    typedef SrcChannelV result_type;
    float32_t operator()(SrcChannelV   x) const { return float32_t(x/float(channel_traits<SrcChannelV>::max_value())); }
};

template <> struct channel_converter_unsigned<float32_t,float32_t> {
    typedef float32_t argument_type;
    typedef float32_t result_type;
    float32_t operator()(float32_t   x) const { return x; }
};

/// \brief 32 bit <-> float channel conversion
template <> struct channel_converter_unsigned<uint32_t,float32_t> {
    typedef uint32_t argument_type;
    typedef float32_t result_type;
    float32_t operator()(uint32_t x) const {
        // unfortunately without an explicit check it is possible to get a round-off error. We must ensure that max_value of uint32_t matches max_value of float32_t
        if (x>=channel_traits<uint32_t>::max_value()) return channel_traits<float32_t>::max_value();
        return float(x) / float(channel_traits<uint32_t>::max_value());
    }
};
/// \brief 32 bit <-> float channel conversion
template <> struct channel_converter_unsigned<float32_t,uint32_t> {
    typedef float32_t argument_type;
    typedef uint32_t result_type;
    uint32_t operator()(float32_t x) const {
        // unfortunately without an explicit check it is possible to get a round-off error. We must ensure that max_value of uint32_t matches max_value of float32_t
        if (x>=channel_traits<float32_t>::max_value())
            return channel_traits<uint32_t>::max_value();

        auto const max_value = channel_traits<uint32_t>::max_value();
        auto const result = x * static_cast<float32_t::base_channel_t>(max_value) + 0.5f;
        return static_cast<uint32_t>(result);
    }
};

/// @}

namespace detail {
// Converting from signed to unsigned integral channel.
// It is both a unary function, and a metafunction (thus requires the 'type' nested typedef, which equals result_type)
template <typename ChannelValue>     // Model ChannelValueConcept
struct channel_convert_to_unsigned : public detail::identity<ChannelValue> {
    typedef ChannelValue type;
};

template <> struct channel_convert_to_unsigned<int8_t> {
    typedef int8_t argument_type;
    typedef uint8_t result_type;
    typedef uint8_t type;
    type operator()(int8_t val) const {
        return static_cast<uint8_t>(static_cast<uint32_t>(val) + 128u);
    }
};

template <> struct channel_convert_to_unsigned<int16_t> {
    typedef int16_t argument_type;
    typedef uint16_t result_type;
    typedef uint16_t type;
    type operator()(int16_t val) const {
        return static_cast<uint16_t>(static_cast<uint32_t>(val) + 32768u);
    }
};

template <> struct channel_convert_to_unsigned<int32_t> {
    typedef int32_t argument_type;
    typedef uint32_t result_type;
    typedef uint32_t type;
    type operator()(int32_t val) const {
        return static_cast<uint32_t>(val)+(1u<<31);
    }
};

// Converting from unsigned to signed integral channel
// It is both a unary function, and a metafunction (thus requires the 'type' nested typedef, which equals result_type)
template <typename ChannelValue>     // Model ChannelValueConcept
struct channel_convert_from_unsigned : public detail::identity<ChannelValue> {
    typedef ChannelValue type;
};

template <> struct channel_convert_from_unsigned<int8_t> {
    typedef uint8_t argument_type;
    typedef int8_t result_type;
    typedef int8_t type;
    type  operator()(uint8_t val) const {
        return static_cast<int8_t>(static_cast<int32_t>(val) - 128);
    }
};

template <> struct channel_convert_from_unsigned<int16_t> {
    typedef uint16_t argument_type;
    typedef int16_t result_type;
    typedef int16_t type;
    type operator()(uint16_t val) const {
        return static_cast<int16_t>(static_cast<int32_t>(val) - 32768);
    }
};

template <> struct channel_convert_from_unsigned<int32_t> {
    typedef uint32_t argument_type;
    typedef int32_t result_type;
    typedef int32_t type;
    type operator()(uint32_t val) const {
        return static_cast<int32_t>(val - (1u<<31));
    }
};

}   // namespace detail

/// \ingroup ChannelConvertAlgorithm
/// \brief A unary function object converting between channel types
template <typename SrcChannelV, typename DstChannelV> // Model ChannelValueConcept
struct channel_converter {
    typedef SrcChannelV argument_type;
    typedef DstChannelV result_type;
    DstChannelV operator()(const SrcChannelV& src) const {
        typedef detail::channel_convert_to_unsigned<SrcChannelV> to_unsigned;
        typedef detail::channel_convert_from_unsigned<DstChannelV>   from_unsigned;
        typedef channel_converter_unsigned<typename to_unsigned::result_type, typename from_unsigned::argument_type> converter_unsigned;
        return from_unsigned()(converter_unsigned()(to_unsigned()(src)));
    }
};

/// \ingroup ChannelConvertAlgorithm
/// \brief Converting from one channel type to another.
template <typename DstChannel, typename SrcChannel> // Model ChannelConcept (could be channel references)
inline typename channel_traits<DstChannel>::value_type channel_convert(const SrcChannel& src) {
    return channel_converter<typename channel_traits<SrcChannel>::value_type,
                             typename channel_traits<DstChannel>::value_type>()(src);
}

/// \ingroup ChannelConvertAlgorithm
/// \brief Same as channel_converter, except it takes the destination channel by reference, which allows
///        us to move the templates from the class level to the method level. This is important when invoking it
///        on heterogeneous pixels.
struct default_channel_converter {
    template <typename Ch1, typename Ch2>
    void operator()(const Ch1& src, Ch2& dst) const {
        dst=channel_convert<Ch2>(src);
    }
};

namespace detail {
    // fast integer division by 255
    inline uint32_t div255(uint32_t in) { uint32_t tmp=in+128; return (tmp + (tmp>>8))>>8; }

    // fast integer divison by 32768
    inline uint32_t div32768(uint32_t in) { return (in+16384)>>15; }
}

/**
\defgroup ChannelMultiplyAlgorithm channel_multiply
\ingroup ChannelAlgorithm
\brief Multiplying unsigned channel values of the same type. Performs scaled multiplication result = a * b / max_value

Example:
\code
uint8_t x=128;
uint8_t y=128;
uint8_t mul = channel_multiply(x,y);
assert(mul == 64);    // 64 = 128 * 128 / 255
\endcode
*/
/// @{

/// \brief This is the default implementation. Performance specializatons are provided
template <typename ChannelValue>
struct channel_multiplier_unsigned {
    typedef ChannelValue first_argument_type;
    typedef ChannelValue second_argument_type;
    typedef ChannelValue result_type;
    ChannelValue operator()(ChannelValue a, ChannelValue b) const {
        return ChannelValue(static_cast<typename base_channel_type<ChannelValue>::type>(a / double(channel_traits<ChannelValue>::max_value()) * b));
    }
};

/// \brief Specialization of channel_multiply for 8-bit unsigned channels
template<> struct channel_multiplier_unsigned<uint8_t> {
    typedef uint8_t first_argument_type;
    typedef uint8_t second_argument_type;
    typedef uint8_t result_type;
    uint8_t operator()(uint8_t a, uint8_t b) const { return uint8_t(detail::div255(uint32_t(a) * uint32_t(b))); }
};

/// \brief Specialization of channel_multiply for 16-bit unsigned channels
template<> struct channel_multiplier_unsigned<uint16_t> {
    typedef uint16_t first_argument_type;
    typedef uint16_t second_argument_type;
    typedef uint16_t result_type;
    uint16_t operator()(uint16_t a, uint16_t b) const { return uint16_t((uint32_t(a) * uint32_t(b))/65535); }
};

/// \brief Specialization of channel_multiply for float 0..1 channels
template<> struct channel_multiplier_unsigned<float32_t> {
    typedef float32_t first_argument_type;
    typedef float32_t second_argument_type;
    typedef float32_t result_type;
    float32_t operator()(float32_t a, float32_t b) const { return a*b; }
};

/// \brief A function object to multiply two channels. result = a * b / max_value
template <typename ChannelValue>
struct channel_multiplier {
    typedef ChannelValue first_argument_type;
    typedef ChannelValue second_argument_type;
    typedef ChannelValue result_type;
    ChannelValue operator()(ChannelValue a, ChannelValue b) const {
        typedef detail::channel_convert_to_unsigned<ChannelValue> to_unsigned;
        typedef detail::channel_convert_from_unsigned<ChannelValue>   from_unsigned;
        typedef channel_multiplier_unsigned<typename to_unsigned::result_type> multiplier_unsigned;
        return from_unsigned()(multiplier_unsigned()(to_unsigned()(a), to_unsigned()(b)));
    }
};

/// \brief A function multiplying two channels. result = a * b / max_value
template <typename Channel> // Models ChannelConcept (could be a channel reference)
inline typename channel_traits<Channel>::value_type channel_multiply(Channel a, Channel b) {
    return channel_multiplier<typename channel_traits<Channel>::value_type>()(a,b);
}
/// @}

/**
\defgroup ChannelInvertAlgorithm channel_invert
\ingroup ChannelAlgorithm
\brief Returns the inverse of a channel. result = max_value - x + min_value

Example:
\code
// uint8_t == uint8_t == unsigned char
uint8_t x=255;
uint8_t inv = channel_invert(x);
assert(inv == 0);
\endcode
*/

/// \brief Default implementation. Provide overloads for performance
/// \ingroup ChannelInvertAlgorithm channel_invert
template <typename Channel> // Models ChannelConcept (could be a channel reference)
inline typename channel_traits<Channel>::value_type channel_invert(Channel x) {

    using base_t = typename base_channel_type<Channel>::type;
    using promoted_t = typename promote_integral<base_t>::type;
    promoted_t const promoted_x = x;
    promoted_t const promoted_max = channel_traits<Channel>::max_value();
    promoted_t const promoted_min = channel_traits<Channel>::min_value();
    promoted_t const promoted_inverted_x = promoted_max - promoted_x + promoted_min;
    auto const inverted_x = static_cast<base_t>(promoted_inverted_x);
    return inverted_x;
}

} }  // namespace boost::gil

#endif


#include <fstream>
#include <sstream>

#include <cstdlib>
#include <ctime>

/****/
#include <boost/lexical_cast.hpp>
/****/

#include <fastcgi++/manager.hpp>
#include <fastcgi++/request.hpp>

#include <Magick++.h>

/****/
//#include "base.hpp"
//#include "bstring.hpp"
/****/
#include "errlogger.hpp"
#include "error.hpp"

using namespace std;
using namespace boost;
using namespace Magick;

namespace SAAIIR {
    class CapRoot;
}

class SAAIIR::CapRoot : public Fastcgipp::Request<wchar_t> {
private:
    bool response();
};

bool SAAIIR::CapRoot::response() {
    try {
        /****/
        //Base::GenCap(BString::StrToInt(BString::WStrToStr(session.queryString).c_str()), true);
        /****/

        /*string captcha(" ");
        captcha += BString::IntToStr(Base::capN1);
        captcha += " X ";
        captcha += BString::IntToStr(Base::capN2);
        captcha += " = ";*/

        /****/
        //string captcha(BString::IntToStr(Base::capN1));
        //captcha += " X ";
        //captcha += BString::IntToStr(Base::capN2);
        /****/

        /****/
        //int seed = boost::lexical_cast<int>(session.queryString) + 1984; //IT SUCKS
        wstringstream ss(session.queryString);
        int seed;
        ss >> seed;
        seed += 1984;
        //

        srand(seed);
        int n1 = (rand() % 10) + 1;
        int n2 = (rand() % 10) + 1;
        int rotate = (rand() % 7 - 3);
        int skew = (rand() % 9 - 4);

        string captcha(boost::lexical_cast<std::string>(n1));
        captcha += " X ";
        captcha += boost::lexical_cast<std::string>(n2);
        /****/

        Image img(Geometry(115, 35), Color("white"));
        list<Drawable> drawList;

        drawList.push_back(DrawableTextAntialias(true));
        drawList.push_back(DrawableFont("../fnt/HAZELN.TTF"));
        drawList.push_back(DrawablePointSize(32));
        drawList.push_back(DrawableStrokeColor(Color("black")));
        drawList.push_back(DrawableFillColor(Color(0, 0, 0, MaxRGB)));
        drawList.push_back(DrawableTextDecoration(UnderlineDecoration));
        drawList.push_back(DrawableGravity(CenterGravity));
        /****/
        //drawList.push_back(DrawableRotation(Base::capRotate));
        //drawList.push_back(DrawableRotation(Base::capSkew));
        drawList.push_back(DrawableRotation(rotate));
        drawList.push_back(DrawableRotation(skew));
        /****/
        drawList.push_back(DrawableText(0, 0, captcha));

        img.draw(drawList);
        //img.blur(2, 0.5);

        string path("../tmp/captcha-");
        /****/
        //path += BString::IntToStr(rand());
        path += boost::lexical_cast<std::string>(rand());
        /****/
        path += ".png";

        img.write(path);

        ifstream file(path.c_str());
        if (file.is_open()) {
            out << "Content-Type: image/png\r\n\r\n";
            out.dump(file);
            file.close();
            /****/
            //Base::EraseFile(path.c_str());
            remove(path.c_str());
            /****/
        }
    }
    catch (std::exception& ex) {
        /****/
        //out << Base::httpHeader.c_str();
        out << "Content-Type: text/html; charset=utf-8\n\n";
        /****/
        //out << ex.what();
        out << "

Internal Server Error...
";
        SAAIIR::ErrLogger::LogError(ex.what());
    }
    catch (...) {
        /****/
        //out << Base::httpHeader.c_str();
        //out << Base::errUnknown.c_str();
        //SAAIIR::ErrLogger::LogError(ex.what());
        out << "Content-Type: text/html; charset=utf-8\n\n";
        out << "

Internal Server Error...
";
        SAAIIR::ErrLogger::LogError(" **  Unknown Error!!");
        /****/
    }

    return true;
}

int main(int argc, char **argv, char **env) {
    try {
        /****/
        //SAAIIR::Base::InitCurPath(argv[0]);
        string path(argv[0]);
        path = path.substr(0, path.rfind("/") + 1);
        if (chdir(path.c_str()) == -1) {
            throw Error(("Error changing directory to " + path).c_str());
        }
        /****/

        Fastcgipp::Manager<SAAIIR::CapRoot> capRoot;
        capRoot.handler();
    }

    catch (SAAIIR::Error ex) {
        SAAIIR::ErrLogger::LogError(ex.GetReason());
    }

    catch (std::exception& ex) {
        SAAIIR::ErrLogger::LogError(ex.what());
        return -2;
    }

    catch (...) {
        /****/
        //SAAIIR::ErrLogger::LogError(SAAIIR::Base::errUnknown.c_str());
        SAAIIR::ErrLogger::LogError(" **  Unknown Error!!");
        /****/
        return -1;
    }

    return 0;
}

///////// CGI Way
/*

#include <iostream>
#include <fstream>

using namespace std;

int main () {
        ifstream file("test.png", ios::in | ios::binary | ios::ate);

        if (file.is_open()) {
//		cout << "Content-Disposition: attachment; filename=captcha.png\n\n";
                cout << "Content-Type: image/png;\n\n";

                ifstream::pos_type size = (int)file.tellg();
                char *buffer = new char [size];
                file.seekg (0, ios::beg);
                file.read (buffer, size);
                cout.write(buffer, size);
                //cout << buffer;
                delete[] buffer;
        }

        return 0;
}

*/


/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/*                                                                  */
/*                      Early Plume Microphysics                    */
/*                              (EPM)                               */
/*                                                                  */
/* odeSolver Header File                                            */
/*                                                                  */
/* Author               : Thibaud M. Fritz                          */
/* Time                 : 1/10/2018                                 */
/* File                 : odeSolver.hpp                             */
/*                                                                  */
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#ifndef ODESOLVER_H_INCLUDED
#define ODESOLVER_H_INCLUDED

#include <iostream>
#include <iomanip>
#include <fstream>
#include <cmath>
#include <vector>
#include <boost/range/algorithm.hpp>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/iterator/adaptive_iterator.hpp>

#include "Util/ForwardDecl.hpp"
#include "Core/Parameters.hpp"
#include "Util/PhysConstant.hpp"
#include "Util/PhysFunction.hpp"

typedef boost::numeric::odeint::runge_kutta_fehlberg78< Vector_1D > error_stepper_type;
typedef boost::numeric::odeint::controlled_runge_kutta< error_stepper_type > controlled_stepper_type;

namespace EPM
{

    template<class System> class odeSolver;
    class streamingObserver;

}

template<class System> class EPM::odeSolver
{

    public:

        odeSolver( System system, Vector_1D &x, bool adaptive = 1, bool stop = 0, RealDouble threshold = 0.0 );
        inline odeSolver( System system, Vector_1D &x, bool stop, RealDouble threshold ) {
            odeSolver( system, x, 0, stop, threshold );
        }
        ~odeSolver();
        odeSolver( const odeSolver &solver );
        odeSolver& operator=( const odeSolver &solver );
        void updateTime( RealDouble t );
        void updateStep( RealDouble dt );

        UInt integrate( RealDouble start_time, RealDouble end_time, RealDouble dt, streamingObserver observer );

        UInt integrate( RealDouble start_time, RealDouble end_time, RealDouble dt );

        void observer( const Vector_1D &x, const RealDouble t );

        void updateThreshold( RealDouble t );

        bool done( const RealDouble &x );
        Vector_1D getState() const;

    protected:

        const System *system;
        Vector_1D vars;
        RealDouble currentTime;
        RealDouble timeStep;
        bool adaptive;
        controlled_stepper_type controlled_stepper;
        bool stop;
        RealDouble threshold;

    private:

};

class EPM::streamingObserver
{

    public:

        streamingObserver( Vector_2D &states, Vector_1D &times, std::vector<UInt> indices, const char* fileName = "", UInt write_every = 100 );
        ~streamingObserver( );
        streamingObserver& operator=( const streamingObserver &obs );
        void operator()( const Vector_1D &x, double t );
        RealDouble getLastElement() const;
        void print2File( ) const;
        bool checkwatersat( ) const;

        UInt m_write_every;
        UInt m_count = 0;
        const char* fileName;

        Vector_2D &m_states;
        Vector_1D &m_times;

    protected:

    private:

        const std::vector<UInt> m_indices;

};

#endif /* ODESOLVER_H_INCLUDED */


/**
 * Copyright 2007-2021 IMP Inventors. All rights reserved.
 */

#include <boost/lexical_cast.hpp>
#include <exception>
#include <iostream>
#include <sstream>
#include <string>

#include "RMF/FileConstHandle.h"
#include "RMF/FileHandle.h"
#include "RMF/ID.h"
#include "RMF/NodeHandle.h"
#include "RMF/Vector.h"
#include "RMF/decorator/physics.h"
#include "RMF/decorator/shape.h"
#include "RMF/infrastructure_macros.h"
#include "RMF/log.h"
#include "RMF/types.h"
#include "RMF/utility.h"
#include "common.h"

#define RMF_TRANSFORM(f, Name, attribute)       \
  if (f.get_is(nh)) {                           \
    RMF::decorator::Name d = f.get(nh);         \
    RMF::Vector3 cs = d.get_##attribute();      \
    for (unsigned int i = 0; i < 3; ++i) {      \
      cs[i] = scale * (cs[i] + translation[i]); \
    }                                           \
    d.set_##attribute(cs);                      \
  }

#define RMF_TRANSFORM_LIST(f, Name, attribute)          \
  if (f.get_is(nh)) {                                   \
    RMF::decorator::Name d = f.get(nh);                 \
    RMF::Vector3s cs = d.get_##attribute();             \
    for (unsigned int j = 0; j < cs.size(); ++j) {      \
      for (unsigned int i = 0; i < 3; ++i) {            \
        cs[j][i] = scale * (cs[j][i] + translation[i]); \
      }                                                 \
    }                                                   \
    d.set_##attribute(cs);                              \
  }

namespace {
std::string description(
    "Transform an rmf file in place. IntermediateParticle, Ball, Cylinder, "
    "Segment, RigidParticle and ReferenceFrame nodes are handled.");

void transform(RMF::NodeHandle nh,
               RMF::decorator::IntermediateParticleFactory ipf,
               RMF::decorator::RigidParticleFactory rpf,
               RMF::decorator::ReferenceFrameFactory rff,
               RMF::decorator::BallFactory bf,
               RMF::decorator::CylinderFactory cf,
               RMF::decorator::SegmentFactory sf, double scale,
               const RMF::Floats& translation) {
  RMF_TRANSFORM(ipf, IntermediateParticle, coordinates)
  else RMF_TRANSFORM(rpf, RigidParticle, coordinates);
  if (ipf.get_is(nh)) {
    RMF::decorator::IntermediateParticle d = ipf.get(nh);
    d.set_radius(scale * d.get_radius());
  }
  RMF_TRANSFORM(rff, ReferenceFrame, translation);
  RMF_TRANSFORM(bf, Ball, coordinates);
  RMF_TRANSFORM_LIST(cf, Cylinder, coordinates_list);
  RMF_TRANSFORM_LIST(sf, Segment, coordinates_list);
  RMF::NodeHandles children = nh.get_children();
  for (unsigned int i = 0; i < children.size(); ++i) {
    transform(children[i], ipf, rpf, rff, bf, cf, sf, scale, translation);
  }
}
}

int main(int argc, char** argv) {
  try {
    double scale = 1;
    std::string translation_str;
    options.add_options()("scale,s",
                          boost::program_options::value<double>(&scale),
                          "What to scale the model by")(
        "translation,t",
        boost::program_options::value<std::string>(&translation_str),
        "What to translate the model by as \"x y z\"");
    RMF_ADD_INPUT_FILE("rmf");
    RMF_ADD_OUTPUT_FILE("rmf");
    process_options(argc, argv);
    RMF::Floats translation;
    if (!translation_str.empty()) {
      std::istringstream iss(translation_str);
      for (unsigned int i = 0; i < 3; ++i) {
        double c;
        iss >> c;
        if (!iss) {
          std::cerr << "Bad translation" << std::endl;
          print_help_and_exit(argv);
        }
        translation.push_back(c);
      }
    } else {
      translation = RMF::Floats(0, 3);
    }
    RMF_INFO("Scaling model by " << scale);
    RMF_INFO("Translating model by " << translation[0] << " " << translation[1]
                                     << " " << translation[2]);

    RMF::FileConstHandle rhi = RMF::open_rmf_file_read_only(input);
    RMF::FileHandle rh = RMF::create_rmf_file(output);
    RMF::clone_file_info(rhi, rh);
    RMF::clone_hierarchy(rhi, rh);
    RMF::clone_static_frame(rhi, rh);
    RMF::decorator::IntermediateParticleFactory ipf(rh);
    RMF::decorator::RigidParticleFactory rpf(rh);
    RMF::decorator::ReferenceFrameFactory rff(rh);
    RMF::decorator::BallFactory bf(rh);
    RMF::decorator::CylinderFactory cf(rh);
    RMF::decorator::SegmentFactory sf(rh);
    RMF_FOREACH(RMF::FrameID frame, rhi.get_frames()) {
      RMF::clone_loaded_frame(rhi, rh);
      RMF_INFO("Processing frame " << frame);
      rh.set_current_frame(frame);
      transform(rh.get_root_node(), ipf, rpf, rff, bf, cf, sf, scale,
                translation);
    }
    return 0;
  }
  catch (const std::exception& e) {
    std::cerr << "Error: " << e.what() << std::endl;
    print_help_and_exit(argv);
  }
}


/*  BSD 3-Clause License
 *
 *  Copyright (c) 2020, FriederPankratz <frieder.pankratz@gmail.com>
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  1. Redistributions of source code must retain the above copyright notice, this
 *     list of conditions and the following disclaimer.
 *
 *  2. Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *
 *  3. Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**/

#include "gtest/gtest.h"
#include <traact/util/Logging.h>
#include "spdlog/sinks/stdout_color_sinks.h"

#include <traact/math/perspective.h>
#include <traact/vision.h>
#include <traact/spatial.h>
#include <Eigen/Dense>
#include <math.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

void test_pose(const Eigen::Affine3d& marker_pose, const traact::vision::CameraCalibration& calibration) {
    using namespace traact::math;
    using namespace traact::vision;
    using namespace Eigen;

    std::vector<Translation3d> marker_corners_local;
    std::vector<Affine3d> marker_corners;

    /*
    marker_corners_local.push_back(Translation3d(0.2,0.2,0));
    marker_corners_local.push_back(Translation3d(0.2,-0.2,0));
    marker_corners_local.push_back(Translation3d(-0.2,-0.2,0));
    marker_corners_local.push_back(Translation3d(-0.2,0.2,0));
*/

    marker_corners_local.push_back(Eigen::Translation3d(0,0,0));
    marker_corners_local.push_back(Eigen::Translation3d(0.384,0,0));
    marker_corners_local.push_back(Eigen::Translation3d(0,0.114,0));
    marker_corners_local.push_back(Eigen::Translation3d(0.225,0,0));

    traact::spatial::Position2DList marker_corners_2d;
    traact::spatial::Position3DList marker_model;
    // project marker corners to 2d
    for(int i=0;i<marker_corners_local.size();++i) {
        marker_corners.push_back(marker_pose * marker_corners_local[i]);
        marker_corners_2d.emplace_back(reproject_point(calibration, marker_corners[i].translation()));
        marker_model.push_back(marker_corners_local[i].translation());
    }

    Affine3d camera_pose, pose_result;
    EXPECT_TRUE(traact::math::estimate_camera_pose(pose_result, marker_corners_2d, calibration, marker_model));
    //pose_result = camera_pose.inverse();

    EXPECT_NEAR(marker_pose.translation().x(), pose_result.translation().x(), 1e-6);
    EXPECT_NEAR(marker_pose.translation().y(), pose_result.translation().y(), 1e-6);
    EXPECT_NEAR(marker_pose.translation().z(), pose_result.translation().z(), 1e-6);

    Quaterniond rot_marker(marker_pose.rotation());
    Quaterniond rot_result(pose_result.rotation());

    EXPECT_NEAR(rot_marker.x(), rot_result.x(), 1e-6);
    EXPECT_NEAR(rot_marker.y(), rot_result.y(), 1e-6);
    EXPECT_NEAR(rot_marker.z(), rot_result.z(), 1e-6);
    EXPECT_NEAR(rot_marker.w(), rot_result.w(), 1e-6);
}

TEST(TraactVisionTestSuite, PoseEstimationTest_NoDistortion) {

    using namespace traact::math;
    using namespace traact::vision;
    using namespace Eigen;
    CameraCalibration calibration;
    calibration.width = 640;
    calibration.height = 480;
    calibration.fx = 500;
    calibration.fy = 500;
    calibration.skew = 0;
    calibration.cx = calibration.width / 2.0 - 0.5;
    calibration.cy = calibration.height / 2.0 - 0.5;

    Affine3d marker_pose;
    marker_pose = Translation3d(1,2,3);
    //test_pose(marker_pose, calibration);
    marker_pose.rotate(AngleAxisd(M_PI, Vector3d::UnitZ()));
    //test_pose(marker_pose, calibration);

    marker_pose = Translation3d(0.1,0.2,4) * AngleAxisd(M_PI/16, Vector3d::UnitY());
    //test_pose(marker_pose, calibration);

    marker_pose = Translation3d(0.3,-0.5,2) * AngleAxisd(M_PI/16, Vector3d::UnitX());
    //test_pose(marker_pose, calibration);

    for(int i=0;i<100;++i) {
        Vector3d pos;
        Vector4d rot;
        pos.setRandom();
        rot.setRandom();
        Quaterniond rotq(rot);
        rotq.normalize();
        pos.z() += 3;

        Affine3d pose = Translation3d (pos) * rotq;
        //test_pose(pose, calibration);

    }

}


#pragma once

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <vector>

#include "../stateobject.hh"
#include "../../OcclusionRay/occlusionray.hh"
#include "../../geometry/LineSegment/LineSegment2/LineSegment2i/linesegment2i.hh"
#include "../../util/Maybe.hh"

namespace bold
{
  typedef std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d>> Vector2dVector;

  class CameraFrameState : public StateObject
  {
  public:
    CameraFrameState(Maybe<Eigen::Vector2d> ballObservation,
                     Vector2dVector goalObservations,
                     Vector2dVector teamMateObservations,
                     std::vector<LineSegment2i> observedLineSegments,
                     std::vector<OcclusionRay<ushort>> occlusionRays,
                     long totalPixelCount, long processedPixelCount,
                     ulong thinkCycleNumber)
    : d_ballObservation(ballObservation),
      d_goalObservations(goalObservations),
      d_teamMateObservations(teamMateObservations),
      d_observedLineSegments(observedLineSegments),
      d_occlusionRays(occlusionRays),
      d_totalPixelCount(totalPixelCount),
      d_processedPixelCount(processedPixelCount),
      d_thinkCycleNumber(thinkCycleNumber)
    {}

    Maybe<Eigen::Vector2d> getBallObservation() const { return d_ballObservation; }
    bool isBallVisible() const { return d_ballObservation.hasValue(); }
    Vector2dVector getGoalObservations() const { return d_goalObservations; }
    Vector2dVector getTeamMateObservations() const { return d_teamMateObservations; }
    int getGoalObservationCount() const { return d_goalObservations.size(); }
    std::vector<OcclusionRay<ushort>> getOcclusionRays() const { return d_occlusionRays; }
    std::vector<LineSegment2i> getObservedLineSegments() const { return d_observedLineSegments; }
    long getTotalPixelCount() const { return d_totalPixelCount; }
    long getProcessedPixelCount() const { return d_processedPixelCount; }
    float getProcessedPixelRatio() const { return (float)d_processedPixelCount/d_totalPixelCount; }
    ulong getThinkCycleNumber() const { return d_thinkCycleNumber; }

    void writeJson(rapidjson::Writer<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::Writer<WebSocketBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }

  private:
    template<typename TBuffer>
    void writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const;

    Maybe<Eigen::Vector2d> d_ballObservation;
    Vector2dVector d_goalObservations;
    Vector2dVector d_teamMateObservations;
    std::vector<LineSegment2i> d_observedLineSegments;
    std::vector<OcclusionRay<ushort>> d_occlusionRays;
    long d_totalPixelCount;
    long d_processedPixelCount;
    ulong d_thinkCycleNumber;
  };

  template<typename TBuffer>
  inline void CameraFrameState::writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const
  {
    writer.StartObject();
    {
      writer.String("thinkCycle");
      writer.Uint64(d_thinkCycleNumber);

      writer.String("ball");
      if (d_ballObservation.hasValue())
      {
        writer.StartArray();
        writer.Double(d_ballObservation->x(), "%.1f");
        writer.Double(d_ballObservation->y(), "%.1f");
        writer.EndArray();
      }
      else
      {
        writer.Null();
      }

      writer.String("goals");
      writer.StartArray();
      {
        for (auto const& goalPos : d_goalObservations)
        {
          writer.StartArray();
          writer.Double(goalPos.x(), "%.1f");
          writer.Double(goalPos.y(), "%.1f");
          writer.EndArray();
        }
      }
      writer.EndArray();

      writer.String("teammates");
      writer.StartArray();
      {
        for (auto const& teamMatePos : d_teamMateObservations)
        {
          writer.StartArray();
          writer.Double(teamMatePos.x(), "%.1f");
          writer.Double(teamMatePos.y(), "%.1f");
          writer.EndArray();
        }
      }
      writer.EndArray();

      writer.String("lines");
      writer.StartArray();
      {
        for (LineSegment2i const& lineSeg : d_observedLineSegments)
        {
          writer.StartArray();
          writer.Double(lineSeg.p1().x(), "%.1f");
          writer.Double(lineSeg.p1().y(), "%.1f");
          writer.Double(lineSeg.p2().x(), "%.1f");
          writer.Double(lineSeg.p2().y(), "%.1f");
          writer.EndArray();
        }
      }
      writer.EndArray();

      writer.String("occlusionRays");
      writer.StartArray();
      {
        for (auto const& ray : d_occlusionRays)
        {
          writer.StartArray();
          writer.Uint(ray.near().x());
          writer.Uint(ray.near().y());
          writer.Uint(ray.far().x());
          writer.Uint(ray.far().y());
          writer.EndArray();
        }
      }
      writer.EndArray();

      writer.String("totalPixelCount");
      writer.Uint64(d_totalPixelCount);

      writer.String("processedPixelCount");
      writer.Uint64(d_processedPixelCount);
    }
    writer.EndObject();
  }
}


#ifndef ASLAM_CAMERA_GEOMETRY_HPP
#define ASLAM_CAMERA_GEOMETRY_HPP

#include <aslam/Frame.hpp>
#include <aslam/cameras/CameraGeometryBase.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <sm/PropertyTree.hpp>
#include <sm/boost/serialization.hpp>
#include <sm/eigen/NumericalDiff.hpp>
#include <sm/kinematics/UncertainHomogeneousPoint.hpp>
#include "FiniteDifferences.hpp"

namespace aslam {
// forward declaration
template <typename C>
class Frame;

namespace cameras {

/**
 * @brief Camera geometry. A camera geometry class based on three configurable policies:
 *  1. A lens type that determines the geometry of the camera
 *  2. A shutter type that determines the timing of each pixel.
 *  3. A mask type.
 * @tparam PROJECTION_T Projection type, project point from world frame to image frame
 * @tparam SHUTTER_T    Shutter type, global shutter or rolling shutter
 * @tparam MASK_T       Mask type
 */
template <typename PROJECTION_T, typename SHUTTER_T, typename MASK_T>
class CameraGeometry : public CameraGeometryBase {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef PROJECTION_T projection_t;
    // Not all projections will have a distortion...is this necessary? Maybe.
    // typedef typename projection_t::distortion_t distortion_t;
    typedef typename projection_t::keypoint_t keypoint_t;
    typedef SHUTTER_T shutter_t;
    typedef MASK_T mask_t;
    typedef aslam::Frame<CameraGeometry<projection_t, shutter_t, mask_t> > frame_t;

    enum { KeypointDimension = projection_t::KeypointDimension };

    typedef Eigen::Matrix<double, KeypointDimension, 4> jacobian_homogeneous_t;
    typedef Eigen::Matrix<double, KeypointDimension, 3> jacobian_t;
    typedef Eigen::Matrix<double, 3, KeypointDimension> inverse_jacobian_t;
    typedef Eigen::Matrix<double, 4, KeypointDimension> inverse_jacobian_homogeneous_t;
    typedef Eigen::Matrix<double, KeypointDimension, KeypointDimension> covariance_t;

    /// \brief default constructor
    CameraGeometry();

    /// \brief Construct from projection
    CameraGeometry(const projection_t& projection);

    /// \brief Construct from projection and shutter
    CameraGeometry(const projection_t& projection, const shutter_t& shutter);

    /// \brief Construct from projection, shutter, and mask
    CameraGeometry(const projection_t& projection, const shutter_t& shutter, const mask_t& mask);

    /// \brief Construct from a property tree
    CameraGeometry(const sm::PropertyTree& config);

    /// \brief simple destructor
    virtual ~CameraGeometry();

    //////////////////////////////////////////////////////////////
    // PROJECTION FUNCTIONS
    //////////////////////////////////////////////////////////////
    template <typename DERIVED_P, typename DERIVED_K>
    bool euclideanToKeypoint(const Eigen::MatrixBase<DERIVED_P>& p,
                             const Eigen::MatrixBase<DERIVED_K>& outKeypoint) const;

    template <typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
    bool euclideanToKeypoint(const Eigen::MatrixBase<DERIVED_P>& p, const Eigen::MatrixBase<DERIVED_K>& outKeypoint,
                             const Eigen::MatrixBase<DERIVED_JP>& outJp) const;

    template <typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
    bool euclideanToKeypointFiniteDifference(const Eigen::MatrixBase<DERIVED_P>& p,
                                             const Eigen::MatrixBase<DERIVED_K>& outKeypoint,
                                             const Eigen::MatrixBase<DERIVED_JP>& outJp) const;

    template <typename DERIVED_P, typename DERIVED_K>
    bool homogeneousToKeypoint(const Eigen::MatrixBase<DERIVED_P>& p,
                               const Eigen::MatrixBase<DERIVED_K>& outKeypoint) const;

    template <typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
    bool homogeneousToKeypoint(const Eigen::MatrixBase<DERIVED_P>& p, const Eigen::MatrixBase<DERIVED_K>& outKeypoint,
                               const Eigen::MatrixBase<DERIVED_JP>& outJp) const;

    template <typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
    bool homogeneousToKeypointFiniteDifference(const Eigen::MatrixBase<DERIVED_P>& p,
                                               const Eigen::MatrixBase<DERIVED_K>& outKeypoint,
                                               const Eigen::MatrixBase<DERIVED_JP>& outJp) const;

    // Project the point and get the associated uncertainty of the projection.
    template <typename DERIVED_K>
    bool homogeneousToKeypoint(const sm::kinematics::UncertainHomogeneousPoint& p,
                               const Eigen::MatrixBase<DERIVED_K>& outKeypoint,
                               covariance_t& outProjectionUncertainty) const;

    // Project the point
    template <typename DERIVED_K>
    bool homogeneousToKeypoint(const sm::kinematics::HomogeneousPoint& p,
                               const Eigen::MatrixBase<DERIVED_K>& outKeypoint) const;

    template <typename DERIVED_K, typename DERIVED_P>
    bool keypointToEuclidean(const Eigen::MatrixBase<DERIVED_K>& keypoint,
                             const Eigen::MatrixBase<DERIVED_P>& outPoint) const;

    template <typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
    bool keypointToEuclidean(const Eigen::MatrixBase<DERIVED_K>& keypoint, const Eigen::MatrixBase<DERIVED_P>& outPoint,
                             const Eigen::MatrixBase<DERIVED_JK>& outJk) const;

    template <typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
    bool keypointToEuclideanFiniteDifference(const Eigen::MatrixBase<DERIVED_K>& keypoint,
                                             const Eigen::MatrixBase<DERIVED_P>& outPoint,
                                             const Eigen::MatrixBase<DERIVED_JK>& outJk) const;

    template <typename DERIVED_K, typename DERIVED_P>
    bool keypointToHomogeneous(const Eigen::MatrixBase<DERIVED_K>& keypoint,
                               const Eigen::MatrixBase<DERIVED_P>& outPoint) const;

    template <typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
    bool keypointToHomogeneous(const Eigen::MatrixBase<DERIVED_K>& keypoint,
                               const Eigen::MatrixBase<DERIVED_P>& outPoint,
                               const Eigen::MatrixBase<DERIVED_JK>& outJk) const;

    template <typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
    bool keypointToHomogeneousFiniteDifference(const Eigen::MatrixBase<DERIVED_K>& keypoint,
                                               const Eigen::MatrixBase<DERIVED_P>& outPoint,
                                               const Eigen::MatrixBase<DERIVED_JK>& outJk) const;

    template <typename DERIVED_P, typename DERIVED_JI>
    void euclideanToKeypointIntrinsicsJacobian(const Eigen::MatrixBase<DERIVED_P>& p,
                                               const Eigen::MatrixBase<DERIVED_JI>& outJi) const;

    template <typename DERIVED_P, typename DERIVED_JD>
    void euclideanToKeypointDistortionJacobian(const Eigen::MatrixBase<DERIVED_P>& p,
                                               const Eigen::MatrixBase<DERIVED_JD>& outJd) const;

    template <typename DERIVED_P, typename DERIVED_JI>
    void euclideanToKeypointIntrinsicsJacobianFiniteDifference(const Eigen::MatrixBase<DERIVED_P>& p,
                                                               const Eigen::MatrixBase<DERIVED_JI>& outJi) const;

    template <typename DERIVED_P, typename DERIVED_JD>
    void euclideanToKeypointDistortionJacobianFiniteDifference(const Eigen::MatrixBase<DERIVED_P>& p,
                                                               const Eigen::MatrixBase<DERIVED_JD>& outJd) const;

    template <typename DERIVED_P, typename DERIVED_JI>
    void homogeneousToKeypointIntrinsicsJacobian(const Eigen::MatrixBase<DERIVED_P>& p,
                                                 const Eigen::MatrixBase<DERIVED_JI>& outJi) const;

    template <typename DERIVED_P, typename DERIVED_JI>
    void homogeneousToKeypointIntrinsicsJacobianFiniteDifference(const Eigen::MatrixBase<DERIVED_P>& p,
                                                                 const Eigen::MatrixBase<DERIVED_JI>& outJi) const;

    template <typename DERIVED_P, typename DERIVED_JS>
    void homogeneousToKeypointDistortionJacobian(const Eigen::MatrixBase<DERIVED_P>& p,
                                                 const Eigen::MatrixBase<DERIVED_JS>& outJs) const;

    template <typename DERIVED_P, typename DERIVED_JD>
    void homogeneousToKeypointDistortionJacobianFiniteDifference(const Eigen::MatrixBase<DERIVED_P>& p,
                                                                 const Eigen::MatrixBase<DERIVED_JD>& outJd) const;

    //////////////////////////////////////////////////////////////
    // SHUTTER SUPPORT
    //////////////////////////////////////////////////////////////

    // The amount of time elapsed between the start of the image and the
    // keypoint. For a global shutter camera, this can return Duration(0).
    template <typename DERIVED_K>
    Duration temporalOffset(const Eigen::MatrixBase<DERIVED_K>& keypoint) const;

    virtual Duration temporalOffset(const Eigen::VectorXd& keypoint) const;

    //////////////////////////////////////////////////////////////
    // VALIDITY TESTING
    //////////////////////////////////////////////////////////////

    template <typename DERIVED_K>
    bool isValid(const Eigen::MatrixBase<DERIVED_K>& keypoint) const;

    template <typename DERIVED_P>
    bool isEuclideanVisible(const Eigen::MatrixBase<DERIVED_P>& p) const;

    template <typename DERIVED_P>
    bool isHomogeneousVisible(const Eigen::MatrixBase<DERIVED_P>& ph) const;

    //////////////////////////////////////////////////////////////
    // SUPERCLASS SUPPORT
    //////////////////////////////////////////////////////////////
    virtual bool vsEuclideanToKeypoint(const Eigen::Vector3d& p, Eigen::VectorXd& outKeypoint) const;
    virtual bool vsEuclideanToKeypoint(const Eigen::Vector3d& p, Eigen::VectorXd& outKeypoint,
                                       Eigen::MatrixXd& outJacobian) const;

    virtual bool vsHomogeneousToKeypoint(const Eigen::Vector4d& ph, Eigen::VectorXd& outKeypoint) const;

    virtual bool vsHomogeneousToKeypoint(const Eigen::Vector4d& ph, Eigen::VectorXd& outKeypoint,
                                         Eigen::MatrixXd& outJacobian) const;

    virtual bool vsKeypointToEuclidean(const Eigen::VectorXd& keypoint, Eigen::Vector3d& outPoint) const;

    virtual bool vsKeypointToEuclidean(const Eigen::VectorXd& keypoint, Eigen::VectorXd& outPoint,
                                       Eigen::MatrixXd& outJacobian) const;

    virtual bool vsKeypointToHomogeneous(Eigen::VectorXd const& keypoint, Eigen::VectorXd& outPoint) const;

    virtual bool vsKeypointToHomogeneous(Eigen::VectorXd const& keypoint, Eigen::VectorXd& outPoint,
                                         Eigen::MatrixXd& outJacobian) const;

    virtual bool vsIsValid(const Eigen::VectorXd& keypoint) const;

    virtual bool vsIsEuclideanVisible(const Eigen::Vector3d& p) const;

    virtual bool vsIsHomogeneousVisible(const Eigen::Vector4d& ph) const;

    // The amount of time elapsed between the start of the image and the
    // keypoint. For a global shutter camera, this can return Duration(0).
    virtual Duration vsTemporalOffset(const Eigen::VectorXd& keypoint) const;

    /// \brief get the camera id.
    virtual CameraId id() const;

    /// \brief set the camera id.
    virtual void setId(CameraId id);

    /// \brief the length of a keypoint
    virtual size_t keypointDimension() const;

    /// \brief is this an invertible camera model
    virtual bool isProjectionInvertible() const;

    //////////////////////////////////////////////////////////////
    // CAMERA CALIBRATION
    //////////////////////////////////////////////////////////////
    /// \brief initialize the intrinsics based on a list of views of a gridded calibration target
    /// \return true on success
    bool initializeIntrinsics(const std::vector<GridCalibrationTargetObservation>& observations);

    /// \brief estimate the transformation of the camera with respect to the calibration target
    ///        On success out_T_t_c is filled in with the transformation that takes points from
    ///        the camera frame to the target frame
    /// \return true on success
    virtual bool estimateTransformation(const GridCalibrationTargetObservation& obs,
                                        sm::kinematics::Transformation& out_T_t_c) const;

    ////////////////////////////////////////////////////////////////
    // OPTIMIZATION SUPPORT
    ////////////////////////////////////////////////////////////////
    /// \brief the minimal dimensions of the projection parameters
    virtual int minimalDimensionsProjection() const;

    /// \brief the minimal dimensions of the distortion parameters
    virtual int minimalDimensionsDistortion() const;

    /// \brief the minimal dimensions of the shutter parameters
    virtual int minimalDimensionsShutter() const;

    // aslam::backend compatibility

    /// \brief update the intrinsics
    virtual void update(const double* v, bool projection, bool distortion, bool shutter);

    /// \brief Get the total number of dimensions of the intrinsic parameters
    virtual int minimalDimensions(bool projection, bool distortion, bool shutter) const;

    /// \brief get the intrinsic parameters.
    virtual void getParameters(Eigen::MatrixXd& P, bool projection, bool distortion, bool shutter) const;

    /// \brief set the intrinsic parameters.
    virtual void setParameters(const Eigen::MatrixXd& P, bool projection, bool distortion, bool shutter);

    /// \brief return the Jacobian of the projection with respect to the intrinsics.
    virtual void euclideanToKeypointIntrinsicsJacobian(const Eigen::Vector3d& p, Eigen::MatrixXd& outJi,
                                                       bool estimateProjection, bool estimateDistortion,
                                                       bool estimateShutter) const;

    /// \brief return the Jacobian of the projection with respect to the intrinsics.
    virtual void homogeneousToKeypointIntrinsicsJacobian(const Eigen::Vector4d& p, Eigen::MatrixXd& outJi,
                                                         bool estimateProjection, bool estimateDistortion,
                                                         bool estimateShutter) const;

    /// \brief return the temporal offset with respect to the intrinsics.
    virtual void temporalOffsetIntrinsicsJacobian(const Eigen::VectorXd& keypoint, Eigen::MatrixXd& outJi,
                                                  bool estimateProjection, bool estimateDistortion,
                                                  bool estimateShutter) const;

    //////////////////////////////////////////////////////////////
    // UNIT TEST SUPPORT
    //////////////////////////////////////////////////////////////
    // \brief creates a random valid keypoint.
    virtual Eigen::VectorXd createRandomKeypoint() const;

    // \brief creates a random visible point. Negative depth means random between 0 and 100 meters.
    virtual Eigen::Vector3d createRandomVisiblePoint(double depth = -1.0) const;

    projection_t& projection() { return _projection; }
    const projection_t& projection() const { return _projection; }

    shutter_t& shutter() { return _shutter; }
    const shutter_t& shutter() const { return _shutter; }

    void setMask(const mask_t& mask) { _mask = mask; }
    mask_t& mask() { return _mask; }
    const mask_t& mask() const { return _mask; }

    virtual bool hasMask() const { return _mask.isSet(); }

    virtual void print(std::ostream& out);

    bool isBinaryEqual(const CameraGeometry<PROJECTION_T, SHUTTER_T, MASK_T>& rhs) const;

    /// \brief This will create a frame with the right type. WARNING: it will not
    ///        fill in the camera geometry in the frame. This must be done manually
    ///        outside the call.
    virtual boost::shared_ptr<FrameBase> createUninitializedFrameBase() const;

    /// \brief This will create a frame with the right type. WARNING: it will not
    ///        fill in the camera geometry in the frame. This must be done manually
    ///        outside the call.
    boost::shared_ptr<frame_t> createUninitializedFrame() const;

    enum { CLASS_SERIALIZATION_VERSION = 3 };
    BOOST_SERIALIZATION_SPLIT_MEMBER();

    template <class Archive>
    void load(Archive& ar, const unsigned int version) {
        SM_ASSERT_LE(std::runtime_error, version, (unsigned int)CLASS_SERIALIZATION_VERSION,
                     "Unsupported serialization version");
        if (version >= 2) {
            ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(CameraGeometryBase);
        }

        ar >> BOOST_SERIALIZATION_NVP(_id);
        ar >> BOOST_SERIALIZATION_NVP(_projection);
        ar >> BOOST_SERIALIZATION_NVP(_shutter);
        ar >> BOOST_SERIALIZATION_NVP(_mask);
    }

    template <class Archive>
    void save(Archive& ar, const unsigned int /* version */) const {
        ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(CameraGeometryBase);
        ar << BOOST_SERIALIZATION_NVP(_id);
        ar << BOOST_SERIALIZATION_NVP(_projection);
        ar << BOOST_SERIALIZATION_NVP(_shutter);
        ar << BOOST_SERIALIZATION_NVP(_mask);
    }

    static CameraGeometry<PROJECTION_T, SHUTTER_T, MASK_T> getTestGeometry();

    /// \todo redo this. Somehow Stefan's stuff needs it.
    virtual int width() const { return _projection.ru(); }
    virtual int height() const { return _projection.rv(); }

  private:
    CameraId _id;
    projection_t _projection;  // projection
    shutter_t _shutter;        // shutter
    mask_t _mask;              // mask
};

}  // namespace cameras
}  // namespace aslam

#include "implementation/CameraGeometry.hpp"

SM_BOOST_CLASS_VERSION_T1T2T3(aslam::cameras::CameraGeometry);

#endif /* ASLAM_CAMERA_GEOMETRY_BASE_HPP */