#pragma once
#include <vector>
#include <iostream>
//#include "Eigen/Eigen/Eigen"

#include <Eigen/Dense>
#include<Eigen/Core>
// fstream
#include <fstream>
#include<sstream>
#include <stdlib.h>
#include <iomanip>

// math
#include <math.h>
//time
#include <time.h>
//algorithm
#include <algorithm>
// Define Infinite (Using INT_MAX caused overflow problems)
#define INF 10000
using namespace Eigen;
using namespace std;
class generateSkymask
{
public: // functions
		generateSkymask(string kml_dir, string saved_skymask_dir,bool mannually_set_ref_llh, double ref_lat,double ref_lon) // construct function
	{
		mannually_set_ref_llh_ = mannually_set_ref_llh;
		ref_lat_fromlaunch = ref_lat;
		ref_lon_fromlaunch = ref_lon;
		//readKml("G:/Dropbox/Dropbox/PNL/57HuaWei_WWS/Shadow_Matching_Huawei/Shadow_Matching_Huawei/NiuToujiao.kml"); // demo_HK_TST_fix
		// readKml("/home/wenws/amsipolyu/src/rtklibros/app/generate_skymask/data/demo_HK_TST_fix.kml"); // demo_HK_TST_fix
		readKml(kml_dir);
		wait(5);
		string2llh(builDataS); // parse llh from string
		wait(5);
		prepareBuildingData(buildingLLHS);  // prepare buildingS to map_
		wait(5);

		/*Eigen::MatrixXd ENU_;
		ENU_.resize(1, 3);
		ENU_(0) = 25;
		ENU_(1) = 3;
		ENU_(2) = 30;
		bool result = identifyPointInsideBuildingENU(map_, ENU_);*/

		Eigen::MatrixXd LLH_;
		LLH_.resize(1, 3);
		LLH_(0) = 114.177911;
		LLH_(1) = 22.298642;
		LLH_(2) = 102;
		bool result = identifyPointInsideBuildingLLH(map_, LLH_);
		cout << "result by convex hull (LLH):   " << result << "\n";

		Eigen::MatrixXd ENU_;
		ENU_.resize(1, 3);
		ENU_(0) = 0;
		ENU_(1) = -84;
		ENU_(2) = 0;
		result = identifyPointInsideBuildingENU(map_, ENU_);
		cout << "result by convex hull (ENU):   " << result << "\n";

		// get the azimuth angle at a certain position (LLH) with a certain azimuth
		Eigen::MatrixXd aziEleResult;
		aziEleResult.resize(2,1);

		vector<double> elevationList;
		vector<double> builingIntersectionNumber;
		bool consistency = 1;
		bool onebuilding = 0; // only intersect with one building

		for (double azi = 0; azi <= 360; azi = azi + map_.aziReso)
		{
			aziEleResult = getMaskElevationFromLLH(map_, LLH_, azi);
			elevationList.push_back(aziEleResult(1));
			if (aziEleResult(1) > 0) // if
			{
				builingIntersectionNumber.push_back(aziEleResult(2));
			}

		}

		createSkymasks(); // generate skymasks
		wait(5);
		saveSkymasks2SCV(map_,saved_skymask_dir); // save skymask to CSV
		wait(5);

	}

	~generateSkymask() // deconstruction function
	{
	}

public: // solve convex hull problem (https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain#C++)
		// Implementation of Andrew's monotone chain 2D convex hull algorithm.
		// Asymptotic complexity: O(n log n).
		// Practical performance: 0.5-1.0 seconds for n=1000000 on a 1GHz computer.
	typedef double coord_t;         // coordinate type
	typedef double coord2_t;  // must be big enough to hold 2*max(|coordinate|)^2

	struct Point {
		coord_t x, y;

		bool operator <(const Point &p) const {
			return x < p.x || (x == p.x && y < p.y);
		}
	};

	// 2D cross product of OA and OB vectors, i.e. z-component of their 3D cross product.
	// Returns a positive value, if OAB makes a counter-clockwise turn,
	// negative for clockwise turn, and zero if the points are collinear.
	coord2_t cross(const Point &O, const Point &A, const Point &B)
	{
		return (A.x - O.x) * (B.y - O.y) - (A.y - O.y) * (B.x - O.x);
	}

	// Returns a list of points on the convex hull in counter-clockwise order.
	// Note: the last point in the returned list is the same as the first one.
	vector<Point> convex_hull(vector<Point> P)
	{
		size_t n = P.size(), k = 0;
		if (n <= 3) return P;
		vector<Point> H(2 * n);

		// Sort points lexicographically
		sort(P.begin(), P.end());

		// Build lower hull
		for (size_t i = 0; i < n; ++i) {
			while (k >= 2 && cross(H[k - 2], H[k - 1], P[i]) <= 0) k--;
			H[k++] = P[i];
		}

		// Build upper hull
		for (size_t i = n - 1, t = k + 1; i > 0; --i) {
			while (k >= t && cross(H[k - 2], H[k - 1], P[i - 1]) <= 0) k--;
			H[k++] = P[i - 1];
		}

		H.resize(k - 1);
		return H;
	}

public: // solve the problem: if one point is inside the building
	struct Point_
	{
		double x;
		double y;
	};

	// Given three colinear Point_s p, q, r, the function checks if
	// Point_ q lies on line segment 'pr'
	bool onSegment(Point_ p, Point_ q, Point_ r)
	{
		if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) &&
			q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y))
			return true;
		return false;
	}

	// To find orientation of ordered triplet (p, q, r).
	// The function returns following values
	// 0 --> p, q and r are colinear
	// 1 --> Clockwise
	// 2 --> Counterclockwise
	int orientation(Point_ p, Point_ q, Point_ r)
	{
		int val = (q.y - p.y) * (r.x - q.x) -
			(q.x - p.x) * (r.y - q.y);

		if (val == 0) return 0; // colinear
		return (val > 0) ? 1 : 2; // clock or counterclock wise
	}

	// The function that returns true if line segment 'p1q1'
	// and 'p2q2' intersect.
	bool doIntersect(Point_ p1, Point_ q1, Point_ p2, Point_ q2)
	{
		// Find the four orientations needed for general and
		// special cases
		int o1 = orientation(p1, q1, p2);
		int o2 = orientation(p1, q1, q2);
		int o3 = orientation(p2, q2, p1);
		int o4 = orientation(p2, q2, q1);

		// General case
		if (o1 != o2 && o3 != o4)
			return true;

		// Special Cases
		// p1, q1 and p2 are colinear and p2 lies on segment p1q1
		if (o1 == 0 && onSegment(p1, p2, q1)) return true;

		// p1, q1 and p2 are colinear and q2 lies on segment p1q1
		if (o2 == 0 && onSegment(p1, q2, q1)) return true;

		// p2, q2 and p1 are colinear and p1 lies on segment p2q2
		if (o3 == 0 && onSegment(p2, p1, q2)) return true;

		// p2, q2 and q1 are colinear and q1 lies on segment p2q2
		if (o4 == 0 && onSegment(p2, q1, q2)) return true;

		return false; // Doesn't fall in any of the above cases
	}

	// Returns true if the Point_ p lies inside the polygon[] with n vertices
	bool isInside(vector<Point_> polygon, int n, Point_ p)
	{
		// There must be at least 3 vertices in polygon[]
		if (n < 3) return false;

		// Create a Point_ for line segment from p to infinite
		Point_ extreme = { INF, p.y };

		// Count intersections of the above line with sides of polygon
		int count = 0, i = 0;
		do
		{
			int next = (i + 1) % n;

			// Check if the line segment from 'p' to 'extreme' intersects
			// with the line segment from 'polygon[i]' to 'polygon[next]'
			if (doIntersect(polygon[i], polygon[next], p, extreme))
			{
				// If the Point_ 'p' is colinear with line segment 'i-next',
				// then check if it lies on segment. If it lies, return true,
				// otherwise false
				if (orientation(polygon[i], p, polygon[next]) == 0)
					return onSegment(polygon[i], p, polygon[next]);

				count++;
			}
			i = next;
		} while (i != 0);

		// Return true if count is odd, false otherwise
		return count & 1; // Same as (count%2 == 1)
	}

public: // variables for particles
	double reserve;
	typedef struct obs_Nav_epoch // reserved
	{

	};

	typedef struct  // satellite information
	{
		double GNSS_time;
		double total_sv; // total satellite in this epoch
		double prn_satellites_index; // satellite prn
		double pseudorange; // satellite pseudorange
		double snr; // satellite snr
		double elevation; // satellite elevation
		double azimuth; // satellite azimuth
		double err_tropo; //satellite erro_tropo
		double err_iono; // satellite ono
		double sat_clk_err; // satellite clock bias
		double sat_pos_x; // satellite position x
		double sat_pos_y; // satellite position y
		double sat_pos_z; // satellite position z
		int visable; // satellite visability
		std::string sat_system; // satellite system ("GPS", "GLONASS")
	}satelliteInfo;

	typedef struct // single grid: determined in ENU coordiante system
	{
		double E_;
		double N_;
		double U_;
		double score;
	}grid;

	typedef struct  //  grid sequences (grids)
	{
		std::vector<grid> grids_;
	}grids;

	typedef struct  // state for a particle
	{
		double GNSS_time = 0; // time stamps
		//satellites information
		std::vector<satelliteInfo>  satInfo; // incluide pseudorange.etc
		int particle_ID; // the ID for particle
		double scores; // scores for this particle
		//position in llh
		double lon; // latitude
		double lat; // longtutude
		double altitude; // altitude

		grids smGrids; 	//grid at present for shadow matching (plane)
		std::vector<grids> smGridss; // all the grids from first epoch to last epoch (multi-plane)

		//position in ENU
		double E;
		double N;
		double U;
		double ini_lon;
		double ini_lat;
		double ini_alt;

	}particle;
	std::vector<particle> particles; // all th particles at present
	std::vector<std::vector<particle>> particless; // all the particles from first epoch to last epoch
public: // for reading data from kml file
	vector<string> builDataS; // building data in string format
	typedef struct
	{
		double lon;
		double lat;
		double alt;
	}LLH;
	vector <LLH> buildingLLH; // determine a building with a set of lon lat alt
	vector<vector <LLH>> buildingLLHS; // all the buildings

public:  // for generate map
	typedef struct
	{
		double E;
		double N;
		double U;
	}ENU;
	typedef struct // struct for a processed building information
	{
		double center_E; // center position of the building in E direction
		double center_N; // center position of the building in N direction
		double center_U; // center position of the building in U direction
		double ini_lon; // initial reference position for ENU
		double ini_lat; // initial reference position for ENU
		double ini_alt; // initial reference position for ENU
		vector <LLH> buildingLLHV; // building node list (each node with respect to a vertex) in llh
		vector <ENU> buildingENUV; // building node list (each node with respect to a vertex) in ENU
		vector<Point> buildingHull; // building hull points (used for later check if the point is inside the building) in ENU
		double sAzimuth; // smallest azimuth
		double bAzimuth; // bigest azimuth

	} building;
	vector<building> buildingS; // processed building information
	typedef struct  // struct a map
	{
		vector<building> buildingS_M; // processed building information save in a map struct
		// lon lat alt boundary
		double lonMax; // max longitude
		double latMax; // max latitude
		double altMax; // max altitude
		MatrixXd llhMax = MatrixXd::Random(3, 1); // available

		double lonMin; // min longitude
		double latMin; // min latitude
		double altMin; // min altitude
		MatrixXd llhMin = MatrixXd::Random(3, 1); // available

		// ENU boundary
		double EMax; // max E
		double NMax; // max N
		double UMax; // max U
		MatrixXd ENUMax = MatrixXd::Random(3, 1); // available

		double EMin; //  min E
		double NMin; //  min N
		double UMin; //  min U
		MatrixXd ENUMin = MatrixXd::Random(3, 1); // available

		// original llh
		double lonOri; // original lon of ENU (refers to reference point)
		double latOri; // original lat of ENU (refers to reference point)
		double altOri; // original alt of ENU (refers to reference point)
		MatrixXd llhOri = MatrixXd::Random(3, 1);

		// minimum elevation angle and corresponding maxinum searching distance
		double MinElevation = 15; // satellites with a elevation less than 15 degree will not be considered
		double maxSearDis; // the maximum searching distance when finding the insections
		// azimuth search resolution
		double aziReso = 1.0; // azimuth search resolution
		double disReso = 2.0; // grid resolution for skymasks
	}map;
	map map_; // the map saving all the buildings

public:  // for generate sky mask
	typedef struct // mask elevation
	{
		double azimuth; // the azimuth
		double elevation; // the mask elevation
	}elevationMask;

	typedef struct
	{
		LLH posLLH; // pose in LLH
		ENU poseENU; // pose in ENU
		double buildingNum; // how many this position can intersect: if point inside building (buildingNum =1)
		bool insideBuilding; // inside buildings (=1) , outside building (=0)
		vector<elevationMask> aziElemask; // a vector
	}skyMask; // the sky mask in a position (refers to the skyplot in one position)
	vector<skyMask> skyMaskS; // save all the skymsk in one vector

public: // identify the intersection
	// Function used to display X and Y coordinates
	// of a point
	void displayPoint(Point_ P)
	{
		cout << "(" << P.x << ", " << P.y
			<< ")" << endl;
	}

	Point_ lineLineIntersection(Point_ A, Point_ B, Point_ C, Point_ D)
	{
		Point_ result;
		// Line AB represented as a1x + b1y = c1
		double a1 = B.y - A.y;
		double b1 = A.x - B.x;
		double c1 = a1*(A.x) + b1*(A.y);

		// Line CD represented as a2x + b2y = c2
		double a2 = D.y - C.y;
		double b2 = C.x - D.x;
		double c2 = a2*(C.x) + b2*(C.y);

		double determinant = a1*b2 - a2*b1;

		double x = (b2*c1 - b1*c2) / determinant;
		double y = (a1*c2 - a2*c1) / determinant;
		result.x = x;
		result.y = y;
		return result;
	}

	double disPP(Point_ A, Point_ B)
	{
		return (double ) sqrt(((A.x - B.x) * (A.x - B.x) + (A.y - B.y) * (A.y - B.y)));
	}

public:  // time delay
	void wait(int seconds)
	{
		clock_t endwait,start;
		start = clock();
		endwait = clock() + seconds * CLOCKS_PER_SEC;
		while (clock() < endwait) {
			if(clock() - start > CLOCKS_PER_SEC)
			{
				start = clock();
				std::cout<<".......1 s"<<std::endl;
			}
		}
	}

public: // functions
		/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: read kml and extract one building lla into a string
	input: kml file
	output: vector<string> variable
	*/
	void readKml(string filepath) // read kml file
	{
		/* test code*/
		/*ifstream OpenFile("NiuToujiao.kml");
		char ch;
		while (!openfile.eof())
		{
			openfile.get(ch);
			cout << ch;
		}
		openfile.close();*/
		std::cout << "[Function]->readkml read .kml files to strings..." << std::endl;
		ifstream in(filepath);
		string filename;
		string line; // save content of one line
		int lineNum = 0; // line number
		string search = "coordinates"; // searching
		vector<int> cLine; // save line numbers
		vector<string> allLines; // save all the lines of the kml file

	 	if (in) // file is available
		{
			std::cout<<"begin read kml files..."<<std::endl;
				while (getline(in, line)) // line not contain line break (enter)
				{
						lineNum++;
						//cout << line << endl;
						//std::cout << "line number" << lineNum << endl;
						allLines.push_back(line);
						if (line.find(search, 0) != string::npos) {
							//cout << "found: " << search << "line number : " << lineNum << endl;
							cLine.push_back(lineNum-1); //  save line number
						}
				}
				cout << "building amounts:" << cLine.size()/2 << endl; // amount of buildings
				for (auto it = cLine.begin(); it != cLine.end(); it++) {
					int temp = *it;
					//cout << "one building data: " << allLines[temp+1] << endl;
					builDataS.push_back(allLines[temp + 1]);
					it++;
				}
				for (auto it = builDataS.begin(); it != builDataS.end(); it++) {
					cout << "-----------------------------------buildings--------------------------------" << "\n" << *it << "\n" << endl;
				}

		}
		else // file is not available
		{
			cout << "no such kml file" << endl;
		}
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: parse llh from string
	input: vector<string>
	output: vector<vector <LLH>> buildingLLHS; // all the buildings
				// the first [] indicates the building index, the second [] indicates the lla index in one building
	*/
	void string2llh(vector<string> data)// extract the llh from the string for each building
	{
		std::cout << "[Function]->string2llh llh string to llh double..." << std::endl;
		vector<string> aBuilding;
		for (auto it = data.begin(); it != data.end(); it++) { // for one building
			string temp = *it;

			string result;
			stringstream input(temp); // split by blank
			while (input >> result)
				aBuilding.push_back(result);
			for (int i = 0; i<aBuilding.size(); i++) { // for one building again
				cout << "each lon lat alt:  "<<aBuilding[i] << endl;
				cout << "size of this building :  " << aBuilding.size() << endl;
				//split the string 114.1772621294604,22.29842880200087,58 and save it to  :vector <LLH> buildingLLH;
				std::stringstream ss(aBuilding[i]); // split into three string
				vector<string> result;
				while (ss.good())
				{
					string substr;
					getline(ss, substr, ',');
					result.push_back(substr);
				}
				LLH llh_;
				for (int k = 0; k < result.size(); k++)
				{
					//std::cout<<"string result: " << result.at(k) << std::endl;
					string word;
					word = result.at(k);
					double value = strtod(word.c_str(), NULL);
					std::cout << std::setprecision(17);
					//std::cout<< "double result: " << value << '\n';

					if (k == 0) {
						llh_.lon = value;
					}
					else if (k == 1) llh_.lat = value;
					else if (k == 2) llh_.alt = value;
				}
				buildingLLH.push_back(llh_);
			}
			aBuilding.clear(); // clear this after process a building (very important)
			cout << "size of llh vector save in one building llh: " << buildingLLH.size() << "\n" << endl;
			cout << "finish one buildings......................................................................................................................." << endl;
			buildingLLHS.push_back(buildingLLH); // each building save in this vector
			buildingLLH.clear(); // clear this after fully finished one building (very important)

		}
		cout << "finish all buildings, total building number is : " << buildingLLHS.size()<<"\n" << endl; //
		std::cout << std::setprecision(17);
		// the first [] indicates the building index, the second [] indicates the lla index in one building
		cout << std::setprecision(17)<< "test: " << buildingLLHS[0][0].lat - buildingLLHS[0][1].lat << "\n" << endl;
		cout << "-----------------------------------------------3D city map have been read into vector buildingLLHS ----------------------------------------\n\n\n\n\n\n\n\n" << endl; //
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: preparing building data
	input:vector<vector <LLH>> data // the first [] indicates the building index, the second [] indicates the lla index in one building
	output:  prepared building information

	Notation:
			first node of the first building will be choose as the initial reference point for ENU
	*/
	void prepareBuildingData(vector<vector <LLH>> data) // prepare the building information: do some pre-processing
	{
		std::cout << "[Function]->prepareBuildingData transfer builidngs points, convex hull solving..." << std::endl;
		//center of the building

		float count = 0;
		double lonMa=-100000; // lon max
		double latMa=-100000; // lat max
		double altMa=-100000; // alt max

		double lonMi=1000000; // lon min
		double latMi=1000000; // lat min
		double altMi=1000000; // alt min

		for (int i = 0; i < data.size() ; i++) // index all the buildings
		{
			building building_; // processed: a buidling information
			building_.ini_lon = data[0][0].lon; // original lon
			building_.ini_lat = data[0][0].lat; // original lat
			building_.ini_alt = 0; // original alt
			for (int j = 0; j < data[i].size() ; j++) // index the llh in one buildings
			{
				count++;
				building_.buildingLLHV.push_back(data[i][j]); // save llh in struct
				// save ENU
				//****obtain ecef
				Eigen::MatrixXd llh;
				llh.resize(3, 1);
				Eigen::MatrixXd ecef;
				ecef.resize(3, 1);
				llh(0) = data[i][j].lon;
				llh(1) = data[i][j].lat;
				llh(2) = data[i][j].alt;
				ecef = llh2ecef(llh); // position in ecef

				//****obtain ENU
				Eigen::MatrixXd llhO;  //original
				llhO.resize(3, 1);
				//llhO(0) = data[0][0].lon; //original lon for ENU reference
				//llhO(1) = data[0][0].lat; //original lat for ENU reference
				//llhO(2) = data[0][0].alt; //original alt for ENU reference

				llhO(0) = building_.ini_lon; //original lon for ENU reference
				llhO(1) = building_.ini_lat; //original lat for ENU reference
				llhO(2) = building_.ini_alt; //original alt for ENU reference
				Eigen::MatrixXd enu;
				enu.resize(3, 1);
				enu = ecef2enu(llhO, ecef);

				//****save ENU into building struct
				ENU ENU_;
				ENU_.E = enu(0); // save E
				ENU_.N = enu(1); // save N
				ENU_.U = enu(2); // save U
				building_.buildingENUV.push_back(ENU_); // save ENU to
				//cout << "total llh->count: " << count << "    building number->data.size()" << data.size() << "      llh number->data[i].size()" << data[i].size() << "   ecef:->" << ecef << "    enu:->" << enu << "\n"<<endl;

				//obtain the boundary of all the buidlings
				if (llh(0) > lonMa) lonMa = llh(0); // save the largest lon
				if (llh(1) > latMa) latMa = llh(1); // save the largest lat
				if (llh(2) > altMa) altMa = llh(2); // save the largest alt

				if (llh(0) < lonMi) lonMi = llh(0); // save the smallest lon
				if (llh(1) < latMi) latMi = llh(1); // save the smallest lat
				if (llh(2) < altMi) altMi = llh(2); // save the smallest alt

			}
			// solve the convex hull probelm, extract the hull of the building in 2D domain (in ENU)
			vector<Point> buildingNodePointFormat;
			for (int index = 0; index < building_.buildingENUV.size(); index++) // index all the buildings
			{
				Point temp_;
				temp_.x = building_.buildingENUV[index].E;
				temp_.y = building_.buildingENUV[index].N;
				buildingNodePointFormat.push_back(temp_); // save all the node (ENU) into the Point format
				//cout << "temp_x:  " << temp_.x<< "     temp_y:  " << temp_.y<<"\n";
			}
			building_.buildingHull = convex_hull(buildingNodePointFormat);
			cout << "before convex hull : " << buildingNodePointFormat.size() << "      after convex hull: " << building_.buildingHull.size()<<"\n";
			cout << "enu size -------------------------------------------- ->: " << building_.buildingENUV.size() << "\n\n\n\n\n\n\n\n\n";
			buildingS.push_back(building_);
		}
		map_.buildingS_M = buildingS; // save all the buildings into the map
		//save the boundary lon lat alt
		map_.lonMax = lonMa;
		map_.latMax = latMa;
		map_.altMax = altMa;
		map_.llhMax(0) = lonMa;
		map_.llhMax(1) = latMa;
		map_.llhMax(2) = altMa;

		map_.maxSearDis = map_.llhMax(2) / tan(15 * (3.1415926 / 180.0)); // calculate the maximum searching distance

		map_.lonMin = lonMi;
		map_.latMin = latMi;
		map_.altMin = altMi;
		map_.llhMin(0) = lonMi;
		map_.llhMin(1) = latMi;
		map_.llhMin(2) = altMi;

		/* initialize the reference point of the map: two methods
		1. initialize by boundary of the 3D city maps
		2. initialize by the provided ref lat and lon from generateSkymask.launch
		*/
		if(mannually_set_ref_llh_ == false)
		{
			map_.llhOri(0) = data[0][0].lon;
			map_.llhOri(1) = data[0][0].lat;
			map_.llhOri(2) = 0;
		}
		else if(mannually_set_ref_llh_ == true)
		{
			map_.llhOri(0) = ref_lon_fromlaunch;
			map_.llhOri(1) = ref_lat_fromlaunch;
			map_.llhOri(2) = 0;
		}

		//cout << "original point " << map_.llhOri << "\n";
		//save the boundary in ENU : boundary means the maximum and minmum
		Eigen::MatrixXd ecefTemp; // save the max
		ecefTemp.resize(3, 1);
		ecefTemp = llh2ecef(map_.llhMax); // position in ecef
		map_.ENUMax = ecef2enu(map_.llhOri, ecefTemp);
		cout << "map enu max   " << map_.ENUMax<<"  map_.llhOri-> "<<map_.llhOri<<"\n";

		ecefTemp.resize(3, 1); // save the min
		ecefTemp = llh2ecef(map_.llhMin); // position in ecef
		map_.ENUMin = ecef2enu(map_.llhOri, ecefTemp);
		cout << "map enu min " << map_.ENUMin << "\n";

		cout << "-----------map information--------" << map_.buildingS_M.size() << endl;
		/*
		map_.lonMax :-122.3002392264451      map_.latMax 37.901411108972418
		map_.lonMin :-122.302402266828      map_.latMin 37.900422296017631
		*/
		cout << "map_.lonMax :" << map_.lonMax << "      map_.latMax " << map_.latMax << "      map_.lonMin :" << map_.lonMin << "      map_.latMin " << map_.latMin<<"\n\n";

		cout << "-------------------------------------------------finish preparing all the buildings information and is saved to a map struct: ->  map_------------------------------------------------\n\n";
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: llh to ecef
	input: llh (Matrix3d)
	output: ecef (Matrix3d)
	*/
	Eigen::MatrixXd llh2ecef(Eigen::MatrixXd data) // transform the llh to ecef
	{
		Eigen::MatrixXd ecef; // the ecef for output
		ecef.resize(3, 1);
		double a = 6378137.0;
		double b = 6356752.314;
		double n, Rx, Ry, Rz;
		double lon = (double)data(0) * 3.1415926 / 180.0; // lon to radis
		double lat  = (double)data(1) * 3.1415926 / 180.0; // lat to radis
		double alt  = (double)data(2); // altitude
		n = a * a / sqrt(a * a * cos(lat) * cos(lat) + b * b * sin(lat) * sin(lat));
		Rx = (n + alt) * cos(lat) * cos(lon);
		Ry = (n + alt) * cos(lat) * sin(lon);
		Rz = (b * b / (a * a) * n + alt) * sin(lat);
		ecef(0) = Rx; // return value in ecef
		ecef(1) = Ry; // return value in ecef
		ecef(2) = Rz; // return value in ecef
		return ecef;

		/**************for test purpose*************************
		Eigen::MatrixXd llh;
		llh.resize(3, 1);
		Eigen::MatrixXd ecef;
		ecef.resize(3, 1);
		llh(0) = 114.1772621294604;
		llh(1) = 22.29842880200087;
		llh(2) = 58;
		ecef = llh2ecef(llh);
		cout << "ecef ->: " << ecef << "\n";
		*/
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: ecef to llh
	input: ecef (Matrix3d)
	output: llh (Matrix3d)
	*/
	Eigen::MatrixXd ecef2llh(Eigen::MatrixXd data) // transform the ecef to llh
	{
		Eigen::MatrixXd llh; // the ecef for output
		double pi = 3.1415926; // pi
		llh.resize(3, 1);
		double x = data(0); // obtain ecef
		double y = data(1);
		double z = data(2);
		double x2 = pow(x, 2);
		double y2 = pow(y, 2);
		double z2 = pow(z, 2);

		double a = 6378137.0000; //earth radius in meters
		double b = 6356752.3142; // earth semiminor in meters
		double e = sqrt(1 - (b / a) * (b / a));
		double b2 = b*b;
		double e2 = e*e;
		double  ep = e*(a / b);
		double  r = sqrt(x2 + y2);
		double  r2 = r*r;
		double  E2 = a * a - b*b;
		double F = 54 * b2*z2;
		double G = r2 + (1 - e2)*z2 - e2*E2;
		double c = (e2*e2*F*r2) / (G*G*G);
		double s = (1 + c + sqrt(c*c + 2 * c));
		s = pow(s, 1 / 3);
		double P = F / (3 * ((s + 1 / s + 1)*(s + 1 / s + 1)) * G*G);
		double Q = sqrt(1 + 2 * e2*e2*P);
		double ro = -(P*e2*r) / (1 + Q) + sqrt((a*a / 2)*(1 + 1 / Q) - (P*(1 - e2)*z2) / (Q*(1 + Q)) - P*r2 / 2);
		double tmp = (r - e2*ro)*(r - e2*ro);
		double U = sqrt(tmp + z2);
		double V = sqrt(tmp + (1 - e2)*z2);
		double zo = (b2*z) / (a*V);

		double height = U*(1 - b2 / (a*V));

		double lat = atan((z + ep*ep*zo) / r);

		double temp = atan(y / x);
		double long_;
		if (x >= 0)
			long_ = temp;
		else if ((x < 0) && (y >= 0))
			long_ = pi + temp;
		else
			long_ = temp - pi;
		llh(0) = (long_)*(180 / pi);
		llh(1) = (lat)*(180 / pi);
		llh(2) = height;
		return llh;

		/**************for test purpose*************************
		Eigen::MatrixXd ecef;
		ecef.resize(3, 1);
		Eigen::MatrixXd llh;
		llh.resize(3, 1);
		ecef(0) = -2418080.9387265667;
		ecef(1) = 5386190.3905763263;
		ecef(2) = 2405041.9305451373;
		llh = ecef2llh(ecef);
		cout << "llh ->: " << llh << "\n";
		*/
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: ecef to enu
	input: original llh, and current ecef (Matrix3d)
	output: enu (Matrix3d)
	*/
	Eigen::MatrixXd ecef2enu(Eigen::MatrixXd originllh, Eigen::MatrixXd ecef) // transform the ecef to enu
	{
		double pi = 3.1415926; // pi
		double DEG2RAD = pi / 180.0;
		double RAD2DEG = 180.0 / pi;

		Eigen::MatrixXd enu; // the enu for output
		enu.resize(3, 1); // resize to 3X1
		Eigen::MatrixXd oxyz; // the original position
		oxyz.resize(3, 1); // resize to 3X1

		double x, y, z; // save the x y z in ecef
		x = ecef(0);
		y = ecef(1);
		z = ecef(2);

		double ox, oy, oz; // save original reference position in ecef
		oxyz = llh2ecef(originllh);
		ox = oxyz(0); // obtain x in ecef
		oy = oxyz(1); // obtain y in ecef
		oz = oxyz(2); // obtain z in ecef

		double dx, dy, dz;
		dx = x - ox;
		dy = y - oy;
		dz = z - oz;

		double lonDeg, latDeg, _; // save the origin lon alt in llh
		lonDeg = originllh(0);
		latDeg = originllh(1);
		double lon = lonDeg * DEG2RAD;
		double lat = latDeg * DEG2RAD;

		//save ENU
		enu(0) = -sin(lon) * dx + cos(lon) * dy;
		enu(1) = -sin(lat) * cos(lon) * dx - sin(lat) * sin(lon) * dy + cos(lat) * dz;
		enu(2) = cos(lat) * cos(lon) * dx + cos(lat) * sin(lon) * dy + sin(lat) * dz;
		return enu;

		/**************for test purpose*****suqare distance is about 37.4 meters********************
		Eigen::MatrixXd llh;  //original
		llh.resize(3, 1);
		llh(0) = 114.1775072541416;
		llh(1) = 22.29817969722738;
		llh(2) = 58;
		Eigen::MatrixXd ecef;
		ecef.resize(3, 1);
		ecef(0) = -2418080.9387265667;
		ecef(1) = 5386190.3905763263;
		ecef(2) = 2405041.9305451373;
		Eigen::MatrixXd enu;
		enu.resize(3, 1);
		enu = ecef2enu(llh, ecef);
		cout << "enu ->: " << enu << "\n";
		*/
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: identify if one point (given in ENU) is inside a building
	input: map mapInput, Eigen::MatrixXd ENU
	output: yes or no (bool)
	*/
	bool identifyPointInsideBuildingENU(map mapInput, Eigen::MatrixXd ENU) // identify if one point is inside the building (ENU)
	{
		//
		bool result=0; // 1: inside a builidng
		Point_  ENUPoint = { ENU (0),ENU(1)}; // prepare the pont needed to be identified

		//index all the buildings
		for (int i = 0; i < mapInput.buildingS_M.size(); i++) // index all the buildings in the map struct
		{
				building building_; // processed: a buidling information
				Point_ pointTemp; // temp
				vector<Point_> buildingPolygon; // save all the convex hull node
				for (int j = 0; j < mapInput.buildingS_M[i].buildingHull.size(); j++) // index the every point in building hull (ENU) in one buildings
				{
					//cout << " ENU--------mapInput.buildingS_M[i].buildingHull[j].x> " << mapInput.buildingS_M[i].buildingHull[j].x << "       ENU--------mapInput.buildingS_M[i].buildingHull[j].x> " << mapInput.buildingS_M[i].buildingHull[j].y<<"\n";
					pointTemp.x = mapInput.buildingS_M[i].buildingHull[j].x;
					pointTemp.y = mapInput.buildingS_M[i].buildingHull[j].y;
					buildingPolygon.push_back(pointTemp);
				}
				if (isInside(buildingPolygon, buildingPolygon.size(), ENUPoint))
				{
					//isInside(buildingPolygon, buildingPolygon.size(), ENUPoint) ? cout << "Yes in building ->:  \n" : cout << "Not in building \n";
					return isInside(buildingPolygon, buildingPolygon.size(), ENUPoint);
				}

				//isInside(buildingPolygon, buildingPolygon.size(), ENUPoint) ? cout << "Yes in building ->:  \n" : cout << "Not in building \n";
				//cout << "mapInput.buildingS_M[i].buildingHull.size()--------> " << mapInput.buildingS_M[i].buildingHull.size() << "\n\n";
		}

		/**************for test purpose*************************
		vector<Point_> polygon1 = { { 0, 0 },{ 10, 0 },{ 10, 10 },{ 0, 10 } };
		//int n = sizeof(polygon1) / sizeof(polygon1[0]);
		int n = polygon1.size();
		Point_ p = { 20, 20 };
		isInside(polygon1, n, p) ? cout << "Yes \n" : cout << "No \n";

		p = { 5, 5 };
		isInside(polygon1, n, p) ? cout << "Yes \n" : cout << "No \n";

		vector<Point_> polygon2 = { { 0, 0 },{ 5, 5 },{ 5, 0 } };
		p = { 3, 3 };
		//n = sizeof(polygon2) / sizeof(polygon2[0]);
		n = polygon2.size();
		isInside(polygon2, n, p) ? cout << "Yes \n" : cout << "No \n";

		p = { 5, 1 };
		isInside(polygon2, n, p) ? cout << "Yes \n" : cout << "No \n";

		p = { 8, 1 };
		isInside(polygon2, n, p) ? cout << "Yes \n" : cout << "No \n";

		vector<Point_> polygon3 = { { 0, 0 },{ 10, 0 },{ 10, 10 },{ 0, 10 } };
		p = { -1,10 };
		//n = sizeof(polygon3) / sizeof(polygon3[0]);
		n = polygon3.size();
		isInside(polygon3, n, p) ? cout << "Yes \n" : cout << "No \n";

		*/
		return result;
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: identify if one point (given in LLH) is inside a building
	input: map mapInput, Eigen::MatrixXd LLH
	output: yes or no (bool)

	Notation:
		1: change LLH into ENU
		2. use identifyPointInsideBuildingENU subsequently
	*/
	bool identifyPointInsideBuildingLLH(map mapInput, Eigen::MatrixXd LLH) // identify if one point is inside the building (LLH)
	{
		//
		bool result = 0; // 1: inside a builidng
		// obtain ENU
		//****obtain ecef
		Eigen::MatrixXd ecef;
		ecef.resize(3, 1);
		ecef = llh2ecef(LLH); // position in ecef

		//****obtain ENU
		Eigen::MatrixXd llhO;  //original
		llhO.resize(3, 1);
		llhO(0) = mapInput.llhOri(0); //original lon for ENU reference
		llhO(1) = mapInput.llhOri(1); //original lat for ENU reference
		llhO(2) = mapInput.llhOri(2); //original alt for ENU reference
		Eigen::MatrixXd enu;
		enu.resize(3, 1);
		enu = ecef2enu(llhO, ecef);

		Point_  ENUPoint = { enu(0),enu(1) }; // prepare the pont needed to be identified
		result = identifyPointInsideBuildingENU(mapInput, enu);
		return result;
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: get the mask elevation angle at a position with certain azimuth
	input: map mapInput, Eigen::MatrixXd LLH, double azimuth
	output: Eigen::MatrixXd (azimuth, maximum elevation angle , nearest intersect building number)

	*/
	Eigen::MatrixXd getMaskElevationFromLLH(map mapInput, Eigen::MatrixXd LLH, double azimuth) // get the maximum elevation angle at a certain position (LLH) with a certain azimuth
	{
		Eigen::MatrixXd result; // result(0)->azimuth  result(1)->elevation
		result.resize(3,1);
		result(2) = 10000;
		double Maxelevation = 0;
		double minDistance = 10000;

		// obtain ENU
		//****obtain ecef
		Eigen::MatrixXd ecef;
		ecef.resize(3, 1);
		ecef = llh2ecef(LLH); // position in ecef

		 //****obtain ENU of the given position
		Eigen::MatrixXd llhO;  //original
		llhO.resize(3, 1);
		llhO(0) = mapInput.llhOri(0); //original lon for ENU reference
		llhO(1) = mapInput.llhOri(1); //original lat for ENU reference
		llhO(2) = mapInput.llhOri(2); //original alt for ENU reference
		Eigen::MatrixXd enu;
		enu.resize(3, 1);
		enu = ecef2enu(llhO, ecef);

		result = getMaskElevationFromENU(mapInput, enu, azimuth);
		return result;
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: get the mask elevation angle at a position with certain azimuth
	input: map mapInput, Eigen::MatrixXd ENU, double azimuth
	output: Eigen::MatrixXd (azimuth, maximum elevation angle  , nearest intersect building number)

	*/
	Eigen::MatrixXd getMaskElevationFromENU(map mapInput, Eigen::MatrixXd ENU, double azimuth) // get the maximum elevation angle at a certain position (LLH) with a certain azimuth
	{
		Eigen::MatrixXd result; // result(0)->azimuth  result(1)->elevation
		result.resize(3, 1);
		double Maxelevation = 0; // maximum elevation of blockage
		double minDistance = 10000;

		// obtain ENU
		Eigen::MatrixXd enu;
		enu.resize(3, 1);
		enu = ENU;

		Eigen::MatrixXd enuEnd; // the segment end
		enuEnd.resize(3, 1);
		enuEnd(0) = enu(0) + mapInput.maxSearDis * sin(azimuth * (3.1415926 / 180.0));
		enuEnd(1) = enu(1) + mapInput.maxSearDis * cos(azimuth * (3.1415926 / 180.0));
		Point_ p1;
		Point_ q1;
		Point_ p2;
		Point_ q2;
		p1.x = enu(0);
		p1.y = enu(1);
		q1.x = enuEnd(0);
		q1.y = enuEnd(1);

		////index all the buildings in the maps
		for (int i = 0; i < mapInput.buildingS_M.size(); i++) // index all the buildings in the map struct
		{
			building building_; // processed: a buidling information
			Point_ p2; // temp
			vector<Point_> buildingPolygon; // save all the convex hull node
											//#pragma omp parallel for
			for (int j = 0; j < mapInput.buildingS_M[i].buildingENUV.size() - 1; j++) // index the every point in building hull (ENU) in one buildings
			{
				p2.x = mapInput.buildingS_M[i].buildingENUV[j].E;
				p2.y = mapInput.buildingS_M[i].buildingENUV[j].N;

				q2.x = mapInput.buildingS_M[i].buildingENUV[j + 1].E;
				q2.y = mapInput.buildingS_M[i].buildingENUV[j + 1].N;

				if (doIntersect(p1, q1, p2, q2)) // identify if intersect between line segment p1q1 and line segment p2q2
				{
					// intersection is available

					result.resize(3, 1);
					Point_ intersection;
					intersection = lineLineIntersection(p1, q1, p2, q2); // obtain the intersection in ENU
					double height = mapInput.buildingS_M[i].buildingENUV[j].U;
					double disP12Insec = disPP(p1, intersection); // distance from p1 to intersection
					if (disP12Insec < minDistance)  result(2) = i; // the building with shortest distance to point with intersection
					double elevation_ = 180.0 / 3.1415926 * atan(height / disP12Insec); // calculate the elevation
					//cout << "intersection available---------------------------------------------------: "<< disP12Insec<<"       elevationMax: "<< elevation_ <<"\n\n";
					if (elevation_ > Maxelevation)  Maxelevation = elevation_;
				}

			}
		}
		result(0) = azimuth;
		result(1) = Maxelevation;
		//cout << "azimuth: " << azimuth << "             Maxelevation---------------------------------: " << Maxelevation << "\n\n";
		return result;
	}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: create skymask
	input: map mapInput
	output: skymasks

	*/
	void createSkymasks() // generate all the skymask
	{
		//generater all the skymasks
		std::cout << "[Function]->createSkymasks begin generate skymask .........." << std::endl;
		//_sleep(10000); // pauses for 10 seconds
		double deltaE = map_.ENUMax(0) - map_.ENUMin(0);
		std::cout<<"map_.ENUMax(0)->"<<map_.ENUMax(0)<<"     map_.ENUMin(0)"<<map_.ENUMin(0)<<std::endl;
		double deltaN = map_.ENUMax(1) - map_.ENUMin(1);
		double skymaskNum = deltaN*deltaE / (pow(map_.disReso, 2));
		double finishNum = 0;
		cout << "delta E ->:  " << deltaE << "             delta N->" << deltaN<<"\n\n\n";
		wait(1);
		for (double N = map_.ENUMin(1); N <= map_.ENUMax(1); N = N + map_.disReso) // search in the by line (N direction)
		{
			for (double E = map_.ENUMin(0); E <=map_.ENUMax(0); E = E + map_.disReso) // search in the by line (E direction)
			{
		//for (double N = map_.ENUMin(1); N <= map_.ENUMin(1) + 3; N = N + map_.disReso) // search in the by line (N direction)
		//{
		//	for (double E = map_.ENUMin(0); E <= map_.ENUMin(0) + 3; E = E + map_.disReso) // search in the by line (E direction)
		//	{
				bool insideBuildings = 0; // inside buildings ?
				// generate the skymask of one position in 360 degree
				Eigen::MatrixXd ENU;
				ENU.resize(1, 3);
				ENU(0) = E;
				ENU(1) = N;
				ENU(2) = 0;
				skyMask skyMask_;

				const clock_t begin_time = clock();

				// identify if this point is inside the building first
				if (identifyPointInsideBuildingENU(map_, ENU)) // if a point is inside a building, set elevation (result(1)) to -1.
				{
					insideBuildings = 1; // this justification rule sometimes does not wok,
				}

				if (insideBuildings != 1) // not inside the buildings
				{
					cout << "outside the building      ";
					for (double azi = 0; azi <= 360; azi = azi + map_.aziReso)
					{
						Eigen::MatrixXd aziEleResult;
						aziEleResult.resize(3, 1);
						elevationMask elevation_;
						aziEleResult = getMaskElevationFromENU(map_, ENU, azi);
						elevation_.azimuth = aziEleResult(0);
						elevation_.elevation = aziEleResult(1);

						skyMask_.poseENU.E = ENU(0);
						skyMask_.poseENU.N = ENU(1);
						skyMask_.poseENU.U = ENU(2);
						skyMask_.insideBuilding = 0; // outside the building
						skyMask_.aziElemask.push_back(elevation_);
					}
				}
				if (insideBuildings == 1) // inside the buildings
				{
					cout << "inside the building      ";
					for (double azi = 0; azi <= 360; azi = azi + map_.aziReso)
					{
						Eigen::MatrixXd aziEleResult;
						aziEleResult.resize(2, 1);
						elevationMask elevation_;
						aziEleResult(0) = azi;
						aziEleResult(1) = -1;
						elevation_.azimuth = aziEleResult(0);
						elevation_.elevation = aziEleResult(1);

						skyMask_.poseENU.E = ENU(0);
						skyMask_.poseENU.N = ENU(1);
						skyMask_.poseENU.U = ENU(2);
						skyMask_.insideBuilding = 1; // inside the building
						skyMask_.aziElemask.push_back(elevation_);
					}
				}

				finishNum++;
				skyMaskS.push_back(skyMask_);
				cout <<"total skymasks-> "<< skymaskNum << "finish sky mask->  " << skyMaskS.size() << "      rest skymask-> "<< skymaskNum - finishNum;
				std::cout << "       this sky mask used  time -> " << float(clock() - begin_time) / CLOCKS_PER_SEC << "\n\n";
			}
		}
		cout << "finishing generate skymask...\n";

	}

	//void saveSkymasks2SCV(map mapInput) // save all the skymask into a csv file
	//{
	//	cout << "save skymask to csv file" << "\n";
	//	std::ofstream out("skymask.txt");
	//	std::ostringstream strs;
	//	// index the
	//	for (int grid = 0; grid <skyMaskS.size(); grid = grid + 1) // index grid
	//	{
	//		strs << "<skymask>";
	//		strs << "\n";
	//		strs << skyMaskS[grid].poseENU.E;
	//		strs << ",";
	//		strs << skyMaskS[grid].poseENU.N;
	//		strs << "\n";
	//		for (double sk = 0; sk < skyMaskS[grid].aziElemask.size(); sk = sk + 1) // index skymask
	//		{
	//			//string content;
	//			strs << skyMaskS[grid].aziElemask[sk].azimuth;// save azimuth
	//			strs << ",";
	//			strs<< skyMaskS[grid].aziElemask[sk].elevation;  // save elevation
	//			strs << ",";
	//			strs << skyMaskS[grid].insideBuilding;  // inside building
	//			strs << "  ";
	//		}
	//		strs << "\n";
	//		strs << "<skymask>";
	//		strs << "\n";
	//		strs << "\n";
	//	}
	//	string head; // save the ENU
	//	head = strs.str();
	//	out << head;
	//	out.close(); // finish save file
	//
	//
	//
	//	////save data to txt (from double to string first)
	//	//std::ofstream out("output.txt");
	//	//std::ostringstream strs;
	//	//double a = value * 100000;
	//	//strs << a;
	//	//std::string str = strs.str();
	//	//out << str;
	//	//out.close();
	//
	//}

	/*
	author: WEN Weisong (17902061r@connect.polyu.hk)
	function: save skymask to CSV file
	input: map mapInput
	output: skymasks

	*/
	void saveSkymasks2SCV(map mapInput,string saved_skymask_dir) // save all the skymask into a csv file
	{
		std::cout << "[Function]->saveSkymasks2SCV save skymask to csv file.........." << std::endl;
		/*std::ofstream out("skymask.txt");
		std::ostringstream strs;*/
		// index the
		for (int grid = 0; grid <skyMaskS.size(); grid = grid + 1) // index grid £¨one grid one skymask file£©
		{
			string filename;
			std::ostringstream filenameStrs; //
			// filenameStrs << "/home/wenws/amsipolyu/src/rtklibros/app/generate_skymask/skymask/";
			filenameStrs << saved_skymask_dir;
			filenameStrs << int(skyMaskS[grid].poseENU.E);
			filenameStrs << " ";
			filenameStrs << int(skyMaskS[grid].poseENU.N);
			filenameStrs << ".txt";
			filename = filenameStrs.str();
			std::ofstream out(filename);
			std::ostringstream strs;
			std::cout << std::setprecision(17);
			double lon = mapInput.altMax;
			strs << map_.llhOri(0);
			strs << ",";
			strs << map_.llhOri(1);
			strs << ",";
			strs << map_.llhOri(2);
			strs << "\n";
			strs << "<skymask>";
			strs << "\n";
			strs << skyMaskS[grid].poseENU.E; // E in ENU
			strs << ",";
			strs << skyMaskS[grid].poseENU.N; // N in ENU
			strs << ",";
			strs << skyMaskS[grid].insideBuilding;  // inside building
			strs << ",";
			strs << 1;  // azimuth resolution
			strs << "\n";
			for (double sk = 0; sk < skyMaskS[grid].aziElemask.size(); sk = sk + 1) // index skymask
			{
				//string content;
				strs << skyMaskS[grid].aziElemask[sk].elevation;  // save elevation
				strs << "  ";
			}
			strs << "\n";
			strs << "<skymask>";
			strs << "\n";
			strs << "\n";

			if (!skyMaskS[grid].insideBuilding)
			{
				string head; // save the ENU
				head = strs.str();
				out << head;
				out.close(); // finish save file
			}

		}
		std::cout << "saved all the skymasks to certain directory.........." << std::endl;
	}

private:
	int reserve1;

	bool mannually_set_ref_llh_;
	double ref_lat_fromlaunch;
	double ref_lon_fromlaunch;
};


///////////////////////////////////////////////////////////////////////////////
// contingency_table2.cpp                                                    //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <cmath>
#include <string>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/vector/vector10.hpp>
#include <boost/mpl/detail/wrapper.hpp>

#include <boost/typeof/typeof.hpp>

#include <boost/assign/list_of.hpp>

#include <boost/fusion/container/map/detail/sequence_to_map.hpp>
#include <boost/fusion/include/make_map.hpp>

#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/framework/parameters/weight.hpp>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/statistics/detail/weighted_count.hpp>

#include <boost/statistics/detail/non_parametric/contingency_table/include/factor.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/include/pearson_chisq/independence.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/include/pearson_chisq/common.hpp>

// Analysis of a dataset, and check that the results agree with the R software.
// Source : http://www.math.wustl.edu/~victor/classes/ma322/r-eg-20.txt

/*
# Read the count data for this problem
count<-scan()
   12  34  23         4  47  11
   35  31  11        34  10  18
   12  32   9        18  13  19
   12  12  14         9  33  25

# Create factor tags:r=rows, c=columns, t=tiers
r <- factor(gl(4, 2*3, 2*3*4, labels=c("r1","r2", "r3", "r4")));  r
c <- factor(gl(3, 1,   2*3*4, labels=c("c1","c2", "c3")));  c
t <- factor(gl(2, 3,   2*3*4, labels=c("t1","t2")));  t

# Cross-tabulation of counts:
xtabs(count~r+c+t)           # all three factors
xtabs(count~r+c)             # RC: sum over tiers
xtabs(count~r+t)             # RT: sum over columns
xtabs(count~c+t)             # CT: sum over rows
xtabs(count~r)       # R: sum over columns and tiers
xtabs(count~c)       # C: sum over rows and tiers
xtabs(count~t)       # T: sum over rows and columns

# 3-way Chi squared test of independence
summary(xtabs(count~r+c+t))
# 2-way Chi squared test of partial independence: R versus CT
summary(xtabs(count~c+t))
# 2-way Chi squared test of partial independence: C versus RT
summary(xtabs(count~r+t))
# 2-way Chi squared test of partial independence: T versus RC
summary(xtabs(count~r+c))
*/

void test_contingency_table2()
{

	namespace ct = boost::statistics::detail::contingency_table;
    namespace ps = ct::pearson_chi_square_statistic;

    typedef double val_;
    typedef boost::mpl::int_<0> r_; typedef std::string data_r_;
    typedef boost::mpl::int_<1> c_; typedef std::string data_c_;
    typedef boost::mpl::int_<2> t_; typedef std::string data_t_;

    typedef boost::fusion::detail::sequence_to_map<
        boost::mpl::vector6<r_,data_r_,c_,data_c_,t_,data_t_>
    >::type sample_;

    typedef boost::mpl::vector3<r_,c_,t_> all_three_factors_;
    typedef boost::mpl::vector2<r_,c_> sum_over_tiers_;
    typedef boost::mpl::vector2<r_,t_> sum_over_cols_;
    typedef boost::mpl::vector2<c_,t_> sum_over_rows_;

    typedef ps::tag::independence_between<all_three_factors_> indep_r_c_t_;
    typedef ps::tag::independence_between<sum_over_tiers_> indep_r_c_;
    typedef ps::tag::independence_between<sum_over_cols_> indep_r_t_;
    typedef ps::tag::independence_between<sum_over_rows_> indep_c_t_;

    typedef boost::accumulators::stats<
        indep_r_c_t_,
        indep_r_c_,
        indep_r_t_,
        indep_c_t_
    > stats_;
    typedef boost::accumulators::accumulator_set< sample_, stats_, long int > acc_;

    using namespace boost::assign;
    acc_ acc(( ct::_map_of_levels = boost::fusion::make_map<r_,c_,t_>(
        list_of("r1")("r2")("r3")("r4"),
        list_of("c1")("c2")("c3"),
        list_of("t1")("t2") )
    ));

    typedef boost::fusion::result_of::make_map<
        r_,c_,t_,data_r_,data_c_,data_t_>::type result_of_make_map_;
    typedef result_of_make_map_(*fp_)(const data_r_&,const data_c_&,const data_t_&);

    fp_ make_sample = boost::fusion::make_map<r_,c_,t_>;

    {
        using namespace boost::accumulators;

        acc( make_sample( "r1", "c1", "t1" ), weight = 12 );
        acc( make_sample( "r1", "c1", "t2" ), weight =  4 );
        acc( make_sample( "r1", "c2", "t1" ), weight = 34 );
        acc( make_sample( "r1", "c2", "t2" ), weight = 47 );
        acc( make_sample( "r1", "c3", "t1" ), weight = 23 );
        acc( make_sample( "r1", "c3", "t2" ), weight = 11 );
        acc( make_sample( "r2", "c1", "t1" ), weight = 35 );
        acc( make_sample( "r2", "c1", "t2" ), weight = 34 );
        acc( make_sample( "r2", "c2", "t1" ), weight = 31 );
        acc( make_sample( "r2", "c2", "t2" ), weight = 10 );
        acc( make_sample( "r2", "c3", "t1" ), weight = 11 );
        acc( make_sample( "r2", "c3", "t2" ), weight = 18 );
        acc( make_sample( "r3", "c1", "t1" ), weight = 12 );
        acc( make_sample( "r3", "c1", "t2" ), weight = 18 );
        acc( make_sample( "r3", "c2", "t1" ), weight = 32 );
        acc( make_sample( "r3", "c2", "t2" ), weight = 13 );
        acc( make_sample( "r3", "c3", "t1" ), weight =  9 );
        acc( make_sample( "r3", "c3", "t2" ), weight = 19 );
        acc( make_sample( "r4", "c1", "t1" ), weight = 12 );
        acc( make_sample( "r4", "c1", "t2" ), weight =  9 );
        acc( make_sample( "r4", "c2", "t1" ), weight = 12 );
        acc( make_sample( "r4", "c2", "t2" ), weight = 33 );
        acc( make_sample( "r4", "c3", "t1" ), weight = 14 );
        acc( make_sample( "r4", "c3", "t2" ), weight = 25 );
    }

    using namespace std;
    val_ stat, p_value;
    long df;

/*
#### Output and interpretation (at significance level alpha=0.05):
   # 3-way Chi squared test of independence
   > summary(xtabs(count~r+c+t))
   Call: xtabs(formula = count ~ r + c + t)
   Number of cases in table: 478
   Number of factors: 3
   Test for independence of all factors:
	   Chisq = 102.17, df = 17, p-value = 3.514e-14
#
# ==> reject H0 in favor of
#            HA: r,c,t are NOT mutually independent
#
*/

   {
        typedef boost::mpl::detail::wrapper<indep_r_c_t_> h0_;
        df = ps::degrees_of_freedom( h0_(), acc);
        stat = ps::value( h0_(), acc );
        p_value = cdf( complement(
            ps::asy_distribution( h0_(), acc ),
            stat
        ) );
        BOOST_CHECK( df == 17 );
        BOOST_CHECK( fabs( stat - 102.17 ) < 0.01 );
        BOOST_CHECK( fabs( p_value - 3.514e-14 ) < 0.001e-14 );
    }

/*
   > # 2-way Chi squared test of partial independence: R versus CT
   > summary(xtabs(count~c+t))
   Call: xtabs(formula = count ~ c + t)
   Number of cases in table: 478
   Number of factors: 2
   Test for independence of all factors:
	   Chisq = 2.3704, df = 2, p-value = 0.3057
#
# ==> do not reject H0: c,t are mutually independent
#
*/

   {
        typedef boost::mpl::detail::wrapper<indep_c_t_> h0_;
        df = ps::degrees_of_freedom( h0_(), acc);
        stat = ps::value( h0_(), acc );
        p_value = cdf( complement(
            ps::asy_distribution( h0_(), acc ),
            stat
        ) );
        BOOST_CHECK( df == 2 );
        BOOST_CHECK( fabs( stat == 2.3704 )< 0.001 );
        BOOST_CHECK( fabs( p_value - 0.3057 ) < 0.0001 );
    }
/*
   >
   > # 2-way Chi squared test of partial independence: C versus RT
   > summary(xtabs(count~r+t))
   Call: xtabs(formula = count ~ r + t)
   Number of cases in table: 478
   Number of factors: 2
   Test for independence of all factors:
	   Chisq = 10.057, df = 3, p-value = 0.01809
#
# ==> reject H0 in favor of
#            HA: r,t are NOT mutually independent

*/
    {
        typedef boost::mpl::detail::wrapper<indep_r_t_> h0_;
        df = ps::degrees_of_freedom( h0_(), acc);
        stat = ps::value( h0_(), acc );
        p_value = cdf( complement(
            ps::asy_distribution( h0_(), acc ),
            stat
        ) );
        BOOST_CHECK( df == 3 );
        BOOST_CHECK( fabs( stat - 10.057 ) < 0.001 );
        BOOST_CHECK( fabs( p_value - 0.01809 ) < 0.00001 );
    }
/*
   > # 2-way Chi squared test of partial independence: T versus RC
   > summary(xtabs(count~r+c))
   Call: xtabs(formula = count ~ r + c)
   Number of cases in table: 478
   Number of factors: 2
   Test for independence of all factors:
	   Chisq = 58.67, df = 6, p-value = 8.363e-11

#
# ==> reject H0 in favor of
#            HA: r,c are NOT mutually independent
#
*/

   {
        typedef boost::mpl::detail::wrapper<indep_r_c_> h0_;
        df = ps::degrees_of_freedom( h0_(), acc);
        stat = ps::value( h0_(), acc );
        p_value = cdf( complement(
            ps::asy_distribution( h0_(), acc ),
            stat
        ) );
        BOOST_CHECK( df == 6 );
        BOOST_CHECK( fabs( stat - 58.67 ) < 0.01 );
        BOOST_CHECK( fabs( p_value - 8.363e-11 ) < 0.001e-11 );
    }

}


/*

 [begin_description]
 Test case for issue 149:
 Error C2582 with msvc-10 when using iterator-based integration
 [end_description]

 Copyright 2011-2015 Karsten Ahnert
 Copyright 2011-2015 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

// disable checked iterator warning for msvc

#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_regression_147

#include <utility>
#include <iostream>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/range/algorithm/find_if.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/algebra/fusion_algebra.hpp>
#include <boost/numeric/odeint/algebra/fusion_algebra_dispatcher.hpp>

#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/acceleration.hpp>
#include <boost/units/systems/si/io.hpp>

#include <boost/fusion/container.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

namespace fusion = boost::fusion;
namespace units = boost::units;
namespace si = boost::units::si;

typedef units::quantity< si::time , double > time_type;
typedef units::quantity< si::length , double > length_type;
typedef units::quantity< si::velocity , double > velocity_type;
typedef units::quantity< si::acceleration , double > acceleration_type;
typedef units::quantity< si::frequency , double > frequency_type;

typedef fusion::vector< length_type , velocity_type > state_type;
typedef fusion::vector< velocity_type , acceleration_type > deriv_type;

struct oscillator
{
    frequency_type m_omega;

    oscillator( const frequency_type &omega = 1.0 * si::hertz ) : m_omega( omega ) { }

    void operator()( const state_type &x , deriv_type &dxdt , time_type t ) const
    {
        fusion::at_c< 0 >( dxdt ) = fusion::at_c< 1 >( x );
        fusion::at_c< 1 >( dxdt ) = - m_omega * m_omega * fusion::at_c< 0 >( x );
    }
};

BOOST_AUTO_TEST_CASE( regression_168 )
{
    typedef runge_kutta_dopri5< state_type , double , deriv_type , time_type > stepper_type;

    state_type x( 1.0 * si::meter , 0.0 * si::meter_per_second );

    integrate_const( make_dense_output( 1.0e-6 , 1.0e-6 , stepper_type() ) , oscillator( 2.0 * si::hertz ) ,
                     x , 0.0 * si::second , 100.0 * si::second , 0.1 * si::second);
}


#include "expt.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include "utility/itoa.hpp"
#include <boost/variant.hpp>
#include <cmath>
#include <stdexcept>
#include <algorithm>
#include <complex>

namespace HT
{
    void expt(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=3)
          throw std::runtime_error("expt should have exact 2 parameters");
        auto & thirdCh = *astnode->ch.rbegin();
        auto & secondCh = *(++astnode->ch.begin());
        ph.parse(secondCh);
        ph.parse(thirdCh);
        if (secondCh->token.tokenType!=Complex )
          throw std::runtime_error("The arguments of expt should be Complex");

        if (thirdCh->token.tokenType!=Complex )
          throw std::runtime_error("The arguments of expt should be Complex");

        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto cast =boost::get<ComplexType>(secondCh->token.info);
        auto third = boost::get<ComplexType>(thirdCh->token.info);
        cast.toinexact();
        third.toinexact();

        if (cast == 0)
            astnode->token.info = ComplexType(0.0);
        else
        {
            // (a+bi)^(c+di) =
            // r1^c * e^(-d*theta1) * e^((thera1*c + dln(r1))i)
            std::complex<long double> a(cast.getRealD(), cast.getImagD());
            std::complex<long double> b(third.getRealD(), third.getImagD());
            auto ans = std::pow(a,b);

            astnode->token.info =
                ComplexType(
                            ans.real(),
                            ans.imag()
                           );
        }

        astnode->remove();
    }
}


#pragma once

#include <Eigen/Core>
#include "math_utils.hpp"
#undef far
#undef near

namespace vgl {
template <typename T>
Eigen::Matrix<T, 4, 4> orthographic_projection(double const left, double const right, double const top,
                                             double const bottom, double const near, double const far) {
    Eigen::Matrix<T, 4, 4> proj_mat = Eigen::Matrix<T, 4, 4>::Identity();
    auto const width = right - left;
    auto const height = top - bottom;
    auto const depth = far - near;
    proj_mat(0, 0) = static_cast<T>(2.0 / width);
    proj_mat(1, 1) = static_cast<T>(2.0 / height);
    proj_mat(2, 2) = static_cast<T>(-2.0 / depth);
    proj_mat(0, 3) = static_cast<T>(-(right + left) / width);
    proj_mat(1, 3) = static_cast<T>(-(top + bottom) / height);
    proj_mat(2, 3) = static_cast<T>(-(far + near) / depth);
    return proj_mat;
}

template <typename T>
Eigen::Matrix<T, 4, 4> orthographic_projection(double const width, double const height, double const near,
                                             double const far) {
    Eigen::Matrix<T, 4, 4> proj_mat = Eigen::Matrix<T, 4, 4>::Identity();
    proj_mat(0, 0) = static_cast<T>(2.0 / width);
    proj_mat(1, 1) = static_cast<T>(2.0 / height);
    auto const depth = far - near;
    proj_mat(2, 2) = static_cast<T>(-2.0 / depth);
    proj_mat(2, 3) = static_cast<T>(-(far + near) / depth);
    return proj_mat;
}

template <typename T>
Eigen::Matrix<T, 4, 4> frustum_projection(double const left, double const right, double const top, double const bottom,
                                        double const near, double const far) {
    Eigen::Matrix<T, 4, 4> proj_mat = Eigen::Matrix<T, 4, 4>::Identity();
    auto const width = right - left;
    auto const height = top - bottom;
    auto const depth = far - near;
    proj_mat(0, 0) = static_cast<T>(2.0 * near / width);
    proj_mat(1, 1) = static_cast<T>(2.0 * near / height);
    proj_mat(0, 2) = static_cast<T>((right + left) / width);
    proj_mat(1, 2) = static_cast<T>((top + bottom) / height);
    proj_mat(2, 2) = static_cast<T>(-(far + near) / depth);
    proj_mat(2, 3) = static_cast<T>(-2.0 * far * near / depth);
    proj_mat(3, 2) = static_cast<T>(-1.0);
    return proj_mat;
}

template <typename T>
Eigen::Matrix<T, 4, 4> perspective_projection(double const aspect_ratio, double const fovy, double const near,
                                            double const far) {
    Eigen::Matrix<T, 4, 4> proj_mat = Eigen::Matrix<T, 4, 4>::Identity();
    auto const tan_half_fovy = math::to_radians(fovy / 2.0);
    auto const depth = far - near;
    proj_mat(0, 0) = static_cast<T>(1.0 / (aspect_ratio * tan_half_fovy));
    proj_mat(1, 1) = static_cast<T>(1.0 / tan_half_fovy);
    proj_mat(2, 2) = static_cast<T>(-(far + near) / depth);
    proj_mat(2, 3) = static_cast<T>(-2.0 * far * near / depth);
    proj_mat(3, 2) = static_cast<T>(-1.0);
    return proj_mat;
}

template <typename T>
void change_aspect_ratio(Eigen::Matrix<T, 4, 4>& proj, double aspect_ratio) {
    proj(0, 0) = static_cast<T>(proj(1, 1) / aspect_ratio);
}

template <typename T>
void change_fovy(Eigen::Matrix<T, 4, 4>& proj, double fovy) {
    auto old = proj(1, 1);
    auto tan_half = std::tan(fovy / 2.0);
    proj(0, 0) = static_cast<T>(proj(0, 0) / old / tan_half);
    proj(1, 1) = static_cast<T>(1.0 / tan_half);
}
} // namespace vgl


#ifndef STAN_MATH_PRIM_SCAL_FUN_LOG_DIFF_EXP_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOG_DIFF_EXP_HPP

#include <stan/math/prim/scal/fun/log1m_exp.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/throw_exception.hpp>
#include <limits>
#include <stdexcept>

namespace stan {
  namespace math {

    /**
     * The natural logarithm of the difference of the natural exponentiation
     * of x1 and the natural exponentiation of x2
     *
     * This function is only defined for x<0
     *
     *
       \f[
       \mbox{log\_diff\_exp}(x, y) =
       \begin{cases}
         \textrm{NaN} & \mbox{if } x \leq y\\
         \ln(\exp(x)-\exp(y)) & \mbox{if } x > y \\[6pt]
         \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
       \end{cases}
       \f]

       \f[
       \frac{\partial\, \mbox{log\_diff\_exp}(x, y)}{\partial x} =
       \begin{cases}
         \textrm{NaN} & \mbox{if } x \leq y\\
         \frac{\exp(x)}{\exp(x)-\exp(y)} & \mbox{if } x > y \\[6pt]
         \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
       \end{cases}
       \f]

       \f[
       \frac{\partial\, \mbox{log\_diff\_exp}(x, y)}{\partial y} =
       \begin{cases}
         \textrm{NaN} & \mbox{if } x \leq y\\
         -\frac{\exp(y)}{\exp(x)-\exp(y)} & \mbox{if } x > y \\[6pt]
         \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
       \end{cases}
       \f]
     *
     */
    template <typename T1, typename T2>
    inline typename boost::math::tools::promote_args<T1, T2>::type
    log_diff_exp(const T1 x, const T2 y) {
      if (x <= y)
        return std::numeric_limits<double>::quiet_NaN();
      return x + log1m_exp(y - x);
    }

  }
}

#endif


#include <boost/multiprecision/cpp_dec_float.hpp>
#include "ARPoly.h"
#include "GaussSeq.h"
#include "utils.h"
using utils::my_float;

/*
 *  Constructors and destructors
 */
ARPoly::ARPoly(std::map<int, my_float> mp, std::map<int, my_float> powers):
    ARSeq(mp),
    powers{powers}
{}
ARPoly::ARPoly(std::map<int, my_float> mp, std::map<int, my_float> powers, my_float bias):
    ARSeq(mp, bias),
    powers{powers}
{}

/*
 *  Member functions
 */
my_float ARPoly::next() {
    namespace mp = boost::multiprecision;
    std::vector<my_float> copy_prev = prev_val;

    for (const auto& ele : powers) {
        // subtract one since the zero-index is the previous value
        copy_prev[ele.first - 1] = mp::pow(prev_val[ele.first - 1], ele.second);
    }
    // Since the lag coefficients are only non-zero at non-zero constants, take the dot-product
    my_float result = utils::dot_product(copy_prev, lag_coeff) + g.next() + bias;
    utils::shift_vector(prev_val);
    prev_val[0] = result;
    return result;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/math/math_kmeans.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>
#include <iostream>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_math_kmeans);

BOOST_AUTO_TEST_CASE(simple_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::matrix3x3_type                       matrix3x3_type;
  typedef math_types::real_type                            real_type;
  typedef math_types::index_type                           index_type;
  typedef math_types::value_traits                         value_traits;
  typedef std::vector<vector3_type>                        vector_container;
  typedef std::vector<size_t>                              index_container;

  vector_container features;
  size_t index = 0;
  features.resize(40);
  real_type lower = - value_traits::half();
  real_type upper = value_traits::half();

  vector3_type center[4];
  center[0] = vector3_type( value_traits::half(), value_traits::half(), -value_traits::two() );
  center[1] = vector3_type( -value_traits::four(), value_traits::zero(), -value_traits::half() );
  center[2] = vector3_type( value_traits::half(), value_traits::four(), value_traits::half() );
  center[3] = vector3_type( value_traits::half(), -value_traits::two(), value_traits::four() );

  for(size_t i = 0;i<10;++i,++index)
  {
    OpenTissue::math::random( features[index], lower, upper );
    features[index] += center[0];
  }
  for(size_t i = 0;i<10;++i,++index)
  {
    OpenTissue::math::random( features[index], lower, upper );
    features[index] += center[1];
  }
  for(size_t i = 0;i<10;++i,++index)
  {
    OpenTissue::math::random( features[index], lower, upper );
    features[index] += center[2];
  }
  for(size_t i = 0;i<10;++i,++index)
  {
    OpenTissue::math::random( features[index], lower, upper );
    features[index] += center[3];
  }

  vector_container cluster_centers;
  index_container cluster_indexes;

  size_t K = 4;
  size_t iteration = 0u;
  size_t max_iterations = 50u;

  // K-means may get caught by local minimas, in such cases clusters
  // seem to ``melt'' together unexpected.
  //
  // If k-means gets caught by a local minima then all the following
  // unit-tests is likely to fail!
  //
  OpenTissue::math::kmeans(
    features.begin()
    , features.end()
    , cluster_centers
    , cluster_indexes
    , K
    , iteration
    , max_iterations
    );

  BOOST_CHECK( iteration < max_iterations );

  for(size_t i = 0;i<40;++i)
    std::cout << cluster_indexes[i] << " ";
  std::cout << std::endl;

  size_t cluster_order[4];
  cluster_order[0] = cluster_indexes[0];
  cluster_order[1] = cluster_indexes[10];
  cluster_order[2] = cluster_indexes[20];
  cluster_order[3] = cluster_indexes[30];

  BOOST_CHECK( cluster_order[0] != cluster_order[1] );
  BOOST_CHECK( cluster_order[0] != cluster_order[2] );
  BOOST_CHECK( cluster_order[0] != cluster_order[3] );
  BOOST_CHECK( cluster_order[1] != cluster_order[2] );
  BOOST_CHECK( cluster_order[1] != cluster_order[3] );
  BOOST_CHECK( cluster_order[2] != cluster_order[3] );

  for(size_t i = 0;i<10;++i)
    BOOST_CHECK( cluster_indexes[i] ==  cluster_order[0] );
  for(size_t i = 10;i<20;++i)
    BOOST_CHECK( cluster_indexes[i] ==  cluster_order[1] );
  for(size_t i = 20;i<30;++i)
    BOOST_CHECK( cluster_indexes[i] ==  cluster_order[2] );
  for(size_t i = 30;i<40;++i)
    BOOST_CHECK( cluster_indexes[i] ==  cluster_order[3] );

  for(size_t c = 0;c<K;++c)
  {
    std::cout << cluster_centers[ cluster_order[c] ] << std::endl;
    real_type dist = OpenTissue::math::length( center[c] - cluster_centers[ cluster_order[c] ] );
    BOOST_CHECK(dist < value_traits::half() );
  }
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef HYPSYS1D_MODEL_HPP
#define HYPSYS1D_MODEL_HPP

#include <cmath>
#include <memory>

#include <Eigen/Dense>
#include <ancse/config.hpp>

/// Interface for implementing different models,
/// eg. Euler equations, Shallow-water equations
///
/// Add more functions to this interface if needed.
class Model {
  public:
    virtual ~Model() = default;

    // Pure virtual functions:
    virtual Eigen::VectorXd flux(const Eigen::VectorXd& u) const = 0;
    virtual Eigen::VectorXd eigenvalues(const Eigen::VectorXd& u) const = 0;
    virtual Eigen::MatrixXd eigenvectors(const Eigen::VectorXd& u) const = 0;
    virtual double max_eigenvalue(const Eigen::VectorXd& u) const = 0;

    virtual Eigen::VectorXd cons_to_prim(const Eigen::VectorXd& u) const = 0;
    virtual Eigen::VectorXd prim_to_cons(const Eigen::VectorXd& u) const = 0;

    virtual int get_nvars() const = 0;
    virtual std::string get_name() const = 0;

    // Virtual functions:
    virtual std::pair<double, double> lo_hi_eigenvalues(const Eigen::VectorXd& u) const {}

    virtual double rho(const Eigen::VectorXd& u_prim) const {}
    virtual double v(const Eigen::VectorXd& u_prim) const {}
    virtual double p(const Eigen::VectorXd& u_prim) const {}
    virtual double m(const Eigen::VectorXd& u_prim) const {}
    virtual double E(const Eigen::VectorXd& u_prim) const {}
    virtual double c(const Eigen::VectorXd& u_prim) const {}
    virtual double H(const Eigen::VectorXd& u_prim) const {}

    virtual void set_gamma(const double gamma_) {}
    virtual double get_gamma() const {}
};

class Burgers : public Model {
  public:

    Eigen::VectorXd flux(const Eigen::VectorXd &u) const override
    {
        Eigen::VectorXd f(n_vars);
        f(0) = 0.5*u(0)*u(0);

        return f;
    }

    Eigen::VectorXd eigenvalues(const Eigen::VectorXd &u) const override
    {
        Eigen::VectorXd eigvals(n_vars);
        eigvals(0) = u(0);

        return eigvals;
    }

    Eigen::MatrixXd eigenvectors(const Eigen::VectorXd &) const override
    {
        Eigen::MatrixXd eigvecs(n_vars, n_vars);
        eigvecs (0,0) = 1;

        return eigvecs;
    }

    double max_eigenvalue(const Eigen::VectorXd &u) const override {
        return (eigenvalues(u).cwiseAbs()).maxCoeff();
    }

    Eigen::VectorXd cons_to_prim(const Eigen::VectorXd &u) const override {
        return u;
    }

    Eigen::VectorXd prim_to_cons(const Eigen::VectorXd &u) const override {
        return u;
    }

    int get_nvars() const override
    {
        return n_vars;
    }

    std::string get_name() const override
    {
        return "burgers";
    }

  private:
    int n_vars = 1;
};

/// Euler equations
class Euler : public Model {
    public:

        Eigen::VectorXd flux(const Eigen::VectorXd &u) const override
        {
            Eigen::VectorXd f(n_vars);

            f << rho(u) * v(u),
                 rho(u) * v(u) * v(u) + p(u),
                 (E(u) + p(u)) * v(u);

            return f;
        }

        Eigen::VectorXd eigenvalues(const Eigen::VectorXd &u) const override
        {
            Eigen::VectorXd eigvals(n_vars);

            eigvals << v(u) - c(u),
                       v(u),
                       v(u) + c(u);

            return eigvals;
        }

        Eigen::MatrixXd eigenvectors(const Eigen::VectorXd &u) const override
        {
            Eigen::MatrixXd eigvecs(n_vars, n_vars);

            eigvecs << 1.0,                    1.0,                 1.0,
                       v(u) - c(u),            v(u),                v(u) + c(u),
                       H(u) - v(u) * c(u),     0.5 * v(u) * v(u),   H(u) + (v(u) * c(u));

            return eigvecs;
        }

        double max_eigenvalue(const Eigen::VectorXd &u) const override
        {
            // actually max(abs(eigenvalues)):
            return (eigenvalues(u).cwiseAbs()).maxCoeff();
        }

        Eigen::VectorXd cons_to_prim(const Eigen::VectorXd &u_cons) const override
        {
            Eigen::VectorXd u_prim(n_vars);

            u_prim << rho(u_cons),
                      v(u_cons),
                      p(u_cons);

            return u_prim;
        }

        Eigen::VectorXd prim_to_cons(const Eigen::VectorXd &u_prim) const override
        {
            Eigen::VectorXd u_cons(n_vars);

            u_cons << u_prim(0),
                      u_prim(0) * u_prim(1),
                      u_prim(2) / (gamma - 1) + 0.5 * u_prim(0) * u_prim(1) * u_prim(1);

            return u_cons;
        }

        inline void set_gamma(const double gamma_) override
        {
            gamma= gamma_;
        }

        inline double get_gamma() const override
        {
            return gamma;
        }

        inline int get_nvars() const override
        {
            return n_vars;
        }

        std::string get_name() const override
        {
            return name;
        }

    private:
        // 3D:
        const int n_vars= 3;
        // Monatomic gas:
        double gamma= 5./3.;

        inline static const std::string name= "euler";

        std::pair<double, double> lo_hi_eigenvalues(const Eigen::VectorXd &u) const override
        {
            // check if we want cwiseAbs here:
            // return (eigenvalues(u).cwiseAbs()).minCoeff();
            Eigen::VectorXd eigenvals= eigenvalues(u);
            return {eigenvals.minCoeff(), eigenvals.maxCoeff()};
        }

        /// Helper functions to deal w/ common names for Euler eqn expressions
        /// Vector u must contain conserved variables: u = (rho, m, E)
        /// as given in tasks initial conditions
        /// ------------------------------------------------------------------

        /// Conserved variables:
        // rho : density
        inline double rho(const Eigen::VectorXd& u) const override
        {
            return u(0);
        }

        // m : momentum
        inline double m(const Eigen::VectorXd& u) const override
        {
            return u(1);
        }

        // E : total energy for ideal polytropic gas (internal energy + kinetic energy)
        inline double E(const Eigen::VectorXd& u) const override
        {
            return u(2);
        }

        /// Primitive variables:
        // rho : density
        // same as for conserved variables

        // v : velocity
        inline double v(const Eigen::VectorXd& u) const override
        {
            return m(u) / rho(u);
        }

        // p : pressure
        inline double p(const Eigen::VectorXd& u) const override
        {
            return (E(u) - 0.5 * m(u) * m(u) / rho(u)) * (gamma - 1);
        }

        /// c : speed of sound
        inline double c(const Eigen::VectorXd& u) const override
        {
            return std::sqrt(gamma * p(u) / rho(u));
        }

        /// H : total specific enthalpy
        inline double H(const Eigen::VectorXd& u) const override
        {
            return (E(u) + p(u)) / rho(u);
        }
};

std::shared_ptr<Model> make_model (const nlohmann::json &config);

#endif // HYPSYS1D_MODEL_HPP


#include <numbers>
#include <stdexcept>

#include "catch2/catch.hpp"

#include <Eigen/Dense>

#include "scapin/hooke.hpp"

template <int DIM>
inline constexpr int sym = (DIM * (DIM + 1)) / 2;

template <int DIM>
using Vector = Eigen::Matrix<double, DIM, 1>;

template <int DIM>
using Tensor2 = Eigen::Matrix<double, sym<DIM>, 1>;

template <int DIM>
using Tensor4 = Eigen::Matrix<double, sym<DIM>, sym<DIM>>;

template <int DIM>
std::pair<int, int> unravel_index(int ij);

template <>
std::pair<int, int> unravel_index<2>(int ij) {
  switch (ij) {
    case 0:
      return std::make_pair(0, 0);
    case 1:
      return std::make_pair(1, 1);
    case 2:
      return std::make_pair(0, 1);
    default:
      throw std::invalid_argument("unexpected value");
  }
}

template <>
std::pair<int, int> unravel_index<3>(int ij) {
  switch (ij) {
    case 0:
      return std::make_pair(0, 0);
    case 1:
      return std::make_pair(1, 1);
    case 2:
      return std::make_pair(2, 2);
    case 3:
      return std::make_pair(1, 2);
    case 4:
      return std::make_pair(2, 0);
    case 5:
      return std::make_pair(0, 1);
    default:
      throw std::invalid_argument("unexpected argument");
  }
}

template <int DIM>
double bulk_modulus(double mu, double nu);

template <>
double bulk_modulus<2>(double mu, double nu) {
  return mu / (1. - 2. * nu);
}

template <>
double bulk_modulus<3>(double mu, double nu) {
  return 2. * mu * (1. + nu) / 3. / (1. - 2. * nu);
}

template <int DIM>
std::pair<Tensor4<DIM>, Tensor4<DIM>> isotropic_projectors() {
  Tensor2<DIM> I2 = Tensor2<DIM>::Zero();
  for (int i = 0; i < DIM; i++) I2(i) = 1.;
  Tensor4<DIM> I = Tensor4<DIM>::Identity();
  Tensor4<DIM> J = I2 * I2.transpose() / DIM;
  Tensor4<DIM> K = I - J;
  return std::make_pair(J, K);
}

std::vector<Vector<2>> gen_directions(int num_theta) {
  double const delta_theta = std::numbers::pi / (num_theta - 1.);
  std::vector<Vector<2>> directions;
  for (int i = 0; i < num_theta; i++) {
    double theta = i * delta_theta;
    directions.push_back(Vector<2>{cos(theta), sin(theta)});
  }
  return directions;
}

std::vector<Vector<3>> gen_directions(int num_theta, int num_phi) {
  double const delta_theta = std::numbers::pi / (num_theta - 1.);
  double const delta_phi = 2 * std::numbers::pi / double(num_phi);
  std::vector<Vector<3>> directions{};
  for (int i = 0; i < num_theta; i++) {
    double const theta = i * delta_theta;
    double const cos_theta = cos(theta);
    double const sin_theta = sin(theta);
    for (int j = 0; j < num_phi; j++) {
      double const phi = j * delta_phi;
      directions.push_back(
          Vector<3>{sin_theta * cos(phi), sin_theta * sin(phi), cos_theta});
    }
  }
  return directions;
}

template <int DIM>
Tensor4<DIM> green_operator_matrix(Vector<DIM> const &n, double nu) {
  Tensor4<DIM> out;
  for (int ij = 0; ij < sym<DIM>; ij++) {
    auto const [i, j] = unravel_index<DIM>(ij);
    double const w_ij = ij < DIM ? 1. : std::numbers::sqrt2;
    for (int kl = 0; kl < sym<DIM>; kl++) {
      auto const [k, l] = unravel_index<DIM>(kl);
      double const w_kl = kl < DIM ? 1. : std::numbers::sqrt2;
      double const delta_ik = i == k ? 1. : 0.;
      double const delta_il = i == l ? 1. : 0.;
      double const delta_jk = j == k ? 1. : 0.;
      double const delta_jl = j == l ? 1. : 0.;
      out(ij, kl) = w_ij * w_kl *
                    (0.25 * (delta_ik * n(j) * n(l) + delta_il * n(j) * n(k) +
                             delta_jk * n(i) * n(l) + delta_jl * n(i) * n(k)) -
                     0.5 * n(i) * n(j) * n(k) * n(l) / (1. - nu));
    }
  }
  return out;
}

template <int DIM>
void test_hooke_apply() {
  std::vector<double> norms{1.2, 3.4, 5.6};
  scapin::Hooke<double, DIM> gamma{1.0, 0.3};
  std::vector<Vector<DIM>> directions;
  if constexpr (DIM == 2) {
    directions = gen_directions(20);
  } else if constexpr (DIM == 3) {
    directions = gen_directions(10, 20);
  }
  for (auto n : directions) {
    auto exp = green_operator_matrix<DIM>(n, gamma.nu);
    Tensor4<DIM> act;
    for (auto const norm : norms) {
      Vector<DIM> k = norm * n;
      for (int i = 0; i < sym<DIM>; i++) {
        Tensor2<DIM> tau = Tensor2<DIM>::Zero();
        tau(i) = 1.;
        Tensor2<DIM> eps;
        gamma.apply(k.data(), tau.data(), eps.data());
        act.col(i) = eps;
      }
      for (int i = 0; i < sym<DIM>; ++i) {
        for (int j = 0; j < sym<DIM>; ++j) {
          REQUIRE(act(i, j) == Approx(exp(i, j)).epsilon(1e-12).margin(1e-12));
        }
      }
    }
  }
}

template <int DIM>
void test_apply_stiffness() {
  scapin::Hooke<double, DIM> gamma{1.2, 0.3};
  auto const [J, K] = isotropic_projectors<DIM>();
  auto kappa = bulk_modulus<DIM>(gamma.mu, gamma.nu);
  Tensor4<DIM> C_exp = DIM * kappa * J + 2 * gamma.mu * K;
  Tensor4<DIM> C_act;
  Tensor2<DIM> eps = Tensor2<DIM>::Zero();
  Tensor2<DIM> sig = Tensor2<DIM>::Zero();
  for (int i = 0; i < sym<DIM>; i++) {
    eps(i) = 1.0;
    gamma.apply_stiffness(eps.data(), sig.data());
    C_act.col(i) = sig;
    eps(i) = 0.0;
  }

  for (int i = 0; i < sym<DIM>; i++) {
    for (int j = 0; j < sym<DIM>; j++) {
      REQUIRE(C_act(i, j) == Approx(C_exp(i, j)).epsilon(1e-15).margin(1e-15));
    }
  }
}

template <int DIM>
void test_apply_compliance() {
  scapin::Hooke<double, DIM> gamma{1.2, 0.3};
  auto const [J, K] = isotropic_projectors<DIM>();
  auto kappa = bulk_modulus<DIM>(gamma.mu, gamma.nu);
  Tensor4<DIM> S_exp = J / (DIM * kappa) + K / (2 * gamma.mu);
  Tensor4<DIM> S_act;
  Tensor2<DIM> eps = Tensor2<DIM>::Zero();
  Tensor2<DIM> sig = Tensor2<DIM>::Zero();
  for (int i = 0; i < sym<DIM>; i++) {
    sig(i) = 1.0;
    gamma.apply_compliance(sig.data(), eps.data());
    S_act.col(i) = eps;
    sig(i) = 0.0;
  }

  for (int i = 0; i < sym<DIM>; i++) {
    for (int j = 0; j < sym<DIM>; j++) {
      REQUIRE(S_act(i, j) == Approx(S_exp(i, j)).epsilon(1e-15).margin(1e-15));
    }
  }
}

TEST_CASE("Continuous Green operator") {
  SECTION("Hooke model") {
    SECTION("Apply") {
      SECTION("2D") { test_hooke_apply<2>(); }
      SECTION("3D") { test_hooke_apply<3>(); }
    }
    SECTION("apply_stiffness") {
      SECTION("2D") { test_apply_stiffness<2>(); }
      SECTION("3D") { test_apply_stiffness<3>(); }
    }
    SECTION("apply_compliance") {
      SECTION("2D") { test_apply_compliance<2>(); }
      SECTION("3D") { test_apply_compliance<3>(); }
    }
  }
}


#include <ql/quantlib.hpp>

#include <iostream>

#include <iomanip>

#include <boost/timer.hpp>

using namespace QuantLib;

using namespace std;

#if defined(QL_ENABLE_SESSIONS)

{

        namespace QuantLib

        {

               Integer sessionId() { return 0; }

        }

#endif

int main(int, char*[])

{

        try

        {

               boost::timer timer;

               std::cout << std::endl;

               Calendar calendar = TARGET();

               Date settlementDate(6, Aug, 2021);

               settlementDate = calendar.adjust(settlementDate);

               Integer fixingDays = 1;

               Natural settlementDays = 1;

               Date todaysDate = calendar..advance(settlementDate, -fixingDays, Days);

               Settings::instance().evaluationDate() = todaysDate;

               // Treasury Bond Setting //

               Real faceAmount = 100;

               Real redemption = 100;

               Date issueDate(17, May, 2021);

               Date maturity(5, May, 2031);

               Real couponRate = 0.01625; // 1 5/8

               Real yield = 0.0165;

               RelinkableHandle<YieldTermStructure> discountingTermStructure;

               boost::shared_ptr<YieldTermStructure> flatTermStructure(new FlatForward(settlementDate, yield, ActualActual(ActualActual::Bond), Compounded, Semiannual));

               discountingTermStructure.linkTo(flatTermStructure);

               boost::shared_ptr<PricingEngine> bondEngine(new DiscountingBondEngine(discountingTermStructure));

               Schedule fixedBondSchedule(issueDate, maturity, Period(Semiannual), UnitedStates(UnitedStates::GovernmentBond), Unadjusted, Unadjusted, DateGeneration::Rule::Backward, false);

               FixedRateBond fixedRateBond(settlementDays, faceAmount, fixedBondSchedule, std::vector<Rate>(1, couponRate), ActualActual(ActualActual::Bond), Unadjusted, redemption, issueDate);

               fixedRateBond.setPricingEngine(bondEngine);

               std::cout << "****** Inputs ******" << std::endl;

               //std::setprecision(20);

               std::cout << " Principal = " << faceAmount << std::endl;

               std::cout << "Issue Date = " << issueDate << std::endl;

               std::cout << " Maturity = " << maturity << std::endl;

               std::cout << " Coupon  = " << io::percent(couponRate) << std::endl;

               std::cout << " Yield To Maturity = " << io::percent(yield) << std::endl;

               std::cout << "****** Result ******" << std::endl;

               std::cout << " Fair Value = " << fixedRateBond.NPV() << std::endl;

               std::cout << " Clean Price = " << fixedRateBond.cleanPrice() << std::endl;

               std::cout << " Dirty Price = " << fixedRateBond.dirtyPrice() << std::endl;

               std::cout << " Accured Coupon = " << fixedRateBond.accruedAmount() << std::endl;

               system("pause");

               return 0;

        }

        catch (std::exception& e)

        {

               std::cerr << e.what() << std::endl;

               return 1;

        }

        catch (...)

        {

               std::cerr << "Unknown Error" << std::endl;

               return 1;

        }

}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <boost/graph/edge_list.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>

int
main()
{
  using namespace boost;
  // ID numbers for the routers (vertices).
  enum
  { A, B, C, D, E, F, G, H, n_vertices };
  const int n_edges = 11;
  typedef std::pair < int, int >Edge;

  // The list of connections between routers stored in an array.
  Edge edges[] = {
  Edge(A, B), Edge(A, C),
        Edge(B, D), Edge(B, E), Edge(C, E), Edge(C, F), Edge(D, H),
        Edge(D, E), Edge(E, H), Edge(F, G), Edge(G, H)
  };

  // Specify the graph type and declare a graph object
  typedef edge_list < Edge*, Edge, std::ptrdiff_t, std::random_access_iterator_tag> Graph;
  Graph g(edges, edges + n_edges);

  // The transmission delay values for each edge.
  float delay[] =
    {5.0, 1.0, 1.3, 3.0, 10.0, 2.0, 6.3, 0.4, 1.3, 1.2, 0.5};

  // Declare some storage for some "external" vertex properties.
  char name[] = "ABCDEFGH";
  int parent[n_vertices];
  for (int i = 0; i < n_vertices; ++i)
    parent[i] = i;
  float distance[n_vertices];
  std::fill(distance, distance + n_vertices, (std::numeric_limits < float >::max)());
  // Specify A as the source vertex
  distance[A] = 0;

  bool r = bellman_ford_shortest_paths(g, int (n_vertices),
                                       weight_map(make_iterator_property_map
                                                  (&delay[0],
                                                   get(edge_index, g),
                                                   delay[0])).
                                       distance_map(&distance[0]).
                                       predecessor_map(&parent[0]));

  if (r)
    for (int i = 0; i < n_vertices; ++i)
      std::cout << name[i] << ": " << distance[i]
        << " " << name[parent[i]] << std::endl;
  else
    std::cout << "negative cycle" << std::endl;

  return EXIT_SUCCESS;
}


#include <iostream>
#include <chrono>
#include <random>
#include <map>

#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

#include "matplotlibcpp.h"

using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;

double g(double x, double a, double b, double c)
{
  // return a*x*x+b*x+c;
  return std::exp(a * x * x + b * x + c);
}

int main(int argc, char **argv) {
  std::cout << "Steepest-Descent \n";

  double aa = 1.0, bb = 2.0, cc = 1.0;
  double a = 2.0, b = -1.0, c = -5.0;

  double obs_sigma = 1.0;
  std::default_random_engine generator;
  std::normal_distribution<double> obs_noise_distrib(0.0, obs_sigma);

  int N = 100;
  std::vector<double> x_data(N), y_data(N);
  for (int i = 0; i < N; ++i)
  {
    x_data[i] = static_cast<double>(i) / 100.0;
    y_data[i] = g(x_data[i], aa, bb, cc) + obs_noise_distrib(generator);

  }

  // Optimize
  int max_iter = 100000;
  for (int it = 0; it < max_iter; ++it)
  {
    std::cout << "iter " << it << " : ";
    std::cout << "abc = " << a << " " << b << " " << c << "\n";
    Eigen::Vector3d J(0.0, 0.0, 0.0);
    double total_err = 0.0;
    for (int i = 0; i < N; ++i)
    {
      double err = y_data[i] - g(x_data[i], a, b, c);
      total_err += err*err;

      // Compute derivative of F(x) = 0.5 * sum(f(x)^2) = 0.5 * sum((y-g(x))^2)
      J[0] += -x_data[i] * x_data[i] * g(x_data[i], a, b, c) * err;
      J[1] += -x_data[i] * g(x_data[i], a, b, c) * err;
      J[2] += -g(x_data[i], a, b, c) * err;
    }

    std::cout << "J = " << J.transpose() << "\n";
    std::cout << "total error: " << total_err << "\n";

    if (J.norm() < 0.01)
      break;

    // Very small step
    double S = 100000;
    a -= J[0] / S;
    b -= J[1] / S;
    c -= J[2] / S;
  }
  std::cout << "final = " << a << " " << b << " " << c << "\n";

  vector<double> final_y_data(N);
  for (int i = 0; i < N; ++i)
  {
    final_y_data[i] = g(x_data[i], a, b, c);
  }

  plt::scatter(x_data, y_data);
  std::map<std::string, std::string> parameters;
  parameters["c"] = "red";
  plt::scatter(x_data, final_y_data, 1.0, parameters);
  plt::show();

  return 0;
}


/**
 * @file advectionfv2d_main.cc
 * @brief NPDE homework AdvectionFV2D code
 * @author Philipp Egg
 * @date 21.06.2020
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>

#include <Eigen/Core>
#include <array>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>
#include <string>
#include <vector>

#include "advectionfv2d.h"

// Use this function to plot your solution
void write_vtk(const lf::assemble::DofHandler &dofh,
               const Eigen::VectorXd &solution, std::string name) {
  std::shared_ptr<const lf::mesh::Mesh> mesh_p = dofh.Mesh();
  lf::io::VtkWriter vtk_writer(mesh_p, name + ".vtk");
  auto cell_data_ref =
      lf::mesh::utils::make_CodimMeshDataSet<double>(mesh_p, 0);
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    int row = dofh.GlobalDofIndices(*cell)[0];
    cell_data_ref->operator()(*cell) = solution[row];
  }
  vtk_writer.WriteCellData(name, *cell_data_ref);
}

int main() {
  /* SAM_LISTING_BEGIN_1 */
  // Define velocity field beta
  // Note that the problem description requires ||B|| <= 1
  auto beta = [](Eigen::Vector2d x) -> Eigen::Vector2d {
    return Eigen::Vector2d(-x[1], x[0]) / std::sqrt(2.0);
  };

  // Functor for initial bump
  auto u0 = [](Eigen::Vector2d x) -> double {
    Eigen::Vector2d x0(0.8, 0.2);
    double d = 0.2;
    double dist = (x - x0).norm();
    if (dist < d) {
      double cos = std::cos(M_PI / (2.0 * d) * dist);
      return cos * cos;
    } else {
      return 0.0;
    }
  };

  // Task 8-8.o
  // Generate a mesh hierarchy
  double T = 1.0;

  //////////////////////////////////////////////////////////////////////////////
  // TODO inconsistancy: g vs. G
  //////////////////////////////////////////////////////////////////////////////
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(0, 1.0 / 3.0);

  auto mesh_seq_p{
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh_p, 6)};

  std::vector<int> vector_num_cells;
  std::vector<double> vector_l2error;

  // Iterate over mesh levels starting from thrid refinement
  int num_meshes = mesh_seq_p->NumLevels();
  for (int level = 3; level < num_meshes; ++level) {
    std::cout << "Computing L2Error for level: " << level << std::endl;

#if SOLUTION
    // Get the current mesh
    auto cur_mesh = mesh_seq_p->getMesh(level);

    // Create a DOF Hander for the current mesh
    const lf::assemble::UniformFEDofHandler cur_dofh(
        cur_mesh, {{lf::base::RefEl::kPoint(), 0},
                   {lf::base::RefEl::kSegment(), 0},
                   {lf::base::RefEl::kTria(), 1},
                   {lf::base::RefEl::kQuad(), 1}});
    int N = cur_dofh.NumDofs();

    // Compute cell normals
    std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<
        Eigen::Matrix<double, 2, Eigen::Dynamic>>>
        normal_vectors = AdvectionFV2D::computeCellNormals(cur_dofh.Mesh());

    // Compute adjecent cells
    std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<
        std::array<const lf::mesh::Entity *, 4>>>
        adjacentCells = AdvectionFV2D::getAdjacentCellPointers(cur_dofh.Mesh());

    // Get approximate solution from simulation
    Eigen::VectorXd mu_approx = AdvectionFV2D::simulateAdvection(
        cur_dofh, beta, u0, adjacentCells, normal_vectors, T);
    write_vtk(cur_dofh, mu_approx, "approx" + std::to_string(level));

    // Get exact solution at barycenters of cells
    Eigen::VectorXd mu_exact = AdvectionFV2D::refSolution(cur_dofh, u0, T);
    write_vtk(cur_dofh, mu_exact, "exact" + std::to_string(level));

    // Compute L2 error in barycenter
    double l2_error = 0;
    for (const lf::mesh::Entity *cell : cur_mesh->Entities(0)) {
      const lf::geometry::Geometry *geo_p = cell->Geometry();
      double area = lf::geometry::Volume(*geo_p);
      int idx = cur_dofh.GlobalDofIndices(*cell)[0];
      l2_error += std::pow((mu_approx[idx] - mu_exact[idx]), 2) * area;
    }
    l2_error = std::sqrt(l2_error);
#else
    //====================
    // Your code goes here
    // Compute the number N of DOFs and the L2-error,
    // and replace the lines below:
    int N = 1;
    double l2_error = 1.0;
    // If you want, you can use the function write_vtk(...),
    // defined in this file, to plot your solution.
    //====================
#endif

    vector_num_cells.push_back(N);
    vector_l2error.push_back(l2_error);
  }

  // Write Output file of Task 8-8.o
  std::ofstream csv_file;
  csv_file.open("advectionfv2d.csv");
  for (int i = 0; i < vector_num_cells.size(); ++i) {
    std::cout << "Cells: " << vector_num_cells.at(i)
              << " | L2Error: " << vector_l2error.at(i) << std::endl;
    csv_file << vector_num_cells.at(i) << "," << vector_l2error.at(i) << "\n";
  }
  csv_file.close();

  std::system("python3 " CURRENT_SOURCE_DIR
              "/advectionfv2d.py " CURRENT_BINARY_DIR
              "/advectionfv2d.csv " CURRENT_BINARY_DIR "/solution.eps");
  /* SAM_LISTING_END_1 */

  /* SAM_LISTING_BEGIN_2 */
  // Task 8-8.q
  // Compute threshold for fourth refinement level
  int level = 4;
  auto mesh = mesh_seq_p->getMesh(level);

#if SOLUTION
  // Create a DOF Hander for the current mesh
  const lf::assemble::UniformFEDofHandler dofh(
      mesh, {{lf::base::RefEl::kPoint(), 0},
             {lf::base::RefEl::kSegment(), 0},
             {lf::base::RefEl::kTria(), 1},
             {lf::base::RefEl::kQuad(), 1}});

  int threshold = AdvectionFV2D::findCFLthreshold(dofh, beta, T);
  int cfl_thres = int((T / AdvectionFV2D::computeHmin(mesh) + 1));
#else
  //====================
  // Your code goes here
  // Replace the two variables below:
  int threshold = 0.0;  // Threshold computed by findCFLthreshold(...)
  int cfl_thres = 0.0;  // Threshold obtained from CLF using computeHmin(mesh)
                        //====================
#endif

  std::cout << "Threshold for level " << level << " is " << threshold
            << " | Threshold from CFL is " << cfl_thres << std::endl;
  /* SAM_LISTING_END_2 */

  return 0;
}


#include "Classes.h"
#include <Eigen/Dense>
using namespace Eigen;

int PlaneProjection::rotatePlane() {
    ///
    /// Function to rotate the PlaneProjection instance so as to get the normal parallel to z-axis
    ///
    Vector3d zaxis(0,0,1);
    Vector3d norml(normal[0],normal[1],normal[2]);
    // cout << "Norml: " << endl << norml << endl;
    double costheta = zaxis.dot(norml);
    double sintheta = zaxis.cross(norml).norm();
    if(costheta < 0) {
        costheta = 0-costheta;
    }
    Vector3d axis = zaxis.cross(norml);
    Matrix3d id = Matrix3d::Identity();
    // cout << "Identity Matrix: " << endl << id << endl;
    Matrix3d ux;
    ux << 0, -axis.z(), axis.y(),
                axis.z(), 0, -axis.x(),
                -axis.y(), axis.x(), 0;
    // cout << "UX Matrix: " << endl << ux << endl;
    Matrix3d tensorprod = axis*axis.transpose();
    // cout << "Tensor Product Matrix: " << endl << tensorprod << endl;
    Matrix3d rotmax = costheta*id + sintheta*ux + (1-costheta)*tensorprod;
    for(auto i = 0; i < visibleEdges.size(); i++) {
        Vector3d point1(visibleEdges[i].p1.x,visibleEdges[i].p1.y,visibleEdges[i].p1.z);
        Vector3d point2(visibleEdges[i].p2.x,visibleEdges[i].p2.y,visibleEdges[i].p2.z);
        point1 = rotmax*point1;
        point2 = rotmax*point2;
        visibleEdges[i].p1.x = point1.x();
        visibleEdges[i].p1.y = point1.y();
        visibleEdges[i].p1.z = point1.z();
        visibleEdges[i].p2.x = point2.x();
        visibleEdges[i].p2.y = point2.y();
        visibleEdges[i].p2.z = point2.z();
    }
    for(auto i = 0; i < hiddenEdges.size(); i++) {
        Vector3d point1(hiddenEdges[i].p1.x,hiddenEdges[i].p1.y,hiddenEdges[i].p1.z);
        Vector3d point2(hiddenEdges[i].p2.x,hiddenEdges[i].p2.y,hiddenEdges[i].p2.z);
        point1 = rotmax*point1;
        point2 = rotmax*point2;
        hiddenEdges[i].p1.x = point1.x();
        hiddenEdges[i].p1.y = point1.y();
        hiddenEdges[i].p1.z = point1.z();
        hiddenEdges[i].p2.x = point2.x();
        hiddenEdges[i].p2.y = point2.y();
        hiddenEdges[i].p2.z = point2.z();
    }
}