#include "globalCenterlineExtraction.hpp"
#include "eigenanalysisOfHessian.hpp"
#include "SIPL/Types.hpp"
#ifdef CPP11
#include <unordered_set>
using std::unordered_set;
#else
#include <boost/unordered_set.hpp>
using boost::unordered_set;
#endif
#include <stack>
#include <queue>
#include <algorithm>

using namespace SIPL;

#define MAX(a,b) a > b ? a : b

#define LPOS(a,b,c) (a)+(b)*(size.x)+(c)*(size.x*size.y)
#define POS(pos) pos.x+pos.y*size.x+pos.z*size.x*size.y
#define M(a,b,c) 1-sqrt(pow(T.Fx[a+b*size.x+c*size.x*size.y],2.0f) + pow(T.Fy[a+b*size.x+c*size.x*size.y],2.0f) + pow(T.Fz[a+b*size.x+c*size.x*size.y],2.0f))
#define SQR_MAG(pos) sqrt(pow(T.Fx[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f) + pow(T.Fy[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f) + pow(T.Fz[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f))
#define SQR_MAG_SMALL(pos) sqrt(pow(T.FxSmall[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f) + pow(T.FySmall[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f) + pow(T.FzSmall[pos.x+pos.y*size.x+pos.z*size.x*size.y],2.0f))

class CrossSectionCompare {
private:
	float * dist;
public:
	CrossSectionCompare(float * dist) { this->dist = dist; };
	bool operator() (const CrossSection * a, const CrossSection * b) {
		return dist[a->index] > dist[b->index];
	};
};

std::vector<CrossSection *> createGraph(TubeSegmentation &T, SIPL::int3 size) {
	// Create vector
	std::vector<CrossSection *> sections;
	float threshold = 0.5f;

	// Go through TS.TDF and add all with TDF above threshold
	int counter = 0;
	float thetaLimit = 0.5;
	for(int z = 1; z < size.z-1; z++) {
	for(int y = 1; y < size.y-1; y++) {
	for(int x = 1; x < size.x-1; x++) {
		int3 pos(x,y,z);
		float tdf = T.TDF[POS(pos)];
		if(tdf > threshold) {
			int maxD = std::min(std::max((int)round(T.radius[POS(pos)]), 1), 5);
		    //std::cout << SQR_MAG(pos) << " " << SQR_MAG_SMALL(pos) << std::endl;
			//std::cout << "radius" << TS.radius[POS(pos)] << std::endl;
			//std::cout << "maxD "<< maxD <<std::endl;
			float3 e1 = getTubeDirection(T, pos, size);
			bool invalid = false;
		    for(int a = -maxD; a <= maxD; a++) {
		    for(int b = -maxD; b <= maxD; b++) {
		    for(int c = -maxD; c <= maxD; c++) {
		        if(a == 0 && b == 0 && c == 0)
		            continue;
		        const int3 n = pos + int3(a,b,c);
		        if(!inBounds(n, size))
		        	continue;
		        float3 r(a,b,c);
		        const float dp = e1.dot(r);
		        float3 r_projected = float3(r.x-e1.x*dp,r.y-e1.y*dp,r.z-e1.z*dp);
		        float3 rn = r.normalize();
		        float3 rpn = r_projected.normalize();
		        float theta = acos((double)rn.dot(rpn));
		        //std::cout << "theta: " << theta << std::endl;

		        if((theta < thetaLimit && r.length() < maxD-0.5f)) {
		        	//std::cout << SQR_MAG(n) << std::endl;
		            /*
		        	if(T.radius[POS(pos)]<= 3) {
		            //if(TS.TDF[POS(n)] > TS.TDF[POS(pos)]) {
		            if(SQR_MAG_SMALL(n) < SQR_MAG_SMALL(pos)) {
		                invalid = true;
		                break;
		            }
		            } else {
		            */
		            if(SQR_MAG(n) < SQR_MAG(pos)) {
		            //if(TS.TDF[POS(n)] > TS.TDF[POS(pos)]) {
		                invalid = true;
		                break;
		            }
		            //}
		        }
		    }}}
		    if(!invalid) {
				CrossSection * cs = new CrossSection;
				cs->pos = pos;
				cs->TDF = tdf;
				cs->label = -1;//counter;
				cs->direction = e1;
				counter++;
				sections.push_back(cs);
		    }
		}
	}}}

	// For each cross section c_i
	for(int i = 0; i < sections.size(); i++) {
		CrossSection * c_i = sections[i];
		// For each cross section c_j
		for(int j = 0; j < i; j++) {
			CrossSection * c_j = sections[j];
			// If all criterias are ok: Add c_j as neighbor to c_i
			if(c_i->pos.distance(c_j->pos) < 4 && !(c_i->pos == c_j->pos)) {
				float3 e1_i = c_i->direction;
				float3 e1_j = c_j->direction;
				int3 cint = c_i->pos - c_j->pos;
				float3 c = cint.normalize();

				if(acos((double)fabs(e1_i.dot(e1_j))) > 1.05) // 60 degrees
					continue;

				if(acos((double)fabs(e1_i.dot(c))) > 1.05)
					continue;

				if(acos((double)fabs(e1_j.dot(c))) > 1.05)
					continue;

				int distance = ceil(c_i->pos.distance(c_j->pos));
				float3 direction(c_j->pos.x-c_i->pos.x,c_j->pos.y-c_i->pos.y,c_j->pos.z-c_i->pos.z);
				bool invalid = false;
				for(int i = 0; i < distance; i++) {
					float frac = (float)i/distance;
					float3 n = c_i->pos + frac*direction;
					int3 in(round(n.x),round(n.y),round(n.z));
					//float3 e1 = getTubeDirection(TS, in, size);
					//cost += (1-fabs(a->direction.dot(e1)))+(1-fabs(b->direction.dot(e1)));
					/*
					if(T.intensity[POS(in)] > 0.5f) {
						invalid = true;
						break;
					}
					*/
				}
				//if(invalid)
				//	continue;

				c_i->neighbors.push_back(c_j);
				c_j->neighbors.push_back(c_i);
				//sectionPairs.push_back(c_i);
			}
			// If no pair is found, dont add it
		}
	}

	std::vector<CrossSection *> sectionPairs;
	for(int i = 0; i < sections.size(); i++) {
		CrossSection * c_i = sections[i];
		if(c_i->neighbors.size()>0) {
			sectionPairs.push_back(c_i);
		}
	}

	return sectionPairs;
}

bool segmentCompare(Segment * a, Segment * b) {
	return a->benefit > b->benefit;
}

bool segmentInSegmentation(Segment * s, unordered_set<int> &segmentation, int3 size) {
	bool in = false;
	for(int i = 0; i < s->sections.size(); i++) {
		CrossSection * c = s->sections[i];
		if(segmentation.find(POS(c->pos)) != segmentation.end()) {
			in = true;
			break;
		}
	}
	return in;
}

float calculateBenefit(CrossSection * a, CrossSection * b, TubeSegmentation &TS, int3 size) {
	float benefit = 0.0f;
	int distance = ceil(a->pos.distance(b->pos));
	float3 direction(b->pos.x-a->pos.x,b->pos.y-a->pos.y,b->pos.z-a->pos.z);
	for(int i = 0; i < distance; i++) {
		float frac = (float)i/distance;
		float3 n = a->pos + frac*direction;
		int3 in(round(n.x),round(n.y),round(n.z));
		benefit += (TS.TDF[POS(in)]);
	}

	return benefit;
}

void inverseGradientRegionGrowing(Segment * s, TubeSegmentation &TS, unordered_set<int> &segmentation, int3 size) {
    std::vector<int3> centerpoints;
	for(int c = 0; c < s->sections.size()-1; c++) {
		CrossSection * a = s->sections[c];
		CrossSection * b = s->sections[c+1];
		int distance = ceil(a->pos.distance(b->pos));
		float3 direction(b->pos.x-a->pos.x,b->pos.y-a->pos.y,b->pos.z-a->pos.z);
		for(int i = 0; i < distance; i++) {
			float frac = (float)i/distance;
			float3 n = a->pos + frac*direction;
			int3 in(round(n.x),round(n.y),round(n.z));
			centerpoints.push_back(in);
		}
		segmentation.insert(POS(a->pos));//test
		segmentation.insert(POS(b->pos));//test
	}

	// Dilate the centerline
	std::vector<int3> dilatedCenterline;
	for(int i = 0; i < centerpoints.size(); i++) {
		int3 pos = centerpoints[i];
		for(int a = -1; a < 2; a++) {
		for(int b = -1; b < 2; b++) {
		for(int c = -1; c < 2; c++) {
			int3 n = pos + int3(a,b,c);
			if(inBounds(n, size) && segmentation.find(POS(n)) == segmentation.end()) {
				segmentation.insert(POS(n));
				dilatedCenterline.push_back(n);
			}
		}}}
	}
	/*

	std::queue<int3> queue;
	for(int3 pos : dilatedCenterline) {
		for(int a = -1; a < 2; a++) {
		for(int b = -1; b < 2; b++) {
		for(int c = -1; c < 2; c++) {
			int3 n = pos + int3(a,b,c);
			if(inBounds(n, size) && segmentation.find(POS(n)) == segmentation.end()) {
				queue.push(n);
			}
		}}}
	}

	while(!queue.empty()) {
		int3 X = queue.front();
		float FNXw = SQR_MAG(X);
		queue.pop();
		for(int a = -1; a < 2; a++) {
		for(int b = -1; b < 2; b++) {
		for(int c = -1; c < 2; c++) {
			if(a == 0 && b == 0 && c == 0)
				continue;

			int3 Y = X + int3(a,b,c);
			if(inBounds(Y, size) && segmentation.find(POS(Y)) == segmentation.end()) {

				float3 FNY;
				FNY.x = TS.Fx[POS(Y)];
				FNY.y = TS.Fy[POS(Y)];
				FNY.z = TS.Fz[POS(Y)];
				float FNYw = FNY.length();
				FNY = FNY.normalize();
				if(FNYw > FNXw || FNXw < 0.1f) {

					int3 Z;
					float maxDotProduct = -2.0f;
					for(int a2 = -1; a2 < 2; a2++) {
					for(int b2 = -1; b2 < 2; b2++) {
					for(int c2 = -1; c2 < 2; c2++) {
						if(a2 == 0 && b2 == 0 && c2 == 0)
							continue;
						int3 Zc;
						Zc.x = Y.x+a2;
						Zc.y = Y.y+b2;
						Zc.z = Y.z+c2;
						float3 YZ;
						YZ.x = Zc.x-Y.x;
						YZ.y = Zc.y-Y.y;
						YZ.z = Zc.z-Y.z;
						YZ = YZ.normalize();
						if(FNY.dot(YZ) > maxDotProduct) {
							maxDotProduct = FNY.dot(YZ);
							Z = Zc;
						}
					}}}

					if(Z.x == X.x && Z.y == X.y && Z.z == X.z) {
						segmentation.insert(POS(X));
						queue.push(Y);
					}
				}
			}
		}}}
	}
	*/

}

std::vector<Segment *> createSegments(OpenCL &ocl, TubeSegmentation &TS, std::vector<CrossSection *> &crossSections, SIPL::int3 size) {
	// Create segment vector
	std::vector<Segment *> segments;

	// Do a graph component labeling
	unordered_set<int> visited;
    int labelCounter = 0;
    std::vector<std::vector<CrossSection *> > labels;
	for(int i = 0; i < crossSections.size(); i++) {
		CrossSection * c = crossSections[i];
		// Do a bfs on c
		// Check to see if point has been processed before doing a BFS
		if(visited.find(c->label) != visited.end())
			continue;

        c->label = labelCounter;
        labelCounter++;
        std::vector<CrossSection *> list;

		std::stack<CrossSection *> stack;
		stack.push(c);
		while(!stack.empty()) {
			CrossSection * current = stack.top();
			stack.pop();
			// Check label of neighbors to see if they have been added
			if(current->label != c->label || c->pos == current->pos) {
                list.push_back(current);
				// Change label of neighbors if not
				current->label = c->label;
				// Add neighbors to stack

				for(int j = 0; j < current->neighbors.size(); j++) {
					CrossSection * n = current->neighbors[j];
					if(n->label != c->label)
						stack.push(n);
				}
			}
		}
		visited.insert(c->label);
        labels.push_back(list);
	}

	std::cout << "finished graph component labeling" << std::endl;

	// Do a floyd warshall all pairs shortest path
	int totalSize = crossSections.size();
	std::cout << "number of cross sections is " << totalSize << std::endl;

    // For each label
	for(int i = 0; i < labels.size(); i++) {
		std::vector<CrossSection *> list = labels[i];
        // Do floyd warshall on all pairs
        int totalSize = list.size();
        float * dist = new float[totalSize*totalSize];
        int * pred = new int[totalSize*totalSize];

        for(int u = 0; u < totalSize; u++) {
            CrossSection * U = list[u];
            U->index = u;
        }
        #define DPOS(U, V) V+U*totalSize
        // For each cross section U
        for(int u = 0; u < totalSize; u++) {
            CrossSection * U = list[u];
            // For each cross section V
            for(int v = 0; v < totalSize; v++) {
                dist[DPOS(u,v)] = 99999999;
                pred[DPOS(u,v)] = -1;
            }
            dist[DPOS(U->index,U->index)] = 0;
            for(int j = 0; j < U->neighbors.size(); j++) {
                CrossSection * V = U->neighbors[j];
                // TODO calculate more advanced weight
                dist[DPOS(U->index,V->index)] = ceil(U->pos.distance(V->pos)) - calculateBenefit(U, V, TS, size); //(1-V->TDF);
                pred[DPOS(U->index,V->index)] = U->index;
            }
        }
        for(int t = 0; t < totalSize; t++) {
            //CrossSection * T = crossSections[t];
            //std::cout << "processing t=" << t << std::endl;
            // For each cross section U
            for(int u = 0; u < totalSize; u++) {
                //CrossSection * U = crossSections[u];
                // For each cross section V
                for(int v = 0; v < totalSize; v++) {
                    //CrossSection * V = crossSections[v];
                    float newLength = dist[DPOS(u, t)] + dist[DPOS(t,v)];
                    if(newLength < dist[DPOS(u,v)]) {
                        dist[DPOS(u,v)] = newLength;
                        pred[DPOS(u,v)] = pred[DPOS(t,v)];
                    }
                }
            }
        }

        for(int s = 0; s < list.size(); s++) { // Source
            CrossSection * S = list[s];
            for(int t = 0; t < list.size(); t++) { // Target
                CrossSection * T = list[t];
                if(S->label == T->label && S->index != T->index) {
                    Segment * segment = new Segment;
                    // add all cross sections in segment
                    float benefit = 0.0f;
                    segment->sections.push_back(T);
                    int current = T->index;
                    while(current != S->index) {
                        CrossSection * C = list[current];
                        segment->sections.push_back(C);
                        current = pred[DPOS(S->index,current)];// get predecessor
                        benefit += calculateBenefit(C, list[current], TS, size);
                    }
                    segment->sections.push_back(list[current]);
                    segment->benefit = benefit;
                    segments.push_back(segment);
                }
            }
        }

        delete[] dist;
        delete[] pred;
    }
	std::cout << "finished performing floyd warshall" << std::endl;

	std::cout << "finished creating segments" << std::endl;
	std::cout << "total number of segments is " << segments.size() << std::endl;

	// Sort the segment vector on benefit
	std::sort(segments.begin(), segments.end(), segmentCompare);
	unordered_set<int> segmentation;

	// Go through sorted vector and do a region growing
	std::vector<Segment *> filteredSegments;
	int counter = 0;
	for(int i = 0; i < segments.size(); i++) {
		Segment * s = segments[i];
		if(!segmentInSegmentation(s, segmentation, size)) {
			//std::cout << "adding segment with benefit: " << s->benefit << std::endl;
			// Do region growing and Add all segmented voxels to a set
			inverseGradientRegionGrowing(s, TS, segmentation, size);
			filteredSegments.push_back(s);
			s->index = counter;
			counter++;
		}
	}

	std::cout << "total number of segments after remove overlapping segments " << filteredSegments.size() << std::endl;

	return filteredSegments;
}

int selectRoot(std::vector<Segment *> segments) {
	int root = 0;
	for(int i = 1; i < segments.size(); i++) {
		if(segments[i]->benefit > segments[root]->benefit)
			root = i;
	}
	return root;
}

void DFS(Segment * current, int * ordering, int &counter, unordered_set<int> &visited) {
	if(visited.find(current->index) != visited.end())
		return;
	ordering[counter] = current->index;
	//std::cout << counter << ": " << current->index << std::endl;
	counter++;
	for(int i = 0; i < current->connections.size(); i++) {
		Connection * edge = current->connections[i];
		DFS(edge->target, ordering, counter, visited);
	}
	visited.insert(current->index);
}

int * createDepthFirstOrdering(std::vector<Segment *> segments, int root, int &Ns) {
	int * ordering = new int[segments.size()];
	int counter = 0;

	// Give imdexes to segments
	for(int i = 0; i < segments.size(); i++) {
		segments[i]->index = i;
	}

	unordered_set<int> visited;

	DFS(segments[root], ordering, counter, visited);

	Ns = counter;
	int * reversedOrdering = new int[Ns];
	for(int i = 0; i < Ns; i++) {
		reversedOrdering[i] = ordering[Ns-i-1];
	}

	delete[] ordering;
	return reversedOrdering;
}

class ConnectionComparator {
public:
	bool operator()(Connection * a, Connection *b) const {
		return a->cost > b->cost;
	}
};

std::vector<Segment *> minimumSpanningTree(Segment * root, int3 size) {
	// Need a priority queue on Connection objects based on the cost
	std::priority_queue<Connection *, std::vector<Connection *>, ConnectionComparator> queue;
	std::vector<Segment *> result;
	unordered_set<int> visited;
	result.push_back(root);
	visited.insert(root->index);

	// Add all connections of the root to the queue
	for(int i = 0; i < root->connections.size(); i++) {
		Connection * c = root->connections[i];
		queue.push(c);
	}
	// Remove connections from root
	root->connections = std::vector<Connection *>();

	while(!queue.empty()) {
	// Select minimum connection
	// Check if target is already added
	// if not, add all of its connection to the queue
	// add this connection to the source
	// Add target segment and clear its connections
	// Also add cost to the segment object
		Connection * c = queue.top();
		//std::cout << c->cost << std::endl;
		queue.pop();
		if(visited.find(c->target->index) != visited.end())
			continue;

		for(int i = 0; i < c->target->connections.size(); i++) {
			Connection * cn = c->target->connections[i];
			if(visited.find(cn->target->index) == visited.end())
				queue.push(cn);
		}

		c->source->connections.push_back(c);
		// c->target->connections.clear(); doest his delete the objects?
		c->target->connections = std::vector<Connection *>();
		c->target->cost = c->cost;
		result.push_back(c->target);
		visited.insert(c->target->index);
	}

	return result;
}

std::vector<Segment *> findOptimalSubtree(std::vector<Segment *> segments, int * depthFirstOrdering, int Ns) {

	float * score = new float[Ns]();
	float r = 2.0;

	// Stage 1 bottom up
	for(int j = 0; j < Ns; j++) {
		int mj = depthFirstOrdering[j];
		score[mj] = segments[mj]->benefit - r * segments[mj]->cost;
		/*std::cout << "cross sections: " << segments[mj]->sections.size() << " benefit: "
				<< segments[mj]->benefit << " cost: " << segments[mj]->cost <<
				" children: " << segments[mj]->connections.size() << std::endl;*/
		// For all children of mj
		for(int n = 0; n < segments[mj]->connections.size(); n++) {
			Connection * c = segments[mj]->connections[n];
			int k = c->target->index; //child
			if(score[k] >= 0)
				score[mj] += score[k];
		}
	}

	// Stage 2 top down
	bool * v = new bool[Ns];
	for(int i = 1; i < Ns; i++)
		v[i] = false;
	v[0] = true;

	for(int j = Ns-1; j >= 0; j--) {
		int mj = depthFirstOrdering[j];
		if(v[mj]) {
			// For all children of mj
			for(int n = 0; n < segments[mj]->connections.size(); n++) {
				Connection * c = segments[mj]->connections[n];
				int k = c->target->index; //child
				if(score[k] >= 0)
					v[k] = true;
			}
		}
	}

	delete[] score;

	std::vector<Segment *> finalSegments;
	for(int i = 0; i < Ns; i++) {
		if(v[i]) {
			finalSegments.push_back(segments[i]);

			// for all children, check if they are true in v, if not remove connections
			std::vector<Connection *> connections;
			for(int n = 0; n < segments[i]->connections.size(); n++) {
				Connection * c = segments[i]->connections[n];
				int k = c->target->index; //child
				if(v[k]) {
					// keep connection
					connections.push_back(c);
				} else {
					delete c;
				}
			}
			segments[i]->connections = connections;
		}
	}
	delete[] v;
	return finalSegments;
}

float calculateConnectionCost(CrossSection * a, CrossSection * b, TubeSegmentation &TS, int3 size) {
	float cost = 0.0f;
	int distance = ceil(a->pos.distance(b->pos));
	float3 direction(b->pos.x-a->pos.x,b->pos.y-a->pos.y,b->pos.z-a->pos.z);
	float maxIntensity = -1.0f;
	for(int i = 0; i < distance; i++) {
		float frac = (float)i/distance;
		float3 n = a->pos + frac*direction;
		int3 in(round(n.x),round(n.y),round(n.z));
		cost += /*SQR_MAG(in) +*/ (1.0f-TS.TDF[POS(in)]);
        //float3 e1 = getTubeDirection(TS, in, size);
        //cost += (1-fabs(a->direction.dot(e1)))+(1-fabs(b->direction.dot(e1)));
		if(TS.intensity[POS(in)] > maxIntensity) {
			maxIntensity = TS.intensity[POS(in)];
		}
	}
	/*
	if(maxIntensity > 0.2 && maxIntensity < 0.3) {
		cost = cost*2;
	}
	if(maxIntensity >= 0.3 && maxIntensity < 0.5) {
		cost = cost*4;
	}
	if(maxIntensity >= 0.5) {
		cost = cost*8;
	}
	*/
	return cost;
}

void createConnections(TubeSegmentation &TS, std::vector<Segment *> segments, int3 size) {
	// For all pairs of segments
	for(int k = 0; k < segments.size(); k++) {
		Segment * s_k = segments[k];
		for(int l = 0; l < k; l++) {
			Segment * s_l = segments[l];
			// For each C_k, cross sections in S_k, calculate costs and select the one with least cost
			float bestCost = 999999999.0f;
			CrossSection * c_k_best, * c_l_best;
			bool found = false;
			for(int i = 0; i < s_k->sections.size(); i++){
				CrossSection * c_k = s_k->sections[i];
				for(int j = 0; j < s_l->sections.size(); j++){
					CrossSection * c_l = s_l->sections[j];
					if(c_k->pos.distance(c_l->pos) > 20)
						continue;

					float3 c(c_k->pos.x-c_l->pos.x, c_k->pos.y-c_l->pos.y,c_k->pos.z-c_l->pos.z);
					c = c.normalize();
					if(acos(fabs(c_k->direction.dot(c))) > 1.05f)
						continue;
					if(acos(fabs(c_l->direction.dot(c))) > 1.05f)
						continue;

					/*
                    float rk = TS.radius[POS(c_k->pos)];
                    float rl = TS.radius[POS(c_l->pos)];

                    if(rk > 2 || rl > 2) {
                        if(std::max(rk,rl) / std::min(rk,rl) >= 2)
                            continue;
                    }
                    */

					float cost = calculateConnectionCost(c_k, c_l, TS, size);
					if(cost < bestCost) {
						bestCost = cost;
						c_k_best = c_k;
						c_l_best = c_l;
						found = true;
					}
				}
			}

			// See if they are allowed to connect
			if(found) {
				/*if(bestCost < 2) {
					std::cout << bestCost << std::endl;
					std::cout << "labels: " << c_k_best->label << " " << c_l_best->label << std::endl;
					std::cout << "distance: " << c_k_best->pos.distance(c_l_best->pos) << std::endl;
				}*/
				// If so, create connection object and add to segemnt
				Connection * c = new Connection;
				c->cost = bestCost;
				c->source = s_k;
				c->source_section = c_k_best;
				c->target = s_l;
				c->target_section = c_l_best;
				s_k->connections.push_back(c);
				Connection * c2 = new Connection;
				c2->cost = bestCost;
				c2->source = s_l;
				c2->source_section = c_l_best;
				c2->target = s_k;
				c2->target_section = c_k_best;
				s_l->connections.push_back(c2);
			}
		}
	}
}


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_GROUPS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_GROUPS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/shift_right.hpp>
#include <boost/dispatch/meta/downgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( groups_
                          , (typename A0)
                          , bs::sse4_1_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                         )
  {
    using result = bd::downgrade_t<A0>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0
                                        , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      A0 mask1 = bs::Ratio<A0,0x7FFFFFFF>();
      A0 mask2 = bs::Ratio<A0,0xF0000000>();
      return _mm_packus_epi32( bitwise_and(a0, mask1) | shift_right(bitwise_and(a0, mask2), 1)
                             , bitwise_and(a1, mask1) | shift_right(bitwise_and(a1, mask2), 1)
                             );
    }
  };

} } }

#endif


#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/optional.hpp>
#include <cctbx/sgtbx/rt_mx.h>
#include <cctbx/sgtbx/rt_mx_hash.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/stl/vector_wrapper.h>
#include <scitbx/boost_python/container_conversions.h>
#include <boost_adaptbx/optional_conversions.h>
#include <boost_adaptbx/hash.h>
#include <tbxx/optional_copy.hpp>

namespace cctbx { namespace sgtbx { namespace boost_python {

namespace {

  struct rt_mx_wrappers : boost_adaptbx::py_hashable<rt_mx>
  {
    typedef rt_mx w_t;

    static rt_mx*
    unpickle_init(af::const_ref<int> const& values)
    {
      CCTBX_ASSERT(values.size() == 14);
      rt_mx result(values[12], values[13]);
      for(std::size_t i=0;i<9;i++) {
        result.r().num()[i] = values[i];
      }
      for(std::size_t i=0;i<3;i++) {
        result.t().num()[i] = values[i+9];
      }
      return new rt_mx(result);
    }

    static std::string
    str(w_t const& o) { return o.as_xyz(); }

    static scitbx::vec3<double>
    mul_double(
      w_t const& o, scitbx::vec3<double> const& rhs) { return o * rhs; }

    static vec3_rat
    mul_rat(
      w_t const& o, vec3_rat const& rhs) { return o * rhs; }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_internal_reference<> rir;
      class_<w_t>("rt_mx", no_init)
        .enable_pickling()
        .def("__init__", boost::python::make_constructor(unpickle_init))
        .def(init<optional<int, int> >((
          arg("r_den")=1,
          arg("t_den")=sg_t_den)))
        .def(init<rot_mx const&, tr_vec const&>((arg("r"), arg("t"))))
        .def(init<rot_mx const&, optional<int> >((
          arg("r"),
          arg("t_den")=sg_t_den)))
        .def(init<tr_vec const&, optional<int> >((
          arg("t"),
          arg("r_den")=1)))
        .def(init<parse_string&, optional<const char*, int, int> >((
          arg("symbol"),
          arg("stop_chars")="",
          arg("r_den")=1,
          arg("t_den")=sg_t_den)))
        .def(init<std::string const&, optional<const char*, int, int> >((
          arg("symbol"),
          arg("stop_chars")="",
          arg("r_den")=1,
          arg("t_den")=sg_t_den)))
        .def(init<
          scitbx::mat3<double> const&,
          scitbx::vec3<double> const&,
          optional<int, int> >((
            arg("r"),
            arg("t"),
            arg("r_den")=1,
            arg("t_den")=sg_t_den)))
        .def("r", (rot_mx const&(w_t::*)() const) &w_t::r, rir())
        .def("t", (tr_vec const&(w_t::*)() const) &w_t::t, rir())
        .def("__eq__", &w_t::operator==)
        .def("__ne__", &w_t::operator!=)
        .def("__lt__", &w_t::operator<)
        .def("__le__", &w_t::operator<=)
        .def("__gt__", &w_t::operator>)
        .def("__ge__", &w_t::operator>=)
        .def("__hash__", py_hash)
        .def("is_valid", &w_t::is_valid)
        .def("unit_mx", &w_t::unit_mx)
        .def("is_unit_mx", &w_t::is_unit_mx)
        .def("as_xyz", &w_t::as_xyz, (
          arg("decimal")=false,
          arg("t_first")=false,
          arg("symbol_letters")="xyz",
          arg("separator")=","))
        .def("__str__", str)
        .def("as_int_array", &w_t::as_int_array)
        .def("as_double_array", &w_t::as_double_array)
        .def("new_denominators",
          (rt_mx(w_t::*)(int, int) const) &w_t::new_denominators, (
            arg("r_den"),
            arg("t_den")=0))
        .def("new_denominators",
          (rt_mx(w_t::*)(rt_mx const&) const) &w_t::new_denominators, (
            arg("other")))
        .def("mod_positive", &w_t::mod_positive)
        .def("mod_short", &w_t::mod_short)
        .def("inverse", &w_t::inverse)
        .def("refine_gridding", (sg_vec3(w_t::*)(sg_vec3 const&) const)
          &w_t::refine_gridding, (arg("grid")))
        .def("cancel", &w_t::cancel)
        .def("inverse_cancel", &w_t::inverse_cancel)
        .def("multiply", &w_t::multiply, (arg("rhs")))
        .def("__mul__", mul_double)
        .def("__mul__", mul_rat)
        .def("__call__",
             (scitbx::vec3<double>
              (w_t::*)(scitbx::vec3<double> const &) const)
             &w_t::operator())
        .def("__call__",
             (scitbx::sym_mat3<double>
              (w_t::*)(scitbx::sym_mat3<double> const &) const)
             &w_t::operator())
        .def("__add__", (rt_mx(w_t::*)(sg_vec3 const&) const)&w_t::operator+)
        .def("__add__", (rt_mx(w_t::*)(tr_vec const&) const)&w_t::operator+)
        .def("unit_shifts_minimum_distance", (
          scitbx::vec3<int>(w_t::*)(
            fractional<double> const&,
            fractional<double> const&) const)
              &w_t::unit_shifts_minimum_distance, (
                arg("site_frac_1"), arg("site_frac_2")))
        .def("add_unit_shifts_minimum_distance", (
          rt_mx(w_t::*)(
            fractional<double> const&,
            fractional<double> const&) const)
              &w_t::add_unit_shifts_minimum_distance, (
                arg("site_frac_1"), arg("site_frac_2")))
      ;

      scitbx::stl::boost_python::vector_wrapper<rt_mx>::wrap(
        "stl_vector_rt_mx");
      {
        using namespace scitbx::boost_python::container_conversions;
        tuple_mapping_variable_capacity<af::shared<rt_mx> >();
      }
      {
        using boost_adaptbx::optional_conversions::to_and_from_python;
        // used in cctbx/geometry_restraints
        to_and_from_python<
          tbxx::optional_container<
            af::shared<cctbx::sgtbx::rt_mx> > >();
        to_and_from_python<
          tbxx::optional_copy<cctbx::sgtbx::rt_mx> >();
      }
    }
  };

  struct translation_part_info_wrappers
  {
    typedef translation_part_info w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("translation_part_info", no_init)
        .def(init<rt_mx const&>((arg("s"))))
        .def("intrinsic_part", &w_t::intrinsic_part, ccr())
        .def("location_part", &w_t::location_part, ccr())
        .def("origin_shift", &w_t::origin_shift, ccr())
      ;
    }
  };

} // namespace <anoymous>

  void wrap_rt_mx()
  {
    rt_mx_wrappers::wrap();
    translation_part_info_wrappers::wrap();
  }

}}} // namespace cctbx::sgtbx::boost_python


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

//
// This tests two things: that multiprecision::number meets our
// conceptual requirements, and that we can instantiate
// all our distributions and special functions on this type.
//
#define BOOST_MATH_ASSERT_UNDEFINED_POLICY false
#define TEST_GROUP_4

#ifdef _MSC_VER
#  pragma warning(disable:4800)
#  pragma warning(disable:4512)
#  pragma warning(disable:4127)
#  pragma warning(disable:4512)
#  pragma warning(disable:4503) // decorated name length exceeded, name was truncated
#endif

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/concepts/real_type_concept.hpp>
#include "compile_test/instantiate.hpp"

using namespace boost::multiprecision;

typedef number<cpp_dec_float<50>, et_on> test_type;

#if !(defined(CI_SUPPRESS_KNOWN_ISSUES) && defined(BOOST_GCC))

void foo()
{
   instantiate(test_type());
}

#endif

int main()
{
   //BOOST_CONCEPT_ASSERT((boost::math::concepts::RealTypeConcept<test_type>));
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE clang arithmetic

#include <string>
#include <iostream>
#include <boost/test/included/unit_test.hpp>

#include "../src/info.hpp"
#include "../src/clang_types.hpp"
#include "../src/clang_arithmetic_backend_interface.hpp"

using namespace std;

BOOST_AUTO_TEST_CASE( dummy_test )
{
  BOOST_TEST ( true );
}

//
// Test arithmetic backend clang wrapper
//

#ifdef __HAS_SEAL__

BOOST_AUTO_TEST_CASE( test_arithmetic_backend_clang_interface )
{
  BOOST_TEST_MESSAGE ( "Test arithmetic backend Clang interface..." );

  string algorithm = "bfv";
  string backend_name = "seal";

  // create the backends
  ArithmeticBackendWrapper wrapper = CreateArithmeticBackend((char *)backend_name.c_str());
  ArithmeticBackendWrapper encrypt_wrapper = CreateArithmeticBackend((char *)backend_name.c_str());

  // generate basic key pair
  SetArithmeticBackendAlgorithm(wrapper, (char *)algorithm.c_str());
  InitArithmeticBackend(wrapper);

  GenerateArithmeticBackendKeys(wrapper);
  Blob public_key = GetArithmeticBackendPublicKey(wrapper);
  Blob secret_key = GetArithmeticBackendSecretKey(wrapper);

  // test the encryption
  SetArithmeticBackendAlgorithm(encrypt_wrapper, (char *)algorithm.c_str());
  InitArithmeticBackend(encrypt_wrapper);

  SetArithmeticBackendPublicKey(encrypt_wrapper, public_key);
  ArithmeticBackendEncrypt(encrypt_wrapper, 10000);

  // do the basic numeric operations
  ArithmeticBackendAdd(encrypt_wrapper, 25);
  ArithmeticBackendNegate(encrypt_wrapper);
  ArithmeticBackendMultiply(encrypt_wrapper, 1000);

  // check the result
  SetArithmeticBackendSecretKey(encrypt_wrapper, secret_key);
  BOOST_TEST ( ArithmeticBackendDecrypt(encrypt_wrapper) == -10025000 );

  // clean the result
  FreeArithmeticBackend(wrapper);
  FreeArithmeticBackend(encrypt_wrapper);
}

#endif


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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.
 */

#include "lut_utils.hpp"

#include <classical/lut/lut_coi.hpp>
#include <core/utils/bitset_utils.hpp>
#include <core/utils/range_utils.hpp>

#include <boost/assign/std/vector.hpp>
#include <boost/format.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

int compute_depth_rec( const lut_graph& graph, const lut_vertex_t& node, std::vector<int>& depths )
{
  if ( depths[node] >= 0 ) return depths[node];

  if ( graph.is_input(node) )
  {
    return depths[node] = 0;
  }
  else
  {
    int depth = 0;
    for ( const auto& c : graph.children( node ) )
    {
      depth = std::max( compute_depth_rec( graph, c, depths ), depth );
    }
    return depths[node] = ( depth + 1 );
  }
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

unsigned lut_compute_depth( const lut_graph& graph )
{
  std::vector<int> depths( graph.size(), -1 );

  int depth = -1;

  for ( const auto& output : graph.outputs() )
  {
    depth = std::max( compute_depth_rec( graph, output.first, depths ), depth );
  }

  return static_cast<unsigned>( depth );
}

void lut_print_stats( const lut_graph& graph, std::ostream& os )
{
  auto name = graph.name();
  if ( name.empty() )
  {
    name = "(unnamed)";
  }

  os << boost::format( "[i] %20s: i/o = %7d / %7d gates = %7d lev = %4d" ) % name % graph.inputs().size() % graph.outputs().size() % graph.num_gates() % lut_compute_depth( graph );

  os << std::endl;
}

std::vector<unsigned> lut_compute_levels( const lut_graph& graph )
{
  std::vector<unsigned> levels( graph.size() );
  for ( const auto& n : graph.topological_nodes() )
  {
    if ( graph.is_input( n ) )
    {
      levels[n] = 0u;
    }
    else
    {
      auto level = 0u;
      for ( const auto& c : graph.children( n ) )
      {
        level = std::max( level, levels[c] );
      }
      levels[n] = level + 1u;
    }
  }
  return levels;
}

std::vector<boost::dynamic_bitset<>> lut_compute_sections( const lut_graph& graph )
{
  using namespace boost::assign;

  std::vector< boost::dynamic_bitset<> > sections;
  for ( const auto& o : graph.outputs() )
  {
    sections += lut_compute_coi_as_bitset( graph, o.first );
  }
  sections = transpose( sections );

  std::vector< boost::dynamic_bitset<> > node_to_section;
  node_to_section.resize( graph.nodes().size() );
  for ( const auto& s : index( sections ) )
  {
    node_to_section[ s.index + 2u ] = s.value;
  }
  return node_to_section;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "obs-precomp.h"  // Precompiled headers

#include <mrpt/math/ops_vectors.h>  // << of std::vector()
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/serialization/CArchive.h>
#include <Eigen/Dense>
#include <iostream>
#if MRPT_HAS_MATLAB
#include <mexplus/mxarray.h>
#endif

using namespace mrpt::obs;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::img;

// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CObservationImage, CObservation, mrpt::obs)

uint8_t CObservationImage::serializeGetVersion() const { return 4; }
void CObservationImage::serializeTo(mrpt::serialization::CArchive& out) const
{
	out << cameraPose << cameraParams << image << timestamp << sensorLabel;
}

void CObservationImage::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		case 3:
		case 4:
		{
			in >> cameraPose;

			if (version >= 4)
			{
				in >> cameraParams;
			}
			else
			{
				CMatrixF intrinsicParams, distortionParams;
				in >> distortionParams >> intrinsicParams;

				if (distortionParams.rows() == 1 &&
					distortionParams.cols() == 5)
				{
					CMatrixDouble15 p;
					p = distortionParams.cast_double();
					cameraParams.setDistortionParamsVector(p);
				}
				else
					cameraParams.dist.fill(0);

				cameraParams.intrinsicParams = mrpt::math::CMatrixDouble33(
					intrinsicParams.block<3, 3>(0, 0).cast<double>());
			}

			in >> image;

			if (version >= 1) in >> timestamp;

			if (version >= 2)
			{
				if (version < 4) in >> cameraParams.focalLengthMeters;
			}
			else
				cameraParams.focalLengthMeters = 0.002;

			if (version >= 3)
				in >> sensorLabel;
			else
				sensorLabel = "";
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

/*---------------------------------------------------------------
  Implements the writing to a mxArray for Matlab
 ---------------------------------------------------------------*/
#if MRPT_HAS_MATLAB
// Add to implement mexplus::from template specialization
IMPLEMENTS_MEXPLUS_FROM(mrpt::obs::CObservationImage)
#endif

mxArray* CObservationImage::writeToMatlab() const
{
#if MRPT_HAS_MATLAB
	const char* fields[] = {"class", "ts",   "sensorLabel",
							"image", "pose", "params"};
	mexplus::MxArray obs_struct(
		mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));

	obs_struct.set("class", this->GetRuntimeClass()->className);
	obs_struct.set("ts", mrpt::Clock::toDouble(timestamp));
	obs_struct.set("sensorLabel", this->sensorLabel);
	obs_struct.set("image", this->image);
	obs_struct.set("pose", this->cameraPose);
	obs_struct.set("params", this->cameraParams);
	return obs_struct.release();
#else
	THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
#endif
}

void CObservationImage::getUndistortedImage(CImage& out_img) const
{
	image.undistort(out_img, cameraParams);
}

void CObservationImage::getDescriptionAsText(std::ostream& o) const
{
	using namespace std;
	CObservation::getDescriptionAsText(o);

	o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
		 "base:\n";
	o << cameraPose.getHomogeneousMatrixVal<CMatrixDouble44>() << cameraPose
	  << "\n";

	o << format(
		"Focal length: %.03f mm\n", cameraParams.focalLengthMeters * 1000);

	o << "Intrinsic parameters matrix for the camera:"
	  << "\n"
	  << cameraParams.intrinsicParams.inMatlabFormat() << "\n"
	  << cameraParams.intrinsicParams << "\n";

	o << "Distorsion parameters for the camera: "
	  << cameraParams.getDistortionParamsAsVector() << "\n";

	if (image.isExternallyStored())
		o << " Image is stored externally in file: "
		  << image.getExternalStorageFile() << "\n";

	if (!image.isEmpty())
	{
		o << format(
			" Image size: %ux%u pixels\n", (unsigned int)image.getWidth(),
			(unsigned int)image.getHeight());

		o << " Channels order: " << image.getChannelsOrder() << "\n";

		o << format(
			" Rows are stored in top-bottom order: %s\n",
			image.isOriginTopLeft() ? "YES" : "NO");
	}
}


//
// $Id: TruncatedLorentzianTest.cpp 4129 2012-11-20 00:05:37Z chambm $
//
//
// Original author: Darren Kessner <darren@proteowizard.org>
//
// Copyright 2006 Louis Warschaw Prostate Cancer Center
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// 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 "TruncatedLorentzian.hpp"
#include "DerivativeTest.hpp"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
namespace ublas = boost::numeric::ublas;

#include "pwiz/utility/misc/Std.hpp"
#include <cstring>

using namespace pwiz::util;
using namespace pwiz::frequency;

ostream* os_ = 0;

int main(int argc, char* argv[])
{
    if (argc>1 && !strcmp(argv[1],"-v")) os_ = &cout;
    if (os_) *os_ << "TruncatedLorentzianTest\n";

    if (os_) *os_ << setprecision(8);

    ublas::vector<double> p(4);

    p(TruncatedLorentzian::AlphaR) = 1;
    p(TruncatedLorentzian::AlphaI) = 5;
    p(TruncatedLorentzian::Tau) = 2;
    p(TruncatedLorentzian::F0) = 0;
    double T = 1;

/*
    p(TruncatedLorentzian::AlphaR) = 5e6;
    p(TruncatedLorentzian::AlphaI) = 0;
    p(TruncatedLorentzian::Tau) = 1;
    p(TruncatedLorentzian::F0) = 159455;
    double T = .384;
*/

    TruncatedLorentzian L(T);

//    L.outputSamples(cout, p);

/*
    for (int i=0; i<10; i++)
        DerivativeTest::testDerivatives(L, i, p, 1e-5, 1e-3);
*/

    if (os_) *os_ << "L(0): " << L(0,p) << endl;

    return 0;
}


#ifndef TEST_UNIT_MATH_FWD_SCAL_FUN_NAN_UTIL_HPP
#define TEST_UNIT_MATH_FWD_SCAL_FUN_NAN_UTIL_HPP

#include <stan/math/fwd/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/tools/promotion.hpp>
#include <Eigen/Dense>
#include <limits>

template <typename F>
void test_nan_fd(const F& f, const double& arg1, const bool& throws) {
  stan::math::fvar<double> arg1_v = arg1;
  arg1_v.d_ = 1.0;
  if (throws) {
    EXPECT_THROW(f(arg1_v), std::domain_error);
  } else {
    EXPECT_TRUE(stan::math::is_nan(f(arg1_v).val_));
  }
}

template <typename F>
void test_nan_ffd(const F& f, const double& arg1, const bool& throws) {
  using stan::math::fvar;
  fvar<fvar<double> > arg1_v(fvar<double>(arg1, 1.0), fvar<double>(1.0, 1.0));

  if (throws) {
    EXPECT_THROW(f(arg1_v), std::domain_error);
  } else {
    EXPECT_TRUE(stan::math::is_nan(f(arg1_v).val_.val_));
  }
}

template <typename F>
void test_nan_fwd(const F& f, const bool& throws) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  test_nan_fd(f, nan, throws);
  test_nan_ffd(f, nan, throws);
}

template <typename F>
void test_nan_fd(const F& f, const double& arg1, const double& arg2,
                 const bool& throws) {
  using stan::math::fvar;
  fvar<double> arg1_v(arg1, 1.0);
  fvar<double> arg2_v(arg2, 1.0);

  std::ostringstream fail_msg1;
  std::ostringstream fail_msg2;
  std::ostringstream fail_msg3;
  fail_msg1
      << "Failed for fvar<double>, fvar<double> version with first argument "
      << arg1_v << " and second argument " << arg2_v;
  fail_msg2 << "Failed for fvar<double>, double version with first argument "
            << arg1_v << " and second argument " << arg2;
  fail_msg3 << "Failed for double, fvar<double> version with first argument "
            << arg1 << " and second argument " << arg2_v;

  if (throws) {
    EXPECT_THROW(f(arg1_v, arg2_v), std::domain_error) << fail_msg1.str();
    EXPECT_THROW(f(arg1_v, arg2), std::domain_error) << fail_msg2.str();
    EXPECT_THROW(f(arg1, arg2_v), std::domain_error) << fail_msg3.str();
  } else {
    EXPECT_TRUE(stan::math::is_nan(f(arg1_v, arg2_v).val_)) << fail_msg1.str();
    EXPECT_TRUE(stan::math::is_nan(f(arg1_v, arg2).val_)) << fail_msg2.str();
    EXPECT_TRUE(stan::math::is_nan(f(arg1, arg2_v).val_)) << fail_msg3.str();
  }
}

template <typename F>
void test_nan_ffd(const F& f, const double& arg1, const double& arg2,
                  const bool& throws) {
  using stan::math::fvar;
  fvar<fvar<double> > arg1_v = arg1;
  arg1_v.val_.d_ = 1.0;
  arg1_v.d_.val_ = 1.0;
  arg1_v.d_.d_ = 1.0;
  fvar<fvar<double> > arg2_v = arg2;
  arg2_v.val_.d_ = 1.0;
  arg2_v.d_.val_ = 1.0;
  arg2_v.d_.d_ = 1.0;

  std::ostringstream fail_msg1;
  std::ostringstream fail_msg2;
  std::ostringstream fail_msg3;
  fail_msg1 << "Failed for fvar<fvar<double>>, fvar<fvar<double>> "
            << "version with first argument " << arg1_v
            << " and second argument " << arg2_v;
  fail_msg2 << "Failed for fvar<fvar<double>>, double  "
            << "version with first argument " << arg1_v
            << " and second argument " << arg2;
  fail_msg3 << "Failed for double, fvar<fvar<double>> "
            << "version with first argument " << arg1 << " and second argument "
            << arg2_v;

  if (throws) {
    EXPECT_THROW(f(arg1_v, arg2_v), std::domain_error) << fail_msg1.str();
    EXPECT_THROW(f(arg1_v, arg2), std::domain_error) << fail_msg2.str();
    EXPECT_THROW(f(arg1, arg2_v), std::domain_error) << fail_msg3.str();
  } else {
    EXPECT_TRUE(stan::math::is_nan(f(arg1_v, arg2_v).val_.val_))
        << fail_msg1.str();
    EXPECT_TRUE(stan::math::is_nan(f(arg1_v, arg2).val_.val_))
        << fail_msg2.str();
    EXPECT_TRUE(stan::math::is_nan(f(arg1, arg2_v).val_.val_))
        << fail_msg3.str();
  }
}

template <typename F>
void test_nan_fwd(const F& f, const double& arg1, const double& arg2,
                  const bool& throws) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  Eigen::VectorXd arg1_vec(3);
  Eigen::VectorXd arg2_vec(3);
  arg1_vec << nan, arg1, nan;
  arg2_vec << arg2, nan, nan;
  for (int i = 0; i < arg1_vec.size(); i++) {
    test_nan_fd(f, arg1_vec(i), arg2_vec(i), throws);
    test_nan_ffd(f, arg1_vec(i), arg2_vec(i), throws);
  }
}

template <typename F>
void test_nan_fd(const F& f, const double& arg1, const double& arg2,
                 const double& arg3, const bool& throws) {
  using boost::math::isnan;
  using stan::math::fvar;
  fvar<double> arg1_v(arg1, 1.0);
  fvar<double> arg2_v(arg2, 1.0);
  fvar<double> arg3_v(arg3, 1.0);

  std::ostringstream fail_msg;
  fail_msg << "Failed for "
           << "first argument " << arg1_v << " second argument " << arg2_v
           << " and third argument " << arg3_v;

  if (throws) {
    EXPECT_THROW(f(arg1, arg2_v, arg3_v), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1_v, arg2, arg3_v), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1_v, arg2_v, arg3), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1_v, arg2, arg3), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1, arg2_v, arg3), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1, arg2, arg3_v), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1_v, arg2_v, arg3_v), std::domain_error)
        << fail_msg.str();
  } else {
    EXPECT_TRUE(isnan(f(arg1_v, arg2_v, arg3_v).val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1, arg2_v, arg3_v).val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1_v, arg2, arg3_v).val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1_v, arg2_v, arg3).val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1, arg2, arg3_v).val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1, arg2_v, arg3).val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1_v, arg2, arg3).val_)) << fail_msg.str();
  }
}
template <typename F>
void test_nan_ffd(const F& f, const double& arg1, const double& arg2,
                  const double& arg3, const bool& throws) {
  using boost::math::isnan;
  using stan::math::fvar;
  fvar<fvar<double> > arg1_v(arg1, 1.0);
  fvar<fvar<double> > arg2_v(arg2, 1.0);
  fvar<fvar<double> > arg3_v(arg3, 1.0);
  arg1_v.val_.d_ = 1.0;
  arg1_v.d_.d_ = 1.0;
  arg2_v.val_.d_ = 1.0;
  arg2_v.d_.d_ = 1.0;
  arg3_v.val_.d_ = 1.0;
  arg3_v.d_.d_ = 1.0;

  std::ostringstream fail_msg;
  fail_msg << "Failed for "
           << "first argument " << arg1_v << " second argument " << arg2_v
           << " and third argument " << arg3_v;

  if (throws) {
    EXPECT_THROW(f(arg1, arg2_v, arg3_v), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1_v, arg2, arg3_v), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1_v, arg2_v, arg3), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1_v, arg2, arg3), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1, arg2_v, arg3), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1, arg2, arg3_v), std::domain_error) << fail_msg.str();
    EXPECT_THROW(f(arg1_v, arg2_v, arg3_v), std::domain_error)
        << fail_msg.str();
  } else {
    EXPECT_TRUE(isnan(f(arg1_v, arg2_v, arg3_v).val_.val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1, arg2_v, arg3_v).val_.val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1_v, arg2, arg3_v).val_.val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1_v, arg2_v, arg3).val_.val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1, arg2, arg3_v).val_.val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1, arg2_v, arg3).val_.val_)) << fail_msg.str();
    EXPECT_TRUE(isnan(f(arg1_v, arg2, arg3).val_.val_)) << fail_msg.str();
  }
}
template <typename F>
void test_nan_fwd(const F& f, const double& arg1, const double& arg2,
                  const double& arg3, const bool& throws) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  Eigen::VectorXd arg1_vec(7);
  Eigen::VectorXd arg2_vec(7);
  Eigen::VectorXd arg3_vec(7);
  arg1_vec << nan, arg1, arg1, nan, nan, arg1, nan;
  arg2_vec << arg2, nan, arg2, nan, arg2, nan, nan;
  arg3_vec << arg3, arg3, nan, arg3, nan, nan, nan;

  for (int i = 0; i < arg1_vec.size(); i++) {
    test_nan_fd(f, arg1_vec(i), arg2_vec(i), arg3_vec(i), throws);
    test_nan_ffd(f, arg1_vec(i), arg2_vec(i), arg3_vec(i), throws);
  }
}

#endif


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/hundred.hpp>
#include <boost/simd/as.hpp>
#include <stf.hpp>

STF_CASE_TPL( "Check hundred behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::hundred;
  using boost::simd::Hundred;

  STF_TYPE_IS(decltype(Hundred<T>()), T);
  STF_EQUAL(Hundred<T>(), T(100));
  STF_EQUAL(hundred( as(T{}) ),T(100));
}

STF_CASE_TPL( "Check hundred behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::hundred;
  using boost::simd::Hundred;

  STF_TYPE_IS(decltype(Hundred<T>()), T);
  STF_IEEE_EQUAL(Hundred<T>(), T(100));
  STF_IEEE_EQUAL(hundred( as(T{}) ), T(100));
}


/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin                               *
* Department: Oden Institute and Institute for Cellular and Molecular Biology  *
* PI: Edward Marcotte                                                          *
* Project: Protein Fluorosequencing                                            *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "k-best.h"

// Standard C++ library headers:
#include <cfloat>
#include <string>
#include <vector>

namespace whatprot {
namespace kd_tree {

namespace {
using boost::unit_test::tolerance;
using std::string;
using std::vector;
const double TOL = 0.000000001;
}  // namespace

// Class we can use as a template parameter for E. Note that we are missing the
// [] operator. This is OK. It's not needed by KBest<E>.
class Str {
public:
    Str(string s) : s(s), hits(1) {}
    Str(string s, int hits) : s(s), hits(hits) {}
    int hits;
    string s;
};

BOOST_AUTO_TEST_SUITE(kd_tree_suite)
BOOST_AUTO_TEST_SUITE(k_best_suite)

BOOST_AUTO_TEST_CASE(constructor_test, *tolerance(TOL)) {
    KBest<Str> kb(3);
    BOOST_TEST(kb.k == 3);
    BOOST_TEST(kb.kth_dist_sq == DBL_MAX);
    BOOST_TEST(kb.hits == 0);
}

BOOST_AUTO_TEST_CASE(empty_test, *tolerance(TOL)) {
    KBest<Str> kb(3);
    vector<Str*> v;
    vector<double> dists_sq;
    kb.fill(&v, &dists_sq);
    BOOST_REQUIRE(v.size() == 0);
    BOOST_TEST(dists_sq.size() == 0);
}

BOOST_AUTO_TEST_CASE(under_capacity_test, *tolerance(TOL)) {
    KBest<Str> kb(3);
    Str s_one("one point zero");
    kb.insert(1.0, &s_one);
    BOOST_TEST(kb.kth_dist_sq == DBL_MAX);
    BOOST_TEST(kb.hits == 1);
    vector<Str*> v;
    vector<double> dists_sq;
    kb.fill(&v, &dists_sq);
    BOOST_REQUIRE(v.size() == 1);
    BOOST_TEST(v[0]->s == "one point zero");
    BOOST_TEST(v[0]->hits == 1);
    BOOST_TEST(dists_sq[0] == 1.0);
}

BOOST_AUTO_TEST_CASE(under_capacity_hits_gt1_test, *tolerance(TOL)) {
    KBest<Str> kb(3);
    Str s_one("one point zero", 2);
    kb.insert(1.0, &s_one);
    BOOST_TEST(kb.kth_dist_sq == DBL_MAX);
    BOOST_TEST(kb.hits == 2);
    vector<Str*> v;
    vector<double> dists_sq;
    kb.fill(&v, &dists_sq);
    BOOST_REQUIRE(v.size() == 1);
    BOOST_TEST(v[0]->s == "one point zero");
    BOOST_TEST(v[0]->hits == 2);
    BOOST_TEST(dists_sq[0] == 1.0);
}

BOOST_AUTO_TEST_CASE(at_capacity_test, *tolerance(TOL)) {
    KBest<Str> kb(3);
    Str s_one("one point zero");
    kb.insert(1.0, &s_one);
    Str s_three("three point zero");
    kb.insert(3.0, &s_three);
    Str s_two("two point zero");
    kb.insert(2.0, &s_two);
    BOOST_TEST(kb.kth_dist_sq == 3.0);
    BOOST_TEST(kb.hits == 3);
    vector<Str*> v;
    vector<double> dists_sq;
    kb.fill(&v, &dists_sq);
    BOOST_REQUIRE(v.size() == 3);
    BOOST_TEST(dists_sq.size() == 3);
    BOOST_TEST(v[0]->s == "three point zero");
    BOOST_TEST(v[0]->hits == 1);
    BOOST_TEST(dists_sq[0] == 3.0);
    BOOST_TEST(v[1]->s == "two point zero");
    BOOST_TEST(v[1]->hits == 1);
    BOOST_TEST(dists_sq[1] == 2.0);
    BOOST_TEST(v[2]->s == "one point zero");
    BOOST_TEST(v[2]->hits == 1);
    BOOST_TEST(dists_sq[2] == 1.0);
}

BOOST_AUTO_TEST_CASE(at_capacity_hits_gt1_test, *tolerance(TOL)) {
    KBest<Str> kb(3);
    Str s_one("one point zero", 1);
    kb.insert(1.0, &s_one);
    Str s_two("two point zero", 2);
    kb.insert(2.0, &s_two);
    BOOST_TEST(kb.kth_dist_sq == 2.0);
    BOOST_TEST(kb.hits == 3);
    vector<Str*> v;
    vector<double> dists_sq;
    kb.fill(&v, &dists_sq);
    BOOST_REQUIRE(v.size() == 2);
    BOOST_TEST(dists_sq.size() == 2);
    BOOST_TEST(v[0]->s == "two point zero");
    BOOST_TEST(v[0]->hits == 2);
    BOOST_TEST(dists_sq[0] == 2.0);
    BOOST_TEST(v[1]->s == "one point zero");
    BOOST_TEST(v[1]->hits == 1);
    BOOST_TEST(dists_sq[1] == 1.0);
}

BOOST_AUTO_TEST_CASE(over_capacity_test, *tolerance(TOL)) {
    KBest<Str> kb(3);
    Str s_one("one point zero");
    kb.insert(1.0, &s_one);
    Str s_three("three point zero");
    kb.insert(3.0, &s_three);
    Str s_two("two point zero");
    kb.insert(2.0, &s_two);
    Str s_first_extra("four point four");
    kb.insert(4.4, &s_first_extra);
    Str s_second_extra("two point two");
    kb.insert(2.2, &s_second_extra);
    BOOST_TEST(kb.kth_dist_sq == 2.2);
    BOOST_TEST(kb.hits == 3);
    vector<Str*> v;
    vector<double> dists_sq;
    kb.fill(&v, &dists_sq);
    BOOST_REQUIRE(v.size() == 3);
    BOOST_TEST(dists_sq.size() == 3);
    BOOST_TEST(v[0]->s == "two point two");
    BOOST_TEST(v[0]->hits == 1);
    BOOST_TEST(dists_sq[0] == 2.2);
    BOOST_TEST(v[1]->s == "two point zero");
    BOOST_TEST(v[1]->hits == 1);
    BOOST_TEST(dists_sq[1] == 2.0);
    BOOST_TEST(v[2]->s == "one point zero");
    BOOST_TEST(v[2]->hits == 1);
    BOOST_TEST(dists_sq[2] == 1.0);
}

BOOST_AUTO_TEST_CASE(over_capacity_hits_gt1_test, *tolerance(TOL)) {
    KBest<Str> kb(3);
    Str s_one("one point zero", 2);
    kb.insert(1.0, &s_one);
    Str s_three("three point zero", 2);
    kb.insert(3.0, &s_three);
    Str s_two("two point zero", 2);
    kb.insert(2.0, &s_two);
    BOOST_TEST(kb.kth_dist_sq == 2.0);
    BOOST_TEST(kb.hits == 4);
    vector<Str*> v;
    vector<double> dists_sq;
    kb.fill(&v, &dists_sq);
    BOOST_REQUIRE(v.size() == 2);
    BOOST_TEST(v[0]->s == "two point zero");
    BOOST_TEST(v[0]->hits == 2);
    BOOST_TEST(dists_sq[0] == 2.0);
    BOOST_TEST(v[1]->s == "one point zero");
    BOOST_TEST(v[1]->hits == 2);
    BOOST_TEST(dists_sq[1] == 1.0);
}

BOOST_AUTO_TEST_SUITE_END()  // k_best_suite
BOOST_AUTO_TEST_SUITE_END()  // kd_tree_suite

}  // namespace kd_tree
}  // namespace whatprot


/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * 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 Willow Garage, Inc. 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.
 */

#ifndef TF2_SENSOR_MSGS_HPP
#define TF2_SENSOR_MSGS_HPP

#include <tf2/convert.h>
#include <tf2/time.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <tf2_ros/buffer_interface.h>

namespace tf2
{

/********************/
/** PointCloud2    **/
/********************/

// method to extract timestamp from object
template <>
inline
tf2::TimePoint getTimestamp(const sensor_msgs::msg::PointCloud2& p) {return tf2_ros::fromMsg(p.header.stamp);}

// method to extract frame id from object
template <>
inline
std::string getFrameId(const sensor_msgs::msg::PointCloud2 &p) {return p.header.frame_id;}

// this method needs to be implemented by client library developers
template <>
inline
void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::PointCloud2 &p_out, const geometry_msgs::msg::TransformStamped& t_in)
{
  p_out = p_in;
  p_out.header = t_in.header;
  Eigen::Transform<float,3,Eigen::Affine> t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y,
                                                                   t_in.transform.translation.z) * Eigen::Quaternion<float>(
                                                                     t_in.transform.rotation.w, t_in.transform.rotation.x,
                                                                     t_in.transform.rotation.y, t_in.transform.rotation.z);

  sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, std::string("x"));
  sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, std::string("y"));
  sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, std::string("z"));

  sensor_msgs::PointCloud2Iterator<float> x_out(p_out, std::string("x"));
  sensor_msgs::PointCloud2Iterator<float> y_out(p_out, std::string("y"));
  sensor_msgs::PointCloud2Iterator<float> z_out(p_out, std::string("z"));

  Eigen::Vector3f point;
  for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
    point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);
    *x_out = point.x();
    *y_out = point.y();
    *z_out = point.z();
  }
}
inline
sensor_msgs::msg::PointCloud2 toMsg(const sensor_msgs::msg::PointCloud2 &in)
{
  return in;
}
inline
void fromMsg(const sensor_msgs::msg::PointCloud2 &msg, sensor_msgs::msg::PointCloud2 &out)
{
  out = msg;
}

} // namespace

#endif // TF2_SENSOR_MSGS_HPP


/**
 *  @file
 *  @copyright defined in evt/LICENSE.txt
 */
#include <evt/chain/name128.hpp>
#include <boost/algorithm/string.hpp>
#include <evt/chain/exceptions.hpp>
#include <fc/variant.hpp>

namespace evt { namespace chain {

void
name128::set(const char* str) {
    const auto len = strnlen(str, 22);
    EVT_ASSERT(len <= 21, name128_type_exception, "Name128 is longer than 21 characters (${name}) ",
               ("name", std::string(str)));
    EVT_ASSERT(len > 0, name128_type_exception, "Name128 cannot be empty");
    value = string_to_name128(str);
    EVT_ASSERT(to_string() == std::string(str), name128_type_exception,
               "Name128 not properly normalized (name: ${name}, normalized: ${normalized}) ",
               ("name", std::string(str))("normalized", to_string()));
}

void
name128::set(const std::string& str) {
    const auto len = str.size();
    EVT_ASSERT(len <= 21, name128_type_exception, "Name128 is longer than 21 characters (${name}) ",
               ("name", str));
    value = string_to_name128(str.c_str());
    EVT_ASSERT(to_string() == str, name128_type_exception,
               "Name128 not properly normalized (name: ${name}, normalized: ${normalized}) ",
               ("name", str)("normalized", to_string()));
}

name128::operator std::string() const {
    static const char* charmap = ".-0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";

    auto str = std::string(21, '.');

    auto stop = 0u;
    auto tmp  = value >> 2;
    auto tag  = (int)value & 0x03;

    switch(tag) {
    case i32: {
        stop = 5;
        break;
    }
    case i64: {
        stop = 10;
        break;
    }
    case i96: {
        stop = 15;
        break;
    }
    case i128: {
        stop = 21;
        break;
    }
    }  // switch

    for(auto i = 0u; i < stop; ++i, tmp >>= 6) {
        auto c = charmap[tmp & 0x3f];
        str[i] = c;
    }

    str.erase(str.find_last_not_of('.', stop) + 1);
    return str;
}

name128
name128::from_number(uint64_t v) {
    auto r = name128();

    int i = 0;
    do {
        auto d = v % 10;
        r.value |= (uint128_t(d + 2) & 0x3f) << (128 - (6 * (i++ + 1)));

        v /= 10;
    }
    while(v > 0);

    r.value >>= 6 * (21 - i);

    if(i <= 5) {
        r.value |= name128::i32;
    }
    else if(i <= 10) {
        r.value |= name128::i64;
    }
    else if(i <= 15) {
        r.value |= name128::i96;
    }
    else {
        r.value |= name128::i128;
    }

    return r;
}

}}  // namespace evt::chain

namespace fc {

void
to_variant(const evt::chain::name128& name, fc::variant& v) {
    v = std::string(name);
}
void
from_variant(const fc::variant& v, evt::chain::name128& name) {
    name = v.get_string();
}

}  // namespace fc


//==============================================================================
//         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_ARITHMETIC_FUNCTIONS_SIMD_SSE_AVX_TOINT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_AVX_TOINT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT
#include <boost/simd/arithmetic/functions/toint.hpp>
#include <boost/simd/include/functions/simd/make.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/functions/simd/bitwise_andnot.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/if_zero_else.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::toint_, boost::simd::tag::avx_,
                        (A0),
                        ((simd_<double_<A0>,boost::simd::tag::avx_>))
                       )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      const A0 aa0 = if_zero_else(is_nan(a0), a0);
      return select(eq(aa0, Inf<A0>()),
        Inf<result_type>(),
        make<result_type>(aa0[0],aa0[1],aa0[2],aa0[3])
      );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::toint_, boost::simd::tag::avx_,
                        (A0),
                        ((simd_<single_<A0>,boost::simd::tag::avx_>))
                       )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      const A0 aa0 = if_zero_else(is_nan(a0), a0);
      result_type that = _mm256_cvttps_epi32(aa0);
      return  select(eq(aa0, Inf<A0>()), Inf<result_type>(), that);
    }
  };
} } }

#endif
#endif


#ifndef EUCLIDEAN_TRAJECTORY_H
#define EUCLIDEAN_TRAJECTORY_H

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

class TrajEuclidean{
public:
	TrajEuclidean();
	TrajEuclidean(const int & dim_in, const int & N_size_in, const double & dt_in);
	virtual ~TrajEuclidean();

	// Getter functions
	double get_dt();
	int get_dim();
	int get_trajectory_length();

	virtual void get_pos(const int & index_in, Eigen::Ref<Eigen::VectorXd> pos_out);
	virtual void get_vel(const int & index_in, Eigen::Ref<Eigen::VectorXd> vel_out);
	virtual void get_acc(const int & index_in, Eigen::Ref<Eigen::VectorXd> acc_out);

	// All of the get_next functions increment the internal index counter
	virtual void get_next_pos(Eigen::Ref<Eigen::VectorXd> pos_out);
	virtual void get_next_vel(Eigen::Ref<Eigen::VectorXd> vel_out);
	virtual void get_next_acc(Eigen::Ref<Eigen::VectorXd> acc_out);

	virtual void get_next_data(Eigen::Ref<Eigen::VectorXd> pos_out);
	virtual void get_next_data(Eigen::Ref<Eigen::VectorXd> pos_out, Eigen::Ref<Eigen::VectorXd> vel_out);
	virtual void get_next_data(Eigen::Ref<Eigen::VectorXd> pos_out, Eigen::Ref<Eigen::VectorXd> vel_out, Eigen::Ref<Eigen::VectorXd> acc_out);

	// Setter functions
	void set_dim_N_dt(const int & dim_in, const int & N_size_in, const double & dt_in);
	void set_dt(const double & dt_in);
	void set_pos(const int & index, const Eigen::VectorXd & pos_in);
	void set_vel(const int & index, const Eigen::VectorXd & vel_in);
	void set_acc(const int & index, const Eigen::VectorXd & acc_in);

	// resets the internal index counter
	void reset_index();

protected:
	std::vector<Eigen::VectorXd> pos;
	std::vector<Eigen::VectorXd> vel;
	std::vector<Eigen::VectorXd> acc;

	int N_size;
	int dim;
	double dt;
	int index = 0;

	void increment_index();
};

class TrajEuclideanConstant: public TrajEuclidean{
public:
	TrajEuclideanConstant();
	TrajEuclideanConstant(const Eigen::VectorXd vec_in);
	~TrajEuclideanConstant();

	virtual void get_pos(const int & index_in, Eigen::Ref<Eigen::VectorXd> pos_out);
	virtual void get_vel(const int & index_in, Eigen::Ref<Eigen::VectorXd> vel_out);
	virtual void get_acc(const int & index_in, Eigen::Ref<Eigen::VectorXd> acc_out);

	virtual void get_next_pos(Eigen::Ref<Eigen::VectorXd> pos_out);
	virtual void get_next_vel(Eigen::Ref<Eigen::VectorXd> vel_out);
	virtual void get_next_acc(Eigen::Ref<Eigen::VectorXd> acc_out);

	virtual void get_next_data(Eigen::Ref<Eigen::VectorXd> pos_out);
	virtual void get_next_data(Eigen::Ref<Eigen::VectorXd> pos_out, Eigen::Ref<Eigen::VectorXd> vel_out);
	virtual void get_next_data(Eigen::Ref<Eigen::VectorXd> pos_out, Eigen::Ref<Eigen::VectorXd> vel_out, Eigen::Ref<Eigen::VectorXd> acc_out);

private:
	Eigen::VectorXd vec_constant;
	Eigen::VectorXd vec_zero;

};

#endif


#include <string>
#include <memory>
#include <tuple>
#include <stdexcept>
#include <algorithm>
#include <cmath>
#include <map>

#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
#include <boost/thread/lock_guard.hpp>

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>

#include "cpl_serv.h"
#include "tiff.h"
#include "libxtiff/xtiffio.h"
#include "geotiffio.h"
#include "geovalues.h"
#include "geo_normalize.h"

#include "rastermap.h"
#include "map_geotiff.h"
#include "projection.h"
#include "util.h"

const char * const DEFAULT_ENCODING = "UTF-8";

// Map between the unit square (x and y in [0, 1]) and a general
// quadrilateral using bilinear interpolation.
// Normalized coordinates are given as UnitSquareCoord.
// Quadrilateral coordinatese are given as MapPixelDelta.
class BilinearInterpolator {
    public:
        // Initialize with the four corners of the general quadrilateral.
        // The coordinates are given in clock-wise order.
        BilinearInterpolator(const MapPixelDelta &P00,
                             const MapPixelDelta &P10,
                             const MapPixelDelta &P11,
                             const MapPixelDelta &P01);

        // Map from input (within the unit square) to the general quad.
        // Example: forward(UnitSquareCoord(0.25, 0)) -> P00 * 0.75 + P10 * 0.25
        MapPixelDelta forward(const UnitSquareCoord &input);

        // Map from the general quad back to the unit square.
        // This is the inverse transformation of forward().
        UnitSquareCoord inverse(const MapPixelDelta &X);
    private:
        template <typename T>
        T lerp(double f, const T &A, const T &B) {
            return (1-f)*A + f*B;
        }
        const MapPixelDelta A, B, C, D;
        const MapPixelDelta E, F, G;
};

BilinearInterpolator::BilinearInterpolator(const MapPixelDelta &P00,
                                           const MapPixelDelta &P10,
                                           const MapPixelDelta &P11,
                                           const MapPixelDelta &P01)
: A(P00), B(P10), C(P11), D(P01), E(B-A), F(D-A), G(A-B-D+C)
{}

MapPixelDelta BilinearInterpolator::forward(const UnitSquareCoord &input) {
    // Straight-forward bilinear interpolation.
    return lerp(input.y, lerp(input.x, A, B), lerp(input.x, D, C));
}

UnitSquareCoord BilinearInterpolator::inverse(const MapPixelDelta &X) {
    // Inverse bilinear interpolation involves solving a quadratic equation.
    // Cf. http://www.iquilezles.org/www/articles/ibilinear/ibilinear.htm
    MapPixelDelta H = X-A;
    double k2 = G.x*F.y - G.y*F.x;
    double k1 = E.x*F.y - E.y*F.x + H.x*G.y - H.y*G.x;
    double k0 = H.x*E.y - H.y*E.x;
    double v = (-k1 + sqrt(k1*k1 - 4*k0*k2)) / (2 * k2);
    if (v < 0 || v > 1) {
        v = (-k1 - sqrt(k1*k1 - 4*k0*k2)) / (2 * k2);
    }
    double u = (H.x - F.x*v)/(E.x + G.x*v);
    return UnitSquareCoord(u, v);
}

class TiffHandle {
    public:
        explicit TiffHandle(const std::wstring &fname)
        // Pass "m" flag to disable memory-mapped IO.
        // Otherwise we fill up the whole virtual address space with mmapped
        // TIF files and run out of space to load libraries or map other data.
        // This gives hard-to-diagnose OOM errors (debug with VMMap on Win32).
        : m_tiff(XTIFFOpenW(fname.c_str(), "rm"))
        {
            if (!m_tiff) {
                throw std::runtime_error("File not found");
            }
        };
        ~TiffHandle() {
            if (m_tiff) {
                XTIFFClose(m_tiff);
                m_tiff = NULL;
            }
        }
        TIFF *GetTIFF() { return m_tiff; };
    private:
        DISALLOW_COPY_AND_ASSIGN(TiffHandle);
        TIFF* m_tiff;
};

class GeoTiffHandle {
    public:
        explicit GeoTiffHandle(TiffHandle &tiffhandle)
            : m_gtif(GTIFNew(tiffhandle.GetTIFF()))
        {
            if (!m_gtif)
                throw std::runtime_error("Opening GeoTiff failed.");
        };
        ~GeoTiffHandle() {
            if (m_gtif) {
                GTIFFree(m_gtif);
                m_gtif = NULL;
            }
        };
        GTIF *GetGTIF() { return m_gtif; };
    private:
        DISALLOW_COPY_AND_ASSIGN(GeoTiffHandle);
        GTIF *m_gtif;
};

class Tiff {
    public:
        explicit Tiff(const std::wstring &fname);
        virtual ~Tiff() { };
        TIFF *GetTIFF() { return m_rawtiff; };
        unsigned int GetWidth() const { return m_width; };
        unsigned int GetHeight() const { return m_height; };
        unsigned int GetBitsPerSample() const { return m_bitspersample; };
        unsigned int GetSamplesPerPixel() const { return m_samplesperpixel; };
        PixelBuf GetRegion(
                const class MapPixelCoordInt &pos,
                const class MapPixelDeltaInt &size) const;

        template <typename T>
        std::tuple<unsigned int, const T*>
        GetField(ttag_t field) const;

        const std::wstring &GetFilename() const { return m_fname; };
        const std::wstring &GetTitle() const { return m_title; };
        const std::wstring &GetDescription() const { return m_description; };
    protected:
        const std::wstring m_fname;
        std::wstring m_title;
        std::wstring m_description;
        TiffHandle m_tiffhandle;
        TIFF *m_rawtiff;

        virtual void Hook_TIFFRGBAImageGet(TIFFRGBAImage &img) const {};
    private:
        unsigned int m_width, m_height;
        unsigned short int m_bitspersample, m_samplesperpixel;

        PixelBuf DoGetRegion(
                const class MapPixelCoordInt &pos,
                const class MapPixelDeltaInt &size) const;
};

class GeoTiff : public Tiff {
    public:
        explicit GeoTiff(const std::wstring &fname);
        virtual ~GeoTiff() { };

        bool CheckVersion() const;
        bool LoadCoordinates();

        bool PixelToPCS(double *x, double *y) const;
        bool PCSToPixel(double *x, double *y) const;
        const std::string &GetProj4String() const { return m_proj; };
        GeoDrawable::DrawableType GetType() const { return m_type; };

        template <typename T>
        std::tuple<unsigned int, std::shared_ptr<T>>
        GetKey(geokey_t key) const;

        template <typename T>
        bool GetKeySingle(geokey_t key, T *result) const;

        bool HasKey(geokey_t) const;

        geocode_t GetModel() const { return m_model; }

    protected:
        GeoTiffHandle m_gtifhandle;
        GTIF *m_rawgtif;

        virtual void Hook_TIFFRGBAImageGet(TIFFRGBAImage &img) const;
    private:
        bool CheckDHMValid() const;

        geocode_t m_model;
        const double *m_tiepoints;
        const double *m_pixscale;
        const double *m_transform;
        unsigned short int m_ntiepoints, m_npixscale, m_ntransform;

        std::string m_proj;
        GeoDrawable::DrawableType m_type;
};

static void put16bitbw_DHM(
    TIFFRGBAImage* img,
    uint32* cp,                     // ptr to write target in TO (dest buf)
    uint32 x, uint32 y,             // x,y offset of write target within TO
    uint32 w, uint32 h,             // pixel count of one row; number of rows
    int32 fromskew, int32 toskew,   // offsets on row change in FROM/TO
    unsigned char* pp)              // ptr to read location in FROM
{
    int samplesperpixel = img->samplesperpixel;
    int16 *wp = reinterpret_cast<int16*>(pp);
    while (h-- > 0) {
        for (x = w; x-- > 0;) {
            // Copy the 16 bit heightmap value over into the RGBA image
            // Take care to sign-extend
            *cp++ = (uint32)*wp;
            wp += samplesperpixel;
        }
        cp += toskew;
        wp += fromskew;
    }
}

Tiff::Tiff(const std::wstring &fname)
    : m_fname(fname), m_title(), m_description(), m_tiffhandle(fname),
      m_rawtiff(m_tiffhandle.GetTIFF())
{
    if (!TIFFGetField(m_rawtiff, TIFFTAG_IMAGEWIDTH, &m_width) ||
        !TIFFGetField(m_rawtiff, TIFFTAG_IMAGELENGTH, &m_height)) {
        throw std::runtime_error("Failed getting TIF dimensions.");
    }

    if (!TIFFGetField(m_rawtiff, TIFFTAG_BITSPERSAMPLE, &m_bitspersample) ||
        !TIFFGetField(m_rawtiff, TIFFTAG_SAMPLESPERPIXEL, &m_samplesperpixel))
    {
        throw std::runtime_error("Failed getting TIF pixel format.");
    }
    char *title;
    if (TIFFGetField(m_rawtiff, TIFFTAG_DOCUMENTNAME, &title)) {
        m_title = WStringFromString(title, DEFAULT_ENCODING);
    }
    char *description;
    if (TIFFGetField(m_rawtiff, TIFFTAG_IMAGEDESCRIPTION, &description)) {
        m_description = WStringFromString(description, DEFAULT_ENCODING);
    }
};

PixelBuf
Tiff::DoGetRegion(const MapPixelCoordInt &pos,
                  const MapPixelDeltaInt &size) const
{
    MapPixelCoordInt endpos = pos + size;
    if (endpos.x <= 0 || endpos.y <= 0 ||
        pos.x >= static_cast<int>(m_width) ||
        pos.y >= static_cast<int>(m_height))
    {
        return PixelBuf(size.x, size.y);
    }

    TIFFRGBAImage img;
    char emsg[1024] = "";
    if (!TIFFRGBAImageOK(m_rawtiff, emsg) ||
        !TIFFRGBAImageBegin(&img, m_rawtiff, 0, emsg)) {
        throw std::runtime_error("TIFF RGBA access not possible.");
    }

    img.col_offset = pos.x;
    img.row_offset = pos.y;

    // Give derived classes a chance to influence TIFFRGBAGetImage
    Hook_TIFFRGBAImageGet(img);

    PixelBuf result(size.x, size.y);
    int ok = TIFFRGBAImageGet(&img, result.GetRawData(), size.x, size.y);
    TIFFRGBAImageEnd(&img);

    if (!ok) {
        throw std::runtime_error("Loading TIFF data failed.");
    }
    return result;
}

PixelBuf
Tiff::GetRegion(const MapPixelCoordInt &pos,
                const MapPixelDeltaInt &size) const
{
    if (TIFFIsTiled(m_rawtiff))
        return DoGetRegion(pos, size);

    // Handle stripped images:
    // TIFFRGBAImageGet ignores img.col_offset for stripped images.
    // It always returns data starting from the first column of the image.
    //
    // So, fetch a full-width strip and copy the relevant portion into the
    // output PixelBuf.
    auto result = PixelBuf(size.x, size.y);
    MapPixelCoordInt end = pos + size;

    int strip_size = -1;
    if (!TIFFGetFieldDefaulted(m_rawtiff, TIFFTAG_ROWSPERSTRIP, &strip_size)) {
        throw std::runtime_error("Failed getting TIF dimensions.");
    }
    if (strip_size < 0) {
        throw std::runtime_error("Stripped TIF image with strip height < 0?!");
    }
    if (static_cast<unsigned int>(strip_size) > m_height) {
        // TIFFRGBAImageGet crashes when reading from an image where
        // rows_per_strip > total_rows_in_image if more than
        // total_rows_in_image rows are requested.
        //
        // Cap the number of rows we try to read, in this case.
        strip_size = m_height;
    }

    int first_ty = pos.y / strip_size;
    int last_ty = (end.y - 1) / strip_size;
    for (int ty = pos.y / strip_size; ty*strip_size < end.y; ty++) {
        PixelBuf tile = DoGetRegion(
                MapPixelCoordInt(0, ty*strip_size),
                MapPixelDeltaInt(m_width, strip_size));
        if (!tile.GetData()) {
            continue;
        }
        auto insert_pos = PixelBufCoord(
                -pos.x,
                (last_ty - ty + first_ty) * strip_size - pos.y);
        result.Insert(insert_pos, tile);
    }
    return result;
}

template <typename T>
std::tuple<unsigned int, const T*>
Tiff::GetField(ttag_t field) const {
    const T *data;
    unsigned int length;
    if (!TIFFGetField(m_rawtiff, field, &length, &data)) {
        length = 0;
        data = NULL;
    }
    return std::make_tuple(length, data);
}

/**
 * Internal callback to resolve csv data filenames to full paths.
 *
 * @locking Acquires an internal ``std::mutex``. No external calls are made
 * with that mutex held.
 */
static const char *CSVFileOverride(const char * input_fname) {
    static boost::mutex strmap_mutex;
    static std::map<std::string, const char *> strmap;

    {
        boost::lock_guard<boost::mutex> lock(strmap_mutex);
        auto result = strmap.find(input_fname);
        if (result != strmap.end()) {
            return result->second;
        }
    }

    std::string csvdir = GetModuleDir_char() + "csv" + ODM_PathSep_char;
    std::string fpath = csvdir + input_fname;
    if (!FileExists(fpath)) {
        throw std::runtime_error("CSV file does not exist: " + fpath);
    }

    // Intentionally leak this memory.
    // The buffer address must never change, which STL containers don't
    // guarantee. Only ~40 input file names are possible, so cache per input
    // filename and stop worrying.
    const char * const fpath_cstr = _strdup(fpath.c_str());
    {
        boost::lock_guard<boost::mutex> lock(strmap_mutex);
        strmap[input_fname] = fpath_cstr;
    }
    return fpath_cstr;
}

// http://rocky.ess.washington.edu/data/raster/geotiff/docs/manual.txt
// ftp://kratmos.gsfc.nasa.gov/pub/jim/imager/latest_version/TIFF_reader2.c
// http://svn.osgeo.org/fdocore/tags/3.2.x_G052/Thirdparty/GDAL1.3/src/frmts/gtiff/geotiff.cpp
// http://ojs.klaki.net/geotiff2ncdf/gt2nc.c

GeoTiff::GeoTiff(const std::wstring &fname)
    : Tiff(fname), m_gtifhandle(m_tiffhandle),
      m_rawgtif(m_gtifhandle.GetGTIF()),
      m_tiepoints(NULL), m_pixscale(NULL), m_transform(NULL),
      m_ntiepoints(0), m_npixscale(0), m_ntransform(0),
      m_proj(), m_type()
{
    SetCSVFilenameHook(&CSVFileOverride);
    if (!CheckVersion()) {
        throw std::runtime_error("GeoTIFF version not supported.");
    }
    if (!LoadCoordinates()) {
        m_type = RasterMap::TYPE_IMAGE;
    }
};

bool GeoTiff::CheckVersion() const {
    enum GTIFVersion { VERSION = 0, REV_MAJOR, REV_MINOR };
    int versions[3];
    GTIFDirectoryInfo(m_rawgtif, versions, NULL);
    return versions[VERSION] <= GvCurrentVersion &&
           versions[REV_MAJOR] <= GvCurrentRevision;
}

bool GeoTiff::LoadCoordinates() {
    if (!GetKeySingle<geocode_t>(GTModelTypeGeoKey, &m_model)) {
        return false;
    }

    // The rest is not implemented
    if (m_model != ModelTypeProjected && m_model != ModelTypeGeographic) {
        throw std::runtime_error("Map type not supported yet");
    }

    GTIFDefn    defn;
    if(!GTIFGetDefn(m_rawgtif, &defn)) {
        return false;
    }

    std::shared_ptr<char> proj_str(GTIFGetProj4Defn(&defn),
                                   [](char* mem) { GTIFFreeMemory(mem); });
    if (!proj_str)
        return false;

    m_proj = proj_str.get();
    if (!m_proj.length() || !m_proj[0])
        return false;

    using std::tie;
    tie(m_ntiepoints, m_tiepoints) = GetField<double>(TIFFTAG_GEOTIEPOINTS);
    tie(m_npixscale, m_pixscale) = GetField<double>(TIFFTAG_GEOPIXELSCALE);
    tie(m_ntransform, m_transform) = GetField<double>(TIFFTAG_GEOTRANSMATRIX);

    unsigned int sample_fmt = 0;
    TIFFGetFieldDefaulted(m_rawtiff, TIFFTAG_SAMPLEFORMAT, &sample_fmt);
    if (HasKey(VerticalUnitsGeoKey)) {
        m_type = RasterMap::TYPE_DHM;
    } else if (GetSamplesPerPixel() == 1 && GetBitsPerSample() == 16 &&
               sample_fmt == SAMPLEFORMAT_INT)
    {
        // Some DHM's don't set VerticalUnitsGeoKey, unfortunately.
        // Use this heuristic to catch those, SAMPLEFORMAT_INT should be
        // a pretty good indicator.
        m_type = RasterMap::TYPE_DHM;
    } else {
        m_type = RasterMap::TYPE_MAP;
    }

    return true;
}

template <typename T>
std::tuple<unsigned int, std::shared_ptr<T>>
GeoTiff::GetKey(geokey_t key) const
{
    int size;
    tagtype_t type;
    unsigned int count = GTIFKeyInfo(m_rawgtif, key, &size, &type);
    if (!count) {
        return make_tuple(0, std::shared_ptr<T>());
    }
    if (sizeof(T) != size) {
        throw std::runtime_error("GeoTIFF key malformed.");
    }

    std::shared_ptr<T> buffer(new T[count](), ArrayDeleter<T>());
    if (GTIFKeyGet(m_rawgtif, key, buffer.get(), 0, count) != count) {
        throw std::runtime_error("GeoTIFF key read error.");
    }
    return std::make_tuple(count, buffer);
};

template <typename T>
bool GeoTiff::GetKeySingle(geokey_t key, T *output) const {
    auto result = GetKey<T>(key);
    if (std::get<0>(result) != 1) {
        return false;
    }
    *output = *std::get<1>(result).get();
    return true;
}

bool GeoTiff::HasKey(geokey_t key) const {
    return 0 != GTIFKeyInfo(m_rawgtif, key, NULL, NULL);
}

bool GeoTiff::PixelToPCS(double *x, double *y) const {
    if (m_type == RasterMap::TYPE_IMAGE)
        return false;

    if (m_ntiepoints > 6 && m_npixscale == 0) {
        // Interpolate between multiple tiepoints
        if (m_ntiepoints != 4*6) {
            // Currently, we only support 4 tiepoints.
            return false;
        };

        unsigned int w = GetWidth();
        unsigned int h = GetHeight();
        std::unique_ptr<MapPixelDelta> p00, p10, p01, p11;

        for (unsigned int i=0; i < m_ntiepoints; i += 6) {
            if (m_tiepoints[i+0] == 0 && m_tiepoints[i+1] == 0)
                p00.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == w && m_tiepoints[i+1] == 0)
                p10.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == 0 && m_tiepoints[i+1] == h)
                p01.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == w && m_tiepoints[i+1] == h)
                p11.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else {
                // For now, we require tie points to be at the image corners.
                return false;
            }
        }
        if (!p00 || !p10 || !p01 || !p11) {
            return false;
        }
        auto interpolator = BilinearInterpolator(*p00, *p10, *p11, *p01);
        auto res = interpolator.forward(UnitSquareCoord(*x/w, *y/h));
        *x = res.x;
        *y = res.y;
    }
    else if (m_ntransform == 16) {
        // Use matrix for transformation
        double x_in = *x, y_in = *y;
        *x = x_in * m_transform[0] + y_in * m_transform[1] + m_transform[3];
        *y = x_in * m_transform[4] + y_in * m_transform[5] + m_transform[7];
    }
    else if (m_npixscale >= 3 && m_ntiepoints >= 6) {
        // Use one tiepoint + pixscale
        *x = (*x - m_tiepoints[0]) * m_pixscale[0] + m_tiepoints[3];
        *y = (*y - m_tiepoints[1]) * (-1 * m_pixscale[1]) + m_tiepoints[4];
    }
    else {
        throw std::runtime_error("Couldn't find GeoTIFF coordinates.");
    }
    return true;
}

bool GeoTiff::PCSToPixel(double *x, double *y) const {
    if (m_type == RasterMap::TYPE_IMAGE)
        return false;

    if (m_ntiepoints > 6 && m_npixscale == 0) {
        // Interpolate between multiple tiepoints
        if (m_ntiepoints != 4*6) {
            // Currently, we only support 4 tiepoints.
            return false;
        }

        unsigned int w = GetWidth();
        unsigned int h = GetHeight();
        std::unique_ptr<MapPixelDelta> p00, p10, p01, p11;

        for (unsigned int i=0; i < m_ntiepoints; i += 6) {
            if (m_tiepoints[i+0] == 0 && m_tiepoints[i+1] == 0)
                p00.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == w && m_tiepoints[i+1] == 0)
                p10.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == 0 && m_tiepoints[i+1] == h)
                p01.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == w && m_tiepoints[i+1] == h)
                p11.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else {
                // For now, we require tie points to be at the image corners.
                return false;
            }
        }
        if (!p00 || !p10 || !p01 || !p11) {
            return false;
        }
        auto interpolator = BilinearInterpolator(*p00, *p10, *p11, *p01);
        auto res = interpolator.inverse(MapPixelDelta(*x, *y));
        *x = res.x * w;
        *y = res.y * h;
    }
    else if (m_ntransform == 16) {
        // Use inverse matrix for transformation.
        const double *mat = m_transform;
        double x_in = *x, y_in = *y;
        double denom = (mat[0] * mat[5] - mat[1] * mat[4]);
        *x = (+(x_in - mat[3]) * mat[5] - (y_in - mat[7]) * mat[1]) / denom;
        *y = (-(x_in - mat[3]) * mat[4] + (y_in - mat[7]) * mat[0]) / denom;
    }
    else if (m_npixscale >= 3 && m_ntiepoints >= 6) {
        // Use one tiepoint + pixscale
        *x = (*x - m_tiepoints[3]) / m_pixscale[0] + m_tiepoints[0];
        *y = (*y - m_tiepoints[4]) / (-1 * m_pixscale[1]) + m_tiepoints[1];
    }
    else {
        throw std::runtime_error("Couldn't find GeoTIFF coordinates.");
    }
    return true;
}

bool GeoTiff::CheckDHMValid() const {
    short int vert_units;
    if (!GetKeySingle<short int>(VerticalUnitsGeoKey, &vert_units))
        return false;

    // Other vertical formats not supported at the moment
    return vert_units == Linear_Meter;
}

// If we have a DHM map, the pixel values typically represent height in m
// The default TIFFRGBAImageGet turns this (typically 16 bit) value into
// a grayscale rgb image (thus truncating the lower 8 bits).
// Prevent this and simply store the 16 bit value within the 32 bit RGBA.
void GeoTiff::Hook_TIFFRGBAImageGet(TIFFRGBAImage &img) const {
    // Only hook DHM maps
    if (GetType() != RasterMap::TYPE_DHM)
        return;

    if (GetBitsPerSample() != 16) {
        assert(false);  // not implemented
    }
    if (!img.isContig) {
        assert(false);  // not implemented
    }
    if (img.photometric != PHOTOMETRIC_MINISWHITE &&
        img.photometric != PHOTOMETRIC_MINISBLACK)
    {
        assert(false);  // not implemented
    }
    img.put.contig = put16bitbw_DHM;
}

TiffMap::TiffMap(const wchar_t *fname)
    : m_geotiff(new GeoTiff(fname)), m_proj(m_geotiff->GetProj4String())
{}

GeoDrawable::DrawableType TiffMap::GetType() const {
    return m_geotiff->GetType();
}
unsigned int TiffMap::GetWidth() const { return m_geotiff->GetWidth(); };
unsigned int TiffMap::GetHeight() const { return m_geotiff->GetHeight(); };
MapPixelDeltaInt TiffMap::GetSize() const {
    return MapPixelDeltaInt(m_geotiff->GetWidth(), m_geotiff->GetHeight());
}

PixelBuf TiffMap::GetRegion(
                      const MapPixelCoordInt &pos,
                      const MapPixelDeltaInt &size) const
{
    auto fixed_bounds_pb = GetRegion_BoundsHelper(*this, pos, size);
    if (fixed_bounds_pb.GetData())
        return fixed_bounds_pb;

    boost::lock_guard<boost::mutex> lock(m_getregion_mutex);
    return m_geotiff->GetRegion(pos, size);
}

bool TiffMap::PixelToPCS(double *x, double *y) const
    { return m_geotiff->PixelToPCS(x, y); }
bool TiffMap::PCSToPixel(double *x, double *y) const
    { return m_geotiff->PCSToPixel(x, y); }
const std::wstring &TiffMap::GetFname() const
    { return m_geotiff->GetFilename(); }
const std::wstring &TiffMap::GetTitle() const
    { return m_geotiff->GetTitle(); }
const std::wstring &TiffMap::GetDescription() const
    { return m_geotiff->GetDescription(); }
Projection TiffMap::GetProj() const
    { return m_proj; }

bool TiffMap::PixelToLatLon(const MapPixelCoord &pos, LatLon *result) const {
    double x = pos.x;
    double y = pos.y;
    if (!PixelToPCS(&x, &y))
        return false;

    if (m_geotiff->GetModel() == ModelTypeProjected) {
        if (!GetProj().PCSToLatLong(x, y)) {
            return false;
        }
    }
    *result = LatLon(y, x);
    return true;
}

bool TiffMap::LatLonToPixel(const LatLon &pos, MapPixelCoord *result) const {
    double x = pos.lon;
    double y = pos.lat;
    if (m_geotiff->GetModel() == ModelTypeProjected) {
        if (!GetProj().LatLongToPCS(x, y)) {
            return false;
        }
    }
    if (!PCSToPixel(&x, &y))
        return false;

    *result = MapPixelCoord(x, y);
    return true;
}


#define BOOST_TEST_MODULE pcraster geo csf_map
#include <boost/test/unit_test.hpp>
#include "com_exception.h"
#include "com_pathinfo.h"
#include "geo_csfmap.h"

struct Fixture
{

    Fixture()
    {
        d_map1 = new geo::CSFMap("map1.map",11, 12, VS_SCALAR, PT_YINCT2B,
            3.0, 14.0, 1.0, 5.0);
    }

    ~Fixture()
    {
      delete d_map1;
      remove("map1.map");
    }

    geo::CSFMap* d_map1;

};

BOOST_AUTO_TEST_CASE(constructor)
{
  using namespace geo;

 // illegal angle
 CSFMap *map2(0);
 bool failureExpected=false;
 try {
    map2 = new CSFMap("csfmap2.map",11, 12, VS_SCALAR, PT_YINCT2B, 3.0, 14.0, 15.0, 5.0);
 } catch(const com::FileError& e) {
  const char *msgExpect =
    "File 'csfmap2.map': error creating raster: Angle < -0.5 pi or > 0.5 pi\n";
  BOOST_CHECK(e.messages() == msgExpect);
  failureExpected=true;
 }
 BOOST_CHECK(failureExpected);
 delete map2;
 BOOST_CHECK(!com::PathInfo("csfmap2.map").exists());
}

BOOST_FIXTURE_TEST_SUITE(csf_map, Fixture)

BOOST_AUTO_TEST_CASE(nr_rows)
{
  using namespace geo;

  BOOST_CHECK(d_map1->nrRows() == 11);
}

BOOST_AUTO_TEST_CASE(nr_cols)
{
  using namespace geo;

  BOOST_CHECK(d_map1->nrCols() == 12);
}

BOOST_AUTO_TEST_CASE(nr_cells)
{
  using namespace geo;

  BOOST_CHECK(d_map1->nrCells() == 11 * 12);
}

BOOST_AUTO_TEST_SUITE_END()