#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <windows.h>

#include "nurbs.h"
#include "geometric.h"
#include "interpolate.h"

#define SERVOPERIOD 0.00025

int main()
{
	FILE* fid=NULL;	//deskryptor pliku
	FILE* fidtim=NULL;	//deskryptor pliku
	nurbs_curve curve;//struktura krzywej NURBS
	double u=0.0;	//aktualny parametr krzywej
	double t=0.0;	//aktualny czas
	double un=0.0;	//nowa wartosc parametru krzywej
	double s=0.0;	//aktualna wartosc drogi
	double ds=0.0;	//przyrost drogi
	double ierr=0.0;//blad interpolacji
	double Len=0.0;	//dlugosc krzywej
	double* v;
	double* a;
	double* j;	//prędkosc, przyspieszenie, zryw styczne do toru ruchu
	int numsamp=0;	//ilosc probek profilu predkosci
	int i=0;//indeks petli
	cart_point axv,axa,axj;//predkosc, przysp, zryw w osiach x,y,z
	cart_point Pt, dPt, ddPt, dddPt;//polozenie, 1-3 pochodne wzgl. parametru krzywej NURBS (w osiach x,y,z)
	static LARGE_INTEGER StartTime, StopTime, Frequency, ElapsedMicroseconds;

	/* wczytanie krzywej NURBS */
	nurbs_curve_alloc_file("S1200X30ptaszek.nurbs", &curve);

	/* wyznaczenie dlugosci krzywej */
	Len = adaptiveSimpsons(&nurbs_arc_len, &curve, 0.0, 1.0,1e-12,30);

	fidtim = fopen("time.txt","a");

	/* wczytanie profilu prędkości z pliku */
	fid = fopen("fprof.txt","r");
	fscanf(fid,"N=%d\n",&numsamp);
	v = malloc(numsamp*sizeof(double));
	a = malloc(numsamp*sizeof(double));
	j = malloc(numsamp*sizeof(double));
	for(i=0;i<numsamp;i++){
		fscanf(fid,"%le %le %le\n",&v[i],&a[i],&j[i]);
		//printf("vaj=%f %f %f\n",v[i],a[i],j[i]);
	}
	fclose(fid);

//	fid=fopen("s_u_FCPeval.txt","w");
//	while(s<=Len){
//		fprintf(fid,"%.17le %.17le\n", s, u);
//		u = FCP_eval(s);
//		s = s + 0.05;
//	}
//	fclose(fid);

	fid = fopen("nurbs_out.txt","w");
	if(fid==NULL){
		printf("ERROR: could not open file nurbs_out.txt\n");
		return -1;
	}

	QueryPerformanceFrequency(&Frequency);
	QueryPerformanceCounter(&StartTime);

	u=0.0; un=0.0; s=0.0;
	/* petla interpolacji T=250us */
	for(i=0;i<numsamp;i++){
		u = un;
		nurbs_deBoor_eval(u,-1,&curve,&Pt,&dPt,NULL,NULL);
		compute_axial_vaj(&axv, &axa, &axj, &dPt,&ddPt,&dddPt,v[i], a[i], j[i]);
		fprintf(fid,"%.17le %.17le %.17le %.17le %.17le %.17le %.17le %.17le %.17le %.17le\n", t, u, s, v[i], a[i], j[i], Pt.x, Pt.y, Pt.z, ierr);
		//printf("%lf\n",u);
		//un = u+(v[i]*SERVOPERIOD)/Len;
		//un = get_u_Taylor3(v[i],a[i],j[i],u,SERVOPERIOD,&dPt,&ddPt,&dddPt);
		//un = get_u_Taylor2(v[i],a[i],u,SERVOPERIOD,&dPt,&ddPt);
		//un = get_u_Taylor2_ds(v[i]*SERVOPERIOD,u,SERVOPERIOD,&dPt,&ddPt);
		un = get_u_Taylor3_ds(v[i]*SERVOPERIOD,u,SERVOPERIOD,&dPt,&ddPt,&dddPt);
		//un = get_u_RK2(v[i], u, SERVOPERIOD, &Pt, &dPt, &curve);
		//un = get_u_NewtonRaphson(u, v[i], SERVOPERIOD, &curve,&Pt, &dPt, &ddPt);
		//un = get_u_FCP(s, v[i], SERVOPERIOD);
		ds = adaptiveSimpsons(&nurbs_arc_len, &curve, u, un,1e-12,30);
		//ierr = v[i]*SERVOPERIOD - ds;
		s = s + ds;
		t= t + SERVOPERIOD;
	}

	QueryPerformanceCounter(&StopTime);
    ElapsedMicroseconds.QuadPart = StopTime.QuadPart - StartTime.QuadPart;
    ElapsedMicroseconds.QuadPart *= 1000000;                //first scale up counts
    ElapsedMicroseconds.QuadPart /= Frequency.QuadPart;     //then convert to us
    printf("deltaT=%lli\n",ElapsedMicroseconds.QuadPart);
    fprintf(fidtim,"%lli\n",ElapsedMicroseconds.QuadPart);

	fclose(fid);
	fclose(fidtim);
	free(v);
	free(a);
	free(j);
	nurbs_curve_free(&curve);

	return 0;
}
