/*
 * interpolate.c
 *
 *  Created on: 2 maj 2020
 *      Author: erwin
 */

#include "interpolate.h"
#include "FCP_init.h"

static double G_ds_target;
static double G_u_start;
static nurbs_curve* G_curve;

//static double dut1, dut2, dut3;
static double um1, um2;

void set_global_nurbs_curve(nurbs_curve* curve)
{
	G_curve = curve;
}

//int BinarySearch(double array[], int start_index, int end_index, double element){
//   while (start_index <= end_index){
//      int middle = start_index + (end_index- start_index )/2;
//      if (array[middle] == element)
//         return middle;
//      if (array[middle] < element)
//         start_index = middle + 1;
//      else
//         end_index = middle - 1;
//   }
//   return -1;
//}

int find_span (int n, int p, double u, double *U)
{

	int low, high, mid;
	if (u >= U[n-1]) // safety >= check
	{
		return (n-1);
	}
	low = p;
	high = n-1;
	mid = (low + high)/2;
	while (u < U[mid] || u >= U[mid+1])
	{
		if ( u < U[mid])
		{ high = mid;
		}
		else
		{ low = mid;
		}
		mid = (low + high)/2;
	}

	return (mid);
}

double diff_arclength(double u)
{
	double ds_act;
	double diff_ds;
	ds_act = adaptiveSimpsons(&nurbs_arc_len, G_curve, G_u_start, u,1e-12,20);
	diff_ds = G_ds_target - ds_act;
	return(diff_ds);
}

double get_u_Taylor3(double v, double a, double j, double u, double T,
					cart_point* dPt, cart_point* ddPt, cart_point* dddPt)
{
	double sig, sig1, sig2;//parametric velocity and its 1st and 2nd deriv
	double u1,u2,u3;// curve parameter 1st, 2nd and 3rd derivs
	double u_new;

	sig = modulus(dPt);
	sig1 = dotprod(dPt,ddPt)/sig;
	sig2 = (dotprod(dPt,dddPt) + sq_modulus(ddPt) - (sig1*sig1))/sig;

	u1 = v/sig;
	u2 = a/sig - ((sig1*v)/(sig*sig))*u1;
	u3 = j/sig - ((3*sig1*v)/(sig*sig))*u2 - ((sig2*v)/(sig*sig))*(u1*u1);

	u_new = u + u1*T + (u2/2)*T*T + (u3/6)*T*T*T;
	if(u_new>=1.0) u_new=1.0;
	return(u_new);
}

//double get_u_Taylor3(double v, double a, double j, double u, double T,
//					cart_point* dPt, cart_point* ddPt, cart_point* dddPt)
//{
//	double u1,u2,u3;// curve parameter 1st, 2nd and 3rd derivs
//	double u_new, U1, U2, U3, U4, U5, U6;
//	double ds = v*T;
//	double sig,sig2,sig3;
//
//	sig = modulus(dPt);
//	sig2 = dotprod(dPt,ddPt);
//	sig3 = dotprod(ddPt,ddPt) + dotprod(dPt,dddPt);
//
//	U1 = ds/sig;
//	U2 = (pow(ds,2)*sig2)/(2*pow(sig,4));
//	U3 = (pow(ds,2)*sig3)/(6*pow(sig,5));
//	U4 = (2*pow(ds,3)*pow(sig2,2))/(3*pow(sig,7));
//	U5 = (a*pow(T,2))/(2*sig);
//	U6 = (pow(T,3)/6)*(j/sig - (3*a*v*sig2)/(pow(sig,4)));
//
//	u_new = u + U1 - U2 - U3 + U4 + U5 + U6;
//	if(u_new>=1.0) u_new=1.0;
//	return(u_new);
//}

double get_u_Taylor2(double v, double a, double u, double T,
					cart_point* dPt, cart_point* ddPt)
{
	double sig, sig1;//parametric velocities and its 1st and 2nd deriv
	double u1,u2;// parameter 1st, 2nd and 3rd derivs
	double u_new;

	sig = modulus(dPt);
	sig1 = dotprod(dPt,ddPt)/sig;

	u1 = v/sig;
	u2 = a/sig - ((sig1*v)/(sig*sig))*u1;

	u_new = u + u1*T + (u2/2)*T*T;
	if(u_new>1.0) u_new=1.0;
	return(u_new);
}

double get_u_Taylor2_ds(double ds, double u, double T,
						cart_point* dPt, cart_point* ddPt)
{
	double sig, dpC1C2;//parametric velocity and its 1st and 2nd deriv
	double u1,u2;// curve parameter 1st, 2nd and 3rd derivs
	double u_new;

	sig = modulus(dPt);
	dpC1C2 = dotprod(dPt,ddPt);

	u1 = 1/sig;
	u2 = -dpC1C2/pow(sig,4);

	u_new = u + u1*ds + (1/2)*u2*pow(ds,2);

	if(u_new>=1.0) u_new=1.0;
	return(u_new);
}

double get_u_Taylor3_ds(double ds, double u, double T,
						cart_point* dPt, cart_point* ddPt, cart_point* dddPt)
{
	double sig, sig22, dpC1C2, dpC3C1;//parametric velocity and its 1st and 2nd deriv
	double du_ds, d2u_ds2, d3u_ds3;// curve parameter 1st, 2nd and 3rd derivs
	double u_new;

	sig = modulus(dPt);//sqrt(x'^2 + y'^2 + z'^2)
	sig22 = sq_modulus(ddPt);//x''^2 + y''^2 + z''^2
	dpC1C2 = dotprod(dPt,ddPt);//<C',C''>
	dpC3C1 = dotprod(dddPt,dPt);//<C''',C'>


	du_ds = 1/sig;
	d2u_ds2 = -dpC1C2/pow(sig,4);
	d3u_ds3 = (3*pow(2*dpC1C2,2))/(4*pow(sig,5)) - (2*(sig22 + dpC3C1))/(2*pow(sig,3));

	u_new = u + du_ds*ds + (1/2)*d2u_ds2*pow(ds,2) + (1/6)*d3u_ds3*pow(ds,3);

	if(u_new>=1.0) u_new=1.0;
	return(u_new);
}

double get_u_RK2(double v, double u, double T, cart_point* Pt, cart_point* dPt, nurbs_curve* curve)
{
	cart_point nPt, ndPt;
	cart_point nPt_Pt;
	double u_tmp, u_new, dL, K1, K2, A, B, D;

	K1 = v/modulus(dPt);
	nurbs_deBoor_eval((u + T*K1), -1, curve, &nPt, &ndPt, NULL, NULL);
	K2 = v/modulus(&ndPt);
	u_tmp = u + 0.5*T*(K1+K2);
	u_new = u_tmp;

	nurbs_deBoor_eval(u_tmp, -1, curve, &nPt, &ndPt, NULL, NULL);
	A = pow(modulus(&ndPt),2);

	nPt_Pt.x = nPt.x - Pt->x;
	nPt_Pt.y = nPt.y - Pt->y;
	nPt_Pt.z = nPt.z - Pt->z;
//	B = 2*dotprod(&ndPt,&nPt_Pt);
//	D = pow(modulus(&nPt_Pt),2) - pow(v,2)*pow(T,2);

	dL = adaptiveSimpsons(&nurbs_arc_len, curve, u, u_tmp,1e-6,5);
	B = 2*dotprod(&ndPt,&nPt_Pt);
	D = pow(dL,2) - pow(v,2)*pow(T,2);

	if((B*B - 4*A*D)>=0.0){
		u_new = u_tmp + (sqrt(pow(B,2)-4*A*D)-B)/(2*A);
	}
	else{
		u_new = u_tmp;
	}

	if(u_new>=1.0) u_new=1.0;
	return u_new;
}

double get_u_T3SIA(double v, double a, double j, double u, double T, nurbs_curve* curve)
{
	double u1,u2,u3;// curve parameter 1st, 2nd and 3rd derivs
	double u_new, u_new_ini, u_new_p, U1, U2, U3, U4, U5, U6;
	double ds = v*T;
	double y,z,ids,vfl;
	unsigned int iter=0;
	double sig,sig2,sig3;
	cart_point Pt, pPt, dPt, ddPt, dddPt;

	nurbs_deBoor_eval(u, -1, curve, &pPt, &dPt, &ddPt, &dddPt);

	sig = modulus(&dPt);
	sig2 = dotprod(&dPt,&ddPt);
	sig3 = dotprod(&ddPt,&ddPt) + dotprod(&dPt,&dddPt);

	U1 = ds/sig;
	U2 = (pow(ds,2)*sig2)/(2*pow(sig,4));
	U3 = (pow(ds,2)*sig3)/(6*pow(sig,5));
	U4 = (2*pow(ds,3)*pow(sig2,2))/(3*pow(sig,7));
	U5 = (a*pow(T,2))/(2*sig);
	U6 = (pow(T,3)/6)*(j/sig - (3*a*v*sig2)/(pow(sig,4)));

	u_new_ini = u + U1 - U2 - U3 + U4 + U5 + U6;
	u_new = u_new_ini;
	u_new_p = u_new;
	if(v==0) return(u_new);
	do{
		//printf("iter=%d\n", iter);
		//fflush(stdout);
		nurbs_deBoor_eval(u_new, -1, curve, &Pt, NULL, NULL, NULL);
		ids = cart_dist(&Pt,&pPt);
		//vfl = fabs((ids/T) - v)/v;
		//ids = adaptiveSimpsons(&nurbs_arc_len, &curve, u, u_new,1e-12,20);
		y = ids/v - T;
		nurbs_deBoor_eval(u_new + y, -1, curve, &Pt, NULL, NULL, NULL);
		ids = cart_dist(&Pt,&pPt);
		//ids = adaptiveSimpsons(&nurbs_arc_len, &curve, u, u_new + y,1e-12,20);
		z = ids/v - T;
		u_new = u_new - (y*y)/(z-y);
		iter=iter+1;
		//printf("iter=%d unew_ini=%lf unew=%lf\n", iter,u_new_ini, u_new);
		//fflush(stdout);
	//}while(vfl>0.001);
	}while(fabs(y)>0.001);

	if(u_new>=1.0) u_new=1.0;
	return(u_new);
}

//double get_u_RK4(double u, double v, double T, double ct, feedProfS* fprof){
//	double u_new, fk1, fk2, fk3, fk4;
//	double vk1, vk2, vk3, vk4;
//	cart_point Pt, dPt;
//	nurbs_curve* curve = fprof->curve;
//
//	nurbs_deBoor_eval(u, -1, curve, &Pt, &dPt, NULL, NULL);
//	fk1 = v/modulus(&dPt);
//
//	eval_feed_prof_S(fprof, ct + T/2);
//	nurbs_deBoor_eval((u + (T*fk1)/2), -1, curve, &Pt, &dPt, NULL, NULL);
//	fk2 = (fprof->v)/modulus(&dPt);
//
//	nurbs_deBoor_eval((u + (T*fk2)/2), -1, curve, &Pt, &dPt, NULL, NULL);
//	fk3 = (fprof->v)/modulus(&dPt);
//
//	eval_feed_prof_S(fprof, ct + T);
//	nurbs_deBoor_eval((u + T*fk3), -1, curve, &Pt, &dPt, NULL, NULL);
//	fk4 = (fprof->v)/modulus(&dPt);
//
//	u_new = u + (T/6)*(fk1 + 2*fk2 + 2*fk3 + fk4);
//
//	return u_new;
//}

//double get_u_AdamsBashforth(double u, double v, double T, double ct, feedProfS* fprof)
//{
//	double u_new;
//	double dut, dutn;
//	cart_point Pt, dPt;
//	nurbs_curve* curve = fprof->curve;
//
//	nurbs_deBoor_eval(u, -1, curve, &Pt, &dPt, NULL, NULL);
//	dut = v/modulus(&dPt);
//
//	if(ct < T*3){
//		nurbs_deBoor_eval(u,-1,curve,&Pt,&dPt,NULL,NULL);
//		u_new = get_u_RK2(v, u, T, &Pt, &dPt, curve);
//	}
//	else{
//		u_new = u + (T/24.0)*(55*dut - 59*dut1 + 37*dut2 - 9*dut3);//Adams-Bashforth predictor
//
//		eval_feed_prof_S(fprof, ct + T);
//		nurbs_deBoor_eval(u_new, -1, curve, &Pt, &dPt, NULL, NULL);
//		dutn = (fprof->v)/modulus(&dPt);
//
//		u_new = u + (T/24.0)*(9*dutn + 19*dut - 5*dut1 + dut2);//Adams-Moulton corrector
//	}
//
//	dut3 = dut2;
//	dut2 = dut1;
//	dut1 = dut;
//
//	if(u_new>=1.0) u_new=1.0;
//	return u_new;
//}

double get_u_AdamsMoulton(double u, double v, double T, double ct, nurbs_curve* curve)
{
	double up1_0,up1_1,up1_2;
	double u_new;
	double dLp1_0,dLp1_1,dLp1_2;
	double q,r,s;
	cart_point Pt, dPt;

	if(ct < 2*T){
		nurbs_deBoor_eval(u,-1,curve,&Pt,&dPt,NULL,NULL);
		u_new = get_u_RK2(v, u, T, &Pt, &dPt, curve);
	}

	up1_0 = (1/8.0)*(13.0*u - 2.0*um1 - 3.0*um2);
	up1_1 = (1/10.0)*(21.0*u - 12.0*um1 + um2);
	up1_2 = (1/11.0)*(20.0*u - 7.0*um1 - 2.0*um2);

	//predictor step
	//nurbs_deBoor_eval(u,-1,curve,&Pt,NULL,NULL,NULL);
	//nurbs_deBoor_eval(up1_0,-1,curve,&Ptpx,NULL,NULL,NULL);
	//dLp1_0 = cart_dist(&Ptpx,&Pt);
	dLp1_0 = adaptiveSimpsons(&nurbs_arc_len, curve, u, up1_0,1e-8,10);

	//nurbs_deBoor_eval(up1_1,-1,curve,&Ptpx,NULL,NULL,NULL);
	//dLp1_1 = cart_dist(&Ptpx,&Pt);
	dLp1_1 = adaptiveSimpsons(&nurbs_arc_len, curve, u, up1_1,1e-8,10);

	//nurbs_deBoor_eval(up1_2,-1,curve,&Ptpx,NULL,NULL,NULL);
	//dLp1_2 = cart_dist(&Ptpx,&Pt);
	dLp1_2 = adaptiveSimpsons(&nurbs_arc_len, curve, u, up1_2,1e-8,10);

	//compute predicted step increment ratios
	q = dLp1_0/dLp1_1;
	r = dLp1_2/dLp1_1;
	s = dLp1_2/dLp1_0;

	//corrector step - quadratic
	u_new = up1_2 - (r*(r-q)*(up1_2 - up1_1) + (1.0-r)*s*(up1_2 - up1_0))/((q-1.0)*(r-1.0)*(s-1.0));

	um2 = um1;
	um1 = u;

	if(u_new>=1.0) u_new=1.0;
	return(u_new);
}

//double get_u_AdamsMoulton(double v, double u, double um1, double um2, nurbs_curve* curve)
//{
//	double up1_0,up1_1,up1_2;
//	double u_new;
//	double dLp1_0,dLp1_1,dLp1_2;
//	double q,r,s;
//	cart_point Pt;
//	cart_point Ptpx;
//
//	up1_0 = (1/8.0)*(13.0*u - 2.0*um1 - 3.0*um2);
//	up1_1 = (1/10.0)*(21.0*u - 12.0*um1 + um2);
//	up1_2 = (1/11.0)*(20.0*u - 7.0*um1 - 2.0*um2);
//
//	//predictor step
//	nurbs_deBoor_eval(u,-1,curve,&Pt,NULL,NULL,NULL);
//	nurbs_deBoor_eval(up1_0,-1,curve,&Ptpx,NULL,NULL,NULL);
//	dLp1_0 = cart_dist(&Ptpx,&Pt);
//
//	nurbs_deBoor_eval(up1_1,-1,curve,&Ptpx,NULL,NULL,NULL);
//	dLp1_1 = cart_dist(&Ptpx,&Pt);
//
//	nurbs_deBoor_eval(up1_2,-1,curve,&Ptpx,NULL,NULL,NULL);
//	dLp1_2 = cart_dist(&Ptpx,&Pt);
//
//	//compute predicted step increment ratios
//	q = dLp1_0/dLp1_1;
//	r = dLp1_2/dLp1_1;
//	s = dLp1_2/dLp1_0;
//
//	//corrector step - quadratic
//	u_new = up1_2 - (r*(r-q)*(up1_2 - up1_1) + (1.0-r)*s*(up1_2 - up1_0))/((q-1.0)*(r-1.0)*(s-1.0));
//
//	if(u_new>=1.0) u_new=1.0;
//	return(u_new);
//}

//double PredCorrNURBS(double v, double a, double j, double u, double T, nurbs_curve *curve)
//{
//	//double fval = nurbs_arc_len(u,curve);
//	double up
//
//}

double get_u_NewtonRaphson(double u, double v, double T, nurbs_curve* curve, cart_point* ePt, cart_point* edPt, cart_point* eddPt)
{
	double ds_target = v*T;
	double ds_act, F, dF;
	double u_new;
	int iter=0;
	cart_point Pt, dPt, ddPt;

	//nurbs_deBoor_eval(u, -1, curve, &Pt, &dPt, &ddPt, NULL);
	u_new = get_u_Taylor2_ds(ds_target,u,T,edPt,eddPt);//predictor (2nd order Taylor)

	do{//corrector (Newton-Raphson)
		nurbs_deBoor_eval(u_new, -1, curve, &Pt, &dPt, &ddPt, NULL);
		//ds_act = adaptiveSimpsons(&nurbs_arc_len, curve, u, u_new,1e-10,10);
		ds_act = adaptiveSimpsons(&nurbs_arc_len, curve, u, u_new,1e-12,30);
		//ds_act = cart_dist(ePt, &Pt);
		F = ds_act - ds_target;
		dF = sqrt( pow(dPt.x,2) + pow(dPt.y,2) + pow(dPt.z,2) );
		u_new = u_new - F/dF;
		iter++;
	}while(fabs(F)>1e-12);
	//}while(iter < 5);
	//}while((fabs(F)>1e-9)&&(iter < 3));
	//}while((fabs(F)>1e-10)&&(fabs(F/dF)>1e-12));

	if(u_new>=1.0) u_new=1.0;
	return(u_new);
}

double get_u_FCP(double s, double v, double T)
{
	double s_new = s + v*T;
	double u_new;
	int i=0,j=0;

	//j = BinarySearch(FCP_SBRK,0,BRK_LEN,s_new);
	j = find_span(BRK_LEN,0,s,FCP_SBRK);

	u_new = FCP_THT[j][0];
	s_new = (s_new - FCP_SBRK[j])/FCP_SBRK[BRK_LEN-1];

	for (i=1; i<FCP_ORD; i++)
        u_new = u_new*s_new + FCP_THT[j][i];

	return u_new;
}

double FCP_eval(double s)
{
	double u,sn;
	int i,j;

	//j = BinarySearch(FCP_SBRK,0,BRK_LEN-1,s);
	j = find_span(BRK_LEN,0,s,FCP_SBRK);
	printf("FCP: s=%f jbrk=%d\n",s,j);
	fflush(stdout);

	sn = (s - FCP_SBRK[j])/FCP_SBRK[BRK_LEN-1];
	u = FCP_THT[j][0];

	for (i=1; i<FCP_ORD; i++)
        u = u*sn + FCP_THT[j][i];

	return u;
}

void compute_axial_vaj(cart_point* v, cart_point* a, cart_point* j,/* output axial vel, acc, jer */
					   cart_point* dPt,cart_point* ddPt,cart_point* dddPt,/* input trajectory curve derivatives */
					   double Vt, double At, double Jt)/* input tangential vel, acc, jer */
{
	cart_point C_s, C_ss, C_sss;

	/* compute derivatives wrt arc-length using derivatives wrt curve parameter */
	//compute_Csss(dPt, ddPt, dddPt, &C_s, &C_ss, &C_sss);
	double u_s = 1/modulus(dPt);
	double u_ss = -dotprod(dPt,ddPt)*pow(u_s,4);
	double u_sss = 4*pow(dotprod(dPt,ddPt),2)*pow(u_s,7) -
					(dotprod(ddPt,ddPt) +
					dotprod(dPt,dddPt))*pow(u_s,5);

	C_s.x = dPt->x*u_s;
	C_s.y = dPt->y*u_s;
	C_s.z = dPt->z*u_s;

	C_ss.x = ddPt->x*u_s*u_s + dPt->x*u_ss;
	C_ss.y = ddPt->y*u_s*u_s + dPt->y*u_ss;
	C_ss.z = ddPt->z*u_s*u_s + dPt->z*u_ss;

	C_sss.x = dddPt->x*u_s*u_s*u_s + 3*ddPt->x*u_s*u_ss + dPt->x*u_sss;
	C_sss.y = dddPt->y*u_s*u_s*u_s + 3*ddPt->y*u_s*u_ss + dPt->y*u_sss;
	C_sss.z = dddPt->z*u_s*u_s*u_s + 3*ddPt->z*u_s*u_ss + dPt->z*u_sss;

	/* axial velocity */
	v->x = C_s.x*Vt;
	v->y = C_s.y*Vt;
	v->z = C_s.z*Vt;

	/* axial acceleration */
	a->x = C_ss.x*Vt*Vt + C_s.x*At;
	a->y = C_ss.y*Vt*Vt + C_s.y*At;
	a->z = C_ss.z*Vt*Vt + C_s.z*At;

	/* axial jerk */
	j->x = C_sss.x*Vt*Vt*Vt + 3*C_ss.x*Vt*At + C_s.x*Jt;
	j->y = C_sss.y*Vt*Vt*Vt + 3*C_ss.y*Vt*At + C_s.y*Jt;
	j->z = C_sss.z*Vt*Vt*Vt + 3*C_ss.z*Vt*At + C_s.z*Jt;
}

