//
// Created by Ragesh on 5/5/18.
//

////////////////////////////////////////////////////////////////////////////////
// My naming conventions for the project
// Put header files in separate categories and in alphabetical order
// unless there some compatibility issue between the header files
// macros and const : ALL_IN_CAPS
// namespace : NS_prefix_small_letter_with_underscore
// class and structs : First_letter_caps_with_underscore
// functions and variables: all_small_letters
/////////////////////////////////////////////////////////////////////////////////

/***
 * This cpp file is used to generate executable to that can make the robot move according to a path planned which
 * maximizes the Mutual Information between the map uncertainty and forward ranger sensor model
 */

// ros libraries
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <ros/console.h>

// C++ library header files
#include <iostream>
#include <string>

// C library header files
#include <stdio.h>

// third party libraries
#include <boost/math/constants/constants.hpp>
#include <opencv2/opencv.hpp>

// local header files
#include "my_robot.h"
#include "planners.h"
#include "command_line_parser.h"
#include "occupancyGrid.hpp"

// Short name for the namespaces
namespace clp=NS_rag_command_line_parser;
namespace rob=NS_my_robot;
namespace map=NS_occupancy_grid;

int main(int argc, char** argv)
{
  // robot speed parameters
  static const double cruisesSpeed = 0.4;
  static const double turnSpeed = 0.2;

  // parse the inputs
  clp::CommandLineParser cml_parser(argc, argv);

  // Parse input arguments for robot id.
  std::string robot_number;
  if (cml_parser["-id"])
  {
    robot_number = cml_parser("-id");
  }else{
    ROS_ERROR("Need to provide robot id");
  }

  std::string no_of_robots;
  if (cml_parser["-n"])
  {
    no_of_robots = cml_parser("-n");
  }else{
    ROS_ERROR("Need to provide number of robots n");
  }

  // Initializing the ROS node
  std::string node_name{"MI_wanderer_node_" + robot_number};
  ros::init(argc, argv, node_name);
  // display the name of the node
  ROS_INFO_STREAM("Initiated node : "<<ros::this_node::getName());
  // The parameters for map object
  const double min_x = -8; // in meters
  const double min_y = -8; // in meters
  const double cell_size_x = 0.02; // in meters
  const double cell_size_y = 0.02; // in meters
  const int n_cell_x = 800; // no of cells along x
  const int n_cell_y = 800; // no of cells along y

  // Create a map object

  map::occupancyGrid2D<double, int> occ_grid{min_x, min_y, cell_size_x, cell_size_y, n_cell_x, n_cell_y};

  // Velocity object
  Velocity velocity;
  velocity.linear.x=cruisesSpeed;
  velocity.linear.y=0;
  velocity.linear.z=0;
  velocity.angular.x=0;
  velocity.angular.y=0;
  velocity.angular.z=turnSpeed;

  Pose startP{0.0, 0.0, 0.0};

  // radial range sensor noise
  double radial_noise = 0.01;

  // forward sensor model parameter
  NS_my_planner::F_S_M_parameters fsm;
  fsm.sigma = radial_noise;

  // Create a planner object
  NS_my_planner::base_planner planner{50, 0, startP, velocity};
  NS_my_planner::MI_levyWalk_planner MI_planner{0, startP, velocity, fsm, NS_my_planner::CSQMI, 5};

  // Create a robot object
  rob::Robot robot{static_cast<uint>(std::stoi(robot_number)), std::string{"robot_"}, &MI_planner,
                   static_cast<uint>(std::stoi(no_of_robots)), radial_noise};

  // Assigning the map object to the robot
  robot.occ_grid_map = &occ_grid;

  // ROS loop rate
  ros::Rate loop_rate(10);

  long unsigned int pre_time=0;

  while (ros::ok()){
    // Publish, Spin and Sleep
    //ROS_INFO("\n Ros time now is %f ", ros::Time::now().toSec());
    robot.move();
    robot.build_map();
    //robot.merge_map();
    // to do some action every 20 seconds
    long unsigned int time_now = static_cast<long unsigned int>(ros::Time::now().toSec());
    if (time_now%30 == 0 &&
        (pre_time != time_now)){
      //ROS_INFO("\n In condition ");
      //robot.write_map_image();
      pre_time = time_now;
    }
    robot.update_neighbors();
    robot.publish_map();
    ros::spinOnce();
    loop_rate.sleep();
  }
}


/*
 *
 * Copyright (C) 2015 Mohammad Javad Dousti, Qing Xie, and Massoud Pedram, SPORT lab,
 * University of Southern California. All rights reserved.
 *
 * Please refer to the LICENSE file for terms of use.
 *
*/
#include "headers/rc_utils.h"
#include <omp.h>
//#include <boost/numeric/odeint/external/openmp/openmp.hpp>
//#include <thrust/device_vector.h>
//#include <boost/numeric/odeint/external/thrust/thrust_algebra.hpp>
//#include <boost/numeric/odeint/external/thrust/thrust_operations.hpp>
//#include <boost/numeric/odeint/external/mkl/mkl_operations.hpp>

//double **RCutils::matrix;
//double *RCutils::invC_vector;
//double **RCutils::A_matrix;

double *RCutils::P_vector;
int RCutils::n;
int** RCutils::a_matrix_indices;
double** RCutils::a_matrix_values;

/* For Eigen */
//SparseMatrix<double> RCutils::a_matrix;
//VectorXd RCutils::p_vector;

#if USE_GPU==1
void RCutils::checkGPUStatus(culaStatus status){
    char buf[256];

    if(!status)
        return;

    culaGetErrorInfoString(status, culaGetErrorInfo(), buf, sizeof(buf));
    cout<<buf<<endl;

    culaShutdown();
    exit(-1);
}

void RCutils::gpuSolveSteady(double **matrix, int N, double* P_vector, double *T_vector){
    int NRHS = 1;

    culaStatus status;

    culaDouble* A = NULL;
    culaInt* IPIV = NULL;

    cout<<"--------------------"<<endl;
    cout<<" Calling GPU Solver"<<endl;
    cout<<"--------------------"<<endl;

    cout<<"Allocating Matrices..."<<endl;
    A = (culaDouble*)malloc(N*N*sizeof(culaDouble));
    IPIV = (culaInt*)malloc(N*sizeof(culaInt));
    if(!A || !IPIV){
        cerr<<"Cannot allocate contiguous memory for solving the linear set of equations using GPU."<<endl;
    	exit(-1);
    }

    cout<<"Initializing CULA..."<<endl;
    status = culaInitialize();
    RCutils::checkGPUStatus(status);

	for (int i = 0; i < N; i++){
	    memmove(&A[i*N], matrix[i], sizeof(double) * N);
	}

    //Making a copy of P_vector into T_vector; This is required per specification
    memcpy(T_vector, P_vector, N*sizeof(culaDouble));

    //memset(IPIV, 0, N*sizeof(culaInt));

    cout<<"Calling the solver..."<<endl;
    //General Solver
//	status = culaDgesv(N, NRHS, A, N, IPIV, T_vector, N);
    //Assuming that A is a positive-definite matrix
    status = culaDposv('U', N, NRHS, A, N, T_vector, N);
    if(status == culaInsufficientComputeCapability){
        cout<<"No Double precision support available."<<endl;
        delete(A);
        delete(IPIV);
        culaShutdown();
        return;
    }
    RCutils::checkGPUStatus(status);

    cout<<"Shutting down CULA..."<<endl<<endl;
    culaShutdown();

    delete(A);
    delete(IPIV);
}
#else

/**
 * This solver uses Eigen library to solve the linear set of equations
 */
void RCutils::cpuSolveSteady(double **matrix, int N, double* P_vector, vector<double>& T_vector){
//	Eigen::setNbThreads(8);
//	MatrixXd A = MatrixXd(N,N);

	SparseMatrix<double> A(N,N);
	vector<Triplet<double> > triplet;

	VectorXd b = VectorXd(N);
	//SparseVector<double> b(N);
	for (int i = 0; i < N; i++){
		for(int j=0; j<N; j++){
			//A(i,j)=matrix[i][j];
			if (matrix[i][j]!=0)
				triplet.push_back(Triplet<double> (i,j,matrix[i][j]));
		}
		b(i)=P_vector[i];
	}
	A.setFromTriplets(triplet.begin(), triplet.end());

	cout<<"Calling the solver..."<<endl;
	SimplicialCholesky<SparseMatrix<double> > chol(A);

	b = chol.solve(b);

	//b=A.llt().solve(b);
//	VectorXd x =  b;

	for (int i = 0; i < N; i++){
		T_vector[i]=b(i);
	}
}

void RCutils::thermEq(const state_type &T_vector, state_type &dTdt, double t){
// Code written for Eigen
//   const double *PtState = &T_vector[0];
//   double *PtChange = &dTdt[0];
//
//   typedef Map<const VectorXd> MapTypeConst; // a read-only map
//   typedef Map<VectorXd> MapType;
//
//
//   MapTypeConst States(PtState, n);
//   MapType Changes(PtChange, n);
//
//   Changes = p_vector - (a_matrix * States);

	#pragma omp parallel for schedule(auto) num_threads(4)
//	#pragma omp parallel for schedule(runtime)
	for(int i=0; i<n; i++){
		dTdt[i]=(P_vector[i]);

		for(int j=0; j<n; j++){
			if (a_matrix_indices[i][j]!=-1){
				dTdt[i]-= a_matrix_values[i][j] * T_vector[a_matrix_indices[i][j]];
			}else
				break;
		}

	}
	/*
	 * GPU:
	 *  	typedef thrust::device_vector<double> state_type;
	 * 		runge_kutta4< state_type , double , state_type , double , thrust_algebra ,	thrust_operations > rk4;
	 *
	 */

	//		for(int j=0; j<n; j++){
	//			if (A_matrix[i][j]!=0){
	//				dTdt[i]-=(A_matrix[i][j]*T_vector[j]);
	//			}
	//		}

}

void RCutils::writeTemp( const state_type &T_vector , const double t ){
	cout<<t<<":\t"<<T_vector[0]-273.15<<" C"<<"\n";
}

//template<std::size_t element_no>
void RCutils::cpuSolveTransient(int n, double ** A_matrix, int** a_matrix_indices, double** a_matrix_values,
		double *invC_vector, double* P_vector, vector<double> &T_vector, double time_start, double time_end){

	RCutils::a_matrix_indices=a_matrix_indices;
	RCutils::a_matrix_values=a_matrix_values;
	RCutils::n = n;

	/* Calculating P' = C^-1 * P  */
	RCutils::P_vector = Utils::vectorAlloc(n);
	Utils::diagmatvectmult(RCutils::P_vector, invC_vector, &P_vector[0], n);

	cout<<T_vector[0]-273.15<<" C"<<endl;

	// For Eigen
//	a_matrix=SparseMatrix<double>(n,n);
//	p_vector = VectorXd(n);
//
//	vector<Triplet<double> > triplet;
//	for (int i = 0; i < n; i++){
//		for(int j=0; j<n; j++){
//			if (A_matrix[i][j]!=0){
//				triplet.push_back(Triplet<double> (i, j, A_matrix[i][j]));
//			}
//		}
//		p_vector(i)=RCutils::P_vector[i];
//	}
//	a_matrix.setFromTriplets(triplet.begin(), triplet.end());

	//integrate( system , x0 , t0 , t1 , dt )
	//http://headmyshoulder.github.io/odeint-v2/doc/boost_numeric_odeint/odeint_in_detail/integrate_functions.html

	/* use the scratch pad vector to find (inv_A)*POWER */
	//vdMul(n, &P_vector[0], invC_vector, &P_vector[0]);

	double dt = 0.00001;

//	runge_kutta_cash_karp54 is better than runge_kutta_dopri5
	typedef  runge_kutta_cash_karp54 < state_type > error_stepper_type;
	typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;
	controlled_stepper_type controlled_stepper;

//    adams_bashforth_moulton< 2 , state_type, double, state_type, double, openmp_range_algebra > stepper_adams;

//    int chunk_size = T_vector.size()/omp_get_max_threads();
//    omp_set_schedule( omp_sched_static , chunk_size );

	struct timespec start, end;

	cout<<"Calling the solver..."<<endl;
	clock_gettime(CLOCK_MONOTONIC, &start);

    integrate_adaptive( make_controlled( 5E-1 , 1E-1 , error_stepper_type() ) , thermEq , T_vector , time_start, time_end , dt);

//	integrate_adaptive(stepper_adams , thermEq , T_vector , time_start, time_end , dt);

//	integrate_adaptive( make_controlled<error_stepper_type>( 1E-3 , 1E-3) , thermEq , T_vector , time_start, time_end , dt);
//  integrate_adaptive(stepper_mew , thermEq , T_vector , time_start, time_end , dt);
//	integrate_adaptive(controlled_stepper, thermEq , T_vector , time_start, time_end , dt);

	clock_gettime(CLOCK_MONOTONIC, &end);
	double elapsed = (end.tv_sec - start.tv_sec);
	elapsed += (end.tv_nsec - start.tv_nsec) / 1e9;
	cout<<endl<<"ODE runtime: "<<elapsed<<" s"<<endl;

	cout<<T_vector[0]-273.15<<" C"<<endl;

	Utils::vectorDealloc(RCutils::P_vector);

//	Baseline: HotSpot code
//	double *power = Utils::vectorAlloc(n);
//	/* use the scratch pad vector to find (inv_A)*POWER */
//    diagmatvectmult(power, invC_vector, &P_vector[0], n);
//
//    /* Obtain temp at time (t+time_elapsed).
//     * Instead of getting the temperature at t+time_elapsed directly, we do it
//     * in multiple steps with the correct step size at each time
//     * provided by rk4
//     */
//	int i=0;
//	double h, time_elapsed = 0.1;
//
//	for (double t = 0, new_h = 1e-7 ; t < time_elapsed && new_h >= 1e-7*1.0e-6; t+=h) {
//        h = new_h;
//
//        /* the slope function callback is typecast accordingly */
//        new_h = rk4(A_matrix, &T_vector[0], &P_vector[0], n, &h, &T_vector[0], (slope_fn_ptr) slope_fn_block);
//        new_h = std::min(new_h, time_elapsed-t-h);
//    	cout << t << ":\t" << T_vector[0]-273.15<<" C"<<endl;
//        i++;
//    }
}

#endif
double RCutils::calcThermalConductivity(double k, double thickness, double area){
	if (thickness==0){
		cerr<<"The thickness of an element cannot be zero."<<endl;
		exit(-1);
	}
	return k * area / thickness;
}

double RCutils::calcSubComponentCapacitance(SubComponent *sc){
	double volume = sc->getLength() * sc->getWidth() * sc->getHeight();

	if (volume==0)
		cout<<sc->getName() << " has nil volume."<<endl;

	else if (sc->getComponent()->getMaterial()->getSpecificHeat()==0)
		cout<<sc->getName() << " has nil specific heat."<<endl;

	else if (sc->getComponent()->getMaterial()->getDensity()==0)
		cout<<sc->getName() << " has nil density."<<endl;

	return C_FACTOR * sc->getComponent()->getMaterial()->getSpecificHeat() *
			sc->getComponent()->getMaterial()->getDensity() * volume;
}

double RCutils::calcAmbientResistance(double h, double area){
	return 1/(h * area);
}

double RCutils::overallParallelConductivity(double k1, double k2){
	return (k1 * k2)/( k1 + k2 );
}

bool RCutils::touchesAirInXDir(SubComponent *sc, Device* device){
	if (Utils::eq(sc->getX(), device->getX()) ||
			Utils::eq(sc->getX() +  sc->getLength(), device->getX() +  device->getLength()))
		return true;
	else
		return false;
}

bool RCutils::touchesAirInYDir(SubComponent *sc, Device* device){
	if (Utils::eq(sc->getY(), device->getY()) ||
			Utils::eq(sc->getY() +  sc->getWidth(), device->getY() +  device->getWidth())){
		return true;
	}else{
		return false;
	}
}

bool RCutils::touchesAirFromTopBot(SubComponent *sc, Device* device){
	if (Utils::eq(sc->getZ() , device->getZ()) ||
			Utils::eq(sc->getZ() + sc->getHeight() , device->getZ() +  device->getHeight()))
		return true;
	else
		return false;
}

double RCutils::calcConductanceToAmbient(SubComponent *sc, Device* device){
	double commonArea;
	double t1=0;
	double k;
	double K=0;
	double Kx=0, Ky=0, Kz=0, Rx, Ry, Rz;
//	double h = 10 * 1.15; //	W/m^2/K, from <http://www.engineeringtoolbox.com/convective-heat-transfer-d_430.html>
	double h = 10 * 1.15;
	if (touchesAirFromTopBot(sc, device)){	//Touches air from top or bottom
		t1 = sc->getHeight() / 2;
		commonArea = sc->getLength() * sc->getWidth();
//		cout<<p1->getName() << " and "<<" have XY common area to ambient: "<<commonArea<<endl;

		k = sc->getComponent()->getMaterial()->getNormalConductivity();

		Kz= RCutils::calcThermalConductivity(k, t1, commonArea);
//		cout<<RCutils::calcThermalConductivity(k1, t1, commonArea)<<endl;
		Rz = RCutils::calcAmbientResistance(h, commonArea);
		Kz = RCutils::overallParallelConductivity(Kz, 1/Rz);
	}
	if (touchesAirInXDir(sc, device)){	//Touches air from the X side
		t1 = sc->getLength() / 2;
		commonArea = sc->getWidth() * sc->getHeight();
//		cout<<p1->getName() << " and "<<" have YZ common area to ambient: "<<commonArea<<endl;

		//Setting the k1 value to the proper value if the planar conductivity differs from the normal conductivity
		if (sc->getComponent()->getMaterial()->hasPlanarConductivity())
			k = sc->getComponent()->getMaterial()->getPlanarConductivity();
		else
			k = sc->getComponent()->getMaterial()->getNormalConductivity();

		Kx = RCutils::calcThermalConductivity(k, t1, commonArea);
		Rx = RCutils::calcAmbientResistance(h, commonArea);
		Kx = RCutils::overallParallelConductivity(Kx, 1/Rx);
	}
	if (touchesAirInYDir(sc , device)){		//Touches air from the Y side

		t1 = sc->getWidth() / 2;
		commonArea = sc->getLength() * sc->getHeight();
//		cout<<p1->getName() << " and "<<" have XZ common area to ambient: "<<commonArea<<endl;

		//Setting the k1 value to the proper value if the planar conductivity differs from the normal conductivity
		if (sc->getComponent()->getMaterial()->hasPlanarConductivity())
			k = sc->getComponent()->getMaterial()->getPlanarConductivity();
		else
			k = sc->getComponent()->getMaterial()->getNormalConductivity();

		Ky = RCutils::calcThermalConductivity(k, t1, commonArea);
		Ry = RCutils::calcAmbientResistance(h, commonArea);
		Ky = RCutils::overallParallelConductivity(Ky, 1/Ry);
	}
	return K= Kx + Ky + Kz;
}

double RCutils::calcCommonConductance(SubComponent *sc1, SubComponent *sc2){
	double commonArea;
	double t1=0, t2=0;
	double k1, k2;

	// common area in the Y & Z planes
	double commonX, commonY, commonZ;

	commonX= min(sc1->getX()+sc1->getLength() , sc2->getX()+sc2->getLength()) - max(sc1->getX() , sc2->getX());
	commonY= min(sc1->getY()+sc1->getWidth() , sc2->getY()+sc2->getWidth()) - max(sc1->getY() , sc2->getY());
	commonZ= min(sc1->getZ()+sc1->getHeight() , sc2->getZ()+sc2->getHeight()) - max(sc1->getZ() , sc2->getZ());

	if ((Utils::eq(sc1->getX() + sc1->getLength() , sc2->getX()) ||
		Utils::eq(sc2->getX() + sc2->getLength() , sc1->getX())) && commonZ>0 && commonY>0){
		commonArea = commonY * commonZ;
		 t1 = sc1->getLength() / 2;
		 t2 = sc2->getLength() / 2;

		 //using planar thermal conductivity if the material has different value for it
		 if (sc1->getComponent()->getMaterial()->hasPlanarConductivity()){
			 k1 = sc1->getComponent()->getMaterial()->getPlanarConductivity();
		 }else{
			 k1 = sc1->getComponent()->getMaterial()->getNormalConductivity();
		 }
		 if (sc2->getComponent()->getMaterial()->hasPlanarConductivity()){
			 k2 = sc2->getComponent()->getMaterial()->getPlanarConductivity();
		 }else{
			 k2 = sc2->getComponent()->getMaterial()->getNormalConductivity();
		 }
//		 cout<<"Common area (YZ) btn "<<p1->getName()<<" and "<<p2->getName()<<" is "<<commonArea<<endl;
	}else if ((Utils::eq(sc1->getY() + sc1->getWidth() , sc2->getY()) ||
			Utils::eq(sc2->getY() + sc2->getWidth() , sc1->getY())) && commonX>0 && commonZ>0){
		commonArea = commonX * commonZ;
		 t1 = sc1->getWidth() / 2;
		 t2 = sc2->getWidth() / 2;

		 //using planar thermal conductivity if the material has different value for it
		 if (sc1->getComponent()->getMaterial()->hasPlanarConductivity()){
			 k1 = sc1->getComponent()->getMaterial()->getPlanarConductivity();
		 }else{
			 k1 = sc1->getComponent()->getMaterial()->getNormalConductivity();
		 }
		 if (sc2->getComponent()->getMaterial()->hasPlanarConductivity()){
			 k2 = sc2->getComponent()->getMaterial()->getPlanarConductivity();
		 }else{
			 k2 = sc2->getComponent()->getMaterial()->getNormalConductivity();
		 }
//		 cout<<"Common area (XZ) btn "<<p1->getName()<<" and "<<p2->getName()<<" is "<<commonArea<<endl;
	}else if ((Utils::eq(sc1->getZ() + sc1->getHeight() , sc2->getZ()) ||
			Utils::eq(sc2->getZ() + sc2->getHeight(), sc1->getZ())) && commonX>0 && commonY>0){
		commonArea = commonX * commonY;
		t1 = sc1->getHeight() / 2;
		t2 = sc2->getHeight() / 2;

		//using normal conductivity since it is in the vertical direction
		 k1 = sc1->getComponent()->getMaterial()->getNormalConductivity();
		 k2 = sc2->getComponent()->getMaterial()->getNormalConductivity();

//		 cout<<"Common area (XY) btn "<<p1->getName()<<" and "<<p2->getName()<<" is "<<commonArea<<endl;
	}else{
		commonArea = 0;
		return 0;
	}
	//cout<<p1->getName() << " and "<<p2->getName() <<" have common area: "<<commonArea<<endl;

	double K1 = RCutils::calcThermalConductivity(k1, t1, commonArea);
	double K2 = RCutils::calcThermalConductivity(k2, t2, commonArea);

	return RCutils::overallParallelConductivity(K1,K2);
}

/* compute the slope vector dy for the transient equation
 * dy + cy = p. useful in the transient solver
 */
void slope_fn_block(double *y, double *p, double *dy, double **c, int n)
{
    /* for our equation, dy = p - cy */
    int i;
    double *t = Utils::vectorAlloc(n);
    Utils::matvectmult(t, c, y, n);
    for (i = 0; i < n; i++)
        dy[i] = p[i]-t[i];
    Utils::vectorDealloc(t);
}

/* slope function pointer - used as a call back by the transient solver */
typedef void (*slope_fn_ptr)(void *y, void *p, void *dy, double **c, int n);

/* core of the 4th order Runge-Kutta method, where the Euler step
 * (y(n+1) = y(n) + h * k1 where k1 = dydx(n)) is provided as an input.
 * to evaluate dydx at different points, a call back function f (slope
 * function) is also passed as a parameter. Given values for y, and k1,
 * this function advances the solution over an interval h, and returns
 * the solution in yout. For details, see the discussion in "Numerical
 * Recipes in C", Chapter 16, from
 * http://www.nrbook.com/a/bookcpdf/c16-1.pdf
 */
void rk4_core(double **c, double *y, double *k1, void *p, int n, double h, double *yout, slope_fn_ptr f)
{
	int i;
	double *t, *k2, *k3, *k4;
	k2 = Utils::vectorAlloc(n);
	k3 = Utils::vectorAlloc(n);
	k4 = Utils::vectorAlloc(n);
	t = Utils::vectorAlloc(n);

	/* k2 is the slope at the trial midpoint (t) found using
	 * slope k1 (which is at the starting point).
	 */
	/* t = y + h/2 * k1 (t = y; t += h/2 * k1) */
	for(i=0; i < n; i++)
		t[i] = y[i] + h/2.0 * k1[i];
	/* k2 = slope at t */
	(*f)(t, p, k2, c, n);

	/* k3 is the slope at the trial midpoint (t) found using
	 * slope k2 found above.
	 */
	/* t =  y + h/2 * k2 (t = y; t += h/2 * k2) */
	for(i=0; i < n; i++)
		t[i] = y[i] + h/2.0 * k2[i];
	/* k3 = slope at t */
	(*f)(t, p, k3, c, n);

	/* k4 is the slope at trial endpoint (t) found using
	 * slope k3 found above.
	 */
	/* t =  y + h * k3 (t = y; t += h * k3) */
	for(i=0; i < n; i++)
		t[i] = y[i] + h * k3[i];
	/* k4 = slope at t */
	(*f)(t, p, k4, c, n);

	/* yout = y + h*(k1/6 + k2/3 + k3/3 + k4/6)	*/

	for (i =0; i < n; i++)
		yout[i] = y[i] + h * (k1[i] + 2*k2[i] + 2*k3[i] + k4[i])/6.0;

	Utils::vectorDealloc(k2);
	Utils::vectorDealloc(k3);
	Utils::vectorDealloc(k4);
	Utils::vectorDealloc(t);
}

void copy_dvector (double *dst, double *src, int n){
    memmove(dst, src, sizeof(double) * n);
}

#define RK4_SAFETY      0.95
#define RK4_MAXUP       5.0
#define RK4_MAXDOWN     10.0
#define RK4_PRECISION   0.01
double rk4(double **c, double *y, void *p, int n, double *h, double *yout, slope_fn_ptr f)
{
    int i;
    double *k1, *t1, *t2, *ytemp, max, new_h = (*h);

    k1 = Utils::vectorAlloc(n);
    t1 = Utils::vectorAlloc(n);
    t2 = Utils::vectorAlloc(n);
    ytemp = Utils::vectorAlloc(n);

    /* evaluate the slope k1 at the beginning */
    (*f)(y, p, k1, c, n);

    /* try until accuracy is achieved   */
    do {
        (*h) = new_h;

        /* try RK4 once with normal step size   */
        rk4_core(c, y, k1, p, n, (*h), ytemp, f);

        /* repeat it with two half-steps    */
        rk4_core(c, y, k1, p, n, (*h)/2.0, t1, f);

        /* y after 1st half-step is in t1. re-evaluate k1 for this  */
        (*f)(t1, p, k1, c, n);

        /* get output of the second half-step in t2 */
        rk4_core(c, t1, k1, p, n, (*h)/2.0, t2, f);

        /* find the max diff between these two results:
         * use t1 to store the diff
         */
		for(i=0; i < n; i++)
			t1[i] = fabs(ytemp[i] - t2[i]);
		max = t1[0];
		for(i=1; i < n; i++)
			if (max < t1[i])
				max = t1[i];

	       /*
	         * compute the correct step size: see equation
	         * 16.2.10  in chapter 16 of "Numerical Recipes
	         * in C"
	         */
	        /* accuracy OK. increase step size  */
	        if (max <= RK4_PRECISION) {
	            new_h = RK4_SAFETY * (*h) * pow(fabs(RK4_PRECISION/max), 0.2);
	            if (new_h > RK4_MAXUP * (*h))
	                new_h = RK4_MAXUP * (*h);
	        /* inaccuracy error. decrease step size and compute again */
	        } else {
	            new_h = RK4_SAFETY * (*h) * pow(fabs(RK4_PRECISION/max), 0.25);
	            if (new_h < (*h) / RK4_MAXDOWN)
	                new_h = (*h) / RK4_MAXDOWN;
	        }

	    } while (new_h < (*h));

	    /* commit ytemp to yout */
	    copy_dvector(yout, ytemp, n);

	    /* clean up */
	    Utils::vectorDealloc(k1);
	    Utils::vectorDealloc(t1);
	    Utils::vectorDealloc(t2);
	    Utils::vectorDealloc(ytemp);

	    /* return the step-size */
	    return new_h;
}

/*
 * LUP decomposition from the pseudocode given in the CLRS book
 * ('Introduction to Algorithms'). The matrix 'a' is
 * transformed into an in-place lower/upper triangular matrix
 * and the vector'p' carries the permutation vector such that
 * Pa = lu, where 'P' is the matrix form of 'p'.
 */

void RCutils::lupdcmp(double**a, int n, int *p){
	int i, j, k, pivot=0;
	double max = 0;

	/* start with identity permutation	*/
	for (i=0; i < n; i++)
		p[i] = i;

	for (k=0; k < n; k++)	 {
		max = 0;
		for (i = k; i < n; i++)	{
			if (fabs(a[i][k]) > max) {
				max = fabs(a[i][k]);
				pivot = i;
			}
		}
		if (Utils::eq (max, 0)){
			cerr<<"Singular matrix in lupdcmp."<<endl;
			exit (-1);
		}

		/* bring pivot element to position	*/
		swap(p[k], p[pivot]);
		for (i=0; i < n; i++){
			swap(a[k][i], a[pivot][i]);
		}

		for (i=k+1; i < n; i++) {
			a[i][k] /= a[k][k];
			for (j=k+1; j < n; j++)
				a[i][j] -= a[i][k] * a[k][j];
		}
	}
}

/*
 * the matrix a is an in-place lower/upper triangular matrix
 * the following macros split them into their constituents
 */

#define LOWER(a, i, j)		((i > j) ? a[i][j] : 0)
#define UPPER(a, i, j)		((i <= j) ? a[i][j] : 0)

/*
 * LU forward and backward substitution from the pseudocode given
 * in the CLRS book ('Introduction to Algorithms'). It solves ax = b
 * where, 'a' is an in-place lower/upper triangular matrix. The vector
 * 'x' carries the solution vector. 'p' is the permutation vector.
 */

void RCutils::lusolve(double **a, int n, int *p, double *b, double *x){
	int i, j;
	double *y = new double[n];
	double sum;

	/* forward substitution	- solves ly = pb	*/
	for (i=0; i < n; i++) {
		for (j=0, sum=0; j < i; j++)
			sum += y[j] * LOWER(a, i, j);
		y[i] = b[p[i]] - sum;
	}

	/* backward substitution - solves ux = y	*/
	for (i=n-1; i >= 0; i--) {
		for (j=i+1, sum=0; j < n; j++)
			sum += x[j] * UPPER(a, i, j);
		x[i] = (y[i] - sum) / UPPER(a, i, i);
	}

	delete[] y;
}


//此源码被清华学神尹成大魔王专业翻译分析并修改
//尹成QQ77025077
//尹成微信18510341407
//尹成所在QQ群721929980
//尹成邮箱 yinc13@mails.tsinghua.edu.cn
//尹成毕业于清华大学,微软区块链领域全球最有价值专家
//https://mvp.microsoft.com/zh-cn/PublicProfile/4033620
//版权所有（c）2012-2018比特币核心开发者
//根据MIT软件许可证分发，请参见随附的
//文件复制或http://www.opensource.org/licenses/mit-license.php。

#include <consensus/merkle.h>
#include <merkleblock.h>
#include <serialize.h>
#include <streams.h>
#include <uint256.h>
#include <arith_uint256.h>
#include <version.h>
#include <test/test_bitcoin.h>

#include <vector>

#include <boost/test/unit_test.hpp>

class CPartialMerkleTreeTester : public CPartialMerkleTree
{
public:
//在其中一个散列中翻转一位-这将中断身份验证
    void Damage() {
        unsigned int n = InsecureRandRange(vHash.size());
        int bit = InsecureRandBits(8);
        *(vHash[n].begin() + (bit>>3)) ^= 1<<(bit&7);
    }
};

BOOST_FIXTURE_TEST_SUITE(pmt_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(pmt_test1)
{
    SeedInsecureRand(false);
    static const unsigned int nTxCounts[] = {1, 4, 7, 17, 56, 100, 127, 256, 312, 513, 1000, 4095};

    for (int i = 0; i < 12; i++) {
        unsigned int nTx = nTxCounts[i];

//使用一些虚拟事务构建块
        CBlock block;
        for (unsigned int j=0; j<nTx; j++) {
            CMutableTransaction tx;
tx.nLockTime = j; //实际事务数据并不重要；只需使nlocktime的唯一性
            block.vtx.push_back(MakeTransactionRef(std::move(tx)));
        }

//计算实际梅克尔根和高度
        uint256 merkleRoot1 = BlockMerkleRoot(block);
        std::vector<uint256> vTxid(nTx, uint256());
        for (unsigned int j=0; j<nTx; j++)
            vTxid[j] = block.vtx[j]->GetHash();
        int nHeight = 1, nTx_ = nTx;
        while (nTx_ > 1) {
            nTx_ = (nTx_+1)/2;
            nHeight++;
        }

//用随机子集检查包含机会1，1/2，1/4，…，1/128
        for (int att = 1; att < 15; att++) {
//构建TxID的随机子集
            std::vector<bool> vMatch(nTx, false);
            std::vector<uint256> vMatchTxid1;
            for (unsigned int j=0; j<nTx; j++) {
                bool fInclude = InsecureRandBits(att / 2) == 0;
                vMatch[j] = fInclude;
                if (fInclude)
                    vMatchTxid1.push_back(vTxid[j]);
            }

//建立部分梅克尔树
            CPartialMerkleTree pmt1(vTxid, vMatch);

//串行化
            CDataStream ss(SER_NETWORK, PROTOCOL_VERSION);
            ss << pmt1;

//验证CPartialmerkletree的大小保证
            unsigned int n = std::min<unsigned int>(nTx, 1 + vMatchTxid1.size()*nHeight);
            BOOST_CHECK(ss.size() <= 10 + (258*n+7)/8);

//反序列化为测试仪副本
            CPartialMerkleTreeTester pmt2;
            ss >> pmt2;

//从副本中提取merkle根和匹配的txid
            std::vector<uint256> vMatchTxid2;
            std::vector<unsigned int> vIndex;
            uint256 merkleRoot2 = pmt2.ExtractMatches(vMatchTxid2, vIndex);

//检查它是否有与原始的相同的merkle根，以及一个有效的根。
            BOOST_CHECK(merkleRoot1 == merkleRoot2);
            BOOST_CHECK(!merkleRoot2.IsNull());

//检查它是否包含匹配的事务（顺序相同！）
            BOOST_CHECK(vMatchTxid1 == vMatchTxid2);

//检查随机位翻转是否破坏身份验证
            for (int j=0; j<4; j++) {
                CPartialMerkleTreeTester pmt3(pmt2);
                pmt3.Damage();
                std::vector<uint256> vMatchTxid3;
                uint256 merkleRoot3 = pmt3.ExtractMatches(vMatchTxid3, vIndex);
                BOOST_CHECK(merkleRoot3 != merkleRoot1);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(pmt_malleability)
{
    std::vector<uint256> vTxid = {
        ArithToUint256(1), ArithToUint256(2),
        ArithToUint256(3), ArithToUint256(4),
        ArithToUint256(5), ArithToUint256(6),
        ArithToUint256(7), ArithToUint256(8),
        ArithToUint256(9), ArithToUint256(10),
        ArithToUint256(9), ArithToUint256(10),
    };
    std::vector<bool> vMatch = {false, false, false, false, false, false, false, false, false, true, true, false};

    CPartialMerkleTree tree(vTxid, vMatch);
    std::vector<unsigned int> vIndex;
    BOOST_CHECK(tree.ExtractMatches(vTxid, vIndex).IsNull());
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, Locus Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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.
 *   * 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 <fuse_core/util.h>
#include <fuse_models/SetPose.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>

#include <boost/make_shared.hpp>

#include <cmath>
#include <random>
#include <vector>

static constexpr double SITE_WIDTH = 100.0;  //!< The width/length of the test area in meters
static constexpr double BEACON_SPACING = 20.0;  //!< The distance between each range beacon
static constexpr double BEACON_SIGMA = 4.0;  //!< Std dev used to create the database of noisy beacon positions
static constexpr double ROBOT_PATH_RADIUS = 0.35 * SITE_WIDTH;  //!< The radius of the simulated robot's path
static constexpr double ROBOT_VELOCITY = 10.0;  //!< The forward velocity of our simulated robot
static constexpr char BASELINK_FRAME[] = "base_link";  //!< The base_link frame id used when publishing sensor data
static constexpr char ODOM_FRAME[] = "odom";  //!< The odom frame id used when publishing wheel odometry data
static constexpr char MAP_FRAME[] = "map";  //!< The map frame id used when publishing ground truth data
static constexpr double IMU_SIGMA = 0.1;  //!< Std dev of simulated Imu measurement noise
static constexpr double ODOM_VX_SIGMA = 0.5;  //!< Std dev of simulated Odometry linear velocity measurement noise
static constexpr double ODOM_VYAW_SIGMA = 0.5;  //!< Std dev of simulated Odometry angular velocity measurement noise
static constexpr double RANGE_SIGMA = 0.5;  //!< Std dev of simulated beacon range measurement noise

/**
 * @brief The position of a "range beacon"
 */
struct Beacon
{
  double x;
  double y;
};

/**
 * @brief The true pose and velocity of the robot
 */
struct Robot
{
  ros::Time stamp;
  double x;
  double y;
  double yaw;
  double vx;
  double vy;
  double vyaw;
};

/**
 * @brief Create the set of range beacons to simulate
 */
std::vector<Beacon> createBeacons()
{
  auto beacons = std::vector<Beacon>();
  for (auto x = -SITE_WIDTH / 2; x <= SITE_WIDTH / 2; x += BEACON_SPACING)
  {
    for (auto y = -SITE_WIDTH / 2; y <= SITE_WIDTH / 2; y += BEACON_SPACING)
    {
      beacons.push_back({x, y});  // NOLINT[whitespace/braces]
    }
  }
  return beacons;
}

/**
 * @brief Create a noisy set of beacon priors from the true set of beacons
 */
std::vector<Beacon> createNoisyBeacons(const std::vector<Beacon>& beacons)
{
  static std::random_device rd{};
  static std::mt19937 generator{rd()};
  static std::normal_distribution<> noise{0.0, BEACON_SIGMA};

  auto noisy_beacons = std::vector<Beacon>();
  for (const auto& beacon : beacons)
  {
    noisy_beacons.push_back({beacon.x + noise(generator), beacon.y + noise(generator)});  // NOLINT[whitespace/braces]
  }
  return noisy_beacons;
}

/**
 * @brief Convert the set of beacons into a pointcloud for visualization purposes
 */
sensor_msgs::PointCloud2::ConstPtr beaconsToPointcloud(const std::vector<Beacon>& beacons)
{
  auto msg = boost::make_shared<sensor_msgs::PointCloud2>();
  msg->header.stamp = ros::Time::now();
  msg->header.frame_id = MAP_FRAME;
  sensor_msgs::PointCloud2Modifier modifier(*msg);
  modifier.setPointCloud2Fields(5, "x", 1, sensor_msgs::PointField::FLOAT32,
                                   "y", 1, sensor_msgs::PointField::FLOAT32,
                                   "z", 1, sensor_msgs::PointField::FLOAT32,
                                   "sigma", 1, sensor_msgs::PointField::FLOAT32,
                                   "id", 1, sensor_msgs::PointField::UINT32);
  modifier.resize(beacons.size());
  sensor_msgs::PointCloud2Iterator<float> x_it(*msg, "x");
  sensor_msgs::PointCloud2Iterator<float> y_it(*msg, "y");
  sensor_msgs::PointCloud2Iterator<float> z_it(*msg, "z");
  sensor_msgs::PointCloud2Iterator<float> sigma_it(*msg, "sigma");
  sensor_msgs::PointCloud2Iterator<unsigned int> id_it(*msg, "id");
  for (auto id = 0u; id < beacons.size(); ++id)
  {
    // Compute the distance to each beacon
    const auto& beacon = beacons.at(id);
    *x_it = static_cast<float>(beacon.x);
    *y_it = static_cast<float>(beacon.y);
    *z_it = 10.0f;
    *sigma_it = static_cast<float>(BEACON_SIGMA);
    *id_it = id;
    ++x_it; ++y_it; ++z_it; ++sigma_it, ++id_it;
  }
  return msg;
}

/**
 * @brief Convert the robot state into a ground truth odometry message
 */
nav_msgs::Odometry::ConstPtr robotToOdometry(const Robot& state)
{
  auto msg = boost::make_shared<nav_msgs::Odometry>();
  msg->header.stamp = state.stamp;
  msg->header.frame_id = MAP_FRAME;
  msg->child_frame_id = BASELINK_FRAME;
  msg->pose.pose.position.x = state.x;
  msg->pose.pose.position.y = state.y;
  msg->pose.pose.position.z = 10.0;
  msg->pose.pose.orientation.w = std::cos(state.yaw / 2);
  msg->pose.pose.orientation.x = 0.0;
  msg->pose.pose.orientation.y = 0.0;
  msg->pose.pose.orientation.z = std::sin(state.yaw / 2);
  msg->pose.covariance[0] = 0.1;
  msg->pose.covariance[7] = 0.1;
  msg->pose.covariance[14] = 0.1;
  msg->pose.covariance[21] = 0.1;
  msg->pose.covariance[28] = 0.1;
  msg->pose.covariance[35] = 0.1;
  msg->twist.twist.linear.x = state.vx;
  msg->twist.twist.linear.y = state.vy;
  msg->twist.twist.linear.z = 0.0;
  msg->twist.twist.angular.x = 0.0;
  msg->twist.twist.angular.y = 0.0;
  msg->twist.twist.angular.z = state.vyaw;
  msg->twist.covariance[0] = 0.1;
  msg->twist.covariance[7] = 0.1;
  msg->twist.covariance[14] = 0.1;
  msg->twist.covariance[21] = 0.1;
  msg->twist.covariance[28] = 0.1;
  msg->twist.covariance[35] = 0.1;
  return msg;
}

/**
 * @brief Send the starting robot pose to the state estimator
 *
 * The state estimator will not run until it has been sent a starting pose.
 */
void initializeStateEstimation(const Robot& state)
{
  // Send the initial localization signal to the state estimator
  auto srv = fuse_models::SetPose();
  srv.request.pose.header.frame_id = MAP_FRAME;
  srv.request.pose.pose.pose.position.x = state.x;
  srv.request.pose.pose.pose.position.y = state.y;
  srv.request.pose.pose.pose.position.z = 0.0;
  srv.request.pose.pose.pose.orientation.w = std::cos(state.yaw / 2);
  srv.request.pose.pose.pose.orientation.x = 0.0;
  srv.request.pose.pose.pose.orientation.y = 0.0;
  srv.request.pose.pose.pose.orientation.z = std::sin(state.yaw / 2);
  srv.request.pose.pose.covariance[0] = 1.0;
  srv.request.pose.pose.covariance[7] = 1.0;
  srv.request.pose.pose.covariance[14] = 1.0;
  srv.request.pose.pose.covariance[21] = 1.0;
  srv.request.pose.pose.covariance[28] = 1.0;
  srv.request.pose.pose.covariance[35] = 1.0;
  ros::service::waitForService("/state_estimation/set_pose", ros::Duration(30.0));
  auto success = false;
  while (!success)
  {
    ros::Duration(0.1).sleep();
    srv.request.pose.header.stamp = ros::Time::now();
    ros::service::call("/state_estimation/set_pose", srv);
    success = srv.response.success;
  }
}

/**
 * @brief Compute the next robot state given the current robot state and a simulated step time
 */
Robot simulateRobotMotion(const Robot& previous_state, const ros::Time& now)
{
  auto dt = (now - previous_state.stamp).toSec();
  auto theta = std::atan2(previous_state.y, previous_state.x) + (dt * previous_state.vyaw);
  auto next_state = Robot();
  next_state.stamp = now;
  next_state.x = ROBOT_PATH_RADIUS * std::cos(theta);
  next_state.y = ROBOT_PATH_RADIUS * std::sin(theta);
  next_state.yaw = fuse_core::wrapAngle2D(theta + (M_PI / 2));
  next_state.vx = previous_state.vx;
  next_state.vy = 0.0;
  next_state.vyaw = previous_state.vyaw;
  return next_state;
}

/**
 * @brief Create a simulated Imu measurement from the current state
 */
sensor_msgs::Imu::ConstPtr simulateImu(const Robot& robot)
{
  static std::random_device rd{};
  static std::mt19937 generator{rd()};
  static std::normal_distribution<> noise{0.0, IMU_SIGMA};

  auto msg = boost::make_shared<sensor_msgs::Imu>();
  msg->header.stamp = robot.stamp;
  msg->header.frame_id = BASELINK_FRAME;
  msg->orientation_covariance[0] = -1;  // Simulated IMU does not provide orientation
  msg->angular_velocity.z = robot.vyaw + noise(generator);
  msg->angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA;
  msg->linear_acceleration_covariance[0] = -1;  // Simulated IMU does not provide acceleration
  return msg;
}

/**
 * @brief Create a simulated Odometry measurement from the current state
 */
nav_msgs::Odometry::ConstPtr simulateWheelOdometry(const Robot& robot)
{
  static std::random_device rd{};
  static std::mt19937 generator{rd()};
  static std::normal_distribution<> vx_noise{0.0, ODOM_VX_SIGMA};
  static std::normal_distribution<> vyaw_noise{0.0, ODOM_VYAW_SIGMA};

  auto msg = boost::make_shared<nav_msgs::Odometry>();
  msg->header.stamp = robot.stamp;
  msg->header.frame_id = ODOM_FRAME;
  msg->child_frame_id = BASELINK_FRAME;
  msg->twist.twist.linear.x = robot.vx + vx_noise(generator);
  msg->twist.twist.linear.y = 0.0;
  msg->twist.twist.angular.z = robot.vyaw + vyaw_noise(generator);
  msg->twist.covariance[0] = ODOM_VX_SIGMA * ODOM_VX_SIGMA;
  msg->twist.covariance[7] = ODOM_VX_SIGMA * ODOM_VX_SIGMA;
  msg->twist.covariance[35] = ODOM_VYAW_SIGMA * ODOM_VYAW_SIGMA;
  return msg;
}

sensor_msgs::PointCloud2::ConstPtr simulateRangeSensor(const Robot& robot, const std::vector<Beacon>& beacons)
{
  static std::random_device rd{};
  static std::mt19937 generator{rd()};
  static std::normal_distribution<> noise{0.0, RANGE_SIGMA};

  auto msg = boost::make_shared<sensor_msgs::PointCloud2>();
  msg->header.stamp = robot.stamp;
  msg->header.frame_id = BASELINK_FRAME;

  // Configure the pointcloud to have the following fields: id, range, sigma
  sensor_msgs::PointCloud2Modifier modifier(*msg);
  modifier.setPointCloud2Fields(3, "id", 1, sensor_msgs::PointField::UINT32,
                                   "range", 1, sensor_msgs::PointField::FLOAT64,
                                   "sigma", 1, sensor_msgs::PointField::FLOAT64);

  // Generate the simulated range to each known beacon
  modifier.resize(beacons.size());
  sensor_msgs::PointCloud2Iterator<unsigned int> id_it(*msg, "id");
  sensor_msgs::PointCloud2Iterator<double> range_it(*msg, "range");
  sensor_msgs::PointCloud2Iterator<double> sigma_it(*msg, "sigma");
  for (auto id = 0u; id < beacons.size(); ++id)
  {
    // Compute the distance to each beacon
    const auto& beacon = beacons.at(id);
    auto dx = robot.x - beacon.x;
    auto dy = robot.y - beacon.y;
    auto range = std::sqrt(dx * dx + dy * dy) + noise(generator);
    // Insert the beacon measurement into the pointcloud
    *id_it = id;
    *range_it = range;
    *sigma_it = RANGE_SIGMA;
    ++id_it; ++range_it; ++sigma_it;
  }
  return msg;
}

/**
 * @brief Simulate a robot traveling in a circular path with wheel encoders, Imu, and a range sensor.
 *
 * The following simulated sensor topics are published:
 *  - The wheel encoders measure forward and rotational velocity and publish nav_msgs::Odometry messages on the
 *    /wheel_odom topic at 10Hz.
 *  - The Imu measures z-axis rotational velocity and publishes sensor_msgs::Imu messages on the /imu topic at 10Hz.
 *  - The "range sensor" publishes special sensor_msgs::PointCloud2 messages on the /ranges topic. The PointCloud2
 *    message contains three channels (id, range, sigma). The id field contains the unique ID of the range beacon
 *    being measured. The range field contains the measured distance between the robot and the range beacon in
 *    meters. The sigma field contains the range measurement uncertainty (standard deviation) in meters.
 *  - A prior known database of noisy beacon positions are published on the /prior_beacons latched topic as a
 *    sensor_msgs::PointCloud2 with the following fields: (x, y, z, sigma, id)
 *
 * In addition to the simulated sensors, the following ground truth topics are published:
 *  - The true position of each beacon is published as a sensor_msgs::PointCloud2 (x, y, z, sigma, id) topic on the
 *    latched topic /true_beacons
 *  - The true position and velocity of the robot is published as a nav_msgs::Odometry message on the /ground_truth
 *    topic at 10Hz
 */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "range_sensor_simulator");

  ros::NodeHandle node_handle;
  ros::Publisher imu_publisher = node_handle.advertise<sensor_msgs::Imu>("imu", 1);
  ros::Publisher true_beacons_publisher = node_handle.advertise<sensor_msgs::PointCloud2>("true_beacons", 1, true);
  ros::Publisher prior_beacons_publisher = node_handle.advertise<sensor_msgs::PointCloud2>("prior_beacons", 1, true);
  ros::Publisher wheel_odom_publisher = node_handle.advertise<nav_msgs::Odometry>("wheel_odom", 1);
  ros::Publisher ground_truth_publisher = node_handle.advertise<nav_msgs::Odometry>("ground_truth", 1);
  ros::Publisher range_publisher = node_handle.advertise<sensor_msgs::PointCloud2>("ranges", 1);

  // Create the true set of range beacons
  auto beacons = createBeacons();
  true_beacons_publisher.publish(beaconsToPointcloud(beacons));

  // Publish a set of noisy beacon locations to act as the known priors
  auto noisy_beacons = createNoisyBeacons(beacons);
  prior_beacons_publisher.publish(beaconsToPointcloud(noisy_beacons));

  // Initialize the robot state
  auto state = Robot();
  state.stamp = ros::Time::now();
  state.x = ROBOT_PATH_RADIUS;
  state.y = 0.0;
  state.yaw = M_PI / 2;
  state.vx = ROBOT_VELOCITY;
  state.vy = 0.0;
  state.vyaw = state.vx / ROBOT_PATH_RADIUS;

  // Send the initial localization pose to the state estimator
  initializeStateEstimation(state);

  // Simulate the robot traveling in a circular path
  auto rate = ros::Rate(10.0);
  while (ros::ok())
  {
    // Simulate the robot motion
    auto new_state = simulateRobotMotion(state, ros::Time::now());
    // Publish the new ground truth
    ground_truth_publisher.publish(robotToOdometry(new_state));
    // Generate and publish simulated measurements from the new robot state
    imu_publisher.publish(simulateImu(new_state));
    wheel_odom_publisher.publish(simulateWheelOdometry(new_state));
    range_publisher.publish(simulateRangeSensor(new_state, beacons));
    // Wait for the next time step
    state = new_state;
    rate.sleep();
  }

  return 0;
}


//          Copyright Danny Havenith Aug 2, 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)

/**
 * @file avr_core.hpp
 * @author Danny Havenith
 *
 * This file contains the implementation of the AVR instructions. These are implemented in the avr_core class.
 *
 */
#ifndef AVR_CORE_HPP_
#define AVR_CORE_HPP_
#include <vector>
#include <boost/cstdint.hpp>
#include <array>
#include <exception>
#include <tuple>

#include "avr_instruction_set.hpp"

namespace avrsim
{

using namespace instructions;

struct flags_t
{
    typedef boost::uint8_t flag_t;
    flag_t I;
    flag_t T;
    flag_t H;
    flag_t S;
    flag_t V;
    flag_t N;
    flag_t Z;
    flag_t C;

    bool operator==( const flags_t &other) const
    {
        return std::tie( I, T, H, S, V, N, Z, C) ==
                std::tie( other.I, other.T, other.H, other.S, other.V, other.N, other.Z, other.C);
    }
};

struct avr_state
{
    avr_state( size_t ram_size)
        : sp( ram_size -1), ram(ram_size)
    {}

    avr_state()
        :avr_state(512){}

    typedef boost::uint16_t instruction_t;
    typedef boost::uint8_t  register_t;
    typedef boost::uint32_t pointer_t;
    typedef std::vector<instruction_t> rom_t;
    typedef std::vector<register_t>    ram_t;

    uint8_t         eind{}; // only available on 22-bit pc cores
    bool            is_sleeping{};
    flags_t         flags{};
    unsigned int    clock_ticks{};
    pointer_t       pc{};
    pointer_t       sp{};
    ram_t           ram;
    rom_t           rom;
};

struct avr_core_utils
{
    static boost::int16_t signed_( boost::uint8_t input)
    {
        using namespace boost;
        return static_cast<int16_t>(static_cast<int8_t>( input));
    }
};

/**
 * Class that implements the core instruction set of the AVR microcontroller.
 *
 * This class implements each instruction as an overload of the 'execute' member function.
 * Each overload takes the instruction tag as its first argument and then, depending on the
 * instruction, takes zero, one or two extra arguments.
 */
class avr_core : public avr_state, avr_core_utils
{
public:

    using avr_state::avr_state; // delegate constructors to the avr_state
    typedef boost::int16_t  signed16;
    typedef boost::uint16_t unsigned16;
    typedef boost::int8_t   signed8;
    typedef boost::uint8_t  unsigned8;
    typedef boost::uint16_t operand;
    static constexpr int register_X = 26;
    static constexpr int register_Y = 28;
    static constexpr int register_Z = 30;
    static constexpr int io_offset = 0x20;
    static constexpr bool is_xmega = false;
    static constexpr bool is_22bit_pc = false;
    static constexpr int rampz_io_address = 0x3b;

    void execute( NOP)
    {
        // do nothing
    }

    void execute( MOVW, operand dest, operand source)
    {
        // move word
        assign8( 2*dest, reg(2*source));
        assign8( 2*dest + 1, reg(2*source + 1));
    }

    void execute( MULS, operand dest, operand source)
    {
        // multiply signed
        assign16( 0, signed_multiply( dest, source));
    }

    void execute( MULSU, operand dest, operand source)
    {
        // multiply signed with unsigned
        assign16( 0, signed_unsigned_multiply(dest, source));
    }

    void execute( FMUL, operand dest, operand source)
    {
        assign16( 0, unsigned_multiply( dest, source) << 1);
    }

    void execute( FMULS, operand dest, operand source)
    {
        assign16( 0, signed_multiply( dest, source) << 1);
    }

    void execute( FMULSU, operand dest, operand source)
    {
        assign16( 0, signed_unsigned_multiply( dest, source) << 1);
    }

    void execute( CPC, operand dest, operand source)
    {
        // ignore subtract results, change flags only
        auto previousZ = flags.Z;
        subtract( reg(dest), static_cast<unsigned16>(reg(source)) + (flags.C?1:0));
        flags.Z = flags.Z and previousZ;
    }

    void execute( SBC, operand dest, operand source)
    {
        auto previousZ = flags.Z;
        assign8( dest, subtract( reg(dest), static_cast<unsigned16>(reg(source)) + (flags.C?1:0)));
        flags.Z = flags.Z and previousZ;
    }

    void execute( ADD, operand dest, operand source)
    {
        assign8( dest, add( reg(dest), reg(source)));
    }

    void execute( CPSE, operand dest, operand source)
    {
        if (reg(dest) == reg(source))
        {
            skip();
        }
    }

    void execute( CP, operand dest, operand source)
    {
        subtract( reg(dest), reg(source));
    }

    void execute( SUB, operand dest, operand source)
    {
        assign8( dest, subtract( reg(dest), reg(source)));
    }

    void execute( ADC, operand dest, operand source)
    {
        assign8( dest, add( reg(dest), flags.C?((unsigned16)(reg(source)+1)):reg(source)));
    }

    unsigned16 fetch_instruction_word()
    {
        return rom[pc++];
    }

    void execute( AND, operand dest, operand source)
    {
        assign8( dest, set_logical_flags(reg(dest) & reg(source)));
    }

    void execute( EOR, operand dest, operand source)
    {
        assign8( dest, set_logical_flags( reg(dest) ^ reg(source)));
    }

    void execute( OR, operand dest, operand source)
    {
        assign8( dest, set_logical_flags( reg(dest) | reg(source)));
    }

    void execute( MOV, operand dest, operand source)
    {
        assign8( dest, reg(source));
    }

    void execute( CPI, operand dest, operand constant)
    {
        subtract( reg(dest + 16), constant);
    }

    void execute( SBCI, operand dest, operand constant)
    {
        assign8( dest+16, subtract( reg(dest+16), flags.C?((unsigned16)(constant+1)):constant));
    }

    void execute( SUBI, operand dest, operand constant)
    {
        assign8( dest+16, subtract( reg(dest+16), constant));
    }

    void execute( ORI, operand dest, operand constant)
    {
        assign8( dest, set_logical_flags( reg(dest) | constant));
    }

    void execute( ANDI, operand dest, operand constant)
    {
        assign8( dest, set_logical_flags( reg(dest) & constant));
    }

    void execute( LDD_Y, operand dest, operand offset)
    {
        assign8( dest, ram( get16( register_Y) + offset)    );
        extra_clocktick();
    }

    void execute( LDD_Z, operand dest, operand offset)
    {
        assign8( dest, ram( get16( register_Z) + offset)    );
        extra_clocktick();
    }

    void execute( STD_Z, operand source, operand offset)
    {
        ram( get16( register_Z) + get_rampz_offset() + offset)     = reg(source);
    }

    void execute( STD_Y, operand source, operand offset)
    {
        ram( get16( register_Y) + offset)     = reg(source);
    }

    void execute( LDS, operand destination)
    {
        assign8( destination, ram( fetch_instruction_word())    );
    }

    void increase16( operand dest)
    {
        assign16( dest, get16( dest) + 1);
    }

    void decrease16( operand dest)
    {
        assign16( dest, get16( dest) - 1);
    }

    void execute( LD_Z_inc, operand dest)
    {
        execute( LDD_Z(), dest, 0);
        increase16( register_Z);
        extra_clocktick();
    }

    void execute( LD_Z_dec, operand dest)
    {
        decrease16( register_Z);
        execute( LDD_Z(), dest, 0);
        extra_clocktick();
    }

    void execute( LD_Z_min, operand dest)
    {
        decrease16( register_Z);
        execute( LDD_Z(), dest, 0);
        increase16( register_Z);
        extra_clocktick();
    }

    void execute( LPM_Z, operand dest)
    {
        assign8( dest, fetch_rom_byte( get16( register_Z)));
        extra_clockticks(2);
    }

    void execute( LPM_Z_inc, operand dest)
    {
        execute( LPM_Z(), dest);
        increase16( register_Z);
    }

    void execute( ELPM_Z, operand dest)
    {
        assign8( dest, fetch_rom_byte( get16( register_Z) + get_rampz_offset()));
        extra_clockticks(2);
    }

    void execute( ELPM_Z_inc, operand dest)
    {
        execute( ELPM_Z(), dest);
        increase16( register_Z);
    }

    void execute( LD_Y_inc, operand dest)
    {
        execute( LDD_Y(), dest, 0);
        increase16( register_Y);
    }

    void execute( LD_Y_dec, operand dest)
    {
        decrease16( register_Y);
        execute( LDD_Y(), dest, 0);
    }

    void execute( LD_X, operand dest)
    {
        assign8( dest, ram( get16( register_X))    );
        extra_clocktick();
    }

    void execute( LD_X_inc, operand dest)
    {
        execute( LD_X(), dest);
        increase16( register_X);
    }

    void execute( LD_X_dec, operand dest)
    {
        decrease16( register_X);
        execute( LD_X(), dest);
    }

    void execute( POP, operand dest)
    {
        assign8( dest, ram( ++sp));
    }

    void execute( STS, operand source)
    {
        execute( STS_direct(), source, fetch_instruction_word());
    }

    void execute( STS_direct, operand source, operand address)
    {
        ram( address + get_rampd_offset())     = reg(source);
    }

    void execute( ST_Z_inc, operand source)
    {
        execute( STD_Z(), source, 0);
        increase16( register_Z);
    }

    void execute( ST_Z_dec, operand source)
    {
        decrease16( register_Z);
        execute( STD_Z(), source, 0);
    }

    void execute( ST_Y_inc, operand source)
    {
        execute( STD_Y(), source, 0);
        increase16( register_Y);
    }

    void execute( ST_Y_dec, operand source)
    {
        decrease16(register_Y);
        execute( STD_Y(), source, 0);
    }

    void execute( ST_Y, operand source)
    {
        execute( STD_Y(), source, 0);
    }

    void execute( ST_X_inc, operand source)
    {
        execute( ST_X(), source);
        increase16( register_X);
    }

    void execute( ST_X, operand source)
    {
        ram( get16( register_X) + get_rampx_offset())     = reg(source);
    }

    void execute( ST_X_dec, operand source)
    {
        decrease16( register_X);
        execute( ST_X(), source);
    }

    void execute( PUSH, operand dest)
    {
        ram( sp--)     = reg(dest);
    }

    void execute( COM, operand dest)
    {
        assign8( dest, ~reg(dest));
        flags.V = 0;
        flags.C = 0x80;
        set_z_and_n( reg(dest));
    }

    void execute( NEG, operand dest)
    {
        register_t result = -reg(dest);
        set_z_and_n( result);
        flags.V = reg(dest) == 0x80;
        flags.C = result != 0;
        flags.H = (result | reg(dest)) & 0x80;
        assign8( dest, result);
    }

    void execute( SWAP, operand dest)
    {
        assign8( dest, (reg(dest) >> 4) | (reg(dest) << 4));
    }

    void execute( INC, operand dest)
    {
        assign8( dest, reg(dest) +1);
        flags.V = reg(dest) == 0x80;
        set_z_and_n( reg(dest));
    }

    void execute( unk2)
    {
    }

    void execute( ASR, operand dest)
    {
        flags.C = (reg(dest) & 0x01) << 7;
        assign8( dest, ((signed8)reg(dest))/2);
        set_z_and_n( reg(dest));
        flags.V = flags.N ^ flags.C;
    }

    void execute( LSR, operand dest)
    {
        flags.C = (reg(dest) & 0x01) << 7;
        assign8( dest, reg(dest) >> 1);
        set_z_and_n( reg(dest));
        flags.V = flags.N ^ flags.C;
    }

    void execute( ROR, operand dest)
    {

        unsigned8 result = reg(dest) >> 1;
        if (flags.C)
        {
            result |= 0x80;
        }
        flags.C = (reg(dest) & 0x01) << 7;
        set_z_and_n( result);
        flags.V = flags.N ^ flags.C;
        assign8( dest, result);
    }

    void execute( DEC, operand dest)
    {
        assign8( dest, reg(dest) - 1);
        set_z_and_n( reg(dest));
        flags.V = reg(dest) == 0x7f;
    }

    void execute( BSET, operand bit)
    {
        number_to_flag( bit) = 0x80;
    }

    void execute( BCLR, operand bit)
    {
        number_to_flag( bit) = 0;
    }

    void execute( IJMP)
    {
        pc = get16( register_Z);
        extra_clocktick();
    }

    /// TODO
    pointer_t get_eind_offset()
    {
        return 0;
    }

    // 16-bit addressing scheme.
    pointer_t pop_address()
    {
        pointer_t result = ram( sp+1)     + (ram( sp+2)     << 8);
        sp += 2;
        extra_clocktick();
        return result;
    }

    void push_address( pointer_t address)
    {
        if (sp > 1)
        {
            ram( sp)     = address >> 8;
            ram( sp-1)     = address;
            sp -= 2;
            extra_clockticks(2);
        }
    }

    void execute( EIJMP)
    {
        pc = get16( register_Z) + get_eind_offset();
        extra_clocktick();
    }

    void execute( RET)
    {
        pc = pop_address();
    }

    void execute( ICALL)
    {
        push_address( pc);
        pc = get16( register_Z);
    }

    void execute( RETI)
    {
        pc = pop_address();
        if (!is_xmega) flags.I = 1;
        extra_clockticks( is_22bit_pc?4:3);
    }

    void execute( EICALL)
    {
        push_address(pc);
        pc = (static_cast<pointer_t>(eind) << 16) + get16( register_Z );
        extra_clockticks( is_xmega?2:3);
    }

    void execute( SLEEP)
    {
        if (is_sleep_enabled())
        {
            is_sleeping = true;
        }
    }

    class break_exception : std::exception
    {
    public:
        break_exception( pointer_t pc, uint64_t clock)
        :pc{pc},clock{clock}
        {}

        virtual const char *what() const noexcept
        {
            return "break";
        }
        pointer_t pc;
        uint64_t  clock;

    };

    void execute( BREAK)
    {
        throw break_exception{pc-1, clock_ticks};
    }

    void execute( WDR)
    {
        // todo: implement
    }

    void execute( LPM)
    {
        execute( LPM_Z{}, 0);
    }

    void execute( ELPM)
    {
        execute( ELPM_Z{}, 0);
    }

    void execute( SPM)
    {
        // todo: implement
    }

    void execute( ESPM)
    {
        // todo: implement
    }

    void execute( JMP, operand upper_bits)
    {
        pc = fetch_instruction_word() + (static_cast<pointer_t>( upper_bits) << 16);
        extra_clockticks(2);
    }

    void execute( CALL, operand upper_bits)
    {
        push_address( pc);
        pc = fetch_instruction_word() + (static_cast<pointer_t>( upper_bits) << 16);
        extra_clockticks(3);
    }

    void execute( ADIW, operand reg_pair, operand value)
    {
        assign16( 2*reg_pair + 24, add16( reg(2*reg_pair + 24), value));
        extra_clocktick();
    }

    void execute( SBIW, operand reg_pair, operand value)
    {
        assign16( 2*reg_pair+24, subtract16( get16(2*reg_pair+24), value));
        extra_clocktick();
    }

    void execute( CBI, operand ioaddr, operand bit)
    {
        set_io( ioaddr, get_io( ioaddr) & ~ (1<<bit));
        extra_clocktick();
    }

    void execute( SBIC, operand ioaddr, operand bit)
    {
        if (!(get_io( ioaddr) & (1<<bit)))
        {
            skip();
        }
    }

    void execute( SBI, operand ioaddr, operand bit)
    {
        set_io( ioaddr, get_io( ioaddr) | (1<<bit));
    }

    void execute( SBIS, operand ioaddr, operand bit)
    {
        if (get_io( ioaddr) & (1<<bit))
        {
            skip();
        }
    }

    void execute( MUL, operand dest, operand source)
    {
       assign16( 0, static_cast<uint16_t>( reg(dest)) * reg(source));
    }

    void execute( IN, operand dest, operand io_address)
    {
        assign8( dest, get_io( io_address));
    }

    void execute( OUT, operand ioaddr, operand source)
    {
        set_io( ioaddr, reg(source));
    }

    void execute( RJMP, operand offset)
    {
        pc += sign_extend12( offset);
        extra_clocktick();
    }

    void execute( RCALL, operand offset)
    {
        push_address( pc);
        pc += sign_extend12( offset);
    }

    void execute( LDI, operand dest, operand constant)
    {
        assign8( 0x10 | dest, constant);
    }

    void execute( BRBS, operand offset, operand bit)
    {
        if (number_to_flag( bit))
        {
            pc += sign_extend7( offset);
            extra_clocktick();
        }
    }

    void execute( BRBC, operand offset, operand bit)
    {
        if (!number_to_flag(bit))
        {
            pc += sign_extend7( offset);
            extra_clocktick();
        }
    }

    void execute( BLD, operand dest, operand bit)
    {
        const register_t mask = 1<<bit;
        assign8( dest, (reg(dest) & ~mask) | ((flags.T)?mask:0));
    }

    void execute( BST, operand dest, operand bit)
    {
        flags.T = reg(dest) & ( 1<<bit);
    }

    void execute( SBRC, operand dest, operand bit)
    {
        if (not (reg(dest)&(1<<bit)))
        {
            skip();
        }
    }

    void execute( SBRS, operand dest, operand bit)
    {
        if (reg(dest) & (1<<bit))
        {
            skip();
        }
    }

    register_t &ram( pointer_t address)
    {
        return avr_state::ram.at(address);
    }

    const register_t &ram( pointer_t address) const
    {
        return avr_state::ram.at(address);
    }

    const register_t &reg( pointer_t dest) const
    {
        return avr_state::ram[dest];
    }

    register_t &reg_write( pointer_t dest)
    {
        return avr_state::ram[dest];
    }

    void assign8( operand dest, uint8_t value)
    {
        reg_write( dest) = value;
    }

private:
    void extra_clocktick()
    {
        ++clock_ticks;
    }

    void extra_clockticks( int ticks)
    {
        clock_ticks += ticks;
    }
    int16_t sign_extend12( operand input)
    {
        return (( static_cast<int16_t>(input) + 0x0800) & 0x0FFF) - 0x0800;
    }

    int8_t sign_extend7( operand input)
    {
        return ((static_cast<int8_t>(input) + 0x40) & 0x7F) - 0x40;
    }

    // not used yet. should replace the two functions above.
    template< typename Integer, int sourceBits>
    Integer sign_extend( Integer source)
    {
        constexpr Integer upperBit = (1 << (sourceBits - 1));
        return ((source ^ upperBit) & ((1<< sourceBits)-1)) - upperBit;
    }

    bool is_sleep_enabled()
    {
        // todo: implement MCUCR
        return true;
    }

    flags_t::flag_t &number_to_flag( operand bit)
    {
        switch (bit)
        {
        case 0: return flags.C;
        case 1: return flags.Z;
        case 2: return flags.N;
        case 3: return flags.V;
        case 4: return flags.S;
        case 5: return flags.H;
        case 6: return flags.T;
        default: return flags.I;
        }

    }

    unsigned8 fetch_rom_byte( pointer_t address) const
    {
        // ROM byte addressing emulates little endian
        // (low byte is address.0 == 0)
        return (address& 0x01)?
                        (rom[address/2] >> 8)
                    :   (rom[address/2]);
    }

    unsigned8 set_logical_flags( unsigned8 result)
    {
        set_z_and_n( result);
        flags.V = 0;
        flags.S = flags.N;

        return result;
    }

    void skip()
    {
        auto instruction = fetch_instruction_word();
        extra_clocktick();

        if (
                is_instruction<CALL>( instruction)
            |   is_instruction<JMP> (instruction)
            |   is_instruction<LDS> (instruction)
            |   is_instruction<STS> (instruction)
         )
        {
            fetch_instruction_word();
            extra_clocktick();
        }
    }

    unsigned8 subtract( unsigned16 dest, unsigned16 source)
    {
        unsigned8 result = dest - source;

        // calculate carry and half-carry
        // we're doing the common subexpression elimination, just
        // in case our compiler doesn't
        unsigned16 carries =
                    (~dest & source)
                |   (source & result)
                |   (result & ~dest);
        flags.C = carries & 0x80;
        flags.H = carries & 0x08;
        flags.V =  (( dest & ~source & ~result)
                |   (~dest &  source &  result))
                & 0x80;
        flags.S = flags.V ^ flags.N;
        set_z_and_n( result);

        return result;
    }

    signed16 subtract16( signed16 left, signed16 right)
    {
        signed16 result = left - right;
        flags.C = (left >= 0) and (result < 0);
        flags.N = result < 0;
        flags.V = (left < 0) and (result >= 0); // check
        flags.Z = result == 0;
        flags.S = !flags.N != !flags.V;
        return result;
    }

    signed16 add16( signed16 left, signed16 right)
    {
        signed16 result = left + right;
        flags.C = result >= 0 and left < 0;
        flags.Z = result == 0;
        flags.N = result < 0;
        flags.V = result < 0 and left >= 0;
        flags.S = !flags.N != !flags.V;
        return result;
    }

    unsigned8 add( unsigned16 dest, unsigned16 source)
    {
        unsigned8 result = dest + source;
        unsigned16 carries =
                    (source & dest)
                |   (source & ~result)
                |   (dest & ~result);

        set_z_and_n( result);
        flags.C = carries & 0x80;
        flags.H = carries & 0x08;
        flags.V =   ((dest & source & ~result)
                |   (~dest & ~source & result))
                & 0x80;
        flags.S = flags.V ^ flags.N;

        return result;
    }

    void set_z_and_n( register_t result)
    {
        flags.Z = result == 0;
        flags.N = result & 0x80;
    }

    void set_flags_after_multiply( signed16 result)
    {
        flags.Z = result == 0;
        flags.C = result & 0x8000;
    }

    unsigned16 unsigned_multiply( operand dest, operand source)
    {
        unsigned16 result = reg(dest+16) * reg(source+16);
        set_flags_after_multiply( result);
        extra_clocktick();
        return result;
    }

    signed16 signed_multiply( operand dest, operand source)
    {
        signed16 result = signed_(reg(dest+16)) * signed_(reg(source+16));
        set_flags_after_multiply( result);
        extra_clocktick();
        return result;
    }

    signed16 signed_unsigned_multiply( operand dest, operand source)
    {
        boost::int16_t result = signed_( reg(dest+16)) * reg(source+16);
        set_flags_after_multiply( result);
        extra_clocktick();
        return result;
    }

    pointer_t get_rampd_offset() const
    {
        return 0;
    }

    // fetch the contents of the RAMPZ register in io space.
    pointer_t get_rampz_offset() const
    {
        return get_io( rampz_io_address);
    }

    // todo: implement
    pointer_t get_rampx_offset() const
    {
        return 0;
    }

    // todo: implement
    pointer_t get_rampy_offset() const
    {
        return 0;
    }

    register_t &get_io_address( uint16_t address)
    {
        return ram( address + io_offset)    ;
    }

    const register_t &get_io_address( uint16_t address) const
    {
        return ram( address + io_offset)    ;
    }

    void set_io( operand address, register_t value)
    {
        get_io_address( address) = value;
    }

    register_t get_io( operand address) const
    {
        return get_io_address( address);
    }

    void assign16( operand dest, boost::int16_t value)
    {
        reg_write(dest)   = value;
        reg_write(dest+1) = value >> 8;
    }

    unsigned16 get16( operand source)
    {
        return reg(source) + (static_cast<unsigned16>(reg(source+1)) << 8);
    }

};

}
#endif /* AVR_CORE_HPP_ */


//***********************************************************************
// program to analyze bands and report some information on changing
//
//***********************************************************************
// version
// 2.0.4    06/12/2019	Rémi Saint-Amant	Add process of no data
// 2.0.3    29/01/2019	Rémi Saint-Amant	Remove option -wm that cause strange result
// 2.0.2    25/12/2018	Rémi Saint-Amant	Eliminate the verification of independent variables
// 2.0.l	21/12/2018	Rémi Saint-Amant	Use Entry to evaluate the number of input variables
// 2.0.0	29/09/2018	Rémi Saint-Amant	Creation from DisturbanceAnalyser

#include "stdafx.h"
#include <float.h>
#include <math.h>
#include <array>
#include <utility>
#include <iostream>
#include <bitset>
#include <boost/dynamic_bitset.hpp>

#include "Basic/UtilTime.h"
#include "Basic/UtilMath.h"
#include "Basic/OpenMP.h"
#include "Geomatic/GDALBasic.h"
#include "Geomatic/See5hooks.h"
#include "Geomatic/LandsatDataset.h"

#pragma warning(disable: 4275 4251)
#include "gdal_priv.h"

using namespace std;
using namespace WBSF;
using namespace WBSF::Landsat;

static const char* version = "2.0.4";

enum TFilePath { SEE5_FILE_PATH, INPUT_FILE_PATH, OUTPUT_FILE_PATH, NB_FILE_PATH };
typedef vector<float> DTCodeVector;
typedef CDecisionTreeBlock CSee5TreeBlock;
typedef CDecisionTreeBaseEx CSee5Tree;
typedef CDecisionTree CSee5TreeMT;
typedef std::vector<float>  OutputData;

class CSee5Option : public CBaseOptions
{
public:
	CSee5Option()
	{
		m_scenesSize = 1;
		m_appDescription = "This software interpolate values (with a decision tree model from See5) from input images";
		//m_BLOCK_THREADS = omp_get_num_procs();

		static const COptionDef OPTIONS[] =
		{
			//{"-BLOCK_THREADS",1,"threads",false,"Number of threads used to process blocks. nb CPU by default. Only used when -multi is define. "},
			{ "See5Model",0,"",false,"See5 model file path."},
			{ "srcfile",0,"",false, "Input image file path."},
			{ "dstfile",0,"",false, "Output image file path."}
		};

		for (int i = 0; i < sizeof(OPTIONS) / sizeof(COptionDef); i++)
			AddOption(OPTIONS[i]);

		RemoveOption("-BLOCK_THREADS");

		static const CIOFileInfoDef IO_FILE_INFO[] =
		{
			{ "Input Model", "DTModel","","","","Decision tree model file generate by See5."},
			{ "Input Image", "srcfile","","nbBands","same number of band as the input See5 model"},
			{ "Output Image", "dstfile","1","","See5 model result"},
		};

		for (int i = 0; i < sizeof(IO_FILE_INFO) / sizeof(CIOFileInfoDef); i++)
			AddIOFileInfo(IO_FILE_INFO[i]);
	}

	virtual ERMsg ParseOption(int argc, char* argv[])
	{
		ERMsg msg = CBaseOptions::ParseOption(argc, argv);

		ASSERT(NB_FILE_PATH == 3);
		if (msg && m_filesPath.size() != NB_FILE_PATH)
		{
			msg.ajoute("ERROR: Invalid argument line. 3 files are needed: See5 model, the input LANDSAT image and the destination image.");
			msg.ajoute("Argument found: ");
			for (size_t i = 0; i < m_filesPath.size(); i++)
				msg.ajoute("   " + to_string(i + 1) + "- " + m_filesPath[i]);
		}

		return msg;
	}

	virtual ERMsg ProcessOption(int& i, int argc, char* argv[])
	{
		ERMsg msg;

		//Look to see if it's a know base option
		msg = CBaseOptions::ProcessOption(i, argc, argv);

		return msg;
	}

};

//***********************************************************************
//
//	Main
//
//***********************************************************************

class CSee5
{
public:

	string GetDescription() { return  string("See5Images version ") + version + " (" + _T(__DATE__) + ")\n"; }
	ERMsg Execute();

	ERMsg OpenAll(CGDALDatasetEx& inputDS, CGDALDatasetEx& maskDS, CGDALDatasetEx& outputDS);
	void ReadBlock(int xBlock, int yBlock, CBandsHolder& bandHolder);
	void ProcessBlock(int xBlock, int yBlock, const CBandsHolder& bandHolder, CSee5Tree& DT, OutputData& output);
	void WriteBlock(int xBlock, int yBlock, CGDALDatasetEx& outputDS, OutputData& output);
	void CloseAll(CGDALDatasetEx& inputDS, CGDALDatasetEx& maskDS, CGDALDatasetEx& outputDS);

	ERMsg LoadModel(string filePath, CSee5TreeMT& DT);

	CSee5Option m_options;

};

ERMsg CSee5::LoadModel(string filePath, CSee5TreeMT& DT)
{
	ERMsg msg;
	if (!m_options.m_bQuiet)
	{
		cout << "Read rules..." << endl;
	}

	CTimer timer(true);

	msg += DT.Load(filePath, m_options.m_CPU, m_options.m_IOCPU);
	timer.Stop();

	if (!m_options.m_bQuiet)
		cout << "Read rules time = " << SecondToDHMS(timer.Elapsed()).c_str() << endl << endl;

	return msg;
}

ERMsg CSee5::OpenAll(CGDALDatasetEx& inputDS, CGDALDatasetEx& maskDS, CGDALDatasetEx& outputDS)
{
	ERMsg msg;

	if (!m_options.m_bQuiet)
		cout << endl << "Open input image..." << endl;

	msg = inputDS.OpenInputImage(m_options.m_filesPath[INPUT_FILE_PATH], m_options);

	if (msg)
	{

		inputDS.UpdateOption(m_options);

		if (!m_options.m_bQuiet)
		{
			CGeoExtents extents = inputDS.GetExtents();
			CProjectionPtr pPrj = inputDS.GetPrj();
			string prjName = pPrj ? pPrj->GetName() : "Unknown";

			cout << "    Size           = " << inputDS->GetRasterXSize() << " cols x " << inputDS->GetRasterYSize() << " rows x " << inputDS.GetRasterCount() << " bands" << endl;
			cout << "    Extents        = X:{" << ToString(extents.m_xMin) << ", " << ToString(extents.m_xMax) << "}  Y:{" << ToString(extents.m_yMin) << ", " << ToString(extents.m_yMax) << "}" << endl;
			cout << "    Projection     = " << prjName << endl;
			cout << "    NbBands        = " << inputDS.GetRasterCount() << endl;
		}
	}

	if (msg && !m_options.m_maskName.empty())
	{
		if (!m_options.m_bQuiet)
			cout << "Open mask image..." << endl;
		msg += maskDS.OpenInputImage(m_options.m_maskName);
	}

	if (msg && m_options.m_bCreateImage)
	{

		if (!m_options.m_bQuiet)
			cout << "Create output image..." << endl;

		CSee5Option option = m_options;
		option.m_nbBands = 1;

		string filePath = option.m_filesPath[OUTPUT_FILE_PATH];
		msg += outputDS.CreateImage(filePath, option);
	}

	return msg;
}

ERMsg CSee5::Execute()
{
	ERMsg msg;

	if (!m_options.m_bQuiet)
	{
		cout << "Output: " << m_options.m_filesPath[OUTPUT_FILE_PATH] << endl;
		cout << "From:   " << m_options.m_filesPath[INPUT_FILE_PATH] << endl;
		cout << "Using:  " << m_options.m_filesPath[SEE5_FILE_PATH] << endl;

		if (!m_options.m_maskName.empty())
			cout << "Mask:   " << m_options.m_maskName << endl;
	}

	GDALAllRegister();

	if (!msg)
		return msg;

	CSee5TreeMT DT;
	msg += LoadModel(m_options.m_filesPath[SEE5_FILE_PATH], DT);

	if (!msg)
		return msg;

	CGDALDatasetEx inputDS;
	CGDALDatasetEx maskDS;
	CGDALDatasetEx outputDS;

	msg = OpenAll(inputDS, maskDS, outputDS);

	if (msg)
	{
		CBandsHolderMT bandHolder(1, m_options.m_memoryLimit, m_options.m_IOCPU, m_options.m_CPU);

		if (maskDS.IsOpen())
			bandHolder.SetMask(maskDS.GetSingleBandHolder(), m_options.m_maskDataUsed);

		msg += bandHolder.Load(inputDS, m_options.m_bQuiet, m_options.m_extents, m_options.m_period);
		if (!msg)
			return msg;

		if (!m_options.m_bQuiet && m_options.m_bCreateImage)
			cout << "Create output images x(" << m_options.m_extents.m_xSize << " C x " << m_options.m_extents.m_ySize << " R ) with " << m_options.m_CPU << " threads..." << endl;

		CGeoExtents extents = bandHolder.GetExtents();
		m_options.ResetBar((size_t)extents.m_xSize*extents.m_ySize);

		vector<pair<int, int>> XYindex = extents.GetBlockList();

		omp_set_nested(1);
#pragma omp parallel for schedule(static, 1) num_threads(m_options.m_CPU) if (m_options.m_bMulti)
		for (int b = 0; b < (int)XYindex.size(); b++)
		{
			int thread = omp_get_thread_num();
			int xBlock = XYindex[b].first;
			int yBlock = XYindex[b].second;

			//data
			OutputData data;
			ReadBlock(xBlock, yBlock, bandHolder[thread]);
			ProcessBlock(xBlock, yBlock, bandHolder[thread], DT[thread], data);
			WriteBlock(xBlock, yBlock, outputDS, data);
		}//for all blocks

		//  Close decision tree and free allocated memory
		DT.FreeMemory();

		//close inputs and outputs
		CloseAll(inputDS, maskDS, outputDS);

	}

	return msg;
}

void CSee5::ReadBlock(int xBlock, int yBlock, CBandsHolder& bandHolder)
{
#pragma omp critical(BlockIORead)
	{

		m_options.m_timerRead.Start();
		bandHolder.LoadBlock(xBlock, yBlock);
		m_options.m_timerRead.Stop();
	}
}

//Get input image reference
void CSee5::ProcessBlock(int xBlock, int yBlock, const CBandsHolder& bandHolder, CSee5Tree& DT, OutputData& data)
{
	CGeoExtents extents = bandHolder.GetExtents();
	CGeoSize blockSize = extents.GetBlockSize(xBlock, yBlock);
	size_t nbCells = (size_t)extents.m_xSize*extents.m_ySize;

	if (bandHolder.IsEmpty())
	{
#pragma omp atomic
		m_options.m_xx += (std::min(nbCells, (size_t)blockSize.m_x*blockSize.m_y));
		m_options.UpdateBar();

		return;
	}

		//allocate process memory
		data.resize(blockSize.m_x*blockSize.m_y, (__int16)m_options.m_dstNodata);

		//process all x and y
		for (int y = 0; y < blockSize.m_y; y++)
		{
			for (int x = 0; x < blockSize.m_x; x++)
			{
				size_t xy = (size_t )y * blockSize.m_x + x;
				//int thread = ::omp_get_thread_num();

				//process all images
				CSee5TreeBlock block(DT.MaxAtt + 1);

				//fill the data structure for decision tree
				size_t c = 0;
				DVal(block, c++) = DT.MaxClass + 1;
				DVal(block, c++) = Continuous(DT, 1) ? DT_UNKNOWN : 0;

				bool bValid = true;
				for (size_t z = 0; z < bandHolder.size(); z++)
				{
					DataType pixel = bandHolder.GetPixel(z, x, y);
					CVal(block, c++) = (ContValue)(pixel);
					bValid = bValid & bandHolder.IsValid(z, pixel);
				}

				ASSERT(c == (bandHolder.size() + 2));

				if (bValid)
				{
					//fill virtual bands
					//assuming virtual band never return no data
					for (; c <= DT.MaxAtt; c++)
					{
						ASSERT(DT.AttDef[c]);
						block[c] = DT.EvaluateDef(DT.AttDef[c], block.data());
					}

					ASSERT(!block.empty());
					int predict = (int)DT.Classify(block.data());

					ASSERT(predict >= 1 && predict <= DT.MaxClass);
					int DTCode = atoi(DT.ClassName[predict]);
					data[xy] = (float)DTCode;
				}
				else
				{
					data[xy] = (float)m_options.m_dstNodata;
				}

#pragma omp atomic
				m_options.m_xx++;

			}//for x
		}//for y

		m_options.UpdateBar();
	//}//for y

	//m_options.m_timerProcess.Stop();

}

void CSee5::WriteBlock(int xBlock, int yBlock, CGDALDatasetEx& outputDS, OutputData& data)
{
	if (m_options.m_bCreateImage)
	{
#pragma omp critical(BlockIOWrite)
		{

			m_options.m_timerWrite.Start();

			CGeoExtents extents = outputDS.GetExtents();
			CGeoSize blockSize = extents.GetBlockSize(xBlock, yBlock);
			CGeoRectIndex outputRect = extents.GetBlockRect(xBlock, yBlock);

			ASSERT(outputRect.Width() == blockSize.m_x);
			ASSERT(outputRect.Height() == blockSize.m_y);

			float noDataOut = (float)outputDS.GetNoData(0);
			ASSERT(data.size() == blockSize.m_x*blockSize.m_y);
			GDALRasterBand *pBand = outputDS.GetRasterBand(0);
			/*for (size_t i = 0; i < data.size(); i++)
			{
				if (data[i] < -32000)
					exit(99);
			}*/

			//for (size_t b = 0; b < outputDS.GetRasterCount(); b++)
			if(!data.empty())
			{
				pBand->RasterIO(GF_Write, outputRect.m_x, outputRect.m_y, outputRect.Width(), outputRect.Height(), &(data[0]), outputRect.Width(), outputRect.Height(), GDT_Float32, 0, 0);
			}//for all output bands
			else
			{
				pBand->RasterIO(GF_Write, outputRect.m_x, outputRect.m_y, outputRect.Width(), outputRect.Height(), &noDataOut, 1, 1, GDT_Float32, 0, 0);
			}

			m_options.m_timerWrite.Stop();
		}
	}//if create image

}

void CSee5::CloseAll(CGDALDatasetEx& inputDS, CGDALDatasetEx& maskDS, CGDALDatasetEx& outputDS)
{
	if (!m_options.m_bQuiet)
		_tprintf("\nClose all files...\n");

	inputDS.Close();
	maskDS.Close();

	//close output
	m_options.m_timerWrite.Start();
	outputDS.Close(m_options);
	m_options.m_timerWrite.Stop();

	m_options.PrintTime();
}

int _tmain(int argc, _TCHAR* argv[])
{
	CTimer timer(true);

	CSee5 see5;
	ERMsg msg = see5.m_options.ParseOption(argc, argv);

	if (!msg || !see5.m_options.m_bQuiet)
		cout << see5.GetDescription() << endl;

	if (msg)
		msg = see5.Execute();

	if (!msg)
	{
		PrintMessage(msg);
		return -1;
	}

	timer.Stop();

	if (!see5.m_options.m_bQuiet)
		cout << endl << "Total time = " << SecondToDHMS(timer.Elapsed()) << endl;

	int nRetCode = 0;
}


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

  Helper types related to the centrifugal component of the complex propagation.

  Copyright (c) 2013
  Alexander Rukletsov <rukletsov@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.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.

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

#ifndef CENTRIFUGAL_PROPAGATION_HPP_63941A92_EA23_11E2_BC2A_2B71033B4CBB
#define CENTRIFUGAL_PROPAGATION_HPP_63941A92_EA23_11E2_BC2A_2B71033B4CBB

#include <boost/shared_ptr.hpp>

#include "bo/config.hpp"
#include "bo/core/vector.hpp"

namespace bo {
namespace surfaces {
namespace detail {

// Base class for any centrifual propagation.
template <typename RealType>
class BaseCentrifugalPropagation
{
public:
    typedef BaseCentrifugalPropagation<RealType> SelfType;
    typedef boost::shared_ptr<SelfType> Ptr;
    typedef Vector<RealType, 3> Point3D;

    virtual Point3D get(const Point3D& current) const = 0;
};

// Centrifugal propagation that does nothing. Use this class if you want to disable
// centrifugal component of the complex propagation where an instance of centrifugal
// component is required.
template <typename RealType>
class EmptyCentrifugalPropagation: public BaseCentrifugalPropagation<RealType>
{
public:
    typedef Vector<RealType, 3> Point3D;

    EmptyCentrifugalPropagation(): zero_vector_(0)
    { }

    virtual Point3D get(const Point3D& current) const
    {
        BO_UNUSED(current);
        return zero_vector_;
    }

protected:
    Point3D zero_vector_;
};

// Standard centrifugal propagation that uses center of mass as a reference.
template <typename RealType>
class CentrifugalPropagation: public BaseCentrifugalPropagation<RealType>
{
public:
    typedef Vector<RealType, 3> Point3D;

    CentrifugalPropagation(const Point3D& center_of_mass, RealType weight):
        center_of_mass_(center_of_mass), weight_(weight)
    { }

    virtual Point3D get(const Point3D& current) const
    {
        // Save some processor tacts and reuse the input variable.
        Point3D inertial = current - center_of_mass_;

        // TODO: remove this by redesigning the algorithm and requiring inertial
        // vector to be non-zero.
        Point3D centrifugal_normalized(0);
        try
        {
            // TODO: remove this block by refactoring bo::Vector class.
            // Normalize vector.
            centrifugal_normalized.assign(inertial.normalized(), 3);
        }
        catch (...)
        { }

        return weight_ * centrifugal_normalized;
    }

protected:
    const Point3D center_of_mass_;
    const RealType weight_;
};

} // namespace detail
} // namespace surfaces
} // namespace bo

#endif // CENTRIFUGAL_PROPAGATION_HPP_63941A92_EA23_11E2_BC2A_2B71033B4CBB


#include <boost/test/unit_test.hpp>
#include <chipmunk.hpp>

using namespace cp;

BOOST_AUTO_TEST_CASE(SegmentShapeTest) {
	auto shape = std::make_shared<SegmentShape>(nullptr, Vect(0, 0), Vect(100, 0), 5);
	auto body = std::make_shared<Body>(1, 1);
	shape->setBody(body);
	Space space;
	space.add(shape);
	BOOST_CHECK(shape->pointQuery(Vect(43, 0)));
	BOOST_CHECK(shape->pointQuery(Vect(50, 4.9)));
	BOOST_CHECK(!shape->pointQuery(Vect(30, 5.1)));
	BOOST_CHECK(shape->pointQuery(Vect(0, -4.9)));
	space.remove(shape);
	shape->setBody(nullptr);
}


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

#ifndef MTL_LEFT_SCALE_INPLACE_INCLUDE
#define MTL_LEFT_SCALE_INPLACE_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/enable_if.hpp>
#include <boost/numeric/mtl/operation/assign_each_nonzero.hpp>
#include <boost/numeric/mtl/operation/mult.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

    namespace impl {

	template <typename Factor, typename Coll>
	inline Coll& left_scale_inplace(const Factor& alpha, tag::scalar, Coll& c)
	{
	    vampir_trace<3014> tracer;
	    assign_each_nonzero(c, tfunctor::scale<Factor, typename Collection<Coll>::value_type>(alpha));

	    return c;
	}

	template <typename Factor, typename Collection>
	inline Collection& left_scale_inplace(const Factor& alpha, tag::matrix, Collection& c)
	{
	    vampir_trace<3014> tracer;
	    Collection tmp(alpha * c);
	    swap(tmp, c);
	    return c;
	}
    }

    namespace matrix {

	/// Scale matrix \p c from left with scalar or matrix factor \p alpha; \p c is altered
	template <typename Factor, typename Matrix>
	typename mtl::traits::enable_if_matrix<Matrix, Matrix&>::type
	inline left_scale_inplace(const Factor& alpha, Matrix& A)
	{
	    return mtl::impl::left_scale_inplace(alpha, typename mtl::traits::category<Factor>::type(), A);
	}
    }

    namespace vector {

	/// Scale vector \p c from left with scalar or matrix factor \p alpha; \p c is altered
	template <typename Factor, typename Vector>
	typename mtl::traits::enable_if_vector<Vector, Vector&>::type
	inline left_scale_inplace(const Factor& alpha, Vector& v)
	{
	    return mtl::impl::left_scale_inplace(alpha, typename mtl::traits::category<Factor>::type(), v);
	}
    }

    using vector::left_scale_inplace;
    using matrix::left_scale_inplace;

} // namespace mtl

#endif // MTL_LEFT_SCALE_INPLACE_INCLUDE


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

#include <Eigen/Dense>

#include <openbr/plugins/openbr_internal.h>

using namespace cv;

namespace br
{

/*!
 * \ingroup transforms
 * \brief Sliding window feature extraction from a multi-channel integral image.
 * \author Josh Klontz \cite jklontz
 */
class IntegralSamplerTransform : public UntrainableTransform
{
    Q_OBJECT
    Q_PROPERTY(int scales READ get_scales WRITE set_scales RESET reset_scales STORED false)
    Q_PROPERTY(float scaleFactor READ get_scaleFactor WRITE set_scaleFactor RESET reset_scaleFactor STORED false)
    Q_PROPERTY(float stepFactor READ get_stepFactor WRITE set_stepFactor RESET reset_stepFactor STORED false)
    Q_PROPERTY(int minSize READ get_minSize WRITE set_minSize RESET reset_minSize STORED false)
    Q_PROPERTY(bool secondOrder READ get_secondOrder WRITE set_secondOrder RESET reset_secondOrder STORED false)
    BR_PROPERTY(int, scales, 6)
    BR_PROPERTY(float, scaleFactor, 2)
    BR_PROPERTY(float, stepFactor, 0.75)
    BR_PROPERTY(int, minSize, 8)
    BR_PROPERTY(bool, secondOrder, false)

    void project(const Template &src, Template &dst) const
    {
        typedef Eigen::Map< const Eigen::Matrix<qint32,Eigen::Dynamic,1> > InputDescriptor;
        typedef Eigen::Map< const Eigen::Matrix<float,Eigen::Dynamic,1> > SecondOrderInputDescriptor;
        typedef Eigen::Map< Eigen::Matrix<float,Eigen::Dynamic,1> > OutputDescriptor;

        const Mat &m = src.m();
        if (m.depth() != CV_32S) qFatal("Expected CV_32S matrix depth.");
        const int channels = m.channels();
        const int rowStep = channels * m.cols;

        int descriptors = 0;
        float idealSize = min(m.rows, m.cols)-1;
        for (int scale=0; scale<scales; scale++) {
            const int currentSize(idealSize);
            const int numDown = 1+(m.rows-currentSize-1)/int(idealSize*stepFactor);
            const int numAcross = 1+(m.cols-currentSize-1)/int(idealSize*stepFactor);
            descriptors += numDown*numAcross;
            if (secondOrder) descriptors += numDown*(numAcross-1) + (numDown-1)*numAcross;
            idealSize /= scaleFactor;
            if (idealSize < minSize) break;
        }
        Mat n(descriptors, channels, CV_32FC1);

        const qint32 *dataIn = (qint32*)m.data;
        float *dataOut = (float*)n.data;
        idealSize = min(m.rows, m.cols)-1;
        int index = 0;
        for (int scale=0; scale<scales; scale++) {
            const int currentSize(idealSize);
            const int currentStep(idealSize*stepFactor);
            for (int i=currentSize; i<m.rows; i+=currentStep) {
                for (int j=currentSize; j<m.cols; j+=currentStep) {
                    InputDescriptor a(dataIn+((i-currentSize)*rowStep+(j-currentSize)*channels), channels, 1);
                    InputDescriptor b(dataIn+((i-currentSize)*rowStep+ j             *channels), channels, 1);
                    InputDescriptor c(dataIn+(i              *rowStep+(j-currentSize)*channels), channels, 1);
                    InputDescriptor d(dataIn+(i              *rowStep+ j             *channels), channels, 1);
                    OutputDescriptor y(dataOut+(index*channels), channels, 1);
                    y = (d-b-c+a).cast<float>()/(currentSize*currentSize);
                    index++;
                }
            }
            if (secondOrder) {
                const int numDown = 1+(m.rows-currentSize-1)/currentStep;
                const int numAcross = 1+(m.cols-currentSize-1)/currentStep;
                const float *dataIn = n.ptr<float>(index - numDown*numAcross);
                for (int i=0; i<numDown; i++) {
                    for (int j=0; j<numAcross; j++) {
                        SecondOrderInputDescriptor a(dataIn + (i*numAcross+j)*channels, channels, 1);
                        if (j < numAcross-1) {
                            OutputDescriptor y(dataOut+(index*channels), channels, 1);
                            y = a - SecondOrderInputDescriptor(dataIn + (i*numAcross+j+1)*channels, channels, 1);
                            index++;
                        }
                        if (i < numDown-1) {
                            OutputDescriptor y(dataOut+(index*channels), channels, 1);
                            y = a - SecondOrderInputDescriptor(dataIn + ((i+1)*numAcross+j)*channels, channels, 1);
                            index++;
                        }
                    }
                }
            }
            idealSize /= scaleFactor;
            if (idealSize < minSize) break;
        }

        if (descriptors != index)
            qFatal("Allocated %d descriptors but computed %d.", descriptors, index);

        dst.m() = n;
    }
};

BR_REGISTER(Transform, IntegralSamplerTransform)

} // namespace br

#include "imgproc/integralsampler.moc"


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_MEMORY_FUNCTIONS_SIMD_SSE_SSE2_SPLAT_HPP_INCLUDED
#define BOOST_SIMD_MEMORY_FUNCTIONS_SIMD_SSE_SSE2_SPLAT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/memory/functions/splat.hpp>
#include <boost/simd/include/functions/simd/make.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( splat_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , (scalar_< fundamental_<A0> >)
                                      ((target_ < simd_ < double_<A1>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                      )
  {
    typedef typename A1::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm_set1_pd(double(a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( splat_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , (scalar_< fundamental_<A0> >)
                                      ((target_ < simd_ < single_<A1>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A1::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm_set1_ps(float(a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( splat_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , (scalar_< fundamental_<A0> >)
                                      ((target_ < simd_ < ints8_<A1>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A1::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm_set1_epi8(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( splat_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , (scalar_< fundamental_<A0> >)
                                      ((target_ < simd_ < ints16_<A1>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A1::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm_set1_epi16(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( splat_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , (scalar_< fundamental_<A0> >)
                                      ((target_ < simd_ < ints32_<A1>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A1::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return _mm_set1_epi32(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( splat_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , (scalar_< fundamental_<A0> >)
                                      ((target_ < simd_ < ints64_<A1>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A1::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const&) const
    {
#ifdef BOOST_SIMD_ARCH_X86_64
      result_type that = _mm_set1_epi64x(a0);
#else
      result_type that = make<result_type>(a0, a0);
#endif
      return that;
    }
  };
} } }

#endif
#endif