//////////////////////////////////////////////////////////////////////
// controller.cpp: implementation of the controller class.
// Author: Brian Leibowitz
//////////////////////////////////////////////////////////////////////

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "control.h"
//#include "../include/control/control.h"

char controller::axisChar[COORD_SIZE] = {NULL, 'X', 'Y', 'Z', 'T', 'U', 'V'};

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

/* controller::controller
 * Constructor for the Controller class
 * Prepares the controller to receive commands.
 * Current implementation is open device file for writing and reset
 */
controller::controller(const char* deviceOrFileName)
:	firstPoint(false), devicefile(NULL), errorflag(0)
  ,	maxAcceleration(MAX_ACCEL), maxVelocity(MAX_VEL)
{
	// set default status = no error
	errorflag = 0;
	open(deviceOrFileName);
}

/* Controller::~Controller
 * Destructor for the controller class
 * if the device file is open, attempt to reset board
 * and close the device file
 */
controller::~controller()
{
	if (devicefile) {
		// don't reset - it will kill current motion in progress
		//reset();
		fclose(devicefile);
	}
}




////////////////////////////////////////////////////////////////
// Method Implementations
////////////////////////////////////////////////////////////////

/* controller::open
 * Opens a file or device for output
 */
bool controller::open(const char* deviceOrFileName)
{
/*
	if ( devicefile != NULL ) {
		setError("Device alrady opened");
		return false;				// file already opened
	}
*/	
	if ((devicefile = fopen(deviceOrFileName, "w+t")) == NULL ) {
		setError("Could not open device file for writing.\n");
		return false;				// error opening file
	} else {
		// don't reset during constructor - it makes the robot jump
		// only use reset wben necessary
		// reset();
		// set positions and position errors to 0
		for (int i=X; i<COORD_SIZE; i++) {
			position[i] = 0L;
			poserror[i] = 0.0;
		}
		// set the default acceleration for all axes
		// if setAccelerationAll is not called, it will lead to problems
		// in moveRelative()
		setAccelerationAll(DEF_ACCEL);
		firstPoint = true;
	}
	return true;
}

void controller::close()
{
	if( !getError() )	stopMVs();		// finish the trajectory
	
	if (devicefile) {
		// don't reset - it will kill current motion in progress
		//reset();
		fclose(devicefile);
		devicefile = NULL;
	}
	errorflag = 0;						// clear errors
}



/* controller::setError
 * Set the error flag and store a message in the error string
 */
void controller::setError(const char *errmsg)
{
	errorflag = 1;
	sprintf(errorstring, "%s", errmsg);
}




/* controller::sendCommand
 * send a command string the to controller
 * can't check for command errors because might be some delay before
 * board discovers and reports the error
 */
void controller::sendCommand(const char *cmdString)
{
	if (!errorflag) {
		fprintf(devicefile, "%s\n", cmdString);
	}
}




/* controller::reset
 * Reset controller at user's request
 */
void controller::reset(void)
{
	sendCommand("aars;");
	setAccelerationAll(DEF_ACCEL);
	errorflag = 0;
}

void controller::stop(void)
{
	sendCommand("st");
	setError("STOPPED manually");
}

void controller::kill(void)
{
	sendCommand("KL");
	setError("KILLED manually");
}


/* controller::setAccelerationAll
 * sets the acceleration (in steps/sec/sec) for all axes
 */
void controller::setAccelerationAll(long accel)
{
	int i;
	char cmdstr[128], tmpstr[32];

	if (!errorflag) {
		// generate command string
		sprintf(cmdstr, "AA AC");
		itoa((int)accel, tmpstr, 10);
		for(i=0; i<NUM_AXES; i++) {
			strcat(cmdstr, tmpstr);
			strcat(cmdstr, ",");
		}
		strcat(cmdstr, tmpstr);
		strcat(cmdstr, ";");

		// send command string
		sendCommand(cmdstr);

		// save accelleration parameter for later
		accelleration = accel;
	}
}


void controller::setMaxAcceleration(double a_max)
{
	char s[256];
	sprintf( s, "AA AC%.0f ", (float)a_max);
	maxAcceleration = (long)a_max;
	sendCommand(s);
}

#include <vcl/dstring.h>

void controller::setMaxVelocity    (double v_max)
{
	char s[256];
	sprintf( s, "AA VL%.0f ", (float)v_max);
	maxVelocity = (long)v_max;
	sendCommand(s);
}


/* controller::moveRelative
 * Command the controller to move the given relative
 * coordinate in the amount of time indicated by the first
 * element of the coordinate
 * ASSUMES: all axes set to "acceleration"
 *          initial and final velocities are 0
 * RETURNS: 0 if axes should all be able to reach in time
 *          1 if one or more axes will not reach in time
 */
int controller::moveRelative(Coordinate c)
{
	char cmdstr[128], tmpstr[32];
	int i, rcode = 0,
		x[NUM_AXES], vreq[NUM_AXES], finalsteps;
	double T, T2, a = accelleration, finalpos;

	if (!errorflag) {
		T = c[0];
		T2 = T*T;
		// calculate position deltas and velocities needed
		// for each axis
		for (i=1; i<=NUM_AXES; i++) {
			// calculate final desired and actual positions
			finalpos = position[i-1] + poserror[i-1] + c[i];
			finalsteps = (int)fround(finalpos);

			// number of steps to move this axis
			x[i-1] = (int)((long)finalsteps-position[i-1]);
			// check if we will get there in time
			if ((double)x[i-1] < a*T2/4.0) {
				// if so, calculate velocity setting needed
				vreq[i-1] = (int)
					(fround(a*T - sqrt(a*a*T2 - 4*a*x[i-1])))/2;
				if (vreq[i-1] > maxVelocity) {
					vreq[i-1] = maxVelocity;
					rcode = 1;
				}
			} else {
				// otherwise go as fast as possible
				// and set the return code to notify caller
				vreq[i-1] = maxVelocity;
				rcode = 1;
			}

			// store new position & error after move
			position[i-1] = finalsteps;
			poserror[i-1] = finalpos-finalsteps;
		}

		// generate the velocity set command
		sprintf(cmdstr, "AA VL");
		for (i=0; i<NUM_AXES; i++) {
			itoa(abs(vreq[i]), tmpstr, 10);
			strcat(cmdstr, tmpstr);
			if (i < (NUM_AXES-1)) {
				strcat(cmdstr, ",");
			} else {
				strcat(cmdstr, ";");
			}
		}
		// send the velocity command
		sendCommand(cmdstr);

		// generate the relative move command
		sprintf(cmdstr, "AA MR");
		for (i=0; i<NUM_AXES; i++) {
			itoa(x[i], tmpstr, 10);
			strcat(cmdstr, tmpstr);
			if (i < (NUM_AXES-1)) {
				strcat(cmdstr, ",");
			} else {
				strcat(cmdstr, " GO ");
			}
		}
		// send the move command
		sendCommand(cmdstr);
	}

	return rcode;
}




/* controller::moveAbsolute
 * move to an absolute position in a given amount of time.
 * NOTE:  moveRelative is used
 * RETURNS: 0 if all axes should reach in time
 *          1 if not all axes will reach in time
 */
int controller::moveAbsolute(Coordinate c)
{
	int i;
	Coordinate relc;

	// copy the temporal coordinate
	relc[0] = c[0];
	for (i=X; i<COORD_SIZE; i++) {
		//     desired - current position
		relc[i] = c[i] - (position[i-1] + poserror[i-1]);
	}

	return (moveRelative(relc));
}




/* controller::followTrajectory
 * follow a trajectory of points
 * NOTE: moveRelative is ultimately used, so motion ceases at each point
 * RETURNS: 0 if all moves should finish in time
 *          1 if any of the moves won't finish in time
 */
int controller::followTrajectory(Trajectory const t)
{
	int i, j, rcode=0;
	Coordinate tc;

	for (i=0; i<t.numcoords; i++) {
		// copy coord into a temporary vector
		for (j=0; j<7; j++)
			tc[j] = t.coord[i][j];
		// convert from time to delta time
		if (i >= 1) 
			tc[0] = t.coord[i][0] - t.coord[i-1][0];
		// perform the move
		rcode |= moveAbsolute(tc);
	}

	return rcode;
}




/* controller::checkStatus
 * check the motion control board for any return messages
 * if there are any, assume there was an error and set the
 * errorflag.  put all error messages in the errorstring
 * RETURNS: value of errorflag after the check
 */
int controller::queryBoardStatus(void)
{
	char line[1024];

	if (devicefile) {
		fgets(line, 1023, devicefile);
		while (!feof(devicefile)) {
			errorflag = 1;
			sprintf(errorstring, "OMS board returned the following error(s):\n");
			if (strlen(errorstring) + strlen(line) < MAX_ERRORLEN) {
				strcat(errorstring, line);
				strcat(errorstring, "\n");
			}
			fgets(line, 1023, devicefile);
		}
	}

	return errorflag;
}



/* controller::getError
 * RETURNS: the status of the error flag
 */
int controller::getError(void)
{
	return errorflag;
}




/* controller::getErrorString
 * passes back the contents of the error string
 * ASSUMES: _enough_ space is already allocated 
 *          for the return string
 */
void controller::getErrorString(char *errstr)
{
	strcpy(errstr, errorstring);
}

char const * controller::getErrorString()
{
	return errorstring;
}


/* controller::followSmoothTrajectory
 * Follow a trajectory of points
 * Unlike followTrajectory, this function implements a "piecewise
 * biquadratic" (explained in log book) interpolation of the data
 * points. This results in motion with no stops (except at the
 * very beginning and end of the trajectory), and no velocity
 * discontinuities.  It also provides Bezier type derivative constraints
 * without the need for third order polynomials.
 * NOTES: relies on the followQuadratic() function for the piecewise moves.
 *            this function is responsible for recognizing changes in sign
 *            of velocity and breaking the segment into smaller pieces.
 *        loop unrolling for comm. optimization is NOT implemented yet
 */
void controller::followSmoothTrajectory(Trajectory t)
{
	for (int i = 0; i < (t.numcoords - 1); i++)     // for all points in the trajectory
		*this << t[i];								// output one trajectory point
	stopMVs();										// stop motion at the end
}

//-----------------------------------------------------------------------------
// operator <<
// 		Outputs commands to the controller
// 	Inputs: coordinate of the next point
//  Outputs:the same controller object
//	Side-effect: commands sent to the board to move to the PREVIOUS point
//  	The function always lags by one point.  
//-----------------------------------------------------------------------------
/*
controller& controller::operator << (Coordinate& const x_next)
{
	static Coordinate v_prev;

	double	  deltat, deltat2;				// useful for calculations
	Coordinate 		 deltax, x_mid
				, v, deltav, v_mid
				, accel ; 					// gen. purpose temp. coordinate

	if (firstPoint){
		firstPoint = false;
		x_prev = x = x_next;				 // set all to the first point
		x     [DT] = x_next[DT];   
		x_prev[DT] = x     [DT];   
		v_prev = 0.0;                        // start with zero velocity
	}

	deltax  = x - x_prev;
	deltat  = x[DT];	
	if( deltat <= 0.0 ) {					 // assert deltat > 0.0
		setError("Time increments must be greater than zero.");
		return *this;
	}
	deltat2 = deltat*deltat;

#if SPLINE
	double _2dt;
	if( ( _2dt = deltat + x_next[DT] ) <= 0.0 ) {
		setError("Time increments must be greater than zero (2).");
		return *this;
	}
	v = (x_next - x_prev) / _2dt;
#else
	v = (x_next - x ) / deltat;
#endif
	deltav = v - v_prev; 

	accel = 4.0*deltax/deltat2 -(v_prev + 3.0*v)/deltat;
	x_mid = x_prev + deltax/2.0 - 3.0/8.0*deltav*deltat;
	v_mid = v_prev + accel*deltat/2.0;
try{	
	// move   from   x_prev to x_mid with final velocity v_mid, acceleration accel
	follow1Parabola( x_prev, x_mid, v_prev, v_mid, accel ); 

	accel = 4.0*deltax/deltat2-(3.0*v_prev + v)/deltat ;
	// move   from   x_mid to x with final velocity v and acceleration accel
	follow1Parabola( x_mid , x    , v_mid, v     , accel );
}
catch(...){
	setError("Exceptions caught in operator << ");
}
	v_prev = v;
	x_prev = x;
	x      = x_next;

	return *this;
}
*/

template <class T>
T sign(T x){
	if( x > (T)0 ) return (T) 1;
	if( x < (T)0 ) return (T)-1;
	if( x ==(T)0 ) return (T) 0;
	return x;   								// in case of NaN
};

#define amax 2000
#define vmax  100

controller& controller::operator << (Coordinate& const x)
{
	static Coordinate x_prev(0.0), v_prev(0.0);

	Coordinate v, a;
	double	   deltat;						// useful for calculations
	
	deltat  = x[DT];	
	if( deltat <= 0.0 ) {					 // assert deltat > 0.0
		setError("Time increments must be greater than zero.");
		return *this;
	}

	v = ( x - x_prev ) / deltat;
	a = ( v - v_prev ) / deltat;

	for(int i = X; i < COORD_SIZE; ++i) {
		if( abs(a[i]) > amax ){
			a[i] = sign(a[i])*amax;    
			v[i] = v_prev[i] + a[i]*deltat;
			x[i] = x_prev[i] + v[i]*deltat;
		}
	}

	for(int i = X; i < COORD_SIZE; ++i) {
		if( v[i] > maxVelocity ){
			v[i] = sign(v[i])*maxVelocity;
			x[i] = x_prev[i] + v[i]*deltat;
		}
	}

	for(int i = X; i < COORD_SIZE; ++i)
		if( 	x[i] == x_prev[i]      		 // if it is a stationary 			
			||  v[i]*v_prev[i] < 0.0 )       // .. or an inflection point
			stopAt(i, x_prev[i]);                 // stop at that point
		else
			moveVelocity(i, x_prev[i], v_prev[i], amax);	// otherwise keep moving
	
	v_prev = v;
	x_prev = x;

	return *this;


}
//--------------------------------------------------------------------------
// finish the trajectory by sending it's last point to the controller
// a couple of times more
//--------------------------------------------------------------------------
void controller::stopMVs()
{
	sendCommand("* stopping");
	*this << x;							// send the same point
	*this << x;							// send the same point
}

#pragma argsused
void controller::follow1Parabola( Coordinate from,   Coordinate to
								, Coordinate v_from, Coordinate v_to
								, Coordinate accel)
{
	double x_stop;

	for( int i = X; i < COORD_SIZE; i=i+1 ) {
		if (v_from[i]*v_to[i] >= 0.0)    // no velocity inflection - let's do it!
			moveVelocity(i, to[i], v_to[i], accel[i]);
		else {							// split into positive and negative going
			// find the point where the speed changes sign
			try{
			x_stop = from[i] - 0.5 * v_from[i]*v_from[i] / accel[i];
			} catch(...) {
				setError("Zero acceleration");
				throw(int(0));
			}
			moveVelocity(i, x_stop, 0.0,     accel[i]);
			moveVelocity(i, to[i] , v_to[i], accel[i]);
	   }
	}
}

void controller::moveVelocity(int axis
					, double position, double endvelocity, double accel)
{
// possible bug: what if rounded velocity is zero?
	static int prev_pos[COORD_SIZE];
	int pos = (int)fround(position);
	char direction = ( pos - prev_pos[axis] >= 0 ? 'P' : 'M');
	prev_pos[axis] = pos;
	char cmdstr[256];
	sprintf(cmdstr, "A%c AC%.2f M%c MV%d,%.2f "
				  , axisChar[axis]                   		// axis
				  , fabs(accel)                				// acceleration
				  , direction								// direction
				  , pos					                    // position
				  , fabs(endvelocity)          				// final velocity
	);
	sendCommand(cmdstr);
}

void controller::stopAt(int axis, double position)
{
	char cmdstr[256];
	sprintf(cmdstr, "A%c SP%d", axisChar[axis], (int)position ); 
	sendCommand(cmdstr);
}


/* controller::follow2Parabolas
 * takes in arrays of coefficients for 2 quadratic segments as well as
 * the initial velocities and generates motion commands for the two segments.
 * NOTES: responsible for checking queue availability for all axes
 *        responsible for finding velocity inflections and breaking the
 *          quadratics into smaller pieces
 *        responsible for recalculating timing if  pieces broken up
 */
void controller::follow2Parabolas(Coordinate v_i, Coordinate v_f
						, float ti, float deltat
						, Coordinate a, Coordinate b, Coordinate c,
						  Coordinate d, Coordinate e, Coordinate f)
{
	Coordinate midv;
	Coordinate accel;

	// query board for remaining queue space - returns min of all 8 queues
	// wait for 32 words to be open before continuing
	// while (getQueueSpace() < 32);

	followParabola(v_i, a, b, c, ti, deltat/2.0);  // do the first parabola

	// calculate velocities after 1st and before 2nd parabola
	midv = v_i + a*(deltat/2.0*2.0);				// midv = v + 2at
	
	followParabola(midv, d, e, f, ti+deltat/2.0, deltat/2.0);   // do the second parabola
}


// follow a parabolic path on all axes
// if there is a velocity inflection, break into two parabolas with a
// stop command in the middle (required by motion board)
void controller::followParabola(Coordinate v_i,
					Coordinate a, Coordinate b, Coordinate c, 
					float ti, float deltat)
{
	int k;
	float accel, endp, endv, t, tp;
	char cmdstr[64], dir;
	
	for (k = 1; k <= NUM_AXES; k++) {
	   accel = 2*a[k];
	   endp = a[k]*deltat*deltat + b[k]*deltat + c[k];
	   endv = v_i[k] + accel*deltat;
	   // set direction character according to initial velocity
	   (v_i[k] > 0.0) ? dir = 'P' : dir = 'M';
														
	   // check for velocity inflection 
	   if (endv*v_i[k] > 0.0) {   // no velocity inflection - let's do it!
		  sprintf(cmdstr, "A%c AC%d M%c MV%d,%d ",
						   axisChar[k], (int)fround(fabs(accel)), dir,
						   (int)fround(endp), (int)fround(fabs(endv))  );
		  sendCommand(cmdstr);
	   }
	   else {                     						// take care of velocity inflection
		  t = -b[k]/(2.0*a[k]);          				// find time where v = 0;
		  tp = a[k]*t*t + b[k]*t + c[k];          		// find position where v = 0;

		  sprintf(cmdstr, "A%c AC%d SP%d ",          	// come to stop at point of inflection
						  axisChar[k], (int)fround(fabs(accel)), (int)fround(tp) );
		  sendCommand(cmdstr);
		  // figure out direction for second half of quadratic
		  (endv > 0.0) ? dir = 'P' : dir = 'M';
		  sprintf(cmdstr, "M%c MV%d,%d ",
						  dir, (int)fround(endp), (int)fround(fabs(endv))  );
		  sendCommand(cmdstr);
	   }  // end if-else
	}  // end "for each axis" loop
}



/* controller::stopMVs
 * stop any current MV activity.  takes the current position and velocities
 * to figure reasonable stopping distances
 */
void controller::stopMVs(Coordinate p, Coordinate v)
{
	int accel = maxAcceleration/2, k;
	float endp, t;
	char cmdstr[64];

	for (k = 1; k <= NUM_AXES; k++) {
		t = v[k]*1.2/accel;
        endp = 0.5*accel*t*t + v[k]*t +p[k];
        // leave some extra stopping space
        (v[k] > 0.0) ? endp += v[k]*t*0.1 : endp -= v[k]*t*0.1;
        sprintf(cmdstr, "A%c SP%d",
                        axisChar[k], endp);
		sendCommand(cmdstr);
    }
}



/* controller::getQueueSpace
 * RETURNS: the space remaining in the most full queue.  I.e., if it
 *          returns 46, that means that each queue has room for at least
 *          46 more words.
 */
int controller::getQueueSpace(void)
{
    int c, i, q[NUM_AXES], least=200;
    char instr[128];

    sendCommand("AA RQ");
    // skip initial line feed and carraige return, exitting on errors
    while ( (c=getc(devicefile)) != '\n' ) {
        switch (c) {
               case EOF : break;
			   case '#' :
               case '@' : exit(0);
        }
    }
    while ( (c = getc(devicefile)) != '\r' );

	// read the rest of the string
    i = 0;
    do {
        while ((c = getc(devicefile)) == EOF);
        instr[i] = (char)c;
        i++;
    } while (c != '\n');
    instr[i] = '\0';

    for (i = 0; i < NUM_AXES; i++)
        q[i] = 200;

    i = sscanf(instr, "%d,%d,%d,%d,%d,%d", &q[0], &q[1], &q[2],
                                           &q[3], &q[4], &q[5]);
	if (i != 6)
      exit(0);

	for (i = 0; i < NUM_AXES; i++)
        if (q[i] < least)
             least = q[i];

    return least;
}




 /*
 Between each two points we fit two quadratics. At each point the slope is
calculated from the difference between the two neighboring points (the same
as a Bezier curve) and the quadratics pass through every point. The switch
from the first quadratic to the second quadratic happens halfway in time
between the two points, where I constraint the two parabolas to meet with
the same slope.  I call the first quadratic at^2+bt+c and the second one
dt^2+et+f, all in absolute coordinates and time since the beginning of the
trajectory.

The trajectory x(t) travels through two points, x0 = 0, x1 = dx,
for the time T 
at the inital velocity v0 and final velocity v1.

x(t) = x1(t) = at^2+bt+c,   for t=0..T/2
x(t) = x2(q) = dq^2+eq+f,   for t=T/2..T, where q = T-t

Conditions are:
x1(0) = 0, x2(0) = dx   		// initial and final point
x1'(0) = v0, x2'(0) = v1        // initial and final velocities
x1(T/2) = x2(T/2)				// the midpoint is the same for both functions
x1'(T/2) = x2'(T/2)				// the velocities at midpoint are the same 

=>
a = -(3v1+ v0)/2T + 2dx/T^2, b = v0, c = 0
d =  ( v1+3v0)/2T - 2dx/T^2, e =-v1, f = dx
=>
x (T/2) = dx/2 - 3/8*(v1-v0)T	// position at mid-point
x'(T/2) = (-3v1+v0)/2 + 2dx/T   // velocity at mid-point
x''(T/2)= 2*a
*/