
#include "stdafx.h"
#include <math.h>
#include "movgen.h"


////////////// Generate ramp profile to track target position //////////////
////////////// One point per millisecond is generated		 ///////////////

void ramp_gen_tpos(Axis *a)
{
	if(a->state == IDLE) return;

	//if(a->phase == AUTOSTART && a->x != a->target_pos) a->phase=TRACKPOS;

	double xx = a->target_pos - a->x;	//position difference

	if(xx>0){
			double maxv=sqrt(2.0*a->acc* xx);		//for ramp: v=a*t
			if(maxv>a->max_vel) maxv=a->max_vel;

			a->vel +=a->acc;
			if(a->vel > maxv) a->vel = maxv;
			if(a->vel > xx) a->vel=xx;
	}
	else if(xx<0){
			double maxv=sqrt(-2.0*a->acc *xx);
			if(maxv>a->max_vel) maxv=a->max_vel;

			a->vel -=a->acc;
			if(a->vel < -maxv) a->vel = -maxv;
			if(a->vel < xx) a->vel=xx;
	}
	else a->vel = 0;

	a->x += a->vel ;

	//if(a->phase == AUTOSTOP && a->x==a->target_pos) a->phase=IDLE;
	if(a->x==a->target_pos) a->state = IDLE;
}


////////////////////// Init axis parameters for next move //////////////////////

void start_move(Axis*a, double target_pos, double acc, double mvel)
{
	a->target_pos = target_pos;
	a->max_vel = mvel/1000.;		//velocity per ms
	a->acc = acc/1000000.;			//acceleration per ms
	a->vel = 0;
	a->state = BUSY;
}


//////////////////////

typedef struct{
	double x,y,z;
}dPoint;

static dPoint vel;
static dPoint acc;

static dPoint p1;

static double dt1,dt2;

static double T,t;
static double tt1,tt2;
static double Acc, Vel;

static BOOL mov_avail=0;

double Res(dPoint p)
{
		return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}


#define TSTEP	(1/1000.)

//////////////// Setup and start combined 3-axis move /////////////////

void start_lin_move(Axis*ax, Axis*ay, Axis*az, double feed)
{

	//p1 = pp1;
	//p2 = pp2;

	p1.x = ax->x;
	p1.y = ay->x;
	p1.z = az->x;

	double mvel=feed;

		dPoint dp;
		dp.x = ax->target_pos - ax->x;
		dp.y = ay->target_pos - ay->x;
		dp.z = az->target_pos - az->x;

		if(Res(dp)==0){		//Nothing to do
			mov_avail = 0;
			return;
		}

		double c=mvel / Res(dp);
		vel.x = dp.x *c;
		vel.y = dp.y *c;
		vel.z = dp.z *c;

		//////////////////////// Velocity limit

		dPoint vc;
		vc.x = ax->max_vel/fabs(vel.x);
		vc.y = ay->max_vel/fabs(vel.y);
		vc.z = az->max_vel/fabs(vel.z);

		c=min(vc.x, vc.y);
		c=min(c, vc.z);
		if(c<1.0){
			vel.x *=c;
			vel.y *=c;
			vel.z *=c;
		}

		//////////////////////// Acceleration limit

		dPoint ac;

		acc.x = vel.x;
		acc.y = vel.y;
		acc.z = vel.z;

		ac.x = ax->acc/fabs(acc.x);	
		ac.y = ay->acc/fabs(acc.y);
		ac.z = az->acc/fabs(acc.z);

		c = min(ac.x, ac.y);
		c= min(c, ac.z);

		acc.x*=c;
		acc.y*=c;
		acc.z*=c;


		dt1 = Res(vel)/Res(acc);

		//////////////////////// path length limit

		dPoint d1;

		d1.x = acc.x*(dt1*dt1/2.);
		d1.y = acc.y*(dt1*dt1/2.);
		d1.z = acc.z*(dt1*dt1/2.);

		double dp1 = Res(d1);
		if(dp1>Res(dp)/2.){
			d1.x=dp.x*0.5;
			d1.y=dp.y*0.5;
			d1.z=dp.z*0.5;
			dt1 = sqrt(Res(dp)/Res(acc));	//half of the path
		}


		dPoint d2;
		d2.x= dp.x - d1.x*2;
		d2.y= dp.y - d1.y*2;
		d2.z= dp.z - d1.z*2;

		dt2 = Res(d2)/Res(vel);

		//////////////////////// Ready to generate


		T= 2*dt1+dt2;
		t=0;

		Acc = Res(acc);
		Vel = Res(vel);

		tt1=Acc*dt1*dt1/2.;
		tt2=Acc*dt1*dt1/2.+Vel*dt2+Acc*dt1*dt1/2;

		mov_avail=1;

		//ax->state = BUSY;
		//ay->state = BUSY;
		//az->state = BUSY;
}


///////// Advance along the path for linear combined move ///////////

BOOL lin_move_gen(Axis*ax, Axis*ay, Axis*az)
{
		double tt;

		if(t>=T){
			t=T;
			mov_avail=0;		//We are done after this point
			//return false;
		}

			if(t<dt1){
				tt = Acc*t*t/2.;
			}
			else if(t<dt1+dt2){
				tt=tt1+Vel*(t-dt1);
			}
			else{
				tt=tt2 - Acc*(T-t)*(T-t)/2.;
			}

			tt/=Vel;

			ax->x = p1.x + vel.x*tt;
			ay->x = p1.y + vel.y*tt;
			az->x = p1.z + vel.z*tt;

			//*cur *= spa;

			t+=TSTEP;

			return true;
}


////////  More points needed?

BOOL lin_mov_avail()
{
	return mov_avail;
}


void stop_lin_move()
{
		mov_avail = 0;
}


