#include "trapez.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

int8_t check_sign(float num);

void trapez4(float qr1, float qr2, float qpr, float qsec, float qter, float qcuar, float tm, float textra, trapezoidal *trapez){
	float s, t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, tfin;
	int8_t sign;
	uint32_t time;
	
	trapezoidal trapz;
	
	s=qr2-qr1;
	sign = check_sign(s);
	
	if ( fabs(qsec) > qter*qter/qcuar){
		t1=qter/qcuar;
		t2=fabs(qsec)/qter;
		t3=t1+t2;
	}
	else{
		t1 = sqrt(fabs(qsec)/qcuar);
		t3 = 2*t1;
		t2 = 0;
	}
	
	t4=qpr/qsec;
	t5=t4+t1;
	t6=t5+t2-t1;
	t7=t6+t1;
	t8=(qr2-qr1)/qpr;
	t9=t8+t1;
	t10=t9+t2-t1;
	t11=t10+t1;
	t12=t11+t4-t3;
	t13=t12+t1;
	t14=t13+t2-t1;
	t15=t14+t1;
	tfin=t15;

	trapez->position = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	trapez->velocity = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	trapez->aceleration = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	trapez->jerk = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	trapez->der_jerk = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	trapez->time = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	
	trapez->position[0] = qr1;
	trapez->velocity[0] = 0;
	trapez->jerk[0] = 0;
	trapez->aceleration[0] = 0;
	trapez->time[0]=0;
	
	for (time=1;time<=((tfin+textra)/tm);time++){
		
		if ((time*tm < t1) || ((time*tm >=t6)&&(time*tm <t7)) || ((time*tm >=t10)&&(time*tm <t11)) || ((time*tm >=t12)&&(time*tm <t13))){
			
			trapez->der_jerk[time] = sign*qcuar;
			trapez->jerk[time] = trapez->jerk[time-1] + trapez->der_jerk[time]*tm;
			trapez->aceleration[time] = trapez->aceleration[time-1]  + trapez->jerk[time]*tm +0.5*trapez->der_jerk[time]*tm*tm;
			trapez->velocity[time]= trapez->velocity[time-1] + trapez->aceleration[time]*tm + 0.5*trapez->jerk[time]*tm*tm + (1/6)*trapez->der_jerk[time]*tm*tm*tm;
			trapez->position[time]= trapez->position[time-1] + trapez->velocity[time]*tm + 0.5*trapez->aceleration[time]*tm*tm + (1/6)*trapez->jerk[time]*tm*tm*tm + (1/24)*trapez->der_jerk[time]*tm*tm*tm*tm;
		
		}

		else if (((time*tm >= t1)&&(time*tm < t2)) || ((time*tm >=t13)&&(time*tm <t14))){
			
			trapez->der_jerk[time] = 0;
			trapez->jerk[time] = sign*qter;
			trapez->aceleration[time] = trapez->aceleration[time-1] + trapez->jerk[time]*tm;
			trapez->velocity[time] =trapez->velocity[time-1] + trapez->aceleration[time]*tm + 0.5*trapez->jerk[time]*tm*tm;
			trapez->position[time]= trapez->position[time-1] + trapez->velocity[time]*tm + 0.5*trapez->aceleration[time]*tm*tm + (1/6)*trapez->jerk[time]*tm*tm*tm;
			
		}
		
		else if (((time*tm >=t2)&&(time*tm <t3)) || ((time*tm >=t4)&&(time*tm <t5)) || ((time*tm >=t8)&&(time*tm <t9)) || ((time*tm >=t14)&&(time*tm <t15))){
			
			trapez->der_jerk[time] = -sign*qcuar;
			trapez->jerk[time] = trapez->jerk[time-1] + trapez->der_jerk[time]*tm;
			trapez->aceleration[time] = trapez->aceleration[time-1] + trapez->jerk[time]*tm +0.5*trapez->der_jerk[time]*tm*tm;
			trapez->velocity[time]= trapez->velocity[time-1] + trapez->aceleration[time]*tm + 0.5*trapez->jerk[time]*tm*tm + (1/6)*trapez->der_jerk[time]*tm*tm*tm;
			trapez->position[time]= trapez->position[time-1] + trapez->velocity[time]*tm + 0.5*trapez->aceleration[time]*tm*tm + (1/6)*trapez->jerk[time]*tm*tm*tm + (1/24)*trapez->der_jerk[time]*tm*tm*tm*tm;
		
		}
			
		else if ((time*tm >=t3)&&(time*tm <t4)){
			
			trapez->der_jerk[time] = 0;
			trapez->jerk[time] = 0;
			trapez->aceleration[time] = sign*qsec;
			trapez->velocity[time] = trapez->velocity[time-1] + trapez->aceleration[time]*tm;
			trapez->position[time] = trapez->position[time-1] + trapez->velocity[time]*tm + 0.5*trapez->aceleration[time]*tm*tm;     
			
		}
			
		else if (((time*tm >=t5)&&(time*tm <t6)) || ((time*tm >=t9)&&(time*tm <t10))){
			
			trapez->der_jerk[time] = 0;
			trapez->jerk[time] = -sign*qter;
			trapez->aceleration[time] = trapez->aceleration[time-1] + trapez->jerk[time]*tm;
			trapez->velocity[time] =trapez->velocity[time-1] + trapez->aceleration[time]*tm + 0.5*trapez->jerk[time]*tm*tm;
			trapez->position[time]= trapez->position[time-1] + trapez->velocity[time]*tm + 0.5*trapez->aceleration[time]*tm*tm + (1/6)*trapez->jerk[time]*tm*tm*tm;
			
		}
			
		else if ((time*tm >=t7)&&(time*tm <t8)){
			
			trapez->der_jerk[time] = 0;
			trapez->jerk[time] = 0;
			trapez->aceleration[time] = 0;
			trapez->velocity[time] = sign*qpr;
			trapez->position[time]= trapez->position[time-1] + trapez->velocity[time]*tm;
			
		}
					
		else if ((time*tm >=t11)&&(time*tm <t12)){
			
			trapez->der_jerk[time] = 0;
			trapez->jerk[time] = 0;
			trapez->aceleration[time] = -sign*qsec;
			trapez->velocity[time] = trapez->velocity[time-1] + trapez->aceleration[time]*tm;
			trapez->position[time] = trapez->position[time-1] + trapez->velocity[time]*tm + 0.5*trapez->aceleration[time]*tm*tm;

		}
						 
		else{
			
			trapez->der_jerk[time] = 0;
			trapez->jerk[time] = 0;
			trapez->aceleration[time] = 0;
			trapez->velocity[time] = 0;
			trapez->position[time] = qr2;  
		}	
		trapez->time[time]=time*tm;
	}
	trapez->size = time;
}    

void trapez(float qr1, float qr2, float qpr, float qsec, float tm, float textra, trapezoidal *trapez){
	float s, t1, t2, t3, tfin;
	int8_t sign;
	uint32_t time;
	
	//trapezoidal trapz;
	
	s=qr2-qr1;
	sign = check_sign(s);
	
	if (fabs(s) > qpr*qpr/qsec){
		t1=qpr/qsec;
		t2=fabs(s)/qpr;
		tfin=fabs(s)/qpr+qpr/qsec;
	}
	else{
		t1 = sqrt(abs(s)/qsec);
		tfin = 2*t1;
		t2=0;
	}
	

	trapez->position = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	trapez->velocity = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	trapez->aceleration = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	trapez->time = (float*)malloc((((tfin+textra)/tm)*sizeof(int)));
	
	trapez->position[0] = qr1;
	trapez->velocity[0] = 0;
	trapez->aceleration[0] = 0;
	trapez->time[0]=0;
	
	
	for (time=1;time<=((tfin+textra)/tm);time++){
	//for (time=0;time<=((tfin+textra)/tm);time++){
		
		if ((time*tm < t1)){
			
			trapez->aceleration[time] = sign*qsec;
			trapez->velocity[time]= trapez->velocity[time-1] + trapez->aceleration[time]*tm;
			trapez->position[time]= trapez->position[time-1] + trapez->velocity[time]*tm + 0.5*trapez->aceleration[time]*tm*tm;
			
		
		}

		else if (((time*tm >= t1)&&(time*tm < t2))){
			
			
			trapez->aceleration[time] = 0;
			trapez->velocity[time] = sign*qpr;
			trapez->position[time]= trapez->position[time-1] + trapez->velocity[time]*tm;
			
		}
		
		else if (((time*tm >=t2)&&(time*tm <tfin))){
			
			trapez->aceleration[time] = -sign*qsec;
			trapez->velocity[time]= trapez->velocity[time-1] + trapez->aceleration[time]*tm;
			trapez->position[time]= trapez->position[time-1] + trapez->velocity[time]*tm + 0.5*trapez->aceleration[time]*tm*tm;	
			//printf("%f",trapez->position[time]);
			}
						 
		else{
			
			trapez->aceleration[time] = 0;
			trapez->velocity[time] = 0;
			trapez->position[time] = qr2; 
			//printf("%d \n",time); 
			
		}	
		trapez->time[time]=time*tm;
		
	}
	trapez->size = time;
}  
	

int8_t check_sign(float num){
	if(num<0) return -1;
	else return 1;
}