#include <boost/filesystem.hpp>

#include <fftw3.h>

#include <vtkXMLImageDataWriter.h>
#include <vtkSmartPointer.h>
#include <vtkDoubleArray.h>
#include <vtkImageData.h>
#include <vtkPointData.h>

#include "ScreenOutput.h"

ScreenOutput::ScreenOutput(
		const RootSettings &settings,
		const PhysicsCoefficients &coefs) :
	PostprocessorBase::PostprocessorBase(
		settings.postprocessor.micrograph_output,
		settings.algorithm.general.results_folder_name,
		coefs),
	iso_layer_thickness(settings.postprocessor.micrograph_output.iso_layer_thickness),
	iso_layer_index(settings.postprocessor.micrograph_output.iso_layer_index),
	focalisation_z_shift(settings.postprocessor.micrograph_output.focalisation_z_shift),
	numerical_aperture(settings.postprocessor.micrograph_output.numerical_aperture),
	wavelengths(coefs.wavelengths()),
	q_vals(coefs.q_vals()) {

	if(iso_layer_thickness.size()!=iso_layer_index.size())
		throw std::string(
			"Inconsistent number of isotropic layers");

	// index of the air between the last iso layer and the objective
	iso_layer_index.push_back(1);
}

void ScreenOutput::apply(ScreenOpticalFieldCollection &screen_optical_fields) {

	std::cout << "Calculating output transverse fields..." << std::endl;

	auto mesh = screen_optical_fields.mesh;
	Nx = mesh.Nx;
	Ny = mesh.Ny;
	delta_x = mesh.delta_x;
	delta_y = mesh.delta_y;

	// We create the FFTW plans for the forward and backward transform
	std::cout << "    Accumulating FFTW wisdom..." << std::endl;

	fftw_complex *in = static_cast<fftw_complex*>(
		fftw_malloc(sizeof(fftw_complex) * Nx*Ny));
	fftw_complex *out = static_cast<fftw_complex*>(
		fftw_malloc(sizeof(fftw_complex) * Nx*Ny));
	fftw_plan forward_plan =
		fftw_plan_dft_2d(Ny, Nx, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_plan backward_plan =
		fftw_plan_dft_2d(Ny, Nx, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);

	// Array of pointers for the vti data structures
	std::vector<vtkSmartPointer<vtkDoubleArray> > vti_data_arrays(
		4*q_vals.size()*wavelengths.size());

	std::cout <<
		"    Filtering optical fields..." << std::endl;
	std::string pol_strs[2] ={"inputX_", "inputY_"};

	#pragma omp parallel for
	for(int wave_idx=0; wave_idx<wavelengths.size(); wave_idx++) {
		// FFTW arrays for this thread
		fftw_complex* field[2];
		fftw_complex* fft_field[2];
		for(int comp=0; comp<2; comp++) {
			field[comp] = static_cast<fftw_complex*>(
				fftw_malloc(sizeof(fftw_complex) * Nx*Ny));
			fft_field[comp] = static_cast<fftw_complex*>(
				fftw_malloc(sizeof(fftw_complex) * Nx*Ny));
		}

		for(int q_idx=0; q_idx<q_vals.size(); q_idx++) {
			// Propagation filter for this wavelength and incoming wavevector
			auto fourier_filter = assemble_fourier_filter(
				wavelengths[wave_idx], q_vals[q_idx]);

			for(int pol_idx=0; pol_idx<2; pol_idx++) {
				auto real_data = vtkSmartPointer<vtkDoubleArray>::New();
				real_data->SetNumberOfComponents(3);
				real_data->SetNumberOfTuples(Nx*Ny);
				auto varname =
					"E_real_"+pol_strs[pol_idx]+
					std::to_string(wave_idx)+"_"+std::to_string(q_idx);
				real_data->SetName(varname.c_str());

				auto imag_data = vtkSmartPointer<vtkDoubleArray>::New();
				imag_data->SetNumberOfComponents(3);
				imag_data->SetNumberOfTuples(Nx*Ny);
				varname =
					"E_imag_"+pol_strs[pol_idx]+
					std::to_string(wave_idx)+"_"+std::to_string(q_idx);
				imag_data->SetName(varname.c_str());

				vti_data_arrays[4*(q_idx+q_vals.size()*wave_idx)+2*pol_idx] = real_data;
				vti_data_arrays[4*(q_idx+q_vals.size()*wave_idx)+2*pol_idx+1] = imag_data;

				// First, we fill the fftw input arrays with our data and switch to
				// Fourier space
				for(int comp=0; comp<2; comp++) {
					for(int i=0; i<Nx*Ny; i++) {
						field[comp][i][0] =
							std::real(screen_optical_fields(wave_idx,q_idx,pol_idx)(i,comp));
						field[comp][i][1] =
							std::imag(screen_optical_fields(wave_idx,q_idx,pol_idx)(i,comp));
					}
					fftw_execute_dft(forward_plan, field[comp], fft_field[comp]);
				}

				// We apply the fourier filter
				for(int i=0; i<Nx*Ny; i++) {
					auto tmp =
						fourier_filter->at(i)*
						Eigen::Vector2cd(
							std::complex<double>(fft_field[0][i][0], fft_field[0][i][1]),
							std::complex<double>(fft_field[1][i][0], fft_field[1][i][1]));
					for(int comp=0; comp<2; comp++) {
						fft_field[comp][i][0] = std::real(tmp[comp]);
						fft_field[comp][i][1] = std::imag(tmp[comp]);
					}
				}

				// We come back to real space and save the transformed data
				for(int comp=0; comp<2; comp++) {
					fftw_execute_dft(backward_plan, fft_field[comp], field[comp]);
					for(int i=0; i<Nx*Ny; i++) {
						real_data->SetComponent(i, comp, field[comp][i][0]/(Nx*Ny));
						imag_data->SetComponent(i, comp, field[comp][i][1]/(Nx*Ny));
					}
				}
				for(int i=0; i<Nx*Ny; i++) {
					real_data->SetComponent(i, 2, 0);
					imag_data->SetComponent(i, 2, 0);
				}
			}
		}

		// We free the FFTW pointers
		for(int comp=0; comp<2; comp++) {
			fftw_free(field[comp]);
			fftw_free(fft_field[comp]);
		}
	}

	// We free the FFTW pointers
	fftw_destroy_plan(forward_plan);
	fftw_destroy_plan(backward_plan);
	fftw_free(in);
	fftw_free(out);

	// We export all the calculated data
	auto vti_data = vtkSmartPointer<vtkImageData>::New();
	vti_data->SetDimensions(Nx, Ny, 1);
	vti_data->SetSpacing(delta_x, delta_y, 0);
	vti_data->SetOrigin(-delta_x*(Nx-1)/2., -delta_y*(Ny-1)/2., 0);

	for(auto v : vti_data_arrays)
		vti_data->GetPointData()->AddArray(v);

	auto wavelength_data = vtkSmartPointer<vtkDoubleArray>::New();
	wavelength_data->SetNumberOfComponents(1);
	wavelength_data->SetNumberOfTuples(wavelengths.size());
	wavelength_data->SetName("lambda");
	for(int wave_idx=0; wave_idx<wavelengths.size(); wave_idx++)
		wavelength_data->SetComponent(wave_idx, 0, wavelengths[wave_idx]);
	vti_data->GetFieldData()->AddArray(wavelength_data);

	auto qx_data = vtkSmartPointer<vtkDoubleArray>::New();
	qx_data->SetNumberOfComponents(1);
	qx_data->SetNumberOfTuples(q_vals.size());
	qx_data->SetName("qx");
	for(unsigned int q_idx=0; q_idx<q_vals.size(); q_idx++)
		qx_data->SetComponent(q_idx, 0, q_vals[q_idx].first);
	vti_data->GetFieldData()->AddArray(qx_data);

	auto qy_data = vtkSmartPointer<vtkDoubleArray>::New();
	qy_data->SetNumberOfComponents(1);
	qy_data->SetNumberOfTuples(q_vals.size());
	qy_data->SetName("qy");
	for(unsigned int q_idx=0; q_idx<q_vals.size(); q_idx++)
		qy_data->SetComponent(q_idx, 0, q_vals[q_idx].second);
	vti_data->GetFieldData()->AddArray(qy_data);

	std::string filename = format_global_path() + ".vti";
	boost::filesystem::path path(filename);

	std::cout << "    Exporting " << path.filename().string() << std::endl;

	auto writer = vtkSmartPointer<vtkXMLImageDataWriter>::New();
	writer->SetFileName(filename.c_str());
	writer->SetInputData(vti_data);
	writer->SetDataModeToAppended();
	writer->EncodeAppendedDataOff();
	writer->Write();
}

void ScreenOutput::apply_no_export(
		ScreenOpticalFieldCollection &screen_optical_fields,
		std::complex<double>* fields_vals) {

	std::cout << "Calculating output transverse fields..." << std::endl;

	auto mesh = screen_optical_fields.mesh;
	Nx = mesh.Nx;
	Ny = mesh.Ny;
	delta_x = mesh.delta_x;
	delta_y = mesh.delta_y;

	// We create the FFTW plans for the forward and backward transform
	std::cout << "    Accumulating FFTW wisdom..." << std::endl;

	fftw_complex *in = static_cast<fftw_complex*>(
		fftw_malloc(sizeof(fftw_complex) * Nx*Ny));
	fftw_complex *out = static_cast<fftw_complex*>(
		fftw_malloc(sizeof(fftw_complex) * Nx*Ny));
	fftw_plan forward_plan =
		fftw_plan_dft_2d(Ny, Nx, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_plan backward_plan =
		fftw_plan_dft_2d(Ny, Nx, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);

	std::cout <<
		"    Filtering optical fields..." << std::endl;

	#pragma omp parallel for
	for(int wave_idx=0; wave_idx<wavelengths.size(); wave_idx++) {
		// FFTW arrays for this thread
		fftw_complex* field[2];
		fftw_complex* fft_field[2];
		for(int comp=0; comp<2; comp++) {
			field[comp] = static_cast<fftw_complex*>(
				fftw_malloc(sizeof(fftw_complex) * Nx*Ny));
			fft_field[comp] = static_cast<fftw_complex*>(
				fftw_malloc(sizeof(fftw_complex) * Nx*Ny));
		}

		for(int q_idx=0; q_idx<q_vals.size(); q_idx++) {
			// Propagation filter for this wavelength and incoming wavevector
			auto fourier_filter = assemble_fourier_filter(
				wavelengths[wave_idx], q_vals[q_idx]);

			for(int pol_idx=0; pol_idx<2; pol_idx++) {
				// First, we fill the fftw input arrays with our data and switch to
				// Fourier space
				for(int comp=0; comp<2; comp++) {
					for(int i=0; i<Nx*Ny; i++) {
						field[comp][i][0] =
							std::real(screen_optical_fields(wave_idx,q_idx,pol_idx)(i,comp));
						field[comp][i][1] =
							std::imag(screen_optical_fields(wave_idx,q_idx,pol_idx)(i,comp));
					}
					fftw_execute_dft(forward_plan, field[comp], fft_field[comp]);
				}

				// We apply the fourier filter
				for(int i=0; i<Nx*Ny; i++) {
					Eigen::Vector2cd tmp =
						fourier_filter->at(i)*
						Eigen::Vector2cd(
							std::complex<double>(fft_field[0][i][0], fft_field[0][i][1]),
							std::complex<double>(fft_field[1][i][0], fft_field[1][i][1]));
					for(int comp=0; comp<2; comp++) {
						fft_field[comp][i][0] = std::real(tmp[comp]);
						fft_field[comp][i][1] = std::imag(tmp[comp]);
					}
				}

				// We come back to real space and save the transformed data
				for(int comp=0; comp<2; comp++) {
					fftw_execute_dft(backward_plan, fft_field[comp], field[comp]);
					for(int i=0; i<Nx*Ny; i++)
						fields_vals[i+Nx*Ny*(comp+2*pol_idx+4*(q_idx+q_vals.size()*wave_idx))]=
							std::complex<double>(field[comp][i][0],field[comp][i][1]) /
							double(Nx*Ny);
				}
			}
		}

		// We free the FFTW pointers
		for(int comp=0; comp<2; comp++) {
			fftw_free(field[comp]);
			fftw_free(fft_field[comp]);
		}
	}

	// We free the FFTW pointers
	fftw_destroy_plan(forward_plan);
	fftw_destroy_plan(backward_plan);
	fftw_free(in);
	fftw_free(out);
}

std::shared_ptr<std::vector<Eigen::Matrix2cd> > ScreenOutput::assemble_fourier_filter(
		double wavelength, std::pair<double,double> q_val) const {

	double z_foc = focalisation_z_shift;
	double z_end = 0;
	for(int i=0; i<iso_layer_thickness.size(); i++) {
		z_foc += iso_layer_thickness[i] * (1-1./iso_layer_index[i]);
		z_end += iso_layer_thickness[i];
	}

	auto fourier_filter = std::make_shared<std::vector<Eigen::Matrix2cd> >(Nx*Ny);
	double k0 = 2*PI/wavelength;

	// total and local transfer matrix for forward and backward modes
	Eigen::Matrix4cd tmat, layer_tmat;
	// temporary matrices for the assembly of the transfer matrix
	Eigen::Matrix2cd tmp;
	Eigen::Matrix2d index_mat, next_inv_index_mat, identity, tp, tm;
	identity.setIdentity();

	#pragma omp parallel for firstprivate(tmat, layer_tmat, \
		index_mat, next_inv_index_mat, identity)
	for(int iy=0; iy<Ny; iy++) {
		for(int ix=0; ix<Nx; ix++) {
			double kx = (ix<double(Nx/2.)) ?
				2*PI*ix/(delta_x*Nx) :
				-2*PI*(Nx-ix)/(delta_x*Nx);
			double ky = (iy<double(Ny/2.)) ?
				2*PI*iy/(delta_y*Ny) :
				-2*PI*(Ny-iy)/(delta_y*Ny);
			Eigen::Vector2d q_vec = {kx/k0+q_val.first, ky/k0+q_val.second};
			double q_norm_sqr = std::pow(q_vec.norm(),2);

			if(q_vec.norm()<numerical_aperture) {
				tmat.setIdentity();

				double final_neff = std::sqrt(1-q_norm_sqr);

				for(int p=0; p<iso_layer_thickness.size(); p++) {
					double np = iso_layer_index[p];
					double next_np = iso_layer_index[p+1];
					double neff = std::sqrt(np*np-q_norm_sqr);
					double next_neff = std::sqrt(next_np*next_np-q_norm_sqr);

					index_mat <<
						neff+q_vec[0]*q_vec[0]/neff,
						q_vec[0]*q_vec[1]/neff,
						q_vec[0]*q_vec[1]/neff,
						neff+q_vec[1]*q_vec[1]/neff;
					next_inv_index_mat <<
						(1-std::pow(q_vec[0]/next_np,2))/next_neff,
						-q_vec[0]*q_vec[1]/(next_np*next_np*next_neff),
						-q_vec[0]*q_vec[1]/(next_np*next_np*next_neff),
						(1-std::pow(q_vec[1]/next_np,2))/next_neff;

					tp = 0.5*(identity+next_inv_index_mat*index_mat);
					tm = identity-tp;

					auto g = std::exp(std::complex<double>(0,
						(neff-final_neff)*k0*iso_layer_thickness[p]));
					layer_tmat.topLeftCorner<2,2>() = tp*g;
					layer_tmat.topRightCorner<2,2>() = tm*std::conj(g);
					layer_tmat.bottomLeftCorner<2,2>() = tm*g;
					layer_tmat.bottomRightCorner<2,2>() = tp*std::conj(g);

					tmat.noalias() = layer_tmat*tmat;
				}

				// We invert the (1,1) block and calculate the reduced transfer matrix for
				// forward modes
				auto det = tmat(2,2)*tmat(3,3)-tmat(2,3)*tmat(3,2);
				tmp << tmat(3,3)/det, -tmat(2,3)/det, -tmat(3,2)/det, tmat(2,2)/det;
				fourier_filter->at(ix+Nx*iy) =
					tmat.topLeftCorner<2,2>() -
					tmat.topRightCorner<2,2>()*tmp*tmat.bottomLeftCorner<2,2>();
				fourier_filter->at(ix+Nx*iy) *=
					std::exp(std::complex<double>(0,final_neff*k0*z_foc));
			}
			else
				fourier_filter->at(ix+Nx*iy).setZero();
		}
	}
	return fourier_filter;
}


/*
Modified in 2004-2007,2010 by Andrew Fox
E-Mail: mailto:foxostro@gmail.com

Original Author:
Ben Humphrey (DigiBen)
DigiBen@GameTutorials.com
Co-Web Host of www.GameTutorials.com
*/

#include <boost/tokenizer.hpp>

#include "stdafx.h"
#include "3dmath.h"

istream& operator>>(istream &in, Engine::vec4 & v)
{
	string str;
	std::getline(in, str, ')');
	str += ")"; // we actually need to keep the delimiter
	v.FromString(str);
	return in;
}

ostream& operator<<(ostream &out, const Engine::vec4 & v)
{
	out << v.ToString();
	return out;
}

namespace Engine {

bool vec4::FromString(string _s)
{
    string s = toLowerCase(_s);

    boost::char_separator<char> delimeters("(,)\t\n");
    boost::tokenizer<boost::char_separator<char> > tokens(s, delimeters);

    boost::tokenizer<boost::char_separator<char> >::const_iterator i = tokens.begin();

	if(i == tokens.end())
        return false;

    if(string(*i) != "vec")
        return false;

    x = stof(*(++i));
	y = stof(*(++i));
	z = stof(*(++i));
	w = 0.0f;

	return true;
}

string vec4::ToString(void) const
{
	string _x, _y, _z;

	_x = boost::lexical_cast<std::string>(x);
	_y = boost::lexical_cast<std::string>(y);
	_z = boost::lexical_cast<std::string>(z);

	return string("&vec(")
	       + _x + string(",")
		   + _y + string(",")
		   + _z + string(")");
}

bool IntersectedPlane(vec3 vPoly[], vec3 vLine[], vec3 &vNormal, float &originDistance)
{
	float distance1=0, distance2=0; // The distances from the 2 points of the line from the plane

	vNormal = vec3(vPoly[2] - vPoly[0]).cross(vec3(vPoly[1] - vPoly[0])).getNormal();

	// Let's find the distance our plane is from the origin.  We can find this value
	// from the normal to the plane (polygon) and any point that lies on that plane (Any vertex)
	originDistance = -vNormal.dot(vPoly[0]);

	// Get the distance from point1 from the plane using: Ax + By + Cz + D = (The distance from the plane)

	distance1 = ((vNormal.x * vLine[0].x)  +					// Ax +
		         (vNormal.y * vLine[0].y)  +					// Bx +
				 (vNormal.z * vLine[0].z)) + originDistance;	// Cz + D

	// Get the distance from point2 from the plane using Ax + By + Cz + D = (The distance from the plane)

	distance2 = ((vNormal.x * vLine[1].x)  +					// Ax +
		         (vNormal.y * vLine[1].y)  +					// Bx +
				 (vNormal.z * vLine[1].z)) + originDistance;	// Cz + D

	// Now that we have 2 distances from the plane, if we times them together we either
	// get a positive or negative number.  If it's a negative number, that means we collided!
	// This is because the 2 points must be on either side of the plane (IE. -1 * 1 = -1).

	if(distance1 * distance2 >= 0)			// Check to see if both point's distances are both negative or both positive
	   return false;						// Return false if each point has the same sign.  -1 and 1 would mean each point is on either side of the plane.  -1 -2 or 3 4 wouldn't...

	return true;							// The line intersected the plane, Return TRUE
}

vec3 IntersectionPoint(vec3 vNormal, vec3 vLine[], double distance)
{
	vec3 vPoint, vLineDir;					// Variables to hold the point and the line's direction
	double Numerator = 0.0, Denominator = 0.0, dist = 0.0;

	// 1)  First we need to get the vector of our line, Then normalize it so it's a length of 1
	vLineDir = vLine[1] - vLine[0];		// Get the vec3 of the line
	vLineDir.normalize();				// normalize the lines vector

	// 2) Use the plane equation (distance = Ax + By + Cz + D) to find the
	// distance from one of our points to the plane.
	Numerator = - (vNormal.x * vLine[0].x +		// Use the plane equation with the normal and the line
				   vNormal.y * vLine[0].y +
				   vNormal.z * vLine[0].z + distance);

	// 3) If we take the dot product between our line vector and the normal of the polygon,
	Denominator = vNormal.dot(vLineDir);		// Get the dot product of the line's vector and the normal of the plane

	// Since we are using division, we need to make sure we don't get a divide by zero error
	// If we do get a 0, that means that there are INFINATE points because the the line is
	// on the plane (the normal is perpendicular to the line - (Normal.vec3 = 0)).
	// In this case, we should just return any point on the line.

	if( Denominator == 0.0)						// Check so we don't divide by zero
		return vLine[0];						// Return an arbitrary point on the line

	dist = Numerator / Denominator;				// Divide to get the multiplying (percentage) factor

	// Now, like we said above, we times the dist by the vector, then add our arbitrary point.
	vPoint.x = (float)(vLine[0].x + (vLineDir.x * dist));
	vPoint.y = (float)(vLine[0].y + (vLineDir.y * dist));
	vPoint.z = (float)(vLine[0].z + (vLineDir.z * dist));

	return vPoint;								// Return the intersection point
}

} // namespace Engine


//============================================================================
// Name        : BoundaryFix.cpp
// Author      : bkatigb
// Version     :
// Copyright   :
// Description :
//============================================================================

#include "clipper.h"
#include <iomanip>
#include <iostream>
#include <cstdlib>
#include <cmath>
#include <stdio.h>
#include <ios>
#include <map>
#include <fstream>
#include <vector>
#include <list>
#include <set>
#include <fts.h>
#include <sstream>

#include <boost/assign.hpp>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include "BoundaryFix.h"

 BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

using namespace std;
using namespace ClipperLib;
using namespace boost::geometry;

void processBoundaryFixing(stringstream &instream, stringstream &outstream)
{
	vector<points> coord;

	/*data id*/
	string rec_id;
	int orig_item_cnt,item_out_cnt;
	string str,xp,yp,sv_rec_id, file_id ;

	using boost::assign::tuple_list_of;
	using boost::make_tuple;
	using boost::geometry::append;
	typedef boost::geometry::model::polygon<boost::tuple<int, int> > polygon;
	typedef boost::geometry::model::polygon
		<
			boost::tuple<int, int>
		> clockwise_closed_polygon;

	clockwise_closed_polygon cwcp,cwcp_in;
	double area_after;
	double area_before;

	typedef boost::geometry::model::d2::point_xy<double> xy;
	boost::geometry::model::linestring<xy> line;
	boost::geometry::model::linestring<xy> simplified;

	typedef boost::geometry::model::d2::point_xy<double> P;
	boost::geometry::model::linestring<P> line1, line2;

	int pos1,pos2,eol_ch;
	bool f1st=true;

	typedef boost::geometry::model::d2::point_xy<double> point_type;
	typedef boost::geometry::model::polygon<point_type> polygon_type;
	point_type p(4, 1);

	IntPoint XY;
	Polygon  poly_in;
	Polygons poly_out1;
	Polygons poly_out;
	polygon poly;
	points pt;

	f1st = true;
	int ctr0=0;
	int ctr1=0;
	int ctr2=0;
	int ln_ctr=0;

	bool b = false;
	bool proc = false;

	cwcp.clear();

	while (instream.good())
	{
		instream >> str;
		if (f1st)
		{
			pos1 = str.find(';');
			if (pos1 > 0)
			{
			sv_rec_id = str;
			}
//			f1st = false;
			ctr0 = 0;
		}
		pos1 = str.find(',');
		pos2 = str.size() - 1;
		eol_ch = instream.peek();
		if (f1st)
		{
			f1st = false;
		}
		else
		{
			if (pos1 != pos2 && pos1>0)
			{
				xp    = str.substr(0,pos1);
				yp    = str.substr(pos1+1,pos2-pos1);
				pt.x  = strtod(xp.c_str(),NULL);
				pt.y  = strtod(yp.c_str(),NULL);
				coord.push_back(pt);
				ctr0 = coord.size();
			}
		}
		if (eol_ch==10 || eol_ch==13)
		{
			rec_id        = sv_rec_id;
			//file_id       = instreamname.c_str();
			ctr0 = coord.size();

			if (ctr0 >= 4)
			{
				proc = true;
			}
			else
			{
				proc = false;
			}

			if (proc)
			{
				/*remove collinear*/
				boost::geometry::clear(line1);
				boost::geometry::clear(line2);
				for (int i=0;i!=ctr0;i++)
				{
					append(line1, make_tuple(coord[i].x, coord[i].y));
				}
				if (ctr0 > 500)
				{
					boost::geometry::simplify(line1, line2, 0.1);
				}
				else if (ctr0 > 200)
				{
					boost::geometry::simplify(line1, line2, 0.25);
				}
				else if (ctr0 < 50)
				{
					boost::geometry::simplify(line1, line2, 2.0);
//					boost::geometry::simplify(line1, line2, 0.0);
				}
				else
				{
					boost::geometry::simplify(line1, line2, 1.75);
				}

				ctr0 = line2.size();
				if (ctr0 >= 4)
				{
					proc = true;
				}
				else
				{
					proc = false;
				}
			}

			if (proc)
			{
				poly_in.clear();
				poly_out.clear();
				/*splits up in the event of self-intersect*/
				for (int i = 0;i!=ctr0;i++)
				{
					XY.X = line2.at(i).get<0>();
					XY.Y = line2.at(i).get<1>();
					poly_in.push_back(XY);
//					cout << XY.X << "," << XY.Y << " ";
				}

				SimplifyPolygon(poly_in,poly_out1);
//				cout << endl << "poly_out1.size()" << poly_out1.size() << endl;
				if (poly_out1.size() <=  1)
				{
					poly_out = poly_out1;
				}
				else
				{
					SimplifyPolygons(poly_out1,poly_out);
					poly_out = poly_out1;
				}

				ctr1 = poly_out.size();
//                cout << "ctr1: " << ctr1 << endl;
				for (int i=0;i<ctr1;i++)
				{
//					cout << "# " << i << " output size " << poly_out[i].size() << endl;
					if (poly_out[i].size()>=3)
					{
						ctr2 = poly_out[i].size();
						boost::geometry::clear(cwcp);

						for (int j=0;j!=ctr2;j++)
						{
							 boost::geometry::exterior_ring(cwcp).push_back(make_tuple(poly_out[i][j].X,poly_out[i][j].Y));
						}

						boost::geometry::correct(cwcp);
						outstream << rec_id;
						outstream << dsv(cwcp,","," ","","","","") << endl;
					}
				}
			}

			f1st = true;
			coord.clear();
		} /*eol*/

	} /*while good*/

	outstream.clear();
	coord.clear();

}


#include <math.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <chrono>

#include <opencv2/opencv.hpp>

#include <opencv2/core/cuda.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudafeatures2d.hpp>
#include <opencv2/xfeatures2d/cuda.hpp>
#include <opencv2/cudaarithm.hpp>
#include <opencv2/cudawarping.hpp>

#include <Eigen/Dense>

using namespace Eigen;
using namespace std;
using namespace chrono;

const float PI = 3.14285714286;

const string DATASETS_DIR = "../../datasets";
const string CURRENT_SET_DIR = "1";
const string INPUT_DIR = "input";
const string PRE_PROCESS_DIR = "pre-process";
const string INTERMEDIATE_EDUCATIONAL_DIR = "intermediate-educational";
const string OUTPUT_DIR = "output";

struct imageData {
    string imageName = "";
    double latitude = 0;
    double longitude = 0;
    double altitudeFeet = 0;
    double altitudeMeter = 0;
    double roll = 0;
    double pitch = 0;
    double yaw = 0;
};

struct imageSize2d {
    float width = 0.;
    float height = 0.;
    float scale = 0.;

    float getNewWidth() {
        if (scale == 0)
        {
            return 0.;
        }
        else
        {
            return width / scale;
        }
    }

    float getNewHeight() {
        if (scale == 0)
        {
            return 0.;
        }
        else
        {
            return height / scale;
        }
    }
};

inline ostream& operator<<(ostream& oss, const imageSize2d& other)
{
    oss << "width: " << other.width << ", height: " << other.height << ", scale: " << other.scale;
    return oss;
}

struct imageRange2d {
    int max_x = 0;
    int min_x = 0;
    int max_y = 0;
    int min_y = 0;

    int getWidth() {
        int w = max_x - min_x;
        //cout << "width = " << w << endl;
        return w;
    }

    int getHeight() {
        int h = max_y - min_y;
        //cout << "height = " << h << endl;
        return h;
    }
};

inline ostream& operator<<(ostream& oss, const imageRange2d& other)
{
    oss << "min_x: " << other.min_x << ", max_x: " << other.max_x << ", min_y: " << other.min_y << ", max_y: " << other.max_y;
    return oss;
}

void getMatFromGpuMat(cv::Mat & mat, cv::cuda::GpuMat & mat_gpu, int original_type = -1) {
    // convert from gpu matrix
    if (original_type != -1)
    {
        // if we are passed an original type we must convert the pixel data type of the gpu matrix
        cv::cuda::GpuMat mat_gpu_original;
        mat_gpu.convertTo (mat_gpu_original, original_type);
        mat_gpu_original.download (mat);
    }
    else
    {
        mat_gpu.download (mat);
    }
}

void printMat(cv::Mat& mat) {
    int rows = mat.rows;
    int cols = mat.cols;
    for (int i = 0; i < rows; i++) {
        for (int j = 0; j < cols; j++) {
            cout << mat.at<double>(i, j) << " ";
        }
        cout << endl;
    }
}

void printGpuMatReport(cv::cuda::GpuMat & mat_gpu, const char * name, int original_type = -1)
{
    cout << ">>> matrix = " << name << endl;

    // convert from gpu matrix
    cv::Mat mat;
    getMatFromGpuMat(mat, mat_gpu, original_type);

    // change to grayscale
    //cv::cvtColor(mat, mat, cv::COLOR_BGR2GRAY);
    //int type = mat.type();
    //int cn = CV_MAT_CN(type);
    //cout << "    type = " << type << ", cn = " << cn << endl;

    // dimensions
    cout << "    width = " << mat.cols << ", height = " << mat.rows << endl;

    // number of non-zero elements
    //cout << "    num non-zero elements = " << cv::countNonZero(mat)  << endl;
}

string getDatasetPath() {
    string path = DATASETS_DIR + "/" + CURRENT_SET_DIR + "/";
    return path;
}

string getInputPath() {
    string path = getDatasetPath() + INPUT_DIR + "/";
    return path;
}

string getPreProcessPath() {
    string path = getDatasetPath() + PRE_PROCESS_DIR + "/";
    return path;
}

string getIntermediateEducationalPath() {
    string path = getDatasetPath() + INTERMEDIATE_EDUCATIONAL_DIR + "/";
    return path;
}

string getOutputPath() {
    string path = getDatasetPath() + OUTPUT_DIR + "/";
    return path;
}

void writeIntermediateImage(cv::cuda::GpuMat& mat_gpu, const char* name, int iteration, int step, int original_type = -1) {
    // convert from gpu matrix
    cv::Mat mat;
    getMatFromGpuMat(mat, mat_gpu, original_type);

    // construct filename
    string intermediate_dir = getIntermediateEducationalPath();
    string intermediate_filename = intermediate_dir + to_string(iteration) + "_" + to_string(step) + "_" + name  +  + ".png";

    // write image
    cv::imwrite(intermediate_filename, mat);
}

void writeIntermediateText(cv::cuda::GpuMat& mat_gpu, const char* name, int iteration, int step, int original_type = -1) {
    // convert from gpu matrix
    cv::Mat mat;
    getMatFromGpuMat(mat, mat_gpu, original_type);

    // construct filename
    string intermediate_dir = getIntermediateEducationalPath();
    string intermediate_filename = intermediate_dir + to_string(iteration) + "_" + to_string(step) + "_" + name  +  + ".txt";

    // write text file
    ofstream myfile;
    myfile.open (intermediate_filename);
    myfile << mat;
    myfile.close();
}

void getTransformationToOriginPlane (imageData& pose, cv::Mat& transformation) {

    // get our 3 rotation angles
    double h = (pose.yaw * PI) / 180;
    double p = (pose.pitch * PI) / 180;
    double b = (pose.roll * PI) / 180;

    // compute the rotation matrix per axis
    Matrix3d Rz;
    Rz << cos (h), -sin (h), 0,
          sin (h), cos (h), 0,
          0, 0, 1;
    Matrix3d Ry;
    Ry << cos (p), 0, sin (p),
          0, 1, 0,
          -sin (p), 0, cos (p);
    Matrix3d Rx;
    Rx << 1, 0, 0,
          0, cos (b), -sin (b),
          0, sin (b), cos (b);

    // rotate back to origin plane in roll-pitch-yaw order
    Matrix3d R = Rz * (Ry * Rx);

    // do not adjust any rotations made around the z-axis
    R(0, 2) = 0;
    R(1, 2) = 0;
    R(2, 2) = 1;

    // create the final transformation
    Matrix3d Rtrans = R.transpose ();
    Matrix3d InvR = Rtrans.inverse ();
    transformation = (cv::Mat_<double>(3,3) << InvR(0,0), InvR(0,1), InvR(0,2),
                                               InvR(1,0), InvR(1,1), InvR(1,2),
                                               InvR(2,0), InvR(2,1), InvR(2,2));
}

void getRangeFromRectangleCorners(vector <vector<cv::Point2f>>& rectangles, imageRange2d& range2d) {
    // create the range to just include all the corners of all the rectangles in the collection

    // mmin & max values start out in opposite postions
    // and are iteratively pushed in the other direction
    float min_x = 1e9, max_x = -1e9;
    float min_y = 1e9, max_y = -1e9;

    // do the loop
    for (auto & rect : rectangles) {
        //cout << "rect = " << rect << endl;
        for (auto & corner : rect) {
            min_x = (min_x > corner.x)? corner.x : min_x;
            max_x = (max_x < corner.x)? corner.x : max_x;
            min_y = (min_y > corner.y)? corner.y : min_y;
            max_y = (max_y < corner.y)? corner.y : max_y;
        }
    }

    // round floats to appropriate integer value
    range2d.min_x = static_cast<int>(min_x - 0.5);
    range2d.max_x = static_cast<int>(max_x + 0.5);
    range2d.min_y = static_cast<int>(min_y - 0.5);
    range2d.max_y = static_cast<int>(max_y + 0.5);
    //cout << "getRangeFromRectangleCorners: range2d = " << range2d << endl;
}

void getScaledPaddedTransformation(cv::Mat& transformation, imageSize2d& size2d, cv::Mat& padded_transformation, imageRange2d& range2d) {
    // translate corners according to the scale factor
    vector<cv::Point2f> corners {
            cv::Point2f(0.,0.),
            cv::Point2f(0.,size2d.height/ size2d.scale),
            cv::Point2f(size2d.width/ size2d.scale,size2d.height/ size2d.scale),
            cv::Point2f(size2d.width/ size2d.scale,0.)
    };
    //cout << "corners = " << corners << endl;

    // apply the transformation to get the warped corners
    vector<cv::Point2f> warpedCorners;
    cv::perspectiveTransform(corners, warpedCorners, transformation);
    //cout << "warpedCorners = " << warpedCorners << endl;
    //cout << "transformation = " << transformation << endl;

    //get x & y range to encompass all the warped corners
    vector <vector<cv::Point2f>> rectangles;
    rectangles.push_back(warpedCorners);
    getRangeFromRectangleCorners(rectangles, range2d);
}

cv::Mat warpPerspectiveWithPadding (const cv::Mat& image, cv::Mat& transformation) {
    // reduce the individual image size so we have enough room to create the final stitched tapestry
    imageSize2d size2d{ static_cast<float>(image.cols), static_cast<float>(image.rows), 2.0 };
    //cout << "size2d = " << size2d << endl;
    cv::Mat small_img;
    cv::resize (image, small_img, cv::Size (size2d.getNewWidth(), size2d.getNewHeight()));

    // get a padded transformation scaled to to our reduced image size
    cv::Mat padded_transformation;
    imageRange2d range2d;
    getScaledPaddedTransformation(transformation, size2d, padded_transformation, range2d);
    cv::Mat translation = (cv::Mat_<double>(3,3) << 1, 0, -range2d.min_x, 0, 1, -range2d.min_y, 0, 0, 1);
    cv::Mat fullTransformation = translation * transformation;

    // perform the image perspective warp
    cv::cuda::GpuMat gpu_img (small_img);
    cv::cuda::GpuMat gpu_img_wp;
    cv::cuda::GpuMat gpu_ft (fullTransformation);
    //cout << "fullTransformation = " << fullTransformation << endl;
    cv::Size gpu_size{ range2d.getWidth(), range2d.getHeight() };
    //cout << "gpu_size = " << gpu_size << endl;
    cv::cuda::warpPerspective(gpu_img, gpu_img_wp, fullTransformation, gpu_size);

    // download the final result matrix
    cv::Mat result (gpu_img_wp.size(), gpu_img_wp.type());
    gpu_img_wp.download (result);
    return result;
}

void getKeypoints(  cv::cuda::SURF_CUDA & detector,
                    cv::cuda::GpuMat & img_gpu,
                    cv::cuda::GpuMat & descriptors_gpu,
                    vector<cv::KeyPoint> & keypoints)
{
    cv::cuda::GpuMat img_gray_gpu;
    cv::cuda::cvtColor (img_gpu, img_gray_gpu, cv::COLOR_BGR2GRAY);
    cv::cuda::GpuMat mask_gpu;
    cv::cuda::threshold (img_gray_gpu, mask_gpu, 1, 255, cv::THRESH_BINARY);

    cv::cuda::GpuMat keypoints1_gpu;
    detector (img_gray_gpu, mask_gpu, keypoints1_gpu, descriptors_gpu);
    detector.downloadKeypoints (keypoints1_gpu, keypoints);
}

void matchKeypointDescriptors(  cv::cuda::GpuMat & descriptors1_gpu, cv::cuda::GpuMat & descriptors2_gpu,
                                vector<cv::KeyPoint> & keypoints1, vector<cv::KeyPoint> & keypoints2,
                                vector<cv::Point2f> & src_pts, vector<cv::Point2f> & dst_pts)
{
    cv::Ptr<cv::cuda::DescriptorMatcher> matcher = cv::cuda::DescriptorMatcher::createBFMatcher ();
    vector<vector<cv::DMatch>> knn_matches;
    matcher->knnMatch (descriptors2_gpu, descriptors1_gpu, knn_matches, 2);

    vector<cv::DMatch> matches;
    vector<vector<cv::DMatch>>::const_iterator it;
    for (it = knn_matches.begin(); it != knn_matches.end(); ++it) {
        if(it->size() > 1 && (*it)[0].distance/(*it)[1].distance < 0.55) {
            matches.push_back((*it)[0]);
        }
    }

    for (auto m : matches) {
        src_pts.push_back (keypoints2[m.queryIdx].pt);
        dst_pts.push_back (keypoints1[m.trainIdx].pt);
    }
}

void getAffineTransformation(  float height1, float width1, float height2, float width2,
                               vector<cv::Point2f> & src_pts, vector<cv::Point2f> & dst_pts,
                               cv::Mat & A, imageRange2d& range2d)
{
    A = cv::estimateRigidTransform(src_pts, dst_pts, false);

    vector<cv::Point2f> corners1{
            cv::Point2f(0.,0.),
            cv::Point2f(0.,height1),
            cv::Point2f(width1,height1),
            cv::Point2f(width1,0.)
    };

    vector<cv::Point2f> corners2{
            cv::Point2f(0.,0.),
            cv::Point2f(0.,height2),
            cv::Point2f(width2,height2),
            cv::Point2f(width2,0.)
    };

    vector<cv::Point2f> warpedCorners2{
            cv::Point2f(0.,0.),
            cv::Point2f(0.,0.),
            cv::Point2f(0.,0.),
            cv::Point2f(0.,0.)
    };

    vector<vector<cv::Point2f>> allCorners;
    allCorners.push_back(corners1);

    for (int i = 0; i < 4; i++) {
        float cornerX = corners2[i].x;
        float cornerY = corners2[i].y;
        warpedCorners2[i].x = A.at<double> (0,0) * cornerX + A.at<double> (0,1) * cornerY + A.at<double> (0,2);
        warpedCorners2[i].y = A.at<double> (1,0) * cornerX + A.at<double> (1,1) * cornerY + A.at<double> (1,2);
    }
    allCorners.push_back(warpedCorners2);

    //get x & y range to encompass all the warped corners
    getRangeFromRectangleCorners(allCorners, range2d);
}

cv::Mat combinePair (cv::Mat& img1, cv::Mat& img2) {
    static int call_count = 0;
    call_count++;
    int step = 0;

    // convert to a form our gpu can handle
    cv::cuda::GpuMat img1_gpu (img1), img2_gpu (img2);

    // save original image type (type is the data type used to store each pixel)
    // we will later convert image types to a floating point for math calculations
    // and then when done we will convert back to the original unsigned 8bit type
    // which is necessary for image display or writing file to disk
    int original_type = img1_gpu.type();

    // get keypoint descriptors for the two images
    cv::cuda::SURF_CUDA detector;
    cv::cuda::GpuMat descriptors1_gpu, descriptors2_gpu;
    vector<cv::KeyPoint> keypoints1, keypoints2;
    getKeypoints(detector, img1_gpu, descriptors1_gpu, keypoints1);
    getKeypoints(detector, img2_gpu, descriptors2_gpu, keypoints2);

    // match the keypoint descriptors
    vector<cv::Point2f> src_pts, dst_pts;
    matchKeypointDescriptors(descriptors1_gpu, descriptors2_gpu, keypoints1, keypoints2, src_pts, dst_pts);

    // get affine transformation from our src pts (image 2) to our dst pts (image 1)
    float height1 = static_cast<float>(img1.rows), width1 = static_cast<float>(img1.cols);
    float height2 = static_cast<float>(img2.rows), width2 = static_cast<float>(img2.cols);
    cv::Mat A;
    imageRange2d range2d;
    getAffineTransformation(height1, width1, height2, width2, src_pts, dst_pts, A, range2d);
    cout << "range2d(" << range2d.getWidth() << "," << range2d.getHeight() << ")" << endl;
    cv::Mat translation = (cv::Mat_<double>(3,3) << 1, 0, -range2d.min_x, 0, 1, -range2d.min_y, 0, 0, 1);

    // homographically warp each image so that it is sized correctly
    // relative to the size of the bounding rectangle
    // who was sized to exactly accomodate the 4 corners of both of the images
    cv::cuda::GpuMat warpedPerspectiveImg1_gpu;
    cv::cuda::warpPerspective (img1_gpu, warpedPerspectiveImg1_gpu, translation, cv::Size (range2d.getWidth(), range2d.getHeight()));
    printGpuMatReport(img1_gpu, "img1_gpu");
    printGpuMatReport(warpedPerspectiveImg1_gpu, "warpedPerspectiveImg1_gpu");
    writeIntermediateImage(warpedPerspectiveImg1_gpu, "warpedPerspectiveImg1_gpu", call_count, ++step);
    //
    cv::cuda::GpuMat warpedPerspectiveImg2_gpu;
    cv::cuda::warpPerspective (img2_gpu, warpedPerspectiveImg2_gpu, translation, cv::Size (range2d.getWidth(), range2d.getHeight()));
    printGpuMatReport(img2_gpu, "img2_gpu");
    printGpuMatReport(warpedPerspectiveImg2_gpu, "warpedPerspectiveImg2_gpu");
    writeIntermediateImage(warpedPerspectiveImg2_gpu, "warpedPerspectiveImg2_gpu", call_count, ++step);

    // affinely warp image 2 so that it is positioned correctly
    // within the bounding rectangle relative to the position of image 1
    // (remember the affine transformation was created by matching keypoints of the two images)
    cv::cuda::GpuMat warpedPerspectiveAffineImg2_gpu;
    cv::cuda::warpAffine (warpedPerspectiveImg2_gpu, warpedPerspectiveAffineImg2_gpu, A, cv::Size (range2d.getWidth(), range2d.getHeight()));
    printGpuMatReport(warpedPerspectiveAffineImg2_gpu, "warpedPerspectiveAffineImg2_gpu");
    writeIntermediateImage(warpedPerspectiveAffineImg2_gpu, "warpedPerspectiveAffineImg2_gpu", call_count, ++step);

    // apply a threshhold to our affinely warped image 2 and save as a mask
    // this mask will indicate the portion of our combined images reserved for our image 2
    cv::cuda::GpuMat mask_gpu;
    cv::cuda::threshold (warpedPerspectiveAffineImg2_gpu, mask_gpu, 1, 255, cv::THRESH_BINARY);
    writeIntermediateImage(mask_gpu, "warpedPerspectiveAffineImg2_mask_gpu", call_count, ++step);
    writeIntermediateText(mask_gpu, "warpedPerspectiveAffineImg2_mask_gpu", call_count, step);

    // convert our 2 images and our mask to 32bit floating point type for our math calculations
    warpedPerspectiveImg1_gpu.convertTo (warpedPerspectiveImg1_gpu, CV_32FC3);
    warpedPerspectiveAffineImg2_gpu.convertTo (warpedPerspectiveAffineImg2_gpu, CV_32FC3);
    mask_gpu.convertTo (mask_gpu, CV_32FC3, 1.0/255);

    // create an empty combined image of the correct size type
    cv::cuda::GpuMat combined_gpu (warpedPerspectiveAffineImg2_gpu.size(), warpedPerspectiveAffineImg2_gpu.type());

    // multiply our affinely warped image 2 by our mask to get only the overlay image data
    cv::cuda::multiply (mask_gpu, warpedPerspectiveAffineImg2_gpu, warpedPerspectiveAffineImg2_gpu);
    writeIntermediateImage(warpedPerspectiveAffineImg2_gpu, "warpedPerspectiveAffineImg2_gpu_multiply_by_mask_gpu", call_count, ++step, original_type);

    // create our difference mask which will be used to carve out a place in our image 1 to receive our image 2
    cv::Mat mask;
    mask_gpu.download (mask);
    cv::Mat difference_mask = cv::Scalar::all (1.0) - mask;
    cv::cuda::GpuMat difference_mask_gpu (difference_mask);
    writeIntermediateImage(difference_mask_gpu, "warpedPerspectiveAffineImg2_difference_mask_gpu", call_count, ++step, original_type);
    writeIntermediateText(difference_mask_gpu, "warpedPerspectiveAffineImg2_difference_mask_gpu", call_count, step);

    // multiply our perspective warped image 1 by our difference mask to get the portion of the image data we will retain
    cv::cuda::multiply(difference_mask_gpu, warpedPerspectiveImg1_gpu, warpedPerspectiveImg1_gpu);
    writeIntermediateImage(warpedPerspectiveImg1_gpu, "warpedPerspectiveImg1_gpu_multiply_by_difference_mask_gpu", call_count, ++step, original_type);

    // add our two correctly masked image 1 and image 2 to get the combined image
    cv::cuda::add (warpedPerspectiveImg1_gpu, warpedPerspectiveAffineImg2_gpu, combined_gpu);
    combined_gpu.convertTo (combined_gpu, original_type);
    writeIntermediateImage(combined_gpu, "combined_gpu", call_count, ++step);

    // get a cpu version of the combined image
    cv::Mat combined;
    combined_gpu.download (combined);
    return combined;
}

cv::Mat combine (vector<cv::Mat>& imageList) {
    cv::Mat result = imageList[0];
    for (int i = 1; i < imageList.size(); i++) {
        cv::Mat image = imageList[i];
        cout << i << endl;
        auto start = high_resolution_clock::now();
        result = combinePair (result, image);
        auto end = high_resolution_clock::now();
        auto duration = duration_cast<microseconds> (end-start);
        cout << "time taken by the functions: " << duration.count() << endl;
        float h = result.rows;
        float w = result.cols;
        if (h > 4000 || w > 4000) {
            if (h > 4000) {
                float hx = 4000.0/h;
                h = h * hx;
                w = w * hx;
            }
            else if (w > 4000) {
                float wx = 4000.0/w;
                w = w * wx;
                h = h * wx;
            }
        }
        cout << "result(" << w << "," << h << ")" << endl;
        cv::resize (result, result, cv::Size (w, h));
        cout << "________________________________________" << endl;
    }
    return result;
}

void readData (string& filename,
               vector<imageData>& dataMatrix) {
    ifstream file;
    file.open (filename);
    if (file.is_open()) {
        string line;
        while (getline (file, line)) {
            stringstream ss (line);
            string word;
            imageData id;
            int i = 0;
            while (getline (ss, word, ',')) {
                if (i == 0)	{ id.imageName = word; }
                else if (i == 1) { id.latitude = stof(word); }
                else if (i == 2) { id.longitude = stof(word); }
                else if (i == 3) {
                    id.altitudeFeet = stof (word);
                    id.altitudeMeter = id.altitudeFeet * 0.3048;
                }
                else if (i == 4) { id.yaw = stof(word); }
                else if (i == 5) { id.pitch = stof(word); }
                else if (i == 6) { id.roll = stof(word); }
                i++;
            }
            dataMatrix.push_back (id);
        }
    }
}

void getImageList (vector<cv::Mat>& imageList,
                   vector<imageData>& dataMatrix,
                   string base_path) {
    for (auto data : dataMatrix) {
        string img_path = base_path + data.imageName;
        cv::Mat img = cv::imread (img_path, 1);
        // cout << img.empty () << endl;
        // cout << img_path << endl;
        imageList.push_back (img);
    }
}

void changePerspective(vector<cv::Mat>& imageList,
    vector<imageData>& dataMatrix) {
    cout << "Warping Images Now" << endl;
    int n = imageList.size();
    for (int i = 0; i < n; i++) {
        // get a transformation to the origin plane
        // to unwind any rotations done to the camera before the image was taken
        cv::Mat M;
        getTransformationToOriginPlane(dataMatrix[i], M);
        cv::Mat correctedImage = warpPerspectiveWithPadding(imageList[i], M);
        cv::imwrite(getPreProcessPath() + dataMatrix[i].imageName, correctedImage);
    }
    cout << "Image Warping Done" << endl;
}

int main() {
    string filename = getInputPath() + "image_catalog.txt";
    vector<imageData> dataMatrix;
    readData(filename, dataMatrix);
    vector<cv::Mat> imageList;
    string base_path = getInputPath();
    getImageList(imageList, dataMatrix, base_path);
    changePerspective(imageList, dataMatrix);
    imageList.clear();
    base_path = getPreProcessPath();
    getImageList(imageList, dataMatrix, base_path);
    cv::Mat result = combine(imageList);
    cv::imwrite(getOutputPath() + "result.png", result);
    return 0;
}


#ifndef _polyvec_curve_line_h_
#define _polyvec_curve_line_h_

// C++ STL
#include <functional>
#include <vector>

// Eigen
#include <Eigen/Core>

#include <polyvec/curve-tracer/curve.hpp>

namespace polyvec {
	// Rather than wrapping curnucopia's line, it seems to be easier to
	// just rewrite this one.
	// The better practice .
	class GlobFitCurve_Line : public  GlobFitCurve {
	public:
		// GlobFitCurve Implementation
		GlobFitCurveType get_type() const override;
		int n_params() const override;

		Eigen::Vector2d pos(const double t) const override;
		Eigen::Vector2d dposdt(const double t) const override;
		Eigen::Vector2d dposdtdt(const double t) const override;
		Eigen::Vector2d dposdtdtdt(const double t) const override;

		Eigen::Matrix2Xd dposdparams(const double t) const override;
		Eigen::Matrix2Xd dposdtdparams(const double t) const override;
		Eigen::Matrix2Xd dposdtdtdparams(const double t) const override;
		Eigen::Matrix2Xd dposdtdtdtdparams(const double t) const override;

		double project(const Eigen::Vector2d& point) const override;

		Eigen::VectorXd dtprojectdparams(const double t, const Eigen::Vector2d& point) const override;
		Eigen::Matrix2Xd dposprojectdparams(const double t, const Eigen::VectorXd& dtprojectdparams) const override;
		Eigen::Matrix2Xd dposdtprojectdparams(const double t, const Eigen::VectorXd& dtprojectdparams) const override;

		void set_params(const Eigen::VectorXd& params) override;
		Eigen::VectorXd get_params() const override;

		Eigen::Matrix2Xd get_tesselation2() const override;
		Eigen::VectorXd get_tesselationt() const override;

		GlobFitCurve* clone() const override;

		geom::aabb get_bounding_box() const;

		// Line functions
		void set_points(const Eigen::Matrix2d& points);
		void set_points(const Eigen::Vector2d& p0, const Eigen::Vector2d& p1);
		Eigen::Matrix2d get_points();

		// Constructors
		GlobFitCurve_Line() = default;
		GlobFitCurve_Line(const Eigen::Matrix2d& points);
		GlobFitCurve_Line(const Eigen::Vector2d& p0, const Eigen::Vector2d& p1);

		std::pair<GlobFitCurve*, GlobFitCurve*> split(double t) const;

	private:
		static constexpr int _n_params = 4;
		bool _are_points_set = false;
		// First column is first point, second point is the second point
		Eigen::Matrix2d _points = Eigen::Matrix2d::Constant(1e10);
	};
}

#endif


/**
 * @addtogroup DFNs
 * @{
 */

#include "StereoRectification.hpp"
#include <Eigen/Core>
#include <iostream>

namespace CDFF
{
namespace DFN
{
namespace StereoRectification
{

StereoRectification::StereoRectification()
:parameters(DEFAULT_PARAMETERS)
{
    parametersHelper.AddParameter<int>("StereoRectificationParams", "xratio", parameters.xratio, DEFAULT_PARAMETERS.xratio);
    parametersHelper.AddParameter<int>("StereoRectificationParams", "yratio", parameters.yratio, DEFAULT_PARAMETERS.yratio);
    parametersHelper.AddParameter<double>("StereoRectificationParams", "scaling", parameters.scaling, DEFAULT_PARAMETERS.scaling);
    parametersHelper.AddParameter<bool>("StereoRectificationParams", "fisheye", parameters.fisheye, DEFAULT_PARAMETERS.fisheye);
    parametersHelper.AddParameter<std::string>("StereoRectificationParams", "calibrationFilePath", parameters.calibrationFilePath, DEFAULT_PARAMETERS.calibrationFilePath);

    configurationFilePath = "";
}

StereoRectification::~StereoRectification()
{
}

void StereoRectification::configure()
{
    if(configurationFilePath != ""){
        parametersHelper.ReadFile(configurationFilePath);
    }
    ValidateParameters();
}

void StereoRectification::process()
{
    cv::Mat inLeft(static_cast<int>(inOriginalStereoPair.left.data.rows), static_cast<int>(inOriginalStereoPair.left.data.cols), CV_MAKETYPE(static_cast<int>(inOriginalStereoPair.left.data.depth), static_cast<int>(inOriginalStereoPair.left.data.channels)), inOriginalStereoPair.left.data.data.arr, inOriginalStereoPair.left.data.rowSize);
    cv::Mat inRight(static_cast<int>(inOriginalStereoPair.right.data.rows), static_cast<int>(inOriginalStereoPair.right.data.cols), CV_MAKETYPE(static_cast<int>(inOriginalStereoPair.right.data.depth), static_cast<int>(inOriginalStereoPair.right.data.channels)), inOriginalStereoPair.right.data.data.arr, inOriginalStereoPair.right.data.rowSize);
    cv::Mat outLeft;
    cv::Mat outRight;

    // Generate correction maps if needed
    if( std::string(reinterpret_cast<char const *>(inOriginalStereoPair.left.intrinsic.sensorId.arr)) != _sensorIdLeft ||
            std::string(reinterpret_cast<char const *>(inOriginalStereoPair.right.intrinsic.sensorId.arr)) != _sensorIdRight ||
            parameters.calibrationFilePath != _calibrationFilePath ||
            parameters.xratio != _xratio ||
            parameters.yratio != _yratio ||
            parameters.scaling != _scaling ||
            parameters.fisheye != _fisheye){

        _sensorIdLeft = std::string(reinterpret_cast<char const *>(inOriginalStereoPair.left.intrinsic.sensorId.arr));
        _sensorIdRight = std::string(reinterpret_cast<char const *>(inOriginalStereoPair.right.intrinsic.sensorId.arr));
        _calibrationFilePath = parameters.calibrationFilePath;
        _xratio = parameters.xratio;
        _yratio = parameters.yratio;
        _scaling = parameters.scaling;
        _fisheye = parameters.fisheye;

        cv::FileStorage fs( _calibrationFilePath + "/" + _sensorIdLeft + std::string("-") + _sensorIdRight + ".yml", cv::FileStorage::READ );
        if( fs.isOpened() ){
            cv::Size imageSize;
            cv::Mat1d cameraMatrixL;
            cv::Mat1d cameraMatrixR;
            cv::Mat1d distCoeffsL;
            cv::Mat1d distCoeffsR;
            cv::Mat1d R;
            cv::Mat1d T;

            fs["image_width"]  >> imageSize.width;
            fs["image_height"] >> imageSize.height;

            fs["camera_matrix_1"] >> cameraMatrixL;
            fs["distortion_coefficients_1"] >> distCoeffsL;

            fs["camera_matrix_2"] >> cameraMatrixR;
            fs["distortion_coefficients_2"] >> distCoeffsR;

            fs["rotation_matrix"] >> R;
            fs["translation_coefficients"] >> T;

            cv::Size newSize;
            newSize.width = imageSize.width / _xratio;
            newSize.height = imageSize.height / _yratio;

            cv::Mat1d RLeft;
            cv::Mat1d RRight;
            cv::Mat1d Q;

            if(_fisheye){
                cv::fisheye::stereoRectify(cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, imageSize, R, T, RLeft, RRight, _PLeft, _PRight, Q, CV_CALIB_ZERO_DISPARITY, newSize, _scaling);

                cv::fisheye::initUndistortRectifyMap(cameraMatrixL, distCoeffsL, RLeft, _PLeft, newSize, CV_32F, _lmapx, _lmapy);
                cv::fisheye::initUndistortRectifyMap(cameraMatrixR, distCoeffsR, RRight, _PRight, newSize, CV_32F, _rmapx, _rmapy);
            }
            else{
                cv::stereoRectify(cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, imageSize, R, T, RLeft, RRight, _PLeft, _PRight, Q, CV_CALIB_ZERO_DISPARITY, _scaling, newSize);

                cv::initUndistortRectifyMap(cameraMatrixL, distCoeffsL, RLeft, _PLeft, newSize, CV_32F, _lmapx, _lmapy);
                cv::initUndistortRectifyMap(cameraMatrixR, distCoeffsR, RRight, _PRight, newSize, CV_32F, _rmapx, _rmapy);
            }

            _baseline = 1.0 / Q.at<double>(3,2);

            _initialized = true;
        }
        else{
            _initialized = false;
            std::cerr << "Can't open the calibration file: " << _calibrationFilePath + "/" + _sensorIdLeft + std::string("-") + _sensorIdRight + ".yml" << std::endl;
        }
    }

    if( _initialized ){
        cv::remap(inLeft, outLeft, _lmapx, _lmapy, cv::INTER_LINEAR);
        cv::remap(inRight, outRight, _rmapx, _rmapy, cv::INTER_LINEAR);

        // Getting image pair
        outRectifiedStereoPair.msgVersion = frame_Version;
        outRectifiedStereoPair.baseline = _baseline;

        // Left image
        {
            asn1SccFrame & img = outRectifiedStereoPair.left;

            // init the structure
            img.msgVersion = frame_Version;

            img.intrinsic = inOriginalStereoPair.left.intrinsic;

            Eigen::Map<Eigen::Matrix3d>(img.intrinsic.cameraMatrix.arr[0].arr, 3, 3) = Eigen::Map<Eigen::Matrix3d, 0, Eigen::OuterStride<>>(reinterpret_cast<double *>(_PLeft.data), 3, 3, Eigen::OuterStride<>(4));

            Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>>(img.intrinsic.distCoeffs.arr, inOriginalStereoPair.left.intrinsic.distCoeffs.nCount, 1) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(inOriginalStereoPair.left.intrinsic.distCoeffs.nCount, 1);
            img.intrinsic.distCoeffs.nCount = inOriginalStereoPair.left.intrinsic.distCoeffs.nCount;

            if(_fisheye){
                img.intrinsic.cameraModel = asn1Scccam_FISHEYE;
            }
            else{
                img.intrinsic.cameraModel = asn1Scccam_PINHOLE;
            }

            img.extrinsic = inOriginalStereoPair.left.extrinsic;
            img.metadata = inOriginalStereoPair.left.metadata;

            // Array3D
            {
                img.data.msgVersion = array3D_Version;
                img.data.rows = static_cast<asn1SccT_UInt32>(outLeft.rows);
                img.data.cols = static_cast<asn1SccT_UInt32>(outLeft.cols);
                img.data.channels = static_cast<asn1SccT_UInt32>(outLeft.channels());
                img.data.depth = static_cast<asn1SccArray3D_depth_t>(outLeft.depth());
                img.data.rowSize = static_cast<asn1SccT_UInt32>(outLeft.step[0]);
                img.data.data.nCount = static_cast<int>(img.data.rows * img.data.rowSize);
                memcpy(img.data.data.arr, outLeft.data, static_cast<size_t>(img.data.data.nCount));
            }
        }

        // Right image
        {
            asn1SccFrame & img = outRectifiedStereoPair.right;

            // init the structure
            img.msgVersion = frame_Version;

            img.intrinsic = inOriginalStereoPair.right.intrinsic;

            Eigen::Map<Eigen::Matrix3d>(img.intrinsic.cameraMatrix.arr[0].arr, 3, 3) = Eigen::Map<Eigen::Matrix3d, 0, Eigen::OuterStride<>>(reinterpret_cast<double *>(_PRight.data), 3, 3, Eigen::OuterStride<>(4));

            Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>>(img.intrinsic.distCoeffs.arr, inOriginalStereoPair.right.intrinsic.distCoeffs.nCount, 1) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(inOriginalStereoPair.right.intrinsic.distCoeffs.nCount, 1);
            img.intrinsic.distCoeffs.nCount = inOriginalStereoPair.right.intrinsic.distCoeffs.nCount;

            if(_fisheye){
                img.intrinsic.cameraModel = asn1Scccam_FISHEYE;
            }
            else{
                img.intrinsic.cameraModel = asn1Scccam_PINHOLE;
            }

            img.extrinsic = inOriginalStereoPair.right.extrinsic;
            img.metadata = inOriginalStereoPair.right.metadata;

            // Array3D
            {
                img.data.msgVersion = array3D_Version;
                img.data.rows = static_cast<asn1SccT_UInt32>(outRight.rows);
                img.data.cols = static_cast<asn1SccT_UInt32>(outRight.cols);
                img.data.channels = static_cast<asn1SccT_UInt32>(outRight.channels());
                img.data.depth = static_cast<asn1SccArray3D_depth_t>(outRight.depth());
                img.data.rowSize = static_cast<asn1SccT_UInt32>(outRight.step[0]);
                img.data.data.nCount = static_cast<int>(img.data.rows * img.data.rowSize);
                memcpy(img.data.data.arr, outRight.data, static_cast<size_t>(img.data.data.nCount));
            }
        }
    }
}

void StereoRectification::ValidateParameters()
{
    ASSERT(parameters.xratio >= 1 && parameters.xratio <= 25, "xratio has to be within [1..25]");
    ASSERT(parameters.yratio >= 1 && parameters.yratio <= 25, "yratio has to be within [1..25]");
    ASSERT((parameters.scaling >= 0 && parameters.scaling <= 1) || parameters.scaling == -1, "scaling has to be within [0..1] or equal to -1");
}

const StereoRectification::StereoRectificationParams StereoRectification::DEFAULT_PARAMETERS = {
    .xratio = 1,
    .yratio = 1,
    .scaling = -1,
    .fisheye = false,
    .calibrationFilePath = ""
};

}
}
}

/** @} */


/*
 * Copyright 2018, LAAS-CNRS
 * Author: Steve Tonneau
 */

#ifndef BEZIER_COM_TRAJ_LIB_SOLVE_H
#define BEZIER_COM_TRAJ_LIB_SOLVE_H

#include <hpp/bezier-com-traj/local_config.hh>
#include <hpp/bezier-com-traj/data.hh>
#include <Eigen/Dense>

namespace bezier_com_traj
{
      /**
      * @brief solve0step Tries to solve the 0-step capturability problem. Given the current contact phase,
      * a COM position, and an initial velocity, tries to compute a feasible COM trajectory that
      * stops the character without falling.
      * In this specific implementation, the considered constraints are:
      * init position and velocity, 0 velocity constraints (acceleration constraints are ignored)
      * @param pData problem Data. Should contain only one contact phase.
      * @param Ts timelength of each contact phase. Should only contain one value
      * @param timeStep time that the solver has to stop.
      * @return ResultData a struct containing the resulting trajectory, if success is true.
      */
     BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solve0step(const ProblemData& pData, const std::vector<double>& Ts,
                                                         const double timeStep = -1);

     /**
     * @brief computeCOMTraj Tries to solve the one step problem :  Given two or three contact phases,
     * an initial and final com position and velocity,
     * try to compute the CoM trajectory (as a Bezier curve) that connect them
     * @param pData problem Data.
     * @param Ts timelength of each contact phase. Should be the same legnth as pData.contacts
     * @param timeStep time step used by the discretization
     * @return ResultData a struct containing the resulting trajectory, if success is true.
     */
    BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj computeCOMTrajFixedSize(const ProblemData& pData, const VectorX& Ts,const unsigned int pointsPerPhase = 3);

    /**
    * @brief computeCOMTraj Tries to solve the one step problem :  Given two or three contact phases,
    * an initial and final com position and velocity,
    * try to compute the CoM trajectory (as a Bezier curve) that connect them
    * @param pData problem Data.
    * @param Ts timelength of each contact phase. Should be the same length as pData.contacts
    * @param timeStep time step used by the discretization, if -1 : use the continuous fomulation
    * @param solver solver used to perform optimization. WARNING: if the continuous force formulation is
    * used, it is highly recommended to use the SOLVER_GLPK solver if available and a quadratic
    * cost is not necessary, as these solvers are increasely more computationnaly efficient for the problem
    * @return ResultData a struct containing the resulting trajectory, if success is true.
    */
   BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts, const double timeStep = -1, const solvers::SolverType solver = solvers::SOLVER_QUADPROG );

} // end namespace bezier_com_traj

#endif


// Copyright 2016 by Glukhov V. O. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "solver.hpp"
#include "util.hpp"
#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <iostream>

namespace cnn
{
	namespace solver
	{
		void SgdSolver::Solve()
		{
			using namespace arma;
#ifndef NDEBUG
			assert(net_->is_initialized());
#endif
			std::vector<std::pair<cnn::tensor4d, cnn::tensor4d>> gradient;
			std::vector<std::pair<cnn::tensor4d, cnn::tensor4d>> iter_gradient;
			for (uword epoch = 0; epoch < max_epoch_; ++epoch) {
				double error = 0.0;
				net_->LoadTrainImage();
				net_->Forward();
				error += net_->Error();
				gradient = net_->Backpropagation();
				// training network on training dataset
				std::cout << boost::format(
					"compute error on training dataset for %1% samples on %2% training epoches..."
				) % batch_size_ % (epoch + 1) << "\n";
				for (uword i = 1; i < batch_size_; ++i) {
					net_->LoadTrainImage();
					net_->Forward();
					error += net_->Error();
					iter_gradient = net_->Backpropagation();
					for (std::size_t n = 0; n < gradient.size(); ++n) {
						// weights
						for (uword item = 0; item < gradient[n].first.data.size(); ++item) {
							gradient[n].first.data[item] += iter_gradient[n].first.data[item];
						}
						//biases
						for (uword item = 0; item < gradient[n].second.data.size(); ++item) {
							gradient[n].second.data[item] += iter_gradient[n].second.data[item];
						}
					}
				}
				// get average deltas
				for (std::size_t n = 0; n < gradient.size(); ++n) {
					// weights
					for (uword item = 0; item < gradient[n].first.data.size(); ++item) {
						gradient[n].first.data[item] /= batch_size_;
					}
					//biases
					for (uword item = 0; item < gradient[n].second.data.size(); ++item) {
						gradient[n].second.data[item] /= batch_size_;
					}
				}
				error /= batch_size_;
				std::cout << "training error = " << error << "\n";
				std::cout << "update weights...\n";
				for (std::size_t n = 0; n < gradient.size(); ++n) {
					cnn::tensor4d& weigths = net_->Weights(n);
					cnn::tensor4d& bias_weigths = net_->BiasWeights(n);
					for (uword item = 0; item < weigths.data.size(); ++item) {
						for (uword slice = 0; slice < weigths.n_slices; ++slice) {
							for (uword col = 0; col < weigths.n_cols; ++col) {
								for (uword row = 0; row < weigths.n_rows; ++row) {
									weigths.data[item](row, col, slice) += -learning_rate_
										* gradient[n].first.data[item](row, col, slice);
								}
							}
						}
					}
					//biases
					for (uword item = 0; item < bias_weigths.data.size(); ++item) {
						for (uword slice = 0; slice < bias_weigths.n_slices; ++slice) {
							for (uword col = 0; col < bias_weigths.n_cols; ++col) {
								for (uword row = 0; row < bias_weigths.n_rows; ++row) {
									bias_weigths.data[item](row, col, slice) += -learning_rate_
										* gradient[n].second.data[item](row, col, slice);
								}
							}
						}
					}
				}

				if (snapshot_interval_ != 0 && (epoch + 1) % snapshot_interval_ == 0) {
					boost::filesystem::ofstream out;
					std::wstring path = snapshot_prefix_ + (boost::wformat(L"_%1%.dat")
															% (epoch + 1)).str();
					out.open(path, std::ios::binary);
					if (!out.is_open()) {
						std::cout << "cannot save weights to file\n";
					} else {
						net_->SaveWeights(out);
						out.close();
					}
				}
			}
		}

		void SdlmSolver::Solve()
		{
			using namespace arma;
#ifndef NDEBUG
			assert(net_->is_initialized());
#endif
			std::vector<std::pair<cnn::tensor4d, cnn::tensor4d>> gradient;
			std::vector<std::pair<cnn::tensor4d, cnn::tensor4d>> iter_gradient;
			std::vector<std::pair<cnn::tensor4d, cnn::tensor4d>> current_hessian;
			std::vector<std::pair<cnn::tensor4d, cnn::tensor4d>> new_hessian;
			std::vector<std::pair<cnn::tensor4d, cnn::tensor4d>> old_hessian;
			std::vector<std::pair<cnn::tensor4d, cnn::tensor4d>> iter_hessian;
			for (uword epoch = 0; epoch < max_epoch_; ++epoch) {
				double error = 0.0;
				if (test_interval_ != 0 && (epoch + 1) % test_interval_ == 0) {
					std::cout << boost::format(
						"compute error on test dataset for %1% samples on %2% training epoches..."
					) % test_size_ % (epoch + 1) << "\n";

					// compute error on test data-set
					for (uword i = 0; i < test_size_; ++i) {
						net_->LoadTestImage();
						net_->Forward();
						error += net_->Error();
					}
					error /= test_size_;
					std::cout << "test error = " << error << "\n";
				}
				error = 0.0;
				net_->LoadTrainImage();
				net_->Forward();
				error += net_->Error();
				gradient = net_->Backpropagation();
				current_hessian = net_->Backpropagation_2nd();
				// training network on training dataset
				std::cout << boost::format(
					"compute error on training dataset for %1% samples on %2% training epoches..."
				) % batch_size_ % (epoch + 1) << "\n";
				for (uword i = 1; i < batch_size_; ++i) {
					net_->LoadTrainImage();
					net_->Forward();
					error += net_->Error();
					iter_gradient = net_->Backpropagation();
					iter_hessian = net_->Backpropagation_2nd();

					for (std::size_t n = 0; n < gradient.size(); ++n) {
						// weights
						for (uword item = 0; item < gradient[n].first.data.size(); ++item) {
							gradient[n].first.data[item] += iter_gradient[n].first.data[item];
							current_hessian[n].first.data[item] += iter_hessian[n].first.data[item];
						}
						//biases
						for (uword item = 0; item < gradient[n].second.data.size(); ++item) {
							gradient[n].second.data[item] += iter_gradient[n].second.data[item];
							current_hessian[n].second.data[item] += iter_hessian[n].second.data[item];
						}
					}
				}
				// get average deltas
				for (std::size_t n = 0; n < gradient.size(); ++n) {
					// weights
					for (uword item = 0; item < gradient[n].first.data.size(); ++item) {
						gradient[n].first.data[item] /= batch_size_;
						current_hessian[n].first.data[item] /= batch_size_;
					}
					//biases
					for (uword item = 0; item < gradient[n].second.data.size(); ++item) {
						gradient[n].second.data[item] /= batch_size_;
						current_hessian[n].second.data[item] /= batch_size_;
					}
				}
				error /= batch_size_;
				std::cout << "training error = " << error << "\n";
				std::cout << "update weights...\n";
				new_hessian = std::move(current_hessian);
				if (!old_hessian.empty()) {
					// new_hessian = (1 - gamma) % old_hessian + gamma % new_hessian
					for (std::size_t n = 0; n < new_hessian.size(); ++n) {
						// weights
						for (uword item = 0; item < new_hessian[n].first.data.size(); ++item) {
							new_hessian[n].first.data[item]
									= (1 - gamma_) * old_hessian[n].first.data[item]
									+ gamma_ * new_hessian[n].first.data[item];
						}
						//biases
						for (uword item = 0; item < new_hessian[n].second.data.size(); ++item) {
							new_hessian[n].second.data[item]
									= (1 - gamma_) * old_hessian[n].second.data[item]
									+ gamma_ * new_hessian[n].second.data[item];
						}
					}
				}
				// found optimal learning rate for all weights and update in-place
				for (std::size_t n = 0; n < new_hessian.size(); ++n) {
					cnn::tensor4d& weigths = net_->Weights(n);
					cnn::tensor4d& bias_weigths = net_->BiasWeights(n);
					double local_learning_rate;
					for (uword item = 0; item < weigths.data.size(); ++item) {
						for (uword slice = 0; slice < weigths.n_slices; ++slice) {
							for (uword col = 0; col < weigths.n_cols; ++col) {
								for (uword row = 0; row < weigths.n_rows; ++row) {
									local_learning_rate = learning_rate_
											/ (new_hessian[n].first.data[item](row, col, slice) + mu_);
									weigths.data[item](row, col, slice) += -local_learning_rate
											* gradient[n].first.data[item](row, col, slice);
								}
							}
						}
					}
					//biases
					for (uword item = 0; item < bias_weigths.data.size(); ++item) {
						for (uword slice = 0; slice < bias_weigths.n_slices; ++slice) {
							for (uword col = 0; col < bias_weigths.n_cols; ++col) {
								for (uword row = 0; row < bias_weigths.n_rows; ++row) {
									local_learning_rate = learning_rate_
											/ (new_hessian[n].second.data[item](row, col, slice) + mu_);
									bias_weigths.data[item](row, col, slice) += -local_learning_rate
											* gradient[n].second.data[item](row, col, slice);
								}
							}
						}
					}

				}
				old_hessian = std::move(new_hessian);

				if (snapshot_interval_ != 0 && (epoch + 1) % snapshot_interval_ == 0) {
					boost::filesystem::ofstream out;
					std::wstring path = snapshot_prefix_ + (boost::wformat(L"_%1%.dat")
															% (epoch + 1)).str();
					out.open(path, std::ios::binary);
					if (!out.is_open()) {
						std::cout << "cannot save weights to file\n";
					} else {
						net_->SaveWeights(out);
						out.close();
					}
				}
			}
		}
	}
}


#include "CALPHADFreeEnergyFunctionsTernary.h"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

namespace pt = boost::property_tree;

int main(int argc, char* argv[])
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::LINEAR;

    double nominalc[2] = { 0.6, 0.035 };

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("calphadFeNbNi_Mathon_et_al.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsTernary cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    const int nTemperatures = 50;
    const double lowT       = 1560.;
    const double highT      = 1680.;
    const double dT         = (highT - lowT) / (double)nTemperatures;

    // initial guesses
    double init_guess[5] = { 0.5, 0.05, 0.6, 0.01, 0.5 };
    std::cout << "Tie line" << std::endl;

    for (int i = 0; i < nTemperatures + 1; i++)
    {
        double temperature = lowT + i * dT;

        double lceq[5] = { init_guess[0], init_guess[1], // liquid
            init_guess[2], init_guess[3], // solid
            init_guess[4] };

        bool found_ceq = cafe.computeTieLine(
            temperature, nominalc[0], nominalc[1], &lceq[0]);
        if (found_ceq)
        {
            std::cout << temperature << ", " << lceq[0] << ", " << lceq[1]
                      << ", " << lceq[2] << ", " << lceq[3] << ", " << lceq[4]
                      << std::endl;
        }
    }
}


/*
    This file is part of Nori, a simple educational ray tracer

    Copyright (c) 2012 by Wenzel Jakob and Steve Marschner.

    Nori is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Nori is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <nori/mesh.h>
#include <nori/bbox.h>
#include <nori/bsdf.h>
#include <nori/luminaire.h>
#include <nori/transform.h>
#include <Eigen/Geometry>

#define NORI_TRICLIP_MAXVERTS 10

NORI_NAMESPACE_BEGIN

Mesh::Mesh(const PropertyList& propList) : m_vertexPositions(0), m_vertexNormals(0),
  m_vertexTexCoords(0), m_indices(0), m_vertexCount(0),
  m_triangleCount(0), m_bsdf(NULL), m_luminaire(NULL), m_originalTransform(propList.getTransform("toWorld", Transform())) { }

Mesh::~Mesh() {
	delete[] m_vertexPositions;
	if (m_vertexNormals)
		delete[] m_vertexNormals;
	if (m_vertexTexCoords)
		delete[] m_vertexTexCoords;
	delete[] m_indices;

	if (m_bsdf)
		delete m_bsdf;
	if (m_luminaire)
		delete m_luminaire;
}

void Mesh::activate() {
	/* Create a discrete distribution for sampling triangles
	   with respect to their surface area */
	m_distr.clear();
	m_distr.reserve(m_triangleCount);
	for (uint32_t i=0; i<m_triangleCount; ++i)
		m_distr.append(surfaceArea(i));
	m_distr.normalize();

	if (!m_bsdf) {
		/* If no material was assigned, instantiate a diffuse BRDF */
		m_bsdf = static_cast<BSDF *>(
			NoriObjectFactory::createInstance("diffuse", PropertyList()));
	}
}

void Mesh::samplePosition(const Point2f &_sample, Point3f &p, Normal3f &n) const {
	Point2f sample(_sample);

	/* First, sample a triangle with respect to surface area */
	size_t index = m_distr.sampleReuse(sample.x());

	/* Lookup vertex positions for the chosen triangle */
	int i0 = m_indices[3*index],
		i1 = m_indices[3*index+1],
		i2 = m_indices[3*index+2];

	const Point3f
		&p0 = m_vertexPositions[i0],
		&p1 = m_vertexPositions[i1],
		&p2 = m_vertexPositions[i2];

	/* Sample a position in barycentric coordinates */
	Point2f b = squareToUniformTriangle(sample);

	p = p0 * (1.0f - b.x() - b.y()) + p1 * b.x() + p2 * b.y();

	/* Also provide a normal (interpolated if vertex normals are provided) */
	if (m_vertexNormals) {
		const Normal3f
			&n0 = m_vertexNormals[i0],
			&n1 = m_vertexNormals[i1],
			&n2 = m_vertexNormals[i2];
		n = (n0 * (1.0f - b.x() - b.y()) + n1 * b.x() + n2 * b.y()).normalized();
	} else {
		n = (p1-p0).cross(p2-p0).normalized();
	}
}

float Mesh::surfaceArea(uint32_t index) const {
	int i0 = m_indices[3*index],
		i1 = m_indices[3*index+1],
		i2 = m_indices[3*index+2];

	const Point3f
		&p0 = m_vertexPositions[i0],
		&p1 = m_vertexPositions[i1],
		&p2 = m_vertexPositions[i2];

	return 0.5f * Vector3f((p1-p0).cross(p2-p0)).norm();
}

bool Mesh::rayIntersect(uint32_t index, const Ray3f &ray, float &u, float &v, float &t) const {
	int i0 = m_indices[3*index],
		i1 = m_indices[3*index+1],
		i2 = m_indices[3*index+2];

	const Point3f
		&p0 = m_vertexPositions[i0],
		&p1 = m_vertexPositions[i1],
		&p2 = m_vertexPositions[i2];

	/* find vectors for two edges sharing v[0] */
	Vector3f edge1 = p1 - p0, edge2 = p2 - p0;

	/* begin calculating determinant - also used to calculate U parameter */
	Vector3f pvec = ray.d.cross(edge2);

	/* if determinant is near zero, ray lies in plane of triangle */
	float det = edge1.dot(pvec);

	if (det > -1e-8f && det < 1e-8f)
		return false;
	float inv_det = 1.0f / det;

	/* calculate distance from v[0] to ray origin */
	Vector3f tvec = ray.o - p0;

	/* calculate U parameter and test bounds */
	u = tvec.dot(pvec) * inv_det;
	if (u < 0.0 || u > 1.0)
		return false;

	/* prepare to test V parameter */
	Vector3f qvec = tvec.cross(edge1);

	/* calculate V parameter and test bounds */
	v = ray.d.dot(qvec) * inv_det;
	if (v < 0.0 || u + v > 1.0)
		return false;

	/* ray intersects triangle -> compute t */
	t = edge2.dot(qvec) * inv_det;

	return true;
}

BoundingBox3f Mesh::getBoundingBox(uint32_t index) const {
	BoundingBox3f result(m_vertexPositions[m_indices[3*index]]);
	result.expandBy(m_vertexPositions[m_indices[3*index+1]]);
	result.expandBy(m_vertexPositions[m_indices[3*index+2]]);
	return result;
}

/// Internally used by getClippedBoundingBox()
static int sutherlandHodgman(Point3d *input, int inCount, Point3d *output, int axis,
		double splitPos, bool isMinimum) {
	if (inCount < 3)
		return 0;

	Point3d cur       = input[0];
	double sign       = isMinimum ? 1.0f : -1.0f;
	double distance   = sign * (cur[axis] - splitPos);
	bool  curIsInside = (distance >= 0);
	int   outCount    = 0;

	for (int i=0; i<inCount; ++i) {
		int nextIdx = i+1;
		if (nextIdx == inCount)
			nextIdx = 0;
		const Point3d &next = input[nextIdx];
		distance = sign * (next[axis] - splitPos);
		bool nextIsInside = (distance >= 0);

		if (curIsInside && nextIsInside) {
			/* Both this and the next vertex are inside, add to the list */
			assert(outCount + 1 < NORI_TRICLIP_MAXVERTS);
			output[outCount++] = next;
		} else if (curIsInside && !nextIsInside) {
			/* Going outside -- add the intersection */
			double t = (splitPos - cur[axis]) / (next[axis] - cur[axis]);
			assert(outCount + 1 < NORI_TRICLIP_MAXVERTS);
			Point3d p = cur + (next - cur) * t;
			p[axis] = splitPos; // Avoid roundoff errors
			output[outCount++] = p;
		} else if (!curIsInside && nextIsInside) {
			/* Coming back inside -- add the intersection + next vertex */
			double t = (splitPos - cur[axis]) / (next[axis] - cur[axis]);
			assert(outCount + 2 < NORI_TRICLIP_MAXVERTS);
			Point3d p = cur + (next - cur) * t;
			p[axis] = splitPos; // Avoid roundoff errors
			output[outCount++] = p;
			output[outCount++] = next;
		} else {
			/* Entirely outside - do not add anything */
		}
		cur = next;
		curIsInside = nextIsInside;
	}
	return outCount;
}

BoundingBox3f Mesh::getClippedBoundingBox(uint32_t index, const BoundingBox3f &bbox) const {
	/* Reserve room for some additional vertices */
	Point3d vertices1[NORI_TRICLIP_MAXVERTS], vertices2[NORI_TRICLIP_MAXVERTS];
	int nVertices = 3;

	/* The kd-tree code will frequently call this function with
	   almost-collapsed bounding boxes. It's extremely important not to introduce
	   errors in such cases, otherwise the resulting tree will incorrectly
	   remove triangles from the associated nodes. Hence, do the
	   following computation in double precision! */
	for (int i=0; i<3; ++i)
		vertices1[i] = m_vertexPositions[m_indices[3*index+i]].cast<double>();

	for (int axis=0; axis<3; ++axis) {
		nVertices = sutherlandHodgman(vertices1, nVertices, vertices2, axis, bbox.min[axis], true);
		nVertices = sutherlandHodgman(vertices2, nVertices, vertices1, axis, bbox.max[axis], false);
	}

	BoundingBox3f result;
	for (int i=0; i<nVertices; ++i)
		result.expandBy(vertices1[i].cast<float>());
	result.clip(bbox);
	return result;
}

Color3f Mesh::getMainColor() const {
        return m_luminaire != NULL ? m_luminaire->getColor() :
            m_bsdf != NULL ? m_bsdf->getColor() : Color3f(1.0f);
}

void Mesh::addChild(NoriObject *obj) {
	switch (obj->getClassType()) {
		case EBSDF:
			if (m_bsdf)
				throw NoriException("Mesh: tried to register multiple BSDF instances!");
			m_bsdf = static_cast<BSDF *>(obj);
			break;

		case ELuminaire: {
				Luminaire *luminaire = static_cast<Luminaire *>(obj);
				if (m_luminaire)
					throw NoriException("Mesh: tried to register multiple Luminaire instances!");
				if (luminaire->isEnvironmentLuminaire())
					throw NoriException("Mesh: Tried to attach an environment luminaire!");
				m_luminaire = luminaire;
			}
			break;

		default:
			throw NoriException(QString("Mesh::addChild(<%1>) is not supported!").arg(
				classTypeName(obj->getClassType())));
	}
}

QString Mesh::toString() const {
	return QString(
		"Mesh[\n"
		"  name = \"%1\",\n"
		"  vertexCount = %2,\n"
		"  triangleCount = %3,\n"
		"  bsdf = %4,\n"
		"  luminaire = %5\n"
		"]")
	.arg(m_name)
	.arg(m_vertexCount)
	.arg(m_triangleCount)
	.arg(indent(m_bsdf->toString()))
	.arg(indent(m_luminaire ? m_luminaire->toString() : QString("null")));
}

QString Intersection::toString() const {
	if (!mesh)
		return "Intersection[invalid]";

	return QString(
		"Intersection[\n"
		"  p = %1,\n"
		"  t = %2,\n"
		"  uv = %3,\n"
		"  shFrame = %4,\n"
		"  geoFrame = %5,\n"
		"  mesh = %6\n"
		"]")
	.arg(p.toString())
	.arg(t)
	.arg(uv.toString())
	.arg(indent(shFrame.toString()))
	.arg(indent(geoFrame.toString()))
	.arg(indent(mesh->toString()));
}

NORI_NAMESPACE_END


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

#include <boost/simd/bitwise/functions/firstbitset.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/complement.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/digits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( firstbitset_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0,unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename  dispatch::meta::as_integer<A0,unsigned>::type int_type;
      return b_and((b_not(simd::bitwise_cast<int_type>(a0))+One<int_type>()), a0);
    }
  };
} } }

#endif


#ifndef INCLUDED_PDS_TOOLS_HPP
#define INCLUDED_PDS_TOOLS_HPP

#include <boost/random.hpp>

namespace pds
{

	namespace impl
	{
		inline boost::mt19937& Rnd()
		{
			static boost::mt19937 rnd;
			return rnd;
		}

		inline void RndSeed(unsigned int x)
		{
			Rnd().seed(x);
		}

		/** Selects a random point in the tree node using uniform distribution */
		inline Eigen::Vector2f RandomCellPoint(int scale, int x, int y, float gamma)
		{
			float sf = static_cast<float>(scale);
			float xf = static_cast<float>(x);
			float yf = static_cast<float>(y);
			boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > delta(
					Rnd(), boost::uniform_real<float>(0.5f-gamma, 0.5f+gamma));
			return Eigen::Vector2f(sf*(xf + delta()), sf*(yf + delta()));
		}

		/** Selects the point in the tree node with highest probability */
		inline Eigen::Vector2f OptimalCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y)
		{
			x *= scale;
			y *= scale;
			const auto& b = m0.block(x, y, scale, scale);
			unsigned int best_j=scale/2, best_i=scale/2;
			float best_val = -1000000.0f;
			for(unsigned int i=0; i<b.cols(); ++i) {
				for(unsigned int j=0; j<b.rows(); ++j) {
					float val = b(j,i);
					if(val > best_val) {
						best_j = j;
						best_i = i;
						best_val = val;
					}
				}
			}
			return Eigen::Vector2f(x + best_j, y + best_i);
		}

		/** Randomly selects a point in the given tree node by considering probabilities
		 * Runtime: O(S*S + log(S*S))
		 */
		inline Eigen::Vector2f ProbabilityCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y)
		{
			x *= scale;
			y *= scale;
			const auto& b = m0.block(x, y, scale, scale);
			// build cdf
			std::vector<float> cdf(scale*scale);
			for(int i=0; i<scale; ++i) {
				for(int j=0; j<scale; ++j) {
					float v = b(j,i);
					int q = scale*i + j;
					if(q > 0) {
						cdf[q] = cdf[q-1] + v;
					}
					else {
						cdf[q] = v;
					}
				}
			}
			// sample in cdf
			boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rnd(
					Rnd(), boost::uniform_real<float>(0.0f, cdf.back()));
			float v = rnd();
			// find sample
			auto it = std::lower_bound(cdf.begin(), cdf.end(), v);
			int pos = std::distance(cdf.begin(), it);
			return Eigen::Vector2f(x + pos%scale, y + pos/scale);
		}

		/** Randomly selects a point in the given tree node by considering probabilities
		 * Assumes that cdf_sum is the probability sum in the given tree node
		 * Runtime: O(S*S/2)
		 */
		inline Eigen::Vector2f ProbabilityCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y, float cdf_sum)
		{
			// sample in cdf
			boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rnd(
					Rnd(), boost::uniform_real<float>(0.0f, cdf_sum));
			float v = rnd();
			// find sample
			x *= scale;
			y *= scale;
			const auto& b = m0.block(x, y, scale, scale);
			for(int i=0; i<scale; ++i) {
				for(int j=0; j<scale; ++j) {
					v -= b(j,i);
					if(v <= 0.0f) {
						return Eigen::Vector2f(x + j, y + i);
					}
				}
			}
			// should never be here
			assert(false);
			return Eigen::Vector2f(x + scale/2, y + scale/2);
		}

		inline void ScalePoints(std::vector<Eigen::Vector2f>& pnts, float scale)
		{
			for(Eigen::Vector2f& u : pnts) {
				u *= scale;
			}
		}

		/** Randomly rounds a float up or down s.t. the expected value is the given value */
		inline unsigned int RandomRound(float x)
		{
			if(x <= 0.0f) {
				return 0;
			}
			float a = std::floor(x);
			float r = x - a;
			boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > uniform01(
					Rnd(), boost::uniform_real<float>(0.0f,1.0f));
			return a + (uniform01() >= r ? 0.0f : 1.0f);
		}

		inline std::vector<unsigned int> RandomSample(const std::vector<float>& v, unsigned int num)
		{
			std::vector<float> a(v.size());
			std::partial_sum(v.begin(), v.end(), a.begin());
//			std::copy(a.begin(), a.end(), std::ostream_iterator<float>(std::cout, ", "));
			float ws = a.back();
			boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rndv(
					Rnd(), boost::uniform_real<float>(0.0f, ws));
			std::vector<unsigned int> idx(num);
			std::generate(idx.begin(), idx.end(),
				[&rndv,&a]() -> unsigned int {
					float x = rndv();
					auto it = std::lower_bound(a.begin(), a.end(), x);
					if(it == a.end()) {
						return a.size() - 1;
					}
					else {
						return it - a.begin();
					}
				});
			return idx;
		}

	}

}

#endif


//
// Created by Xinyu Zhang on 3/26/21.
//
// gcc -I/Users/xinyuzhang/Desktop/Spring2021/c++pattern/project/cosan /Users/xinyuzhang/Desktop/Spring2021/c++pattern/project/cosan/test/preprocessor/PCATest.cpp

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <cosan/data/CosanData.h>
#include <gsl/gsl>
// #include <cosan/preprocessing/overunderflow.h>
// #include <cosan/preprocessing/missingvalues.h>
// #include <cosan/preprocessing/minmaxscaler.h>
// #include <cosan/preprocessing/principalcomponentanalysis.h>
// #include <cosan/preprocessing/polynomialfeatures.h>
#include <vector>
#ifndef FMT_HEADER_ONLY
#define FMT_HEADER_ONLY
#endif
#include <fmt/format.h>
#include <numeric>
//using namespace Eigen;
//using namespace std;

void transform( CosanMatrix<double> func ( CosanMatrix<double>) ) {
	return CosanMatrix<double>;
}

int main() {

	// std::cout<<	value1<<value2<<std::endl;
	// std::cout<<X.col(i).array().isNaN().sum()<<std::endl;
	Cosan::CosanMatrix X{5,3};
	X << 1,2,2,
	     4,3,4,
	     2,4,6,
	     12,51,6,
	     435,4,15;
	// (X.array().colwise()-X.colwise().minCoeff())/X.colwise().maxCoeff()-X.colwise().minCoeff();
    std::cout<<X<<std::endl;

    X.conservativeResize(X.rows(),X.cols()+1);
    X.col(X.cols()-1) = X.col(0).array().pow(1.2);

    std::cout<<X<<std::endl;

	// Cosan::CosanRawData CRD("./example_data/toy2/X_.csv","./example_data/toy2/Y_.csv");
	// std::cout<<CRD.GetSummaryMessageX()<<CRD.GetSummaryMessageY()<<std::endl;
	// Cosan::OverUnderFlow ouf(CRD);
	// Cosan::MissingValues MissV(CRD);
	// Cosan::MinmaxScaler MMS(CRD,3,5);
	// auto [value1, value2] = CRD.GetMissingNumber();
	// std::cout<<	value1<<value2<<std::endl;
	// Cosan::PrincipalComponentAnalysis PCA(CRD,4);

	// std::cout<<M.array().isNaN().sum()<<std::endl;
	// CRD.GetTarget().rows()-CRD.GetTarget().array().isNaN().sum()
	// CRD.GetTarget().array().isNaN().select(0,CRD.GetTarget())

	// std::cout<<CRD.GetTarget().array().isNaN().select(0,CRD.GetTarget()).sum()<<std::endl;

	// std::cout<<(1-CRD.GetTarget().array().isNaN()).cast<double>().array()*CRD.GetTarget().array()<<std::endl;

    return 0;
}


#include <iostream>
#include <string>
#include <armadillo>

using namespace std;
using namespace arma;

mat loadTime(int lenght_)
{
  ifstream myFile;

  // fix size
  vec t(lenght_);

  // open again to get data
  myFile.open("data/DataTimed.txt");
  double num = 0.0;
  int l = 0;

  while (myFile >> num)
  {
    t(l) = num;
    l++;
  }

  myFile.close();
  return t;
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ONEMINUS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ONEMINUS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing oneminus capabilities

    Returns the one minus the entry.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = oneminus(x);
    @endcode

    For signed type is similar to:

    @code
    T r = 1-x
    @endcode

    @par Decorator

    saturated_ decorator is available,  but for unsigned integral types the saturated_
    function is equivalent to `x == 0 ? 1 : 0`

  **/
  Value oneminus(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/oneminus.hpp>
#include <boost/simd/function/simd/oneminus.hpp>

#endif