#include "msp430x16x.h"  //register and I/O definitions
#include "dwarfcore.h"
#include "trapservo.h"
#include "parser.h"
#define abs(a) (((a)>=0)?(a):(-a))

void trapServoLoop(MotorChannel* mc)
{
long int error=0;
long int dist_remain=0;
long int dist_stop=0;
long int current_velocity=0;
long int PID=0;
long int accel=2;


	/*if(abs((long)(mc->position-mc->goal_position))<6)
	goto exitServoLoop;*/
	dist_remain=mc->goalPosition-mc->lastPosition;
	
    current_velocity = mc->intermediate-mc->lastPosition;
  	dist_stop=0.5*current_velocity*current_velocity/accel;

   //and save for next time
  mc->lastPosition = mc->intermediate;
 	
  if((mc->cmdVel*dist_remain)>=0)
  {
  	// is it time to begin stopping?
  	if((abs(dist_remain)>abs(dist_stop))&(mc->stopFlag==0))
  	{	
  		//keep going, not time to stop
  		if(dist_remain>0)
  		{
  		mc->cmdVel+=accel;
  		if (mc->cmdVel > mc->maxSpeed)mc->cmdVel = mc->maxSpeed;
  		}
  		
  		else
  		{
  		mc->cmdVel-=accel;
  		if (-mc->cmdVel > mc->maxSpeed)mc->cmdVel = -mc->maxSpeed;
  		}	
  	}
  	else
	{ 
		//time to decel
		mc->stopFlag=1;
		if (mc->cmdVel >0) mc->cmdVel -= accel;
		else mc->cmdVel+=accel;
 	}
  }
 else
  {
  	if(mc->cmdVel>0)
  	mc->cmdVel-=accel;
  	else mc->cmdVel+=accel;
  }	
  
  
  //we are very close, just servo to zero error
 if(abs(dist_remain)<20)
  {	
  	mc->cmdVel=0;
  //	stop_flag=2;
  	mc->intermediate=mc->goalPosition;
  
  }
  else
  {
  
   //adjust commanded position
   mc->intermediate += mc->cmdVel;
  }

 
  error = mc->intermediate - mc->position;
 //calc proportional term 
  PID = (mc->gain.P * error);
  
  //add in error to form intergral
  mc->sum = mc->sum + error;
  //check it's limits
  
 if (mc->sum > INT_LIMIT) mc->sum = INT_LIMIT;
  else if ( mc->sum < -INT_LIMIT) mc->sum = -INT_LIMIT;
  //calc the integral portion
 	PID += mc->sum / mc->gain.I;
  
 //  PID += mc->Kd * (error - mc->last_error);
  //update errors
 //	mc->last_error = error;
  PID=PID/5;
  //now output the result
  //if negative then negate result but set dir
  if (PID < 0 )
  {
  P5OUT |= 0x01;
  P5OUT &= 0xfd;
  PID = PID * -1;   //flip the PID value to positive
  }
  else {
  P5OUT &= 0xfc;//set for forward
  }
  
 // if(PID>mc->max_pwm)mc->max_pwm=PID;
 // encoders can't handle much faster than 700 for old demo board
 if (PID > 95) PID = 95;  //must leave some off time for the
                              //the driver to recharge
 //if(PID>mc->max_pwm)mc->max_pwm=PID;                            
	//maintain small dead band to eliminate unnecessary vibration  
	/*                            
 if(((abs((long)(mc->position-mc->goal_position))<8)&(PID<60))) 
 {	
 	PID=0;	
 }
 */
 //update the PWM                           
  TBCCR1 = PID;   
  goto exitServoLoop;
  exitServoLoop:;

}

void getNewPos(MotorChannel* mc)
{
  unsigned int encoderValue;
  int32 delta, newPosition, oldPosition = mc->position;
   _DINT(); _NOP();  
  encoderValue = readDWRFfirst(DCOR0CNTLOW);
  encoderValue |= readDWRFlast()<<8;
  _EINT();
  newPosition = (oldPosition & 0xffff0000) | encoderValue;  //1st guess at it
  delta = newPosition - oldPosition;  //delta = how far we appear to have moved
  if (delta > 0x7fff)                 //too far forward?
   // newPosition -= 0x10000;
   newPosition -= 0xffff;
  else if (delta < -0x7fff)           //too far backward?
   // newPosition += 0x10000;
    newPosition += 0xffff;
  mc->position= newPosition;
  
}
void haltMotor(MotorChannel* mc,unsigned char gMISCout)
{
		gMISCout=0x1f;
		gMISCout &= (unsigned char)(~(DCORMOTORPWR));
		writeDWRF(DCOROUT,gMISCout);
		mc->relative=0;
		startmove(mc->relative,mc);
	
}

void configMotorChannel(MotorChannel* mc)
{

	mc->position=0;
	getNewPos(mc);
	mc->lastPosition=mc->position;
	mc->goalPosition=mc->position;
	mc->intermediate=mc->position;
	mc->maxSpeed=100;
	mc->stopFlag=0;
	mc->cmdVel=0;
	mc->gain.P=3;
	mc->gain.D=1;
	mc->gain.I=10;
	mc->sum=0;
	mc->lastError=0;
	mc->accel=2;
	
}
void configureMotorChannel(MotorChannel* mc, ServoConfigureReply* scr)
{
	mc->optionMask=scr->optionMask;
	mc->gain=scr->gain;
	mc->maxSpeed=scr->maxSpeed;	//does this need to be 24bits??
	mc->minSpeed=scr->minSpeed;
	mc->maxPositionErr=scr->maxPositionErr;
	mc->maxForce=scr->maxForce;
	mc->maxPressureDelta=scr->maxPressureDelta;
	mc->maxPressure[2]=scr->maxPressure[2];
	mc->minPressure[2]=scr->minPressure[2];	
}

void getMotorConfig(MotorChannel* mc,ServoConfigureReply* scr)
{
	scr->optionMask=mc->optionMask;
	scr->gain=mc->gain;
	scr->maxSpeed=mc->maxSpeed;	//does this need to be 24bits??
	scr->minSpeed=mc->minSpeed;
	scr->maxPositionErr=mc->maxPositionErr;
	scr->maxForce=mc->maxForce;
	scr->maxPressureDelta=mc->maxPressureDelta;
	scr->maxPressure[2]=mc->maxPressure[2];
	scr->minPressure[2]=mc->minPressure[2];	

}
void startmove(long int relative,MotorChannel* mc)
{
  getNewPos(mc);	//get current position
  mc->lastPosition=mc->position;
  mc->sum=0;
  mc->goalPosition = mc->position + relative;	//calculate goal position
  
  mc->stopFlag=0;
 
}