#include <time.h>
#include <fstream>
#include <string>
#include <vector>
#include "Bspline.h"
#include "mosek.h"

// import std namespace
using namespace std;
// import most common Eigen types
USING_PART_OF_NAMESPACE_EIGEN;

#define ERROR 0;

int sign(double x);
void writeMatrix(MatrixXd * M, FILE * pFile);
void writeSparseMatrix(MatrixXd * M, FILE * pFile, int NUMANZ);
void buildC(VectorXd* t,int k, int degree, vector<MatrixXd>* out );
MatrixXd buildB(vector<Bspline> basis, VectorXd X);
const int getNumanz(MatrixXd A);
void sparseARowWise(MatrixXd* A, vector<MSKlidxt>* aptrb,
	vector<MSKlidxt>* aptre, MSKidxt asub[], double aval[]);

int main(int argc,char *argv[])
{
	// nodige variabelen instellen
	const int t0 = 0;			// vast beginpunt van het gebruikte tijdsinterval
	const int t1 = 10;			// eindpunt van gebruikte tijdsinterval
	const int g = 10;			// aantal knopen
	const int k = 3;			// graad van spline

	const int sizex = 2*g+k+1;	// lengte van variabelenvector
	const int lt = g+2;			// lengte van t-vector

	double scal = (double) t1-t0; // scaleringsfactor, nodig bij het verwerken van de preprocessing

	/*
	Knooppuntenrij initiäliseren
	----------------------------
	*/

	double dt = (double) (t1-t0)/(g+1);
	VectorXd t(lt);
	for(int i = 0 ; i <= g+1; i++)
	{
		t(i) = t0+dt*i;
	}
	VectorXd front_boundary = VectorXd::Constant(k,t(0));
	VectorXd back_boundary = VectorXd::Constant(k,t(lt-1));

	VectorXd ext_t(lt+2*k);
	ext_t.start(k)		= front_boundary;
	ext_t.end(k)		= back_boundary;
	ext_t.segment(k,lt)	= t;

	// cout<<"ext_t: \n"<<ext_t<<endl; // tijdsvector uitprinten

	/*
	C-matrices
	(C : zet x = [c v]  om in [ck v])
	(C1: zet x = [c v]  om in [c1  ])
	(C2: zet x = [c v]  om in [c2  ])
	--------------------------------------
	*/
	vector<MatrixXd> Cs;
	buildC(&ext_t,k,k,&Cs);

	MatrixXd C_part = Cs.at(Cs.size()-1);
	MatrixXd C = MatrixXd::Zero(sizex-k,sizex);
	C.corner(Eigen::TopLeft,g+1,g+k+1) = C_part;
	C.corner(Eigen::BottomRight,g,g) = MatrixXd::Identity(g,g);

	// cout<<"C: \n"<<C<<endl; // C-matrix uitprinten

	MatrixXd C1_part = Cs.at(0);
	MatrixXd C1 = MatrixXd::Zero(g+k,sizex);
	C1.corner(Eigen::TopLeft,g+k,g+k+1) = C1_part;

	// cout<<"C1: \n"<<C1<<endl; // C1-matrix uitprinten

	MatrixXd C2_part = Cs.at(1);
	MatrixXd C2 = MatrixXd::Zero(g+k-1,sizex);
	C2.corner(Eigen::TopLeft,g+k-1,g+k+1) = C2_part;

	// cout<<"C2: \n"<<C2<<endl; // C2-matrix uitprinten

	/*
	Opstellen van de splinebasis d0B, d1B en d2B (s(x) = d0B*c)
	-----------------------------------------------------------
	*/

	vector<Bspline> d0basis;
	for(int i=0; i<ext_t.size()-k-1;i++)
	{
		d0basis.push_back(Bspline(ext_t.segment(i,k+2)));
	}
	MatrixXd d0B1 = buildB(d0basis,t);

	vector<Bspline> d1basis;
	for(int i=1; i<ext_t.size()-k-1;i++)
	{
		d1basis.push_back(Bspline(ext_t.segment(i,k+1)));
	}
	MatrixXd d1B = buildB(d1basis,t);

	vector<Bspline> d2basis;
	for(int i=2; i<ext_t.size()-k-1;i++)
	{
		d2basis.push_back(Bspline(ext_t.segment(i,k)));
	}
	MatrixXd d2B = buildB(d2basis,t);

	MatrixXd d0B  = MatrixXd::Zero(lt,sizex);
	d0B.corner(Eigen::TopLeft,lt,g+k+1) = d0B1;
	MatrixXd vel = (d1B*C1)*3;
	MatrixXd acc = (d2B*C2)*6;

	// cout<<"d0B: \n"<<d0B<<endl; // B-matrix uitprinten
	// cout<<"d1B: \n"<<d1B<<endl; // d1B-matrix uitprinten
	// cout<<"d2B: \n"<<d2B<<endl; // d1B-matrix uitprinten
	// cout<<"vel: \n"<<vel<<endl; // velocity-matrix uitprinten
	// cout<<"acc: \n"<<acc<<endl; // acceleration-matrix uitprinten

	/*
	Beginvoorwaarden in matrixvorm instellen
	----------------------------------------
	*/

	MatrixXd A1 = MatrixXd::Zero(6,sizex);
	// begin- en eindpunt
	A1(0,0) = 1;
	A1(1,g+k) = 1;
	// begin- en eindsnelheid
	A1.row(2) = C1.row(0);
	A1.row(3) = C1.row(g+k-1);
	// begin- en eindversnelling
	A1.row(4) = C2.row(0);
	A1.row(5) = C2.row(g+k-2);

	// cout<<"A1: \n"<<A1<<endl; // A-matrix uitprinten

	/*
	Voorwaarde norm(Ck(2:end)-Ck(1:end-1),1)<v
	------------------------------------------
	*/

	MatrixXd onevw = MatrixXd::Zero(2*g,sizex-k);

	onevw.corner(Eigen::TopLeft,g,g) = onevw.corner(Eigen::TopLeft,g,g)-MatrixXd::Identity(g,g);
	onevw.block(0,1,g,g) = onevw.block(0,1,g,g)+MatrixXd::Identity(g,g);
	onevw.block(0,g+1,g,g) = -MatrixXd::Identity(g,g);

	onevw.corner(Eigen::BottomLeft,g,g) = onevw.corner(Eigen::BottomLeft,g,g)-MatrixXd::Identity(g,g);
	onevw.block(g,1,g,g) = onevw.block(g,1,g,g)+MatrixXd::Identity(g,g);
	onevw.block(g,g+1,g,g) = MatrixXd::Identity(g,g);

	MatrixXd A2 = onevw*C;

	// cout<<"onevw: \n"<<onevw<<endl;
	// cout<<"A2: \n"	<<onevw<<endl;

	/*
	snelheidsbeperkingen in vectorvorm
	----------------------------------
	*/

	MatrixXd A4 = C1*3;

	/*
	Data voor mosek
	---------------
	*/
	const int NUMCON1 = A1.rows();
	const int NUMCON2 = A2.rows();
	const int NUMCON3 =vel.rows();
	const int NUMCON4 = A4.rows();
	const int NUMCON = NUMCON1 + NUMCON2 + NUMCON3 + NUMCON4;

	const int NUMANZ1 = getNumanz(A1);
	const int NUMANZ2 = getNumanz(A2);
	const int NUMANZ3 = getNumanz(5*vel+acc);
	const int NUMANZ4 = getNumanz(A4);
	const int NUMANZ = NUMANZ1 + NUMANZ2 + NUMANZ3 + NUMANZ4;

	/*
	wegschrijven van alle berekende data
	-------------------------------------
	*/

	FILE * pFile;
	char loc[] = "..\\..\\Main_no_prepro\\Main_no_prepro\\testFile.bin";
	if((pFile= fopen(loc,"wb"))==NULL)
	{
		printf("Error occurred while opening \"%s\" for reading.\n", loc);
		return ERROR;
	}

	fwrite (&scal	, sizeof(double)	, 1 , pFile );
	fwrite (&g		, sizeof(int)		, 1 , pFile );
	fwrite (&k		, sizeof(int)		, 1 , pFile );
	fwrite (&NUMCON	, sizeof(int)		, 1 , pFile );
	fwrite (&NUMANZ	, sizeof(int)		, 1 , pFile );
	writeSparseMatrix(&A1,	pFile, NUMANZ1);
	writeMatrix(&A2,	pFile);
	writeMatrix(&vel,	pFile);
	writeMatrix(&acc,	pFile);
	writeSparseMatrix(&A4,	pFile, NUMANZ4);

	fclose (pFile);

	cout<<"done"<<endl;
	string stop;
	cin>>stop;

	return 0;
}
// geef het teken terug van x:
// return =	 1 if x >= 0
// return = -1 if x <  0
int sign(double x)
{
	if(x>=0)
	{return 1;}
	else
	{return -1;}
}

// Schrijf een matrix weg naar het gespecifieerd bestand
void writeMatrix(MatrixXd * M, FILE * pFile)
{
	int cols = M->cols();
	fwrite (&cols , sizeof(int) , 1 , pFile );
	int rows = M->rows();
	fwrite (&rows , sizeof(int) , 1 , pFile );
	double data;
	for(int i=0; i<rows; i++)
	{
		for(int j=0; j<cols; j++)
		{
			data = M->coeff(i,j);
			fwrite (&data , sizeof(double) , 1 , pFile );
		}
	}
}

// Schrijf een matrix weg naar het gespecifieerd bestand in spaarse voorstelling
void writeSparseMatrix(MatrixXd * M, FILE * pFile, int NUMANZ)
{
	// Matrix naar spaarse vorm omzetten
	MSKidxt	(*asub) = new	MSKidxt[NUMANZ];
	double	(*aval) = new	double[NUMANZ];
	vector<MSKlidxt> aptrb, aptre;
	sparseARowWise(M, &aptrb, &aptre, asub, aval);

	// Wegschrijven van de eerste 2 arrays
	MSKidxt data1;
	double data2;
	fwrite (&NUMANZ	, sizeof(int)	, 1 , pFile );
	for(int i=0; i<NUMANZ; i++)
	{
		data1 = asub[i];
		fwrite (&data1	, sizeof(MSKidxt)	, 1 , pFile );
	}

	fwrite (&NUMANZ	, sizeof(int)	, 1 , pFile );
	for(int i=0; i<NUMANZ; i++)
	{
		data2 = aval[i];
		fwrite (&data2	, sizeof(double)	, 1 , pFile );
	}

	// Wegschrijven van de 2 vectoren
	MSKlidxt data;
	int length = aptrb.size();
	fwrite (&length	, sizeof(int)	, 1 , pFile );
	for(int i=0; i<length; i++)
	{
		data = aptrb.at(i);
		fwrite (&data , sizeof(MSKlidxt)	, 1 , pFile );
	}

	length = aptre.size();
	fwrite (&length	, sizeof(int)	, 1 , pFile );
	for(int i=0; i<length; i++)
	{
		data = aptre.at(i);
		fwrite (&data , sizeof(MSKlidxt)	, 1 , pFile );
	}
}

// Bouw de verschillende transformatiematrices C
void buildC(VectorXd* t,int k, int degree, vector<MatrixXd>* out )
{
	int g = t->size()-2*(k+1);
	MatrixXd C = MatrixXd::Identity(g+k+1,g+k+1);

	MatrixXd Ci;
	VectorXd dt;

	for(int i=1;i<=degree;++i)
	{
		dt = t->segment(1+k,g+k+1-i)- t->segment(i,g+k+1-i); // lengte = g+k+1-i

		Ci = MatrixXd::Zero(g+k+1-i, g+k+2-i);
		for(int j = 0; j<g+k+1-i;++j)
		{
			Ci(j,j) = -1/dt(j);
			Ci(j,j+1) = 1/dt(j);
			// Ci(j,j) = -1/(t->coeff(1+k+j)-t->coeff(i+j));
			// Ci(j,j+1) = 1/(t->coeff(1+k+j)-t->coeff(i+j));
		}
		C = Ci*C;
		out->push_back(C);
	}
}

// Bouw de B-matrix op met de waarde van de splinebasis op de verschillende punten in X
MatrixXd buildB(vector<Bspline> basis, VectorXd X)
{
	MatrixXd B = MatrixXd::Zero(X.size(),basis.size());
	for(int j=0; j< (int) basis.size();++j)
	{
		for(int i=0; i<X.size();++i)
		{
            B(i,j)= basis[j].evalBspline(X(i));
		}
	}
	B(X.size()-1,basis.size()-1) = 1;
	return B;
}

// Bepaal het aantal elementen in A verschillend van nul
const int getNumanz(MatrixXd A)
{
	int count = 0;
	int cols = A.cols();
	int rows = A.rows();

	for(int i=0; i<rows; i++)
	{
		for(int j=0; j<cols; j++)
		{
			if(abs(A(i,j)>0.0001))
			{
				count++;
			}
		}
	}
	return count;
}

// Zet A om naar zijn spaarse vorm, geordend per rij
void sparseARowWise(MatrixXd* A, vector<MSKlidxt>* aptrb,
	vector<MSKlidxt>* aptre, MSKidxt asub[], double aval[])
{
	int k = 0;
	int count = 0;
	int rows = A->rows();
	int cols = A->cols();
	for(int i=0; i<A->rows();++i)
	{
		aptrb->push_back(count);
		for(int j=0; j<A->cols(); ++j)
		{

			if(abs(A->coeff(i,j))>0.001)
			{
				count++;
				//asub[k] = j;
				//aval[k] = A->coeff(i,j);
				k++;
			}
		}
		aptre->push_back(count);
	}
}