#include "FOPTICS.h"
#include "gaboom.h"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
boost::random::mt19937 gen;

struct RNG
{
    int operator() (int n) {
        return std::rand() / (1.0 + RAND_MAX) * n;
    }
};

bool definitelyGreaterThan(float a, float b, float epsilon)
{
    return (a - b) > ( (fabs(a) < fabs(b) ? fabs(b) : fabs(a)) * epsilon);
}

bool definitelyLessThan(float a, float b, float epsilon)
{
    return (b - a) > ( (fabs(a) < fabs(b) ? fabs(b) : fabs(a)) * epsilon);
}

//Normalizes any number to an arbitrary range
//by assuming the range wraps around when going below min or above max
float normalize_IC_interval(const genlim* gene_lim, float value)
{
	float start = gene_lim->min;
	float end = gene_lim->max;
	float width = end - start;
	float offsetValue = value - start; // value relative to 0
	return ( offsetValue - ( std::floor( offsetValue / width) * width) ) + start;
}

ClusterOrdering::ClusterOrdering(int id, int predID, float reach) : objectID(id), predecessorID(predID), reachability(reach)
{}

inline bool const ClusterOrdering::operator==(const ClusterOrdering& rhs)
{
	if(*this == rhs)
		return true;
	if(typeid(rhs) != typeid(ClusterOrdering))
		return false;

	return ( this->objectID == rhs.objectID );
}

inline bool const ClusterOrdering::operator< (const ClusterOrdering& rhs)
{
	if(this->reachability > rhs.reachability || isUndefinedDist(this->reachability))
			return false;
		else if(this->reachability < rhs.reachability)
			return true;
		if(this->objectID > rhs.objectID)
			return true;
		else if(this->objectID < rhs.objectID)
			return false;
		// if nothing else is true, return 0
		return 0;
}

inline bool const ClusterOrdering::operator> (const ClusterOrdering& rhs)
{
	if(this->reachability > rhs.reachability || isUndefinedDist(this->reachability))
		return true;
	else if(this->reachability < rhs.reachability)
			return false;
	if(this->objectID > rhs.objectID)
		return false;
	else if(this->objectID < rhs.objectID)
		return true;
		// if nothing else is true, return 0
		return 0;
}
/*****************************************\
				FastOPTICS
\*****************************************/
// Constructor and Algorithm main+only call
// int FastOPTICS::iOrder;
FastOPTICS::FastOPTICS(FA_Global* FA, GB_Global* GB, VC_Global* VC, chromosome* chrom, genlim* gen_lim, atom* atoms, resid* residue, gridpoint* cleftgrid, int nChrom, BindingPopulation& Population, int nPoints)
{
// Declarations
	///////////////////////////////////////////////////////
	// Entropy
	this->Population = &Population;
	// FlexAID
    this->FA = FA;
    this->GB = GB;
    this->VC = VC;
    this->cleftgrid = cleftgrid;
    this->atoms = atoms;
    this->residue = residue;
    this->chroms = chrom;
    this->gene_lim = gen_lim;
	this->N = nChrom;

	// FastOPTICS
    this->nDimensions = this->FA->num_het_atm*3;	// use with Vectorized_Cartesian_Coordinates()
    // this->nDimensions = this->FA->npar + 2; 	// use with Vectorized_Chromosome()

    this->minPoints = nPoints;

    // FastOPTICS::iOrder = 0;
    this->iOrder = 0;
	this->order.reserve(this->N);
	this->reachDist.reserve(this->N);
	this->processed.reserve(this->N);
	this->inverseDensities.reserve(this->N);
	this->points.reserve(this->N);
	this->neighbors.reserve(this->N);

	for(int i = 0; i < this->N; ++i)
	{
		// need to transform the chromosomes into vector f
		// std::vector<float> vChrom(this->FastOPTICS::Vectorized_Chromosome(&chrom[i])); // Copy constructor
		std::vector<float> vChrom( this->Vectorized_Cartesian_Coordinates(i) ); // Copy constructor
		if(vChrom.size() == this->nDimensions)
		{
			// std::pair<first, second> is pushed to this->points[]
			// 	first  -> chromosome* pChrom (pointer to chromosome)
			// 	second -> vChrom[FA->npar+n] (vectorized chromosome)
			this->order.push_back(-1);
			this->reachDist.push_back(UNDEFINED_DIST);
			this->processed.push_back(false);
			this->inverseDensities.push_back(0.0f);
			this->points.push_back( std::make_pair( (chromosome*)&chrom[i], vChrom) ) ;
		}
	}

};

void FastOPTICS::Execute_FastOPTICS(char* end_strfile, char* tmp_end_strfile)
{
	// vector of point indexes
	std::vector< int > ptInd;
    ptInd.reserve(this->N);
    for(int k = 0; k < this->N; ++k)
    {
        ptInd.push_back(k);
    }

	// Build object, compute projections, density estimates and density neighborhoods (in serial order of function calls below)
	RandomProjectedNeighborsAndDensities::RandomProjectedNeighborsAndDensities MultiPartition(this->points, this->minPoints, this); // use minPoints amount of random projections
	MultiPartition.computeSetBounds(ptInd);
	MultiPartition.getInverseDensities(this->inverseDensities);
	MultiPartition.getNeighbors(this->neighbors);

	// Compute OPTICS ordering
	for(int ipt = 0; ipt < this->N; ipt++) 		// starting from 0
//	for(int ipt = this->N-1; ipt >= 0; --ipt)	// starting from this->N-1
    {
		if(!this->processed[ipt]) this->ExpandClusterOrder(ipt);
    }

    // Would it be useful to normalize 'reachability distance' ?
    // this->normalizeDistances();
    // output the projected distances

    MultiPartition.output_projected_distance(end_strfile, tmp_end_strfile);

	// Order chromosome and their reachDist in OPTICS
    //  points pairs contain :
    //   first  -> pair<chromosome*, index>
    //   second -> float reachDist
	for(int i = 0; i < this->N; ++i)
	{
		if( (this->points[i]).first != NULL && (this->points[i].first)->app_evalue < 0 )
		{
			// Calling Pose constructor for the current chromosome
			Pose::Pose Pose((this->points[i]).first, i, this->order[i], this->reachDist[i], this->Population->Temperature, (this->points[i]).second);
			// OPTICS.push(Pose);
            this->OPTICS.push_back(Pose);
		}
	}
    std::sort(this->OPTICS.begin(), this->OPTICS.end(), PoseClassifier::PoseClassifier());

	// Build BindingModes (aggregation of Poses in BindingModes)
    int i = 0; // used to have an idea of the number of loop completed (iterators are less convenient for that information while debugging)
 	BindingMode::BindingMode Current(this->Population);
	for(std::vector< Pose >::iterator it = this->OPTICS.begin(); it != this->OPTICS.end(); ++i, ++it)
	{

        if(isUndefinedDist(it->reachDist) && it->order == 0) { Current.add_Pose(*it); }
        else if(it->reachDist <= this->FA->cluster_rmsd*(1 + RandomProjectedNeighborsAndDensities::sizeTolerance)) { Current.add_Pose(*it); }

       	if(it->reachDist > this->FA->cluster_rmsd*(1 + RandomProjectedNeighborsAndDensities::sizeTolerance) || isUndefinedDist(it->reachDist))
        {
			if(Current.get_BindingMode_size() >= this->minPoints)
			{
				this->Population->add_BindingMode(Current);
                Current.clear_Poses();
            }
        }
	}
}

void FastOPTICS::output_OPTICS(char* end_strfile, char* tmp_end_strfile)
{
	// output variables
    char sufix[25];

    // priting OPTICS variable to '__minPoints.optics' file
    sprintf(sufix,"__%d.optics",this->minPoints);
    strcpy(tmp_end_strfile,end_strfile);
	strcat(tmp_end_strfile,sufix);
    FILE* outfile;// = fopen(tmp_end_strfile,"w");
	if(!OpenFile_B(tmp_end_strfile,"w",&outfile))
	{
		Terminate(6);
	}
	else
	{
        fprintf(outfile, "#ORDER\t#INDEX\t#reachDist\t#CF\t#prevRMSD\t#nextRMSD\n");
		for(std::vector<Pose>::iterator it = this->OPTICS.begin(); it != this->OPTICS.end(); ++it)
		{
			float prevDist = (it == this->OPTICS.begin()) 	? 0.0f : this->compute_vect_distance(it->vPose, (it-1)->vPose);
			float nextDist = ((it+1) == this->OPTICS.end()) ? 0.0f : this->compute_vect_distance(it->vPose, (it+1)->vPose);
            if(!isUndefinedDist(it->reachDist))	fprintf(outfile, "%d\t%d\t%8g\t%8g\t%8g\t%8g\n", it->order, it->chrom_index, it->reachDist, it->CF, prevDist, nextDist);
            else fprintf(outfile, "%d\t%d\t%8g\t%8g\t%8g\t%8g\n", it->order, it->chrom_index, UNDEFINED_DIST, it->CF, prevDist, nextDist);
		}
   }
   CloseFile_B(&outfile,"w");;//fclose(outfile);
}

void FastOPTICS::output_3d_OPTICS_ordering(char* end_strfile, char* tmp_end_strfile)
{
	// File and Output variables declarations
    cfstr CF; /* complementarity function value */
    resid *pRes = NULL;
    cfstr* pCF = NULL;
	char sufix[25];
    char remark[MAX_REMARK];
	char tmpremark[MAX_REMARK];

	sprintf(sufix, "__%d.optics.pdb", this->minPoints);
	strcpy(tmp_end_strfile, end_strfile);
	strcat(tmp_end_strfile,sufix);
	FILE* outfile;
	if(!OpenFile_B(tmp_end_strfile, "w", &outfile))
	{
		Terminate(5);
	}
	else
	{
        int nModel = 1;
		for(std::vector<Pose>::iterator Pose = this->OPTICS.begin(); Pose != this->OPTICS.end(); ++Pose, ++nModel)
		{
			for(int k = 0; k < this->GB->num_genes; ++k) this->FA->opt_par[k] = Pose->chrom->genes[k].to_ic;
			CF = ic2cf(this->FA, this->VC, this->atoms, this->residue, this->cleftgrid, this->GB->num_genes, this->FA->opt_par);
			strcpy(remark,"REMARK optimized structure\n");
			sprintf(tmpremark,"REMARK Fast OPTICS clustering algorithm used to order Poses in OPTICS\n");
			strcat(remark,tmpremark);
			sprintf(tmpremark,"REMARK CF=%8.5f\n",get_cf_evalue(&CF));
			strcat(remark,tmpremark);
			sprintf(tmpremark,"REMARK CF.app=%8.5f\n",get_apparent_cf_evalue(&CF));
			strcat(remark,tmpremark);

			for(int j = 0; j < this->FA->num_optres; ++j)
			{
				pRes = &this->residue[this->FA->optres[j].rnum];
				pCF  = &this->FA->optres[j].cf;

		        sprintf(tmpremark,"REMARK optimizable residue %s %c %d\n", pRes->name, pRes->chn, pRes->number);
		        strcat(remark,tmpremark);

		        sprintf(tmpremark ,"REMARK CF.com=%8.5f\n", pCF->com);
		        strcat(remark, tmpremark);
		        sprintf(tmpremark ,"REMARK CF.sas=%8.5f\n", pCF->sas);
		        strcat(remark, tmpremark);
		        sprintf(tmpremark ,"REMARK CF.wal=%8.5f\n", pCF->wal);
		        strcat(remark, tmpremark);
		        sprintf(tmpremark ,"REMARK CF.con=%8.5f\n", pCF->con);
		        strcat(remark, tmpremark);
		        sprintf(tmpremark, "REMARK Residue has an overall SAS of %.3f\n", pCF->totsas);
		        strcat(remark, tmpremark);
			}

		    for(int j=0; j < this->FA->npar; ++j)
			{
				sprintf(tmpremark, "REMARK [%8.3f]\n",this->FA->opt_par[j]);
				strcat(remark,tmpremark);
			}

			// 4. if(REF) prints RMSD to REF
			if(this->FA->refstructure == 1)
			{
				bool Hungarian = false;
				sprintf(tmpremark,"REMARK %8.5f RMSD to ref. structure (no symmetry correction)\n",
				calc_rmsd(this->FA,this->atoms,this->residue,this->cleftgrid,this->FA->npar,this->FA->opt_par, Hungarian));
				strcat(remark,tmpremark);
				Hungarian = true;
				sprintf(tmpremark,"REMARK %8.5f RMSD to ref. structure     (symmetry corrected)\n",
				calc_rmsd(this->FA,this->atoms,this->residue,this->cleftgrid,this->FA->npar,this->FA->opt_par, Hungarian));
				strcat(remark,tmpremark);
			}

			// 5. write_pdb(FA,atoms,residue,tmp_end_strfile,remark)
			if(Pose == this->OPTICS.begin() && Pose+1 == this->OPTICS.end())
			{
				// case where there is only one pose to write (Pose == OPTICS.begin() && Pose++ == OPTICS.end())
				write_MODEL_pdb(true, true, nModel, this->FA,this->atoms,this->residue,tmp_end_strfile,remark);
			}
			else if(Pose == this->OPTICS.begin())
			{
				// first MODEL to be written
				write_MODEL_pdb(true, false, nModel, this->FA,this->atoms,this->residue,tmp_end_strfile,remark);
			}
			else if(Pose+1 == this->OPTICS.end())
			{
				// last MODEL to be written
				write_MODEL_pdb(false, true, nModel, this->FA,this->atoms,this->residue,tmp_end_strfile,remark);
			}
			else
			{
				// any MODEL in between to be written
				write_MODEL_pdb(false, false, nModel, this->FA,this->atoms,this->residue,tmp_end_strfile,remark);
			}
	}
}
}

std::vector<float> FastOPTICS::Vectorized_Chromosome(chromosome* chrom)
{
    float norm = 0.0f;
	std::vector<float> vChrom(this->nDimensions, 0.0f);
	// getting nDim-2 because the Dim=0 fills 3 memory cases
	for(int j = 0; j < this->nDimensions-2; ++j)
	{
		if(j == 0) //  building the first 3 comp. from genes[0] which are CartCoord x,y,z
		{
			for(int i = 0; i < 3; ++i)
			{
				// vChrom[i] = static_cast<float>(this->cleftgrid[static_cast<unsigned int>((*chrom).genes[j].to_ic)].coor[i] - this->FA->ori[i]);
				if(i == 0)
				{
                    vChrom[i] = static_cast<float>(this->cleftgrid[static_cast<unsigned int>((*chrom).genes[j].to_ic)].coor[i] - this->FA->ori[i]);
					// vChrom[i] = static_cast<float>(this->cleftgrid[static_cast<unsigned int>((*chrom).genes[j].to_ic)].dis);
					// vChrom[i] *= vChrom[i];
				}
				if(i == 1)
				{
					vChrom[i] = static_cast<float>(this->cleftgrid[static_cast<unsigned int>((*chrom).genes[j].to_ic)].coor[i] - this->FA->ori[i]);
					// vChrom[i] = static_cast<float>(this->cleftgrid[static_cast<unsigned int>((*chrom).genes[j].to_ic)].ang);
					// vChrom[i] = static_cast<float>( RandomDouble( (*chrom).genes[j].to_int32) );
					// vChrom[i] *= vChrom[i];
				}
				if(i == 2)
				{
                    vChrom[i] = static_cast<float>(this->cleftgrid[static_cast<unsigned int>((*chrom).genes[j].to_ic)].coor[i] - this->FA->ori[i]);
					// vChrom[i] = static_cast<float>(this->cleftgrid[static_cast<unsigned int>((*chrom).genes[j].to_ic)].dih);
					// vChrom[i] = static_cast<float>( genetoic(&this->gene_lim[i],(*chrom).genes[j].to_int32) );
					// vChrom[i] *= vChrom[i];
				}
                norm += vChrom[i]*vChrom[i];
			}
		}
		else
		{
			// j+2 is used from {j = 1 to N} to build further comp. of genes[j]
			// vChrom[j+2] = static_cast<float>(genetoic(&gene_lim[j], (*chrom).genes[j].to_int32));
			vChrom[j+2] = static_cast<float>((*chrom).genes[j].to_ic);
			// vChrom[j+2] = static_cast<float>( RandomDouble( (*chrom).genes[j].to_int32) );
            norm += vChrom[j+2]*vChrom[j+2];
		}
	}

  // norm = sqrtf(norm);
  // for(int k = 0; k < this->nDimensions; ++k) { vChrom[k]/=norm; }

   return vChrom;
}

std::vector<float> FastOPTICS::Vectorized_Cartesian_Coordinates(int chrom_index)
{
	int i = 0,j = 0,l = 0,m = 0;
	int cat;
	int rot;

	uint grd_idx;
	int normalmode=-1;
	int rot_idx=0;

    std::vector<float> vChrom(this->nDimensions);

	int npar = this->GB->num_genes;

	j = chrom_index;

	for(i=0;i<npar;i++){ this->FA->opt_par[i] = this->chroms[j].genes[i].to_ic; }

	for(i=0;i<npar;i++)
	{
		//printf("[%8.3f]",FA->opt_par[i]);

		if(this->FA->map_par[i].typ==-1)
		{ //by index
			grd_idx = (uint)this->FA->opt_par[i];
			//printf("this->FA->opt_par(index): %d\n", grd_idx);
			//PAUSE;
			this->atoms[this->FA->map_par[i].atm].dis = this->cleftgrid[grd_idx].dis;
			this->atoms[this->FA->map_par[i].atm].ang = this->cleftgrid[grd_idx].ang;
			this->atoms[this->FA->map_par[i].atm].dih = this->cleftgrid[grd_idx].dih;

		}
		else if(this->FA->map_par[i].typ == 0)
		{
			this->atoms[this->FA->map_par[i].atm].dis = (float)this->FA->opt_par[i];
		}
		else if(this->FA->map_par[i].typ == 1)
		{
			this->atoms[this->FA->map_par[i].atm].ang = (float)this->FA->opt_par[i];
		}
		else if(this->FA->map_par[i].typ == 2)
		{
			this->atoms[this->FA->map_par[i].atm].dih = (float)this->FA->opt_par[i];

			j=this->FA->map_par[i].atm;
			cat=this->atoms[j].rec[3];
			if(cat != 0)
			{
				while(cat != this->FA->map_par[i].atm)
				{
					this->atoms[cat].dih=this->atoms[j].dih + this->atoms[cat].shift;
					j=cat;
					cat=this->atoms[j].rec[3];
				}
			}
		}else if(this->FA->map_par[i].typ == 3)
		{ //by index
			grd_idx = (uint)this->FA->opt_par[i];

			// serves as flag , but also as grid index
			normalmode=grd_idx;

		}else if(this->FA->map_par[i].typ == 4)
		{
			rot_idx = (int)(this->FA->opt_par[i]+0.5);

			this->residue[this->atoms[this->FA->map_par[i].atm].ofres].rot=rot_idx;
		}

	}

	if(normalmode > -1) alter_mode(this->atoms,this->residue,this->FA->normal_grid[normalmode],this->FA->res_cnt,this->FA->normal_modes);

	/* rebuild cartesian coordinates of optimized residues*/
    for(i=0;i<this->FA->nors;i++) buildcc(this->FA,this->atoms,this->FA->nmov[i],this->FA->mov[i]);

	// residue that is optimized geometrically (ligand)
	l=this->atoms[this->FA->map_par[0].atm].ofres;

	rot=this->residue[l].rot;
    m=0;
	for(i=this->residue[l].fatm[rot];i<=this->residue[l].latm[rot];i++)
	{
		for(j=0;j<3;j++) vChrom[m*3+j] = this->atoms[i].coor[j];
        ++m;
	}
	return vChrom;
}

void FastOPTICS::ExpandClusterOrder(int ipt)
{
    std::priority_queue< ClusterOrdering, std::vector<ClusterOrdering>, ClusterOrderingComparator::ClusterOrderingComparator > queue;
	ClusterOrdering tmp(ipt,0,1e6f);
	queue.push(tmp);

    while(!queue.empty())
	{
		ClusterOrdering current = queue.top();
		queue.pop();
		int currPt = current.objectID;

		if(this->processed[currPt] == true) continue;

		this->order[this->iOrder] = currPt;
		// incrementing STATIC rank ordering ()
		// FastOPTICS::iOrder++;
		this->iOrder++;
		this->processed[currPt] = true;

		float coredist = this->inverseDensities[currPt];
		for( std::vector<int>::iterator it = this->neighbors[currPt].begin(); it != this->neighbors[currPt].end(); ++it)
		{
			int iNeigh = *it;
			if(this->processed[iNeigh] == true) continue;

			float nrdist = this->compute_distance(this->points[iNeigh], this->points[currPt]);

			if(coredist > nrdist)
				nrdist = coredist;
			if(isUndefinedDist(this->reachDist[iNeigh]))
				this->reachDist[iNeigh] = nrdist;
			else if(nrdist < this->reachDist[iNeigh])
				this->reachDist[iNeigh] = nrdist;
			tmp = ClusterOrdering::ClusterOrdering(iNeigh, currPt, nrdist);
			queue.push(tmp);
		}
        std::make_heap(const_cast<ClusterOrdering*>(&queue.top()),
                       const_cast<ClusterOrdering*>(&queue.top() + queue.size()),
                       ClusterOrderingComparator::ClusterOrderingComparator()
                       );
	}
}

float FastOPTICS::compute_distance(std::pair< chromosome*,std::vector<float> > & a, std::pair< chromosome*,std::vector<float> > & b)
{
	float distance = 0.0f;

	// simple distance calculation below
	for(int i = 0; i < this->nDimensions; ++i)
	{
		float tempDist = (a.second[i]-b.second[i]);
        distance +=  tempDist * tempDist;
	}

   			return sqrtf(distance);

   	// if(boost::math::isfinite(distance))
   	// {
   	// 		return sqrtf(distance);
   	// }
    // else if( boost::math::isinf(distance) && distance > FLT_EPSILON )
    // {
    //     	return UNDEFINED_DIST;
    // }
    // else 	return 0.0f;
   	}

float FastOPTICS::compute_vect_distance(std::vector<float> a, std::vector<float> b)
{
	float distance = 0.0f;

	// simple distance calculation below
	for(int i = 0; i < this->nDimensions; ++i)
    {
		float tempDist = (a[i]-b[i]);
        distance +=  tempDist * tempDist;
    }

	return sqrtf(distance);
}

int FastOPTICS::get_minPoints() { return this->minPoints; }
/*****************************************\
			RandomProjections
\*****************************************/

// STATIC variables declaration
int const RandomProjectedNeighborsAndDensities::logOProjectionConstant;
float RandomProjectedNeighborsAndDensities::sizeTolerance;

// Constructor
RandomProjectedNeighborsAndDensities::RandomProjectedNeighborsAndDensities(std::vector< std::pair< chromosome*,std::vector<float> > >& inPoints, int minSplitSize, FastOPTICS* top)
{
	this->top = top;
	this->minSplitSize = minSplitSize;
	RandomProjectedNeighborsAndDensities::sizeTolerance = static_cast<float>(2.0f/3.0f);

	if( inPoints.size() < 1 )
	{
		this->N = 0;
		this->nDimensions = 0;
		this->nProject1D = 0;
		return;
	}
	else
	{
        this->points = inPoints;
		this->N = this->points.size();
		this->nDimensions = this->top->nDimensions;
		this->nPointsSetSplits = static_cast<int>(RandomProjectedNeighborsAndDensities::logOProjectionConstant * log(this->N * this->nDimensions + 1)/log(2));
		this->nProject1D = static_cast<int>(RandomProjectedNeighborsAndDensities::logOProjectionConstant * log(this->N * this->nDimensions + 1)/log(2));
		// line below calls copy-constructor
	}
	this->projectedPoints.reserve(this->nProject1D);
	for(int i = 0; i < this->nProject1D; ++i)
	{
        this->projectedPoints.push_back(std::vector<float>(this->N));
	}
//	this->splitsets.reserve(this->nPointsSetSplits);
//	for(int j = 0; j< this->nPointsSetSplits; ++j)
//	{
		// this->splitsets.push_back(std::vector<int>());
//	}
}

void RandomProjectedNeighborsAndDensities::computeSetBounds(std::vector< int > & ptList)
{
	std::vector< std::vector<float> > tempProj(this->nProject1D);
	// perform projection of points
	for(int j = 0; j<this->nProject1D; ++j)
	{
        // std::vector<float> currentRp = this->Randomized_InternalCoord_Vector();
        std::vector<float> currentRp(this->Randomized_CartesianCoord_Vector());

		int k = 0;
		std::vector<int>::iterator it = ptList.begin();
		while(it != ptList.end())
		{
			float sum = 0.0f;
			// std::vector<float>::iterator vecPt = this->points[(*it)].second.begin();
			std::vector<float> vecPt(this->points[(*it)].second);
			std::vector<float>::iterator currPro = (this->projectedPoints[j]).begin();
			for(int m = 0; m < this->nDimensions; ++m)
				sum += currentRp[m] * vecPt[m];

			currPro[k] = sum;

			++k;
            ++it;
		}
	}

	// Split Points Set

	std::vector<int> projInd(this->nProject1D);
	projInd.reserve(this->nProject1D);
	for(int j = 0; j < this->nProject1D; ++j)
	{
//		projInd.push_back(j);
        projInd[j] = j;
	}
	for(int avgP = 0; avgP < this->nPointsSetSplits; avgP++)
	{
		// Shuffle projections
		for(int i = 0; i < this->nProject1D; ++i)
        {
//			tempProj.push_back(this->projectedPoints[i]);
            tempProj[i] = this->projectedPoints[i];
        }
//        std::random_shuffle(projInd.begin(), projInd.end(), ([](int n) { return rand() % n; }) );
        std::random_shuffle(projInd.begin(), projInd.end());

		int i = 0;
        for(std::vector<int>::iterator it = projInd.begin(); it != projInd.end(); ++it, i++)
		{
			int cind = (*it);
			// look this line to be sure that the vector is pushed in this->projectedPoints
			this->projectedPoints[cind] = tempProj[i];
		}

		//split points set
		int nPoints = ptList.size();
		std::vector<int> ind(nPoints);
		ind.reserve(nPoints);
		for(int l = 0; l < nPoints; ++l)
		{
			ind[l] = l;
		}
		this->SplitUpNoSort(ind,0);
	}
}

void RandomProjectedNeighborsAndDensities::SplitUpNoSort(std::vector< int >& ind, int dim)
{
	int nElements = ind.size();
	dim = dim % this->nProject1D;
	std::vector<float>::iterator tProj = (this->projectedPoints[dim]).begin();
	int splitPos;

	// save set such that used for density or neighborhood computation
	if(nElements > this->minSplitSize*(1 - RandomProjectedNeighborsAndDensities::sizeTolerance) && nElements < this->minSplitSize*(1 + RandomProjectedNeighborsAndDensities::sizeTolerance))
	{
		std::vector<float> cpro(nElements);
		for(int i = 0; i < nElements; ++i)
		{
//			cpro.push_back(tProj[ind[i]]);
            cpro[i] = tProj[ind[i]];
		}
		// sprting cpro[] && ind[] concurrently
		this->quicksort_concurrent_Vectors(cpro, ind, 0, nElements-1);
		this->splitsets.push_back(ind);
	}

	// compute splitting element
	if(nElements > this->minSplitSize)
	{
		//pick random splitting element based on position
		int randInt = static_cast<int>(std::floor( (RandomDouble()*static_cast<double>(nElements)) ));
		float rs = tProj[ind[randInt]];
		int minInd = 0;
		int maxInd = nElements - 1;
		while(minInd < maxInd)
		{
			float currEle = tProj[ind[minInd]];
			if(currEle > rs)
			{
				while(minInd < maxInd && tProj[ind[maxInd]] > rs) maxInd--;
				if(minInd == maxInd) break;

				int currInd = ind[minInd];
				ind[minInd] = ind[maxInd];
				ind[maxInd] = currInd;
				maxInd--;
			}
			minInd++;
		}
		if( minInd == nElements-1 ) minInd = nElements/2;

		// split set recursively
		splitPos = minInd + 1;

		// std::vector<int> ind2(splitPos);
		std::vector<int> ind2(splitPos);
		for(int l = 0; l < splitPos; ++l)
		{
			 ind2[l] = ind[l];
//			ind2.push_back(ind[l]);
		}
		this->SplitUpNoSort(ind2,dim+1);

        std::vector<int> ind3(nElements - splitPos);
//        ind2 = std::vector<int>( nElements-splitPos );
		for(int l = 0; l < nElements-splitPos; ++l)
		{
			 ind3[l] = ind[l+splitPos];
//			ind3.push_back(ind[l+splitPos]);
		}
		this->SplitUpNoSort(ind3,dim+1);
	}
}

void RandomProjectedNeighborsAndDensities::getInverseDensities(std::vector< float > & inverseDensities)
{
	inverseDensities.reserve(this->N);
	std::vector<int> nDists(this->N);
	nDists.reserve(this->N);
//	for(std::vector< std::vector< int> >::iterator it1 = this->splitsets.begin(); it1 != this->splitsets.end(); ++it1)
    for(int i = 0; i < this->splitsets.size(); ++i)
	{
//		std::vector<int>::iterator pinSet = it1->begin();
        std::vector<int> & pinSet = this->splitsets.at(i);

//		int len = it1->size();
        int len = pinSet.size();
		int indoff = static_cast<int>(round(len/2));
		int oldind = pinSet[indoff];
		for(int i = 0; i < len; ++i)
		{
			int ind = pinSet[i];
			if(ind == indoff) continue;

			float dist = this->top->compute_distance(this->points[ind],this->points[oldind]);
			inverseDensities[oldind] += dist;
			nDists[oldind]++;
			inverseDensities[ind] += dist;
			nDists[ind]++;
		}
	}
	for(int l = 0; l < this->N; ++l)
	{
		if(nDists[l] == 0) inverseDensities[l] = UNDEFINED_DIST;
		else inverseDensities[l] /= nDists[l];
	}
}

void RandomProjectedNeighborsAndDensities::getNeighbors(std::vector< std::vector< int > > & neighs)
{
	neighs.reserve(this->N);
	for(int l = 0; l < this->N; l++)
	{
		std::vector<int> list;
		neighs.push_back(list);
	}

	// go through all sets
	for(std::vector< std::vector< int > >::iterator it1 = this->splitsets.begin(); it1 != this->splitsets.end(); ++it1)
	{
		// for each set (each projected line)
		std::vector<int>::iterator pinSet = it1->begin();
		int len = it1->size();
		int ind = pinSet[0];
		int indoff = static_cast<int>(round(len/2));
		int oldind = pinSet[indoff];

		// add all points as neighbors to middle point
		//  +
		// add the middle point to all other points in set
		for(int i = 0; i < len; ++i)
		{
			ind = pinSet[i];

			// The following block of code uses an iterator to check
			std::vector<int> & cneighs = neighs.at(ind);
			// std::vector<int>::iterator itPos = std::find(cneighs.begin(),cneighs.end(),oldind);
			// if(itPos == cneighs.end()) //element not found in cneigh
			if( !std::binary_search(cneighs.begin(), cneighs.end(), oldind) )
			{
				cneighs.push_back(oldind);
				std::sort(cneighs.begin(),cneighs.end());
				cneighs.erase(std::unique(cneighs.begin(), cneighs.end()), cneighs.end());
			}

			std::vector<int> & cneighs2 = neighs.at(oldind);
			// itPos = std::find(cneighs2.begin(), cneighs2.end(), ind);
			// if(itPos == cneighs2.end()) // element not found in cneighs2
			if( !std::binary_search(cneighs2.begin(), cneighs2.end(), ind) )
			{
				cneighs2.push_back(oldind);
				std::sort(cneighs2.begin(),cneighs2.end());
				cneighs2.erase(std::unique(cneighs2.begin(), cneighs2.end()), cneighs2.end());
			}
		}

	}
}

void FastOPTICS::normalizeDistances()
{
	float max = 0.0f;
	std::vector<float> & Distances = this->reachDist;
	for(std::vector<float>::iterator it = Distances.begin(); it != Distances.end(); ++it) if(*it > max && !isUndefinedDist(*it)) max = *it;
	for(std::vector<float>::iterator it = Distances.begin(); it != Distances.end(); ++it) if(!isUndefinedDist(*it)) *it /= max;
}

std::vector<float> RandomProjectedNeighborsAndDensities::Randomized_InternalCoord_Vector()
{
    std::vector<float> vChrom(this->nDimensions);

	float sum = 0.0f;
	for(int j = 0; j < this->nDimensions-2; ++j)
	{
		if(j == 0) //  building the first 3 comp. from genes[0] which are CartCoord x,y,z
		{
            double doubleGeneIC = genetoic(&this->top->gene_lim[j],roll_die());
            for(int i = 0; i < 3; ++i)
			{
				// vChrom[i] = static_cast<float>(this->top->cleftgrid[static_cast<unsigned int>(doubleGeneIC)].coor[i] - this->top-÷>FA->ori[i]);
				if(i == 0)
				{
					vChrom[i] = static_cast<float>(this->top->cleftgrid[static_cast<unsigned int>(doubleGeneIC)].coor[i] - this->top->FA->ori[i]);
//					vChrom[i] = static_cast<float>(this->top->cleftgrid[static_cast<unsigned int>(doubleGeneIC)].dis);
//                    vChrom[i] *= vChrom[i];
				}
				if(i == 1)
				{
                    vChrom[i] = static_cast<float>(this->top->cleftgrid[static_cast<unsigned int>(doubleGeneIC)].coor[i] - this->top->FA->ori[i]);
//					vChrom[i] = static_cast<float>(this->top->cleftgrid[static_cast<unsigned int>(doubleGeneIC)].ang);
//                    vChrom[i] *= vChrom[i];
				}
				if(i == 2)
				{
                    vChrom[i] = static_cast<float>(this->top->cleftgrid[static_cast<unsigned int>(doubleGeneIC)].coor[i] - this->top->FA->ori[i]);
//					vChrom[i] = static_cast<float>(this->top->cleftgrid[static_cast<unsigned int>(doubleGeneIC)].dih);
//                    vChrom[i] *= vChrom[i];
				}
				sum += vChrom[i]*vChrom[i];
			}
		}
		else
		{
			// j+2 is used from {j = 1 to N} to build further comp. of genes[j]
			// vChrom[j+2] = static_cast<float>(RandomDouble(random_dice()));
			vChrom[j+2] = static_cast<float>(genetoic(&this->top->gene_lim[j],roll_die()));
			sum += vChrom[j+2]*vChrom[j+2];
		}
	}
	sum = sqrtf(sum);
	// for(int k = 0; k < this->nDimensions; ++k) { vChrom[k]/=sum; }

	return vChrom;
}

std::vector<float> RandomProjectedNeighborsAndDensities::Randomized_CartesianCoord_Vector()
{
    float norm = 0.0f;

    int i = 0,j = 0,l = 0,m = 0;
	int cat;
	int rot;

	uint grd_idx;
	int normalmode=-1;
	int rot_idx=0;

    std::vector<float> vChrom(this->nDimensions);

	int npar = this->top->GB->num_genes;

	for(i=0;i<npar;i++){ this->top->FA->opt_par[i] = genetoic(&this->top->gene_lim[i],roll_die()); }

	for(i=0;i<npar;i++)
	{

		if(this->top->FA->map_par[i].typ==-1)
		{ //by index
			grd_idx = (uint)this->top->FA->opt_par[i];

			this->top->atoms[this->top->FA->map_par[i].atm].dis = this->top->cleftgrid[grd_idx].dis;
			this->top->atoms[this->top->FA->map_par[i].atm].ang = this->top->cleftgrid[grd_idx].ang;
			this->top->atoms[this->top->FA->map_par[i].atm].dih = this->top->cleftgrid[grd_idx].dih;

		}
		else if(this->top->FA->map_par[i].typ == 0)
		{
			this->top->atoms[this->top->FA->map_par[i].atm].dis = (float)this->top->FA->opt_par[i];
		}
		else if(this->top->FA->map_par[i].typ == 1)
		{
			this->top->atoms[this->top->FA->map_par[i].atm].ang = (float)this->top->FA->opt_par[i];
		}
		else if(this->top->FA->map_par[i].typ == 2)
		{
			this->top->atoms[this->top->FA->map_par[i].atm].dih = (float)this->top->FA->opt_par[i];

			j=this->top->FA->map_par[i].atm;
			cat=this->top->atoms[j].rec[3];
			if(cat != 0)
			{
				while(cat != this->top->FA->map_par[i].atm)
				{
					this->top->atoms[cat].dih=this->top->atoms[j].dih + this->top->atoms[cat].shift;
					j=cat;
					cat=this->top->atoms[j].rec[3];
				}
			}
		}else if(this->top->FA->map_par[i].typ == 3)
		{ //by index
			grd_idx = (uint)this->top->FA->opt_par[i];

			// serves as flag , but also as grid index
			normalmode=grd_idx;

		}else if(this->top->FA->map_par[i].typ == 4)
		{
			rot_idx = (int)(this->top->FA->opt_par[i]+0.5);

			this->top->residue[this->top->atoms[this->top->FA->map_par[i].atm].ofres].rot=rot_idx;
		}

	}

	if(normalmode > -1) alter_mode(this->top->atoms,this->top->residue,this->top->FA->normal_grid[normalmode],this->top->FA->res_cnt,this->top->FA->normal_modes);

	/* rebuild cartesian coordinates of optimized residues*/
    for(i=0;i<this->top->FA->nors;i++) buildcc(this->top->FA,this->top->atoms,this->top->FA->nmov[i],this->top->FA->mov[i]);

	// residue that is optimized geometrically (ligand)
	l=this->top->atoms[this->top->FA->map_par[0].atm].ofres;

	rot=this->top->residue[l].rot;
    m=0;
	for(i=this->top->residue[l].fatm[rot];i<=this->top->residue[l].latm[rot];i++)
	{
		for(j=0;j<3;j++)
		{
			vChrom[m*3+j] = this->top->atoms[i].coor[j];
			norm += vChrom[m*3+j]*vChrom[m*3+j];
		}
        ++m;
	}
	norm = sqrtf(norm);
//   	for(i = 0; i < this->nDimensions; ++i) vChrom[i] /= norm;
	return vChrom;
}

void RandomProjectedNeighborsAndDensities::quicksort_concurrent_Vectors(std::vector<float>& data, std::vector<int>& index, int beg, int end)
{
	int l, r, p;
	float pivot;
	std::vector<float>::iterator xData, yData;
	std::vector<int>::iterator xIndex, yIndex;
	while(beg < end)
	{
		l = beg; p = beg + (end-beg)/2; r = end;
		pivot = data[p];

		while(1)
		{
			while( (l<=r) && QS_ASC(data[l],pivot) <= 0 ) ++l;
			while( (l<=r) && QS_ASC(data[r],pivot)  > 0 ) --r;

			if (l > r) break;
			xData = data.begin()+l; yData = data.begin()+r;
			xIndex = index.begin()+l; yIndex = index.begin()+r;
			this->swap_element_in_vectors(xData, yData, xIndex, yIndex);
			if (p == r) p = l;
			++l; --r;
		}
		xData = data.begin()+p; yData = data.begin()+r;
		xIndex = index.begin()+p; yIndex = index.begin()+r;
		this->swap_element_in_vectors(xData, yData, xIndex, yIndex);

		--r;

		if( (r-beg) < (end-l) )
		{
			this->quicksort_concurrent_Vectors(data, index, beg, r);
			beg = l;
		}
		else
		{
			this->quicksort_concurrent_Vectors(data, index, l, end);
			end = r;
		}
	}
}

void RandomProjectedNeighborsAndDensities::swap_element_in_vectors(std::vector<float>::iterator xData, std::vector<float>::iterator yData, std::vector<int>::iterator xIndex, std::vector<int>::iterator yIndex)
{
	float tData = *xData; *xData = *yData; *yData = tData;
	int tIndex = *xIndex; *xIndex = *yIndex; *yIndex = tIndex;
}

void RandomProjectedNeighborsAndDensities::output_projected_distance(char* end_strfile, char* tmp_end_strfile)
{
	char sufix[25];
	sprintf(sufix, "__%d.projDist", this->top->minPoints);
	strcpy(tmp_end_strfile, end_strfile);
	strcat(tmp_end_strfile,sufix);
	FILE* outfile;
	if(!OpenFile_B(tmp_end_strfile,"w",&outfile))
	{
		Terminate(5);
	}
	else
	{
		for(int i = 0; i < this->N; ++i)
		{
            for(int j = 0; j < this->nProject1D; ++j)
			{
                std::vector<float> & it = this->projectedPoints.at(j);
				fprintf(outfile, "%6f", it[i]);
                if(j < this->nProject1D-1) fprintf(outfile, "\t");
                else fprintf(outfile, "\n");
}
		}
	}
	CloseFile_B(&outfile,"w");;//fclose(outfile);
}

// This function generates a RandomInt32 who can be used as *genes->to_int32 value
// int RandomProjectedNeighborsAndDensities::Dice()
// {
// 	unsigned int tt = static_cast<unsigned int>(time(0));
// 	srand(tt);
// 	RNGType rng(tt);
// 	boost::uniform_int<> one_to_max_int32( 0, MAX_RANDOM_VALUE );
// 	boost::variate_generator< RNGType, boost::uniform_int<> > dice(rng, one_to_max_int32);
// 	return dice();
// }

int roll_die()
{
    boost::random::uniform_int_distribution<> dist(0, MAX_RANDOM_VALUE);
    return dist(gen);
}
int roll_rand_die()
{
	int n;
	return rand()%n;
}


#include <cstdint>
#include <cfloat>
#include <cassert>
#include <string>
#include <queue>
#include <map>
#include <vector>
#include <boost/format.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/utility/value_init.hpp>
#include "Alignment.hpp"
#include "AlnGraphBoost.hpp"

AlnGraphBoost::AlnGraphBoost(const std::string& backbone) {
    // initialize the graph structure with the backbone length + enter/exit
    // vertex
    size_t blen = backbone.length();
    _g = G(blen+2);
    for (size_t i = 0; i < blen+1; i++)
        boost::add_edge(i, i+1, _g);

    VtxIter curr, last;
    boost::tie(curr, last) = boost::vertices(_g);
    _enterVtx = *curr++;
    _g[_enterVtx].base = '^';
    _g[_enterVtx].backbone = true;
    for (size_t i = 0; i < blen; i++, ++curr) {
        VtxDesc v = *curr;
        _g[v].backbone = true;
        _g[v].weight = 1;
        _g[v].base = backbone[i];
        _bbMap[v] = v;
    }
    _exitVtx = *curr;
    _g[_exitVtx].base = '$';
    _g[_exitVtx].backbone = true;
}

AlnGraphBoost::AlnGraphBoost(const size_t blen) {
    _g = G(blen+2);
    for (size_t i = 0; i < blen+1; i++)
        boost::add_edge(i, i+1, _g);

    VtxIter curr, last;
    boost::tie(curr, last) = boost::vertices(_g);
    _enterVtx = *curr++;
    _g[_enterVtx].base = '^';
    _g[_enterVtx].backbone = true;
    for (size_t i = 0; i < blen; ++i, ++curr) {
        VtxDesc v = *curr;
        _g[v].backbone = true;
        _g[v].weight = 1;
        _g[v].deleted = false;
        _g[v].base = 'N';
        _bbMap[v] = v;
    }
    _exitVtx = *curr;
    _g[_exitVtx].base = '$';
    _g[_exitVtx].backbone = true;
}

void AlnGraphBoost::addAln(dagcon::Alignment& aln) {
    IndexMap index = boost::get(boost::vertex_index, _g);
    // tracks the position on the backbone
    uint32_t bbPos = aln.start;
    VtxDesc prevVtx = _enterVtx;
    for (size_t i = 0; i < aln.qstr.length(); i++) {
        char queryBase = aln.qstr[i], targetBase = aln.tstr[i];
        assert(queryBase != '.');
        assert(targetBase != '.');
        VtxDesc currVtx = index[bbPos];
        // match
        if (queryBase == targetBase) {
            _g[_bbMap[currVtx]].coverage++;

            // NOTE: for empty backbones
            _g[_bbMap[currVtx]].base = targetBase;

            _g[currVtx].weight++;
            addEdge(prevVtx, currVtx);
            bbPos++;
            prevVtx = currVtx;
        // query deletion
        } else if (queryBase == '-' && targetBase != '-') {
            _g[_bbMap[currVtx]].coverage++;

            // NOTE: for empty backbones
            _g[_bbMap[currVtx]].base = targetBase;

            bbPos++;
        // query insertion
        } else if (queryBase != '-' && targetBase == '-') {
            // create new node and edge
            VtxDesc newVtx = boost::add_vertex(_g);
            _g[newVtx].base = queryBase;
            _g[newVtx].weight++;
            _g[newVtx].backbone = false;
            _g[newVtx].deleted = false;
            _bbMap[newVtx] = bbPos;
            addEdge(prevVtx, newVtx);
            prevVtx = newVtx;
        }
    }
    addEdge(prevVtx, _exitVtx);
}

void AlnGraphBoost::addEdge(VtxDesc u, VtxDesc v) {
    // Check if edge exists with prev node.  If it does, increment edge counter,
    // otherwise add a new edge.
    InEdgeIter ii, ie;
    bool edgeExists = false;
    for (boost::tie(ii, ie) = boost::in_edges(v, _g); ii != ie; ++ii) {
        EdgeDesc e = *ii;
        if (boost::source(e , _g) == u) {
            // increment edge count
            _g[e].count++;
            edgeExists = true;
        }
    }
    if (! edgeExists) {
        // add new edge
        std::pair<EdgeDesc, bool> p = boost::add_edge(u, v, _g);
        _g[p.first].count++;
    }
}

void AlnGraphBoost::mergeNodes() {
    std::queue<VtxDesc> seedNodes;
    seedNodes.push(_enterVtx);

    while(true) {
        if (seedNodes.size() == 0)
            break;

        VtxDesc u = seedNodes.front();
        seedNodes.pop();
        mergeInNodes(u);
        mergeOutNodes(u);

        OutEdgeIter oi, oe;
        for (boost::tie(oi, oe) = boost::out_edges(u, _g); oi != oe; ++oi) {
            EdgeDesc e = *oi;
            _g[e].visited = true;
            VtxDesc v = boost::target(e, _g);
            InEdgeIter ii, ie;
            int notVisited = 0;
            for (boost::tie(ii, ie) = boost::in_edges(v, _g); ii != ie; ++ii) {
                if (_g[*ii].visited == false)
                    notVisited++;
            }

            // move onto the boost::target node after we visit all incoming edges for
            // the boost::target node
            if (notVisited == 0)
                seedNodes.push(v);
        }
    }
}

void AlnGraphBoost::mergeInNodes(VtxDesc n) {
    std::map<char, std::vector<VtxDesc>> nodeGroups;
    InEdgeIter ii, ie;
    // Group neighboring nodes by base
    for(boost::tie(ii, ie) = boost::in_edges(n, _g); ii != ie; ++ii) {
        VtxDesc inNode = boost::source(*ii, _g);
        if (out_degree(inNode, _g) == 1) {
            nodeGroups[_g[inNode].base].push_back(inNode);
        }
    }

    // iterate over node groups, merge an accumulate information
    for(auto kvp = nodeGroups.cbegin(); kvp != nodeGroups.end(); ++kvp) {
        std::vector<VtxDesc> nodes = (*kvp).second;
        if (nodes.size() <= 1)
            continue;

        std::vector<VtxDesc>::const_iterator ni = nodes.cbegin();
        VtxDesc an = *ni++;
        OutEdgeIter anoi, anoe;
        boost::tie(anoi, anoe) = boost::out_edges(an, _g);

        // Accumulate out edge information
        for (; ni != nodes.cend(); ++ni) {
            OutEdgeIter oi, oe;
            boost::tie(oi, oe) = boost::out_edges(*ni, _g);
            _g[*anoi].count += _g[*oi].count;
            _g[an].weight += _g[*ni].weight;
        }

        // Accumulate in edge information, merges nodes
        ni = nodes.cbegin();
        ++ni;
        for (; ni != nodes.cend(); ++ni) {
            InEdgeIter ii, ie;
            VtxDesc n = *ni;
            for (boost::tie(ii, ie) = boost::in_edges(n, _g); ii != ie; ++ii) {
                VtxDesc n1 = boost::source(*ii, _g);
                EdgeDesc e;
                bool exists;
                boost::tie(e, exists) = edge(n1, an, _g);
                if (exists) {
                    _g[e].count += _g[*ii].count;
                } else {
                    std::pair<EdgeDesc, bool> p = boost::add_edge(n1, an, _g);
                    _g[p.first].count = _g[*ii].count;
                    _g[p.first].visited = _g[*ii].visited;
                }
            }
            markForReaper(n);
        }
        mergeInNodes(an);
    }
}

void AlnGraphBoost::mergeOutNodes(VtxDesc n) {
    std::map<char, std::vector<VtxDesc>> nodeGroups;
    OutEdgeIter oi, oe;
    for(boost::tie(oi, oe) = boost::out_edges(n, _g); oi != oe; ++oi) {
        VtxDesc outNode = boost::target(*oi, _g);
        if (in_degree(outNode, _g) == 1) {
            nodeGroups[_g[outNode].base].push_back(outNode);
        }
    }

    for(auto kvp = nodeGroups.cbegin(); kvp != nodeGroups.end(); ++kvp) {
        std::vector<VtxDesc> nodes = (*kvp).second;
        if (nodes.size() <= 1)
            continue;

        std::vector<VtxDesc>::const_iterator ni = nodes.cbegin();
        VtxDesc an = *ni++;
        InEdgeIter anii, anie;
        boost::tie(anii, anie) = boost::in_edges(an, _g);

        // Accumulate inner edge information
        for (; ni != nodes.cend(); ++ni) {
            InEdgeIter ii, ie;
            boost::tie(ii, ie) = boost::in_edges(*ni, _g);
            _g[*anii].count += _g[*ii].count;
            _g[an].weight += _g[*ni].weight;
        }

        // Accumulate and merge outer edge information
        ni = nodes.cbegin();
        ++ni;
        for (; ni != nodes.cend(); ++ni) {
            OutEdgeIter oi, oe;
            VtxDesc n = *ni;
            for (boost::tie(oi, oe) = boost::out_edges(n, _g); oi != oe; ++oi) {
                VtxDesc n2 = boost::target(*oi, _g);
                EdgeDesc e;
                bool exists;
                boost::tie(e, exists) = edge(an, n2, _g);
                if (exists) {
                    _g[e].count += _g[*oi].count;
                } else {
                    std::pair<EdgeDesc, bool> p = boost::add_edge(an, n2, _g);
                    _g[p.first].count = _g[*oi].count;
                    _g[p.first].visited = _g[*oi].visited;
                }
            }
            markForReaper(n);
        }
    }
}

void AlnGraphBoost::markForReaper(VtxDesc n) {
    _g[n].deleted = true;
    clear_vertex(n, _g);
    _reaperBag.push_back(n);
}

void AlnGraphBoost::reapNodes() {
    int reapCount = 0;
    std::sort(_reaperBag.begin(), _reaperBag.end());
    std::vector<VtxDesc>::iterator curr = _reaperBag.begin();
    for (; curr != _reaperBag.end(); ++curr) {
        assert(_g[*curr].backbone==false);
        remove_vertex(*curr-reapCount++, _g);
    }
}

const std::string AlnGraphBoost::consensus(int minWeight) {
    // get the best scoring path
    std::vector<AlnNode> path = bestPath();

    // consensus sequence
    std::string cns;

    // track the longest consensus path meeting minimum weight
    int offs = 0, bestOffs = 0, length = 0, idx = 0;
    bool metWeight = false;
    std::vector<AlnNode>::iterator curr = path.begin();
    for (; curr != path.end(); ++curr) {
        AlnNode n = *curr;
        if (n.base == _g[_enterVtx].base || n.base == _g[_exitVtx].base)
            continue;

        cns += n.base;

        // initial beginning of minimum weight section
        if (!metWeight && n.weight >= minWeight) {
            offs = idx;
            metWeight = true;
        } else if (metWeight && n.weight < minWeight) {
        // concluded minimum weight section, update if longest seen so far
            if ((idx - offs) > length) {
                bestOffs = offs;
                length = idx - offs;
            }
            metWeight = false;
        }
        idx++;
    }

    // include end of sequence
    if (metWeight && (idx - offs) > length) {
        bestOffs = offs;
        length = idx - offs;
    }

    return cns.substr(bestOffs, length);
}

void AlnGraphBoost::consensus(std::vector<CnsResult>& seqs, int minWeight, size_t minLen) {
    seqs.clear();

    // get the best scoring path
    std::vector<AlnNode> path = bestPath();

    // consensus sequence
    std::string cns;

    // track the longest consensus path meeting minimum weight
    int offs = 0, idx = 0;
    bool metWeight = false;
    std::vector<AlnNode>::iterator curr = path.begin();
    for (; curr != path.end(); ++curr) {
        AlnNode n = *curr;
        if (n.base == _g[_enterVtx].base || n.base == _g[_exitVtx].base)
            continue;

        cns += n.base;

        // initial beginning of minimum weight section
        if (!metWeight && n.weight >= minWeight) {
            offs = idx;
            metWeight = true;
        } else if (metWeight && n.weight < minWeight) {
        // concluded minimum weight section, add sequence to supplied vector
            metWeight = false;
            CnsResult result;
            result.range[0] = offs;
            result.range[1] = idx;
            size_t length = idx - offs;
            result.seq = cns.substr(offs, length);
            if (length >= minLen) seqs.push_back(result);
        }
        idx++;
    }

    // include end of sequence
    if (metWeight) {
        size_t length = idx - offs;
        CnsResult result;
        result.range[0] = offs;
        result.range[1] = idx;
        result.seq = cns.substr(offs, length);
        if (length >= minLen) seqs.push_back(result);
    }
}

const std::vector<AlnNode> AlnGraphBoost::bestPath() {
    EdgeIter ei, ee;
    for (boost::tie(ei, ee) = edges(_g); ei != ee; ++ei)
        _g[*ei].visited = false;

    std::map<VtxDesc, EdgeDesc> bestNodeScoreEdge;
    std::map<VtxDesc, float> nodeScore;
    std::queue<VtxDesc> seedNodes;

    // start at the end and make our way backwards
    seedNodes.push(_exitVtx);
    nodeScore[_exitVtx] = 0.0f;

    while (true) {
        if (seedNodes.size() == 0)
            break;

        VtxDesc n = seedNodes.front();
        seedNodes.pop();

        bool bestEdgeFound = false;
        float bestScore = -FLT_MAX;
        EdgeDesc bestEdgeD = boost::initialized_value;
        OutEdgeIter oi, oe;
        for(boost::tie(oi, oe) = boost::out_edges(n, _g); oi != oe; ++oi) {
            EdgeDesc outEdgeD = *oi;
            VtxDesc outNodeD = boost::target(outEdgeD, _g);
            AlnNode outNode = _g[outNodeD];
            float newScore, score = nodeScore[outNodeD];
            if (outNode.backbone && outNode.weight == 1) {
                newScore = score - 10.0f;
            } else {
                AlnNode bbNode = _g[_bbMap[outNodeD]];
                newScore = _g[outEdgeD].count - bbNode.coverage*0.5f + score;
            }

            if (newScore > bestScore) {
                bestScore = newScore;
                bestEdgeD = outEdgeD;
                bestEdgeFound = true;
            }
        }

        if (bestEdgeFound) {
            nodeScore[n]= bestScore;
            bestNodeScoreEdge[n] = bestEdgeD;
        }

        InEdgeIter ii, ie;
        for (boost::tie(ii, ie) = boost::in_edges(n, _g); ii != ie; ++ii) {
            EdgeDesc inEdge = *ii;
            _g[inEdge].visited = true;
            VtxDesc inNode = boost::source(inEdge, _g);
            int notVisited = 0;
            OutEdgeIter oi, oe;
            for (boost::tie(oi, oe) = boost::out_edges(inNode, _g); oi != oe; ++oi) {
                if (_g[*oi].visited == false)
                    notVisited++;
            }

            // move onto the target node after we visit all incoming edges for
            // the target node
            if (notVisited == 0)
                seedNodes.push(inNode);
        }
    }

    // construct the final best path
    VtxDesc prev = _enterVtx, next;
    std::vector<AlnNode> bpath;
    while (true) {
        bpath.push_back(_g[prev]);
        if (bestNodeScoreEdge.count(prev) == 0) {
            break;
        } else {
            EdgeDesc bestOutEdge = bestNodeScoreEdge[prev];
            _g[prev].bestOutEdge = bestOutEdge;
            next = boost::target(bestOutEdge, _g);
            _g[next].bestInEdge = bestOutEdge;
            prev = next;
        }
    }

    return bpath;
}

void AlnGraphBoost::printGraph() {
    reapNodes();
    boost::write_graphviz(std::cout, _g,
        make_label_writer(get(&AlnNode::base, _g)),
        make_label_writer(get(&AlnEdge::count, _g)));
}

bool AlnGraphBoost::danglingNodes() {
    VtxIter curr, last;
    boost::tie(curr, last) = boost::vertices(_g);
    bool found = false;
    for (;curr != last; ++curr) {
        if (_g[*curr].deleted)
            continue;
        if (_g[*curr].base == _g[_enterVtx].base || _g[*curr].base == _g[_exitVtx].base)
            continue;

        int indeg = out_degree(*curr, _g);
        int outdeg = in_degree(*curr, _g);
        if (outdeg > 0 && indeg > 0) continue;

        found = true;
    }
    return found;
}

AlnGraphBoost::~AlnGraphBoost(){}


#ifndef FSTSEARCHBRANCHANDBOUNDPARTIALPREDICTION_H
#define FSTSEARCHBRANCHANDBOUNDPARTIALPREDICTION_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    search_branch_and_bound_partial_prediction.hpp
   \brief   Implements Branch and Bound with Partial Prediction (BBPP) method, i.e., with predicted node ordering
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    March 2011
   \version 3.1.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is availables free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz).
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <ctime>
#include <cstdlib> //rand
#include "branch_and_bound_predictor.hpp"
#include "search_branch_and_bound.hpp"
#include "error.hpp"
#include "global.hpp"
#include "result_tracker.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

/*! \brief Implements Branch and Bound with Partial Prediction (BBPP) method, i.e., with predicted node ordering

	BBPP is generally slower than FBB but faster than IBB. Its principle is
	identical to that of Improved Branch & Bound (IBB), but it replaces
	large amount of criterion evaluations needed for ordering nodes in tree levels
	by quickly predicted values. The optimality of final result is not jeopardized
	but considerable amount of time is saved. See FBB for a more radical but still
	optimality preserving use of value prediction.

	\warning All Branch & Bound feature selection algorithms require the used
	CRITERION to be monotonic with respect to cardinality. More precisely, it must
	hold that removing a feature from a set MUST NOT increase criterion value.
	Otherwise there is no guarantee as of the optimality of obtained results
	with respect to the used criterion.

	\note All Branch & Bound algorithms by definition yield the solution with the
	same maximum criterion value, therefore the main concern regarding particular
	Branch & Bound algorithm is only its search speed.

	\note Due to possibly high number of subsets to be tested expect
	excessive computational time.

	\note Result tracking in case of Branch & Bound algorithms records only results
	of target cardinality.
*/

template<class RETURNTYPE, typename DIMTYPE, class SUBSET, class CRITERION, class PREDICTOR>
class Search_Branch_And_Bound_Partial_Prediction : public Search_Branch_And_Bound<RETURNTYPE,DIMTYPE,SUBSET,CRITERION>{
public:
	typedef Search_Branch_And_Bound<RETURNTYPE,DIMTYPE,SUBSET,CRITERION> parent;
	typedef typename parent::PCriterion PCriterion;
	typedef typename parent::PSubset PSubset;
	typedef typename parent::PNode PNode;
	typedef typename parent::Node Node;
	typedef typename parent::NodeType NodeType;
	Search_Branch_And_Bound_Partial_Prediction():Search_Branch_And_Bound<RETURNTYPE,DIMTYPE,SUBSET,CRITERION>() {notify("Search_Branch_And_Bound_Partial_Prediction constructor.");}
	virtual ~Search_Branch_And_Bound_Partial_Prediction() {notify("Search_Branch_And_Bound_Partial_Prediction destructor.");}

	// for search() implementation see parent class

	virtual std::ostream& print(std::ostream& os) const {os << "Branch & Bound with Partial Prediction (BBPP)  [Search_Branch_And_Bound_Partial_Prediction() with " << _predictor << "]"; return os;};
protected:
	PREDICTOR _predictor;

	// the following may be overriden in descendant Branch and Bound specialization classes
	virtual void initialize(const DIMTYPE d, const DIMTYPE n, const PCriterion crit) {_predictor.initialize(n);} //!< called before search - enables set-up of additional structures in descendants
	virtual void process_leafs();
	virtual void pre_evaluate_availables(); //!< assign values to each feature in availables - to be used for node ordering
	virtual void post_process_tree_level(); //!< enables to substitute missing COMPUTED values in nodes just after level creation, if needed
	virtual bool cut_possible(); //!< tests current node for the possibility to cut its sub-branch
};

template<class RETURNTYPE, typename DIMTYPE, class SUBSET, class CRITERION, class PREDICTOR>
void Search_Branch_And_Bound_Partial_Prediction<RETURNTYPE,DIMTYPE,SUBSET,CRITERION,PREDICTOR>::process_leafs()
{
	assert(parent::get_criterion());
	const PSubset &currentset=parent::get_currentset();
	Node &parentnode=parent::get_parent_node();
	RETURNTYPE value;
	PNode avail;
	for(bool got=getFirstAvailable(avail);got;got=getNextAvailable(avail))
	{
		assert(currentset->selected_raw(avail->feature));
		currentset->deselect_raw(avail->feature);
		if(!parent::get_criterion()->evaluate(value,currentset)) throw FST::fst_error("Criterion evaluation failure.");
		update_bound(value,currentset); //adds to tracker
		// conditionally update prediction info
		if(parentnode.type==parent::COMPUTED) _predictor.learn(avail->feature,parentnode.value-value);
		currentset->select_raw(avail->feature);
	}
}

template<class RETURNTYPE, typename DIMTYPE, class SUBSET, class CRITERION, class PREDICTOR>
void Search_Branch_And_Bound_Partial_Prediction<RETURNTYPE,DIMTYPE,SUBSET,CRITERION,PREDICTOR>::pre_evaluate_availables()
{
	assert(parent::get_criterion());
	const PSubset &currentset=parent::get_currentset(); assert(currentset);
	Node &parentnode=parent::get_parent_node(); assert(parentnode.type==parent::COMPUTED);

	// pre-evaluate available features before next tree level construction
	PNode avail;
	RETURNTYPE val;
	for(bool got=getFirstAvailable(avail);got;got=getNextAvailable(avail))
	{
		if(_predictor.predict(avail->feature,val)) { //value could be predicted
			avail->value=parentnode.value-val;
			avail->type=parent::PREDICTED;
		} else { //value could not be predicted
			assert(currentset->selected_raw(avail->feature));
			currentset->deselect_raw(avail->feature);
			if(!parent::get_criterion()->evaluate(avail->value,currentset)) throw FST::fst_error("Criterion evaluation failure.");
			avail->type=parent::COMPUTED;
			currentset->select_raw(avail->feature);
			// conditionally update prediction info
			if(parentnode.type==parent::COMPUTED) _predictor.learn(avail->feature,parentnode.value-avail->value);
		}
	}
}

template<class RETURNTYPE, typename DIMTYPE, class SUBSET, class CRITERION, class PREDICTOR>
void Search_Branch_And_Bound_Partial_Prediction<RETURNTYPE,DIMTYPE,SUBSET,CRITERION,PREDICTOR>::post_process_tree_level()
{
	assert(parent::get_criterion());
	const PSubset &currentset=parent::get_currentset();	assert(currentset);
	Node &parentnode=parent::get_parent_node();	assert(parentnode.type==parent::PREDICTED || parentnode.type==parent::COMPUTED);

	// enable to re-evaluate chosen candidate values (e.g., supply missing true criterion values, etc.)
	PNode nod;
	for(bool got=getFirstNode(nod);got;got=getNextNode(nod))
	{
		if(nod->type!=parent::COMPUTED) {
			assert(currentset->selected_raw(nod->feature));
			currentset->deselect_raw(nod->feature);
			if(!parent::get_criterion()->evaluate(nod->value,currentset)) throw FST::fst_error("Criterion evaluation failure.");
			nod->type=parent::COMPUTED;
			currentset->select_raw(nod->feature);
			// conditionally update prediction info
			if(parentnode.type==parent::COMPUTED) _predictor.learn(nod->feature,parentnode.value-nod->value);
		}
	}
}

template<class RETURNTYPE, typename DIMTYPE, class SUBSET, class CRITERION, class PREDICTOR>
bool Search_Branch_And_Bound_Partial_Prediction<RETURNTYPE,DIMTYPE,SUBSET,CRITERION,PREDICTOR>::cut_possible()
{
	if(parent::is_bound_valid()) {
		Node &currentnode=parent::get_current_node(); assert(currentnode.type==parent::COMPUTED);
		if(currentnode.value<=parent::get_bound_value()) { // BBPP ensures that the value must have been COMPUTED by now
			return true;
		}
	}
	return false;
}

} // namespace
#endif // FSTSEARCHBRANCHANDBOUNDPARTIALPREDICTION_H ///:~


/*
Robotic Arm Mouse Control with Inverse Kinematics

Licenced under the MIT License:

Copyright (c) 2015 Pontus Lundström

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

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

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

--
This program controls a servo-driven, turntable-shoulder-elbow topology robotic
arm by mouse with a static Inverse Kinematics (IK) solver. It sends servo PWM
commands over serial connection to an ATmega MCU which does the actual servo
driving.

Written hastily in (ugly) C++11 - downgradeable to C++03 with minimal changes.

Dependencies:
  GLFW 3.x
  boost::asio

On UNIX-like systems, compile with:
$ g++ -std=c++11 arm_control.cpp -o arm_control -lglfw -lboost_system

Example of use:
$ ./arm_control /dev/ttyUSB0

*/

#include <boost/asio.hpp>

#include <GLFW/glfw3.h>

#include <array>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <thread>

#define NDEBUG // Comment/remove to enable debug output

#ifdef NDEBUG
#define DEBUG_OUTPUT(a)
#else
#define DEBUG_OUTPUT(a) a
#endif

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace
{

// Class for mapping angles to servo PWM data
class servo_mapping
{
public:
    servo_mapping(double min_angle, double max_angle,
                  double min_delay, double max_delay) :
        m_min_angle(min_angle),
        m_max_angle(max_angle),
        m_min_delay(min_delay),
        m_max_delay(max_delay)
    {}
    double min_angle() const { return m_min_angle; }
    double max_angle() const { return m_max_angle; }
    double min_delay() const { return m_min_delay; }
    double max_delay() const { return m_max_delay; }
    double delay_from_angle(double angle) const
    {
        // Interpolate linearly & clamp to [m_min_delay, m_max_delay]
        double delay = m_min_delay + (angle - m_min_angle) *
                       (m_max_delay - m_min_delay) / (m_max_angle - m_min_angle);
        if (m_min_delay > m_max_delay)
        {
            return (delay > m_min_delay) ? m_min_delay :
                   ((delay < m_max_delay) ? m_max_delay : delay);
        }
        return (delay < m_min_delay) ? m_min_delay :
               ((delay > m_max_delay) ? m_max_delay : delay);
    }
    double angle_from_delay(double delay) const
    {
        // Interpolate linearly & clamp to [m_min_angle, m_max_angle]
        double angle = m_min_angle + (delay - m_min_delay) *
                       (m_max_angle - m_min_angle) / (m_max_delay - m_min_delay);
        if (m_min_angle > m_max_angle)
        {
            return (angle > m_min_angle) ? m_min_angle :
                   ((angle < m_max_angle) ? m_max_angle : angle);
        }
        return (angle < m_min_angle) ? m_min_angle :
               ((angle > m_max_angle) ? m_max_angle : angle);
    }
private:
    double m_min_angle;
    double m_max_angle;
    double m_min_delay;
    double m_max_delay;
};

// Data structures for IK
struct joint
{
    double d;
    double s;
    double theta;
    double theta_min;
    double theta_max;
};

struct point
{
    double x;
    double y;
    double z;
};

std::array<joint*, 3> arm_joints{nullptr, nullptr, nullptr};
point* wrist_position = nullptr;

// Static IK scheme for turntable-shoulder-elbow topology
// Returns the number of valid solutions (0...2)
int ik(point const& goal, joint& j1, joint& j2, joint& j3)
{
    double epsilon = 1e-10;

    const auto& px = goal.x;
    const auto& py = goal.y;
    const auto& pz = goal.z;

    const auto& s1 = j1.s;

    // Solve for turntable
    double D = sqrt(py*py*(px*px + py*py - s1*s1));
    j1.d = D / py;
    j1.theta = atan2((py*py*s1 - px*D) / (px*px*py + py*py*py),
                     (px*s1 + D) / (px*px + py*py));

    const auto& d1 = j1.d;
    const auto& d2 = j2.d;
    const auto& d3 = j3.d;

    const auto& s2 = j2.s;
    const auto& s3 = j3.s;

    double d12 = d1*d1;
    double d13 = d1*d12;
    double d14 = d12*d12;
    double d22 = d2*d2;
    double d23 = d2*d22;
    double d24 = d22*d22;
    double d32 = d3*d3;
    double d33 = d3*d32;
    double pz2 = pz*pz;
    double pz3 = pz*pz2;
    double s22 = s2*s2;
    double s23 = s2*s22;
    double s32 = s3*s3;
    double s33 = s3*s32;

    double temp1 = d2*pz - d1*s2;
    double temp2 = d12 + pz2;
    double radicand = -temp1*temp1*(d14 - 2*(d22 + d32 - pz2 + s22 + s32)*d12 + d24 + d32*d32 + pz2*pz2 + s22*s22 + s32*s32 - 2*d32*pz2 - 2*d32*s22 - 2*pz2*s22 + 2*d32*s32 - 2*pz2*s32 - 2*s22*s32 - 2*d22*(d32 + pz2 - s22 + s32));

    // Neither division by zero nor negative radicand of square root is allowed
    if (abs(temp1) < epsilon || temp2 < epsilon || radicand < 0)
    {
        if (abs(temp1) < epsilon)
        {
            DEBUG_OUTPUT(std::cerr << "abs(temp1) < epsilon" << std::endl);
        }
        if (temp2 < epsilon)
        {
            DEBUG_OUTPUT(std::cerr << "temp2 < epsilon" << std::endl);
        }
        if (radicand < 0)
        {
            DEBUG_OUTPUT(std::cerr << "radicand < 0" << std::endl);
        }
        // NO SOLUTION!
        return 0;
    }
    else
    {
        double temp3 = sqrt(radicand);

        // Solution 1
        double theta2_1 = atan2(
        -(s22*d14 - 2*d2*pz*s2*d13 + ((pz2 + s22)*d22 + s22*(-d32 + pz2 + s22 - s32))*d12 + (-2*s2*pz3 - 2*s23*pz + 2*s2*s32*pz - 2*d22*s2*pz + 2*d32*s2*pz + temp3)*d2*d1 + (pz*d24 + pz*(-d32 + pz2 + s22 - s32)*d22 + temp3*s2)*pz)/(-temp1*temp2),
        (d2*d13 + pz*s2*d12 + d2*(d22 - d32 + pz2 + s22 - s32)*d1 + pz*s23 - pz*s2*s32 - temp3 + pz3*s2 + d22*pz*s2 - d32*pz*s2)/temp2);

        double theta3_1 = atan2(
        (pz*s3*d24 + pz*s33*d22 - pz3*s3*d22 + pz*s22*s3*d22 + d32*pz*s3*d22 - d3*pz*s23*d2 - d3*pz*s2*s32*d2 - d3*temp3*d2 + d3*pz3*s2*d2 - d33*pz*s2*d2 + d12*pz*(d3*s2 - d2*s3)*d2 - s2*s3*temp3 - d23*d3*pz*s2 + d13*s2*(d2*s3 - d3*s2) + d1*s2*(d3*s2 - d2*s3)*(d22 + d32 - pz2 + s22 + s32))/temp1,
        -(d3*pz*d24 + pz*s2*s3*d23 + d3*pz*(d32 - pz2 + s22 + s32)*d22 + (-s2*pz3 + s23*pz + s2*s32*pz + d32*s2*pz + temp3)*s3*d2 - d12*pz*(d2*d3 + s2*s3)*d2 - d3*s2*temp3 + d13*s2*(d2*d3 + s2*s3) - d1*s2*(d2*d3 + s2*s3)*(d22 + d32 - pz2 + s22 + s32))/temp1);

        // Solution 2
        double theta2_2 = atan2(
        (-s22*d14 + 2*d2*pz*s2*d13-((pz2 + s22)*d22 + s22*(-d32 + pz2 + s22 - s32))*d12 + (2*s2*pz3 + 2*s23*pz - 2*s2*s32*pz + 2*d22*s2*pz - 2*d32*s2*pz + temp3)*d2*d1 + (pz*(d32 - pz2 - s22 + s32)*d22 - d24*pz + temp3*s2)*pz)/(-temp1*temp2),
        (d2*d13 + pz*s2*d12 + d2*(d22 - d32 + pz2 + s22 - s32)*d1 + pz*s23 - pz*s2*s32 + temp3 + pz3*s2 + d22*pz*s2 - d32*pz*s2)/temp2);

        double theta3_2 = atan2(
        (pz*s3*d24 + pz*s33*d22 - pz3*s3*d22 + pz*s22*s3*d22 + d32*pz*s3*d22 - d3*pz*s23*d2 - d3*pz*s2*s32*d2 + temp3*d3*d2 + d3*pz3*s2*d2 - d33*pz*s2*d2 + d12*pz*(d3*s2 - d2*s3)*d2 - d23*d3*pz*s2 + temp3*s2*s3 + d13*s2*(d2*s3 - d3*s2) + d1*s2*(d3*s2 - d2*s3)*(d22 + d32 - pz2 + s22 + s32))/temp1,
        -(d3*pz*d24 + pz*s2*s3*d23 - d3*pz3*d22 + d3*pz*s22*d22 + d3*pz*s32*d22 + d33*pz*d22 + pz*s2*s33*d2 - s3*temp3*d2 + pz*s23*s3*d2 - pz3*s2*s3*d2 + d32*pz*s2*s3*d2 - d12*pz*(d2*d3 + s2*s3)*d2 + temp3*d3*s2 + d13*s2*(d2*d3 + s2*s3) - d1*s2*(d2*d3 + s2*s3)*(d22 + d32 - pz2 + s22 + s32))/temp1);

        // Choose which solution is better
        bool solution1_inbound = true;
        if (theta2_1 < j2.theta_min || theta2_1 > j2.theta_max || theta3_1 < j3.theta_min || theta3_1 > j3.theta_max)
        {
            // Solution 1 is out of bounds
            solution1_inbound = false;
        }
        if (theta2_2 < j2.theta_min || theta2_2 > j2.theta_max || theta3_2 < j3.theta_min || theta3_2 > j3.theta_max)
        {
            // Solution 2 is out of bounds
            DEBUG_OUTPUT(std::cerr << "Solution 2 is out of bounds." << std::endl);
            if (solution1_inbound)
            {
                // Solution 1 is the only valid solution
                j2.theta = theta2_1;
                j3.theta = theta3_1;
                return 1;
            }
            else
            {
                DEBUG_OUTPUT(std::cerr << "Both solutions are out of bounds." << std::endl);
                // NO VALID SOLUTION!
                return 0;
            }
        }
        else
        {
            if (solution1_inbound)
            {
                // Both solutions are valid -> choose one for which theta2 is greater
                if (theta2_1 > theta2_2)
                {
                    j2.theta = theta2_1;
                    j3.theta = theta3_1;
                    return 2;
                }
                else
                {
                    j2.theta = theta2_2;
                    j3.theta = theta3_2;
                    return 2;
                }
            }
            else
            {
                DEBUG_OUTPUT(std::cerr << "Solution 1 is out of bounds." << std::endl);
                // Solution 2 is the only valid solution
                j2.theta = theta2_2;
                j3.theta = theta3_2;
                return 1;
            }
        }
    }
}

// Invokes IK for input point and updates position if solution exists
void move_arm(point const& new_position)
{
    joint new_joints[3];
    for (size_t i = 0; i < 3; ++i)
    {
        new_joints[i] = *arm_joints[i];
    }

    if (ik(new_position, new_joints[0], new_joints[1], new_joints[2]) > 0)
    {
        *wrist_position = new_position;
        for (size_t i = 0; i < 3; ++i)
        {
            *arm_joints[i] = new_joints[i];
        }
    }
}

// Wrist planar position (xy-plane) control callback
void cursor_callback(GLFWwindow*, double cursor_x, double cursor_y)
{
    const double cursor_sensitivity = 0.02;

    static double old_x = 0.0;
    static double old_y = 0.0;

    double delta_x = cursor_x - old_x;
    double delta_y = cursor_y - old_y;

    point new_position{wrist_position->x + cursor_sensitivity * delta_x,
                       wrist_position->y - cursor_sensitivity * delta_y,
                       wrist_position->z};

    // Ignore initial mouse position event
    static bool debounce = true;
    if (!debounce)
    {
        move_arm(new_position);
    }
    else
    {
        debounce = false;
    }

    old_x = cursor_x;
    old_y = cursor_y;
}

// Wrist height (z-axis) control callback
void scroll_callback(GLFWwindow*, double, double offset)
{
    const double scroll_sensitivity = 3.0;

    point new_position{wrist_position->x, wrist_position->y,
                       wrist_position->z + scroll_sensitivity * offset};

    // Ignore initial mouse position event
    static bool debounce = true;
    if (!debounce)
    {
        move_arm(new_position);
    }
    else
    {
        debounce = false;
    }
}

template <typename T>
T deg2rad(T a)
{
    return a*T(M_PI/180);
}

template <typename T>
T rad2deg(T a)
{
    return a*T(180/M_PI);
}

}

int main(int argc, char const *argv[])
{
    try
    {
        // Set fixed float width for stdout
        std::cout.setf(std::ios::fixed, std:: ios::floatfield);
        std::cout.precision(1);

        if (argc < 2)
        {
            std::cout << "Usage:\narm_control <serial_device>" << std::endl;
            return 0;
        }

        auto serial_device = argv[1];
        std::cout << "Begin robot arm control on serial interface '" << serial_device << "'\n\n"
                  << "Can't find the mouse cursor? "
                  << "Don't panic, this is necessary to capture mouse input.\n"
                  << "Press Q or Alt+F4 to kill the program and regain mouse control.\n" << std::endl;

        //=== Arm configuration parameters =========================================

        // Joint angle limits
        const double turntable_min_angle = deg2rad( -90.0);
        const double turntable_max_angle = deg2rad(  90.0);
        const double shoulder_min_angle  = deg2rad( -11.5);
        const double shoulder_max_angle  = deg2rad( 136.0);
        // TODO: measure
        const double elbow_min_angle     = deg2rad(-170.0);
        const double elbow_max_angle     = deg2rad(  10.0);

        std::array<servo_mapping, 3> servos =
            {servo_mapping{turntable_min_angle, turntable_max_angle,  600, 2315},
             servo_mapping{shoulder_min_angle,  shoulder_max_angle,  2100,  650},
             servo_mapping{elbow_min_angle,     elbow_max_angle,     2100,  700}};

        // Joint topology, lengths in millimeters (mm)
        joint j1{0.0,   -23.0, 0.0, turntable_min_angle, turntable_max_angle};
        joint j2{248.0, -25.0, 0.0,  shoulder_min_angle,  shoulder_max_angle};
        joint j3{272.0,   0.0, 0.0,     elbow_min_angle,     elbow_max_angle};
        arm_joints[0] = &j1;
        arm_joints[1] = &j2;
        arm_joints[2] = &j3;

        // Initial wrist position
        point goal{0.0, 100.0, 50.0};
        wrist_position = &goal;
        //=== Arm configuration parameters =========================================

        // Initialize joint angles with IK
        if (ik(goal, j1, j2, j3) == 0)
        {
            std::cerr << "Initial position is not reachable!" << std::endl;
            return -1337;
        }

        // Initialize serial communication
        boost::asio::io_service io;
        boost::asio::serial_port serial(io, serial_device);
        serial.set_option(boost::asio::serial_port::baud_rate(57600));

        // Initialize GLFW
        if (!glfwInit())
            return -1;

        // Create a windowed mode window and its OpenGL context */
        auto window = glfwCreateWindow(320, 240, "Robot arm control", NULL, NULL);
        if (!window)
        {
            glfwTerminate();
            return -1;
        }

        // Make the window's rendering context current
        glfwMakeContextCurrent(window);

        // This hides the cursor and enables infinite cursor movement
        glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED);

        // Setup mouse event callbacks
        glfwSetScrollCallback(window, scroll_callback);
        glfwSetCursorPosCallback(window, cursor_callback);

        // Loop until the user closes the window */
        while (!glfwWindowShouldClose(window))
        {
            glfwSwapBuffers(window);

            // Poll and process events
            glfwPollEvents();

            std::cout << "\rWrist position: [" << goal.x << ", " << goal.y << ", " << goal.z << "]"
                      << "  Joint angles: [" << rad2deg(arm_joints[0]->theta) << ", "
                                             << rad2deg(arm_joints[1]->theta) << ", "
                                             << rad2deg(arm_joints[2]->theta) << "] ";
            for (size_t i = 0; i < 3; ++i)
            {
                DEBUG_OUTPUT(std::cerr << "Send command for servo #" << i << std::endl);
                // Get delay from angle & encode servo index
                uint16_t command = servos[i].delay_from_angle(arm_joints[i]->theta);

                DEBUG_OUTPUT(std::cerr << "\t" << command << std::endl);
                command |= (i << 12);

                // Send servo command
                boost::asio::write(serial, boost::asio::buffer(&command, 2));
            }
            std::cout << "          " << std::flush;

            std::this_thread::sleep_for(std::chrono::milliseconds(5));

            if (glfwGetKey(window, GLFW_KEY_Q) == GLFW_PRESS)
                break;
        }
        std::cout << "\n" << std::endl;
        glfwTerminate();

        return 0;
    }
    catch (boost::system::system_error& e)
    {
        std::cerr << "\n\nSerial communication error: " << e.what() << "\n" << std::endl;
        glfwTerminate();
        return -1;
    }
    catch (std::exception& e)
    {
        std::cerr << "\n\nSomething went wrong: " << e.what() << "\n" << std::endl;
        glfwTerminate();
        return -2;
    }
    catch (...)
    {
        std::cerr << "\n\nUnknown error! :(" << "\n" << std::endl;
        glfwTerminate();
        return -666;
    }
}


/*
 * The Biomechanical ToolKit
 * Copyright (c) 2009-2014, Arnaud Barré
 * 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(s) of the copyright holders 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 OWNER 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 "btkWrenchDirectionAngleFilter.h"

#include <Eigen/Geometry>

namespace btk
{
  /**
   * @class WrenchDirectionAngleFilter btkWrenchDirectionAngleFilter.h
   * @brief Calculate the direction angle of the force vector for each wrench.
   *
   * The direction angle is projected on each plane of the global frame in this order:
   *  - Angle yOz: set in the X component of the output ;
   *  - Angle xOz: set in the Y component of the output ;
   *  - Angle xOy: set in the Z component of the output.
   *
   * The output angles are expressed in degrees and the range is between 0 and 360 degrees.
   * Then a shift from 360 to 0 is possible if the force turns around itself.
   *
   * @ingroup BTKBasicFilters
   */

  /**
   * @typedef WrenchDirectionAngleFilter::Pointer
   * Smart pointer associated with a WrenchDirectionAngleFilter object.
   */

  /**
   * @typedef WrenchDirectionAngleFilter::ConstPointer
   * Smart pointer associated with a const WrenchDirectionAngleFilter object.
   */

  /**
   * @fn static Pointer WrenchDirectionAngleFilter::New();
   * Creates a smart pointer associated with a WrenchDirectionAngleFilter object.
   */

  /**
   * @fn WrenchCollection::Pointer WrenchDirectionAngleFilter::GetInput()
   * Gets the input registered with this process.
   */

  /**
   * @fn void WrenchDirectionAngleFilter::SetInput(Wrench::Pointer input)
   * Sets the input required with this process. This input is transformed in a collection of wrenches with a single force platform.
   */

  /**
   * @fn void WrenchDirectionAngleFilter::SetInput(WrenchCollection::Pointer input)
   * Sets the input required with this process.
   */

  /**
   * @fn PointCollection::Pointer WrenchDirectionAngleFilter::GetOutput()
   * Gets the output created with this process.
   */

  /**
   * Constructor. Sets the number of inputs and outputs to 1.
   */
  WrenchDirectionAngleFilter::WrenchDirectionAngleFilter()
  : ProcessObject()
  {
    this->SetInputNumber(1);
    this->SetOutputNumber(1);
  };

  /**
   * @fn ForcePlatformCollection::Pointer WrenchDirectionAngleFilter::GetInput(int idx)
   * Returns the input at the index @a idx.
   */

  /**
   * @fn WrenchCollection::Pointer WrenchDirectionAngleFilter::GetOutput(int idx)
   * Returns the output at the index @a idx.
   */

  /**
   * Creates a PointCollection:Pointer object and return it as a DataObject::Pointer.
   */
  DataObject::Pointer WrenchDirectionAngleFilter::MakeOutput(int /* idx */)
  {
    return PointCollection::New();
  };

  /**
   * Generates the outputs' data.
   */
  void WrenchDirectionAngleFilter::GenerateData()
  {
    PointCollection::Pointer output = this->GetOutput();
    output->Clear();
    WrenchCollection::Pointer input = this->GetInput();
    const double radToDeg = 180.0 / M_PI;
    if (input)
    {
      for (WrenchCollection::ConstIterator it = input->Begin() ; it != input->End() ; ++it)
      {
        int numFrames = (*it)->GetForce()->GetFrameNumber();
        Point::Pointer dirAngle = Point::New((*it)->GetPosition()->GetLabel() + ".DA", numFrames, Point::Angle);
        for (int i = 0 ; i < numFrames ; ++i)
        {
          if ((*it)->GetPosition()->GetResiduals().coeff(i) >= 0)
          {
            dirAngle->GetValues().coeffRef(i,0) = atan2(-(*it)->GetForce()->GetValues().coeff(i,2), -(*it)->GetForce()->GetValues().coeff(i,1)) * radToDeg + 180.0;
            dirAngle->GetValues().coeffRef(i,1) = atan2(-(*it)->GetForce()->GetValues().coeff(i,2), -(*it)->GetForce()->GetValues().coeff(i,0)) * radToDeg + 180.0;
            dirAngle->GetValues().coeffRef(i,2) = atan2(-(*it)->GetForce()->GetValues().coeff(i,1), -(*it)->GetForce()->GetValues().coeff(i,0)) * radToDeg + 180.0;
          }
          else
          {
            dirAngle->GetResiduals().coeffRef(i) = -1.0;
          }
        }
        output->InsertItem(dirAngle);
      }
    }
  };
};