#include "msp430x16x.h"  //register and I/O definitions
#include "dwarfcore.h"
#include "trapservo.h"
#include "parser.h"
#include "cpld.h"
#include "espdwarf.h"
#include "i2cnode.h"
#include "jogservo.h"


void ServoDoNothing(MotorChannel* mcChan)
{
 updateAvgSpeed(mcChan);
}


static inline void powerDown(void)
{
  //clear_cpld_bit(DCOROUT,DCORMOTORPWR);  
  _DINT();_NOP();
   outImage&= ~DCORMOTORPWR;
   updateOutImage();
}


static void coast(MotorChannel *mcChan)
{
   if(mcChan->channel==0){
     TBCCR1 = 0;
     MoveType0 = ServoDoNothing;
     if (!TBCCR2 && MoveType1 == ServoDoNothing) powerDown();
   }
   else{
     TBCCR2 = 0;
     MoveType1 = ServoDoNothing;
     if (!TBCCR1 && MoveType0 == ServoDoNothing) powerDown();
   }
}


static void clearServo(MotorChannel *mcChan)
{
   mcChan->intermediate = mcChan->position;
   mcChan->goalPosition = mcChan->position;
   mcChan->cmdVel = 0;
   mcChan->sum = 0;
   mcChan->PIDavg = 0;
   mcChan->avgSpeed = 0;
   mcChan->settleCounter=0;
   clear8(mcChan->statusMask, servoOn);
}


void servoOff(MotorChannel* mcChan, int pwm) 
{
uint16 servoBit;
MASK_TIMER_INTERRUPTS();
   
   if (mcChan->goalPosition!=mcChan->intermediate)
   {
     //Flag the failure
     mcChan->reason = servoAborted;     
     servoBit=servo0<<mcChan->channel;
     set16(opComplete,servoBit);//Signal not sent yet so send it!
     signalEV();
   }
   
   clearServo(mcChan);
   if (pwm) {
     if (pwm < 0)
     {
       set8(DCORMOTOROUT,(DCOR0DIR)<<mcChan->channel*2);
       clear8(DCORMOTOROUT,(DCOR0BRAKE)<<mcChan->channel*2);  
       pwm = -pwm;   //flip the pwm value to positive
     }
     else 
     { 
      clear8(DCORMOTOROUT,(DCOR0DIR|DCOR0BRAKE)<<mcChan->channel*2);  
     }
     if (mcChan->channel==0)
       TBCCR1 = pwm;
     else
       TBCCR2 = pwm;
   }else
     coast(mcChan);   
UNMASK_TIMER_INTERRUPTS();
}

void errorOut(MotorChannel* mcChan)
//caller must ensure that servo timer cannot interrupt this
{
   uint16 servoBit;
   if(mcChan->channel==0)
   {                       
     TBCCR1 = 0; //don't flag error if servo is off
     if (MoveType0 == ServoDoNothing) return;   
     MoveType0 = ServoDoNothing;
   }
   else
   {
     TBCCR2 = 0; //don't flag error if servo is off
     if (MoveType1 == ServoDoNothing) return;   
     MoveType1 = ServoDoNothing;
   }
   
   clearServo(mcChan);
   coast(mcChan);

   servoBit=servo0<<mcChan->channel;
   set16(opComplete,servoBit);//Signal not sent yet so send it!
   signalEV();
}

void MotorOverCurrent(MotorChannel* mcChan)
{
    MASK_TIMER_INTERRUPTS();  //ADC may be asynchronous with servo sampling
    mcChan->reason = servoOvercurrent;     
    errorOut(mcChan);
    UNMASK_TIMER_INTERRUPTS();  
}

void MotorPositionError(MotorChannel* mcChan, enum servoReason reason)
//caller must ensure that servo timer cannot interrupt this
{
      mcChan->reason = reason;
      errorOut(mcChan);
}

void MotorPressureError(MotorChannel* mcChan, long int stop_distance)
//caller must ensure that servo timer cannot interrupt this
{
   mcChan->reason = servoPressureError;   
//update goal to where we are now!   
   mcChan->goalPosition = mcChan->intermediate+stop_distance;
   //reset the settle counter
   mcChan->settleCounter = mcChan->maxSettling;
}

void trapServoLoop(MotorChannel* mcChan)
{
int32 dist_remain=mcChan->goalPosition-mcChan->intermediate-mcChan->cmdVel;
int32 dist_stop=(((int32)mcChan->cmdVel+(int32)mcChan->accel)*
                        (int32)mcChan->cmdVel)/(2*(int32)mcChan->accel);
int32 absError = abs(mcChan->goalPosition - mcChan->position);
int32 velocity = updateAvgSpeed(mcChan);
int32 PIDerror = mcChan->intermediate - mcChan->position; 
 
  if(!mcChan->cmdVel || !dist_remain ||
     !((mcChan->cmdVel ^ (int16)(dist_remain>>16))&0x8000))  //same signs?
  {
  	// is it time to begin stopping?
  	if((abs(dist_remain)>abs(dist_stop)))
  	{	
  		//keep going, not time to stop
  		if(dist_remain>0)
  		{
  		  mcChan->cmdVel+=mcChan->accel;
  		  if (mcChan->cmdVel > mcChan->maxSpeed)
  		     mcChan->cmdVel = mcChan->maxSpeed;
    		}
  		else
  		{
  		  mcChan->cmdVel-=mcChan->accel;
  		  if (-mcChan->cmdVel > mcChan->maxSpeed)
  		     mcChan->cmdVel = -mcChan->maxSpeed;
  		}	
  	}
  	else
	{ 
		//time to decel
		if (mcChan->cmdVel >0) mcChan->cmdVel -= mcChan->accel;
		else mcChan->cmdVel+=mcChan->accel;
 	}
  }
 else
  {
  	if(mcChan->cmdVel>0)
  	  mcChan->cmdVel-=mcChan->accel;
  	else
          mcChan->cmdVel+=mcChan->accel;
  }	
  
  
  //we are very close, just servo to zero error
  {
    uint16 servoBit=servo0<<mcChan->channel;
    if(abs(dist_remain) <= mcChan->accel)
    {
        //Force cmdVel to 0 so it is exactly 0
       mcChan->cmdVel=0;
        //set interm. and goal = which stops trajectory
  	   mcChan->intermediate=mcChan->goalPosition;
  	   PIDerror = mcChan->intermediate - mcChan->position;
        //check if time to send signal of completion
       if (mcChan->settleCounter)
       {
        //check if the servo has slowed down
        if ( mcChan->avgSpeed < 2 && absError <= mcChan->maxPositionErr/2 )
          // if so, end move NOW if the error settled (well) into the window
               {
               //used for debugging during implementation
//           reportErr("clearing settleCounter, was %01d", mcChan->settleCounter);
           mcChan->settleCounter = 1;  //compensate for decrement following this
               }
        mcChan->settleCounter--;
       } //end of settleCounter != 0
      else
      {
        //the settling time has run out, if the servo ever moves
        //beyond these bounds flag the error
       if ( absError > mcChan->maxPositionErr)
        {
         reportErr("pos error");
         MotorPositionError(mcChan, servoPositionErr);
         return;    
        }
       if(!(opComplete & servoBit) && validTag(mcChan->tag) )
        { //close enough, report move done if we haven't already
    	 opComplete |= servoBit;
      	 signalEV();
        } 
      }//end of settleCounter == 0
     }//end of, end-of-move 

  else  //moving on a trajectory...
  {  
   if (abs(PIDerror) > 2*mcChan->maxSpeed)
   {
      reportErr("trajectory error");
      MotorPositionError(mcChan, servoTrajectoryErr);
      return;    
   }  
   //adjust commanded position
   mcChan->intermediate += mcChan->cmdVel;
   PIDerror += mcChan->cmdVel;
  }
 }
 if( mcChan->inPressure >= mcChan->maxInPressure-1 ||
     mcChan->outPressure >= mcChan->maxOutPressure-1 ||
     mcChan->inPressure < mcChan->minInPressure ||
     mcChan->outPressure < mcChan->minOutPressure )
  {
    if (!(mcChan->flags & pressureErrorSent))
      {
       mcChan->flags |= pressureErrorSent;  //store that this error has been responded to
       reportErr("pressure error");
       MotorPressureError(mcChan, dist_stop);     
      } 
  }
  //Add a condition for delta pressure in the future
  
  {
   //calc proportional term 
    int32 PID = mcChan->gain.P * PIDerror;
    //calc deriv term 
    //mcChan->DERRavg = (mcChan->DERRavg +(PIDerror - mcChan->lastError))/2;

    PID += mcChan->gain.D * (PIDerror - mcChan->lastError); //mcChan->DERRavg;

     //add in error to form intergral
    //if I term == 0 then skip I

    if (mcChan->gain.I)
    {
        if (abs(PID) > ACT_LIMIT) 
                 mcChan->sum = 0;
        else {  //if stopped near goal, don't allow integrator to wind up
           if (mcChan->settleCounter || abs(velocity)>1)
             mcChan->sum = mcChan->sum +  PIDerror;
           PID +=  mcChan->sum * mcChan->gain.I;
        }
    }
    else mcChan->sum = 0;

    //update errors
    mcChan->lastError = PIDerror;
    PID/=PIDSCALE;
    mcChan->PIDavg += PID;
    mcChan->PIDavg /= 2;
  
    //now output the result
    //if negative then negate result but set dir
    if (PID < 0 )
    {
      set8(DCORMOTOROUT,(DCOR0DIR)<<mcChan->channel*2);
      clear8(DCORMOTOROUT,(DCOR0BRAKE)<<mcChan->channel*2);  
      PID = -PID;   //flip the PID value to positive
    }
    else 
    { 
     clear8(DCORMOTOROUT,(DCOR0DIR|DCOR0BRAKE)<<mcChan->channel*2);  
    }

    if( abs(mcChan->PIDavg)>VEL_LIMITING_HIGH)
    {
      mcChan->flags |= limited;
      mcChan->maxSpeed =  mcChan->avgSpeed;

      if((mcChan->maxSpeed<mcChan->minSpeed)&&
         (mcChan->goalPosition!=mcChan->intermediate))
      {
        mcChan->reason = servoSpeedError;     
        errorOut(mcChan);
      }
    }
    else if((mcChan->flags & limited)&&(PID<VEL_LIMITING_LOW))
    {
      mcChan->flags &= ~limited;
      mcChan->maxSpeed=mcChan->defaultMaxSpeed;
    }

   // encoders can't handle much faster than 700 for old demo board
   if (PID > MAXPWM) PID = MAXPWM;  //must leave some off time for the
                                //the driver to recharge

   //update the PWM    


   if(mcChan->channel==0)                       
     TBCCR1 = PID;   
   else
     TBCCR2 = PID;  
  }
}


static inline int32 nextPos (int32 previous, uint16 encoderValue)
// return nearest 32-bit position to previous at (current) encoderValue
{
  int32 current = (previous & 0xffff0000) | encoderValue;  //1st guess at it
  int32 delta = current - previous;  //delta = how far we appear to have moved
  if (delta > 0x7fff)                 //too far forward?
   current -= 0x10000; 
  else if (delta < -0x7fff)           //too far backward?
   current += 0x10000;
  return current;
}

void getNewPos(MotorChannel* mc)
{
  uint16 encoderValue0,encoderValue1;
  _DINT(); _NOP();
  encoderValue0 = readDWRFfirst(DCOR0CNTLOW);
  encoderValue0 |= readDWRFnext()<<8;
  
  encoderValue1 = readDWRFnext();
  encoderValue1 |= readDWRFlast()<<8;
  _EINT();
  mc[0].position = nextPos (mc[0].position, encoderValue0);  
  mc[1].position = nextPos (mc[1].position, encoderValue1);
}

int32 getLatchPos(MotorChannel* mc)
{
  uint16 latchValue,latchValue1;
  _DINT(); _NOP();
  latchValue = readDWRFfirst(DCOR0THRESLOW);
  latchValue |= readDWRFnext()<<8;
  
  latchValue1 = readDWRFnext();
  latchValue1 |= readDWRFlast()<<8;
  _EINT();
  if (mc->channel) latchValue = latchValue1;
  return nextPos (mc->position, latchValue);
}

void configMotorChannel(MotorChannel* mc,byte channel)
{
    mc->statusMask = servoLostHome; //have not homed yet
	mc->lastPosition=mc->position;
	mc->goalPosition=mc->position;
	mc->intermediate=mc->position;
        mc->homePosition=0;
	mc->maxSpeed=mc->defaultMaxSpeed=0;
	mc->cmdVel=0;
	mc->gain.P=0;
	mc->gain.D=0;
	mc->gain.I=0;
	mc->sum=0;
	mc->maxCurrent=4096;
	mc->lastError=0;
	mc->accel=0;
	mc->channel = channel;
	mc->optionMask = 0;
	mc->reason = 0;
	mc->inPressure=0;
	mc->outPressure=0;
	mc->flags=0;
	mc->servoTics=0;
    mc->PIDavg = 0;
    mc->avgSpeed = 0;
    mc->settleCounter=0;
}
void initServo(void)
{
 // first turn everything off!
 //encoders
 _DINT();_NOP();
 //clear_cpld_bit(DCORENCPWR,DCOR0ENC5 | DCOR1ENC5);
  encpwrImage &= ~(DCOR0ENC5 | DCOR1ENC5);
 //and motor power
 //clear_cpld_bit(DCOROUT,DCORMOTORPWR);
  outImage &= ~DCORMOTORPWR;
  writeDWRF(DCORENCPWR,encpwrImage);
  writeDWRF(DCOROUT,outImage);
  _EINT();

  
 //PWM to the motors are on channel PWM1 and PWM2.
 //this is so that CCR0 can be used with the timer up/down mode
 //define the two PWM's on 4.1 and 4.2 as outptus and second fucntion
 DCORPWMDIR|=(DCOR1PWM|DCOR2PWM|DCOR3PWM|DCOR5PWM);
 DCORPWMSEL|=(DCOR1PWM|DCOR2PWM|DCOR3PWM|DCOR5PWM);

 //need to define the brake and DIR pins as outputs.
 //This also sets the brake on the motor!
 DCORMOTOROUT =(DCOR1BRAKE|DCOR1DIR|DCOR0BRAKE|DCOR0DIR);
 DCORMOTORDIR |= (DCOR1BRAKE|DCOR1DIR|DCOR0BRAKE|DCOR0DIR);

 //Now set timer B for PWM mode using SMCLK
 //first stop and clear the counter AND USE smclk
 TBCTL = (TBSSEL1 | TBCLR);
 //Then set what to count up to.
 TBCCR0 = PWMmaxCOUNT;  //Which makes timer B divided by 500
 //Then set up compare fucntions
 TBCCTL1 = PWMMODE;
 TBCCTL2 = PWMMODE;
 TBCCTL3 = PWMMODE;
 TBCCTL5 = PWMMODE;
 //Then the width of the pulse for ch 0 SET TO 0
 TBCCR1 = 0;
 //Then the width of the pulse for ch 1 SET TO 0
 TBCCR2 = 0;
 //Set starting pulse width to 0 for heater
 TBCCR3 = 0;
 //Set starting pulse width to 0 for rotary valve
 TBCCR5 = 80;
 //Now startup Timer B IN UP/DOWN MODE
 TBCTL |= (MC1 | MC0);
 //clear interrupt
 //Im not sure what this was for - disableDWRFirqs (DCOR0BRAKE);//do we need this?
 //release the brakes
 clear8(DCORMOTOROUT, DCOR0BRAKE | DCOR1BRAKE); // &=0xfd;
 // dont want to do this here - enableDWRFirqs (DCOR0HOMED);

}


enum servoStatusBits updateStatus(MotorChannel* mc)
{
  enum servoStatusBits *sMask;

  byte limitReg, ifg;
 
  _DINT();_NOP();
  limitReg = readDWRF(DCORLIMITS);
  ifg = readDWRF(DCORIFG);
  _EINT();
  
  sMask = &(mc->statusMask);
  *sMask &= ~(servoRLS|servoFLS|servoHomeFlag|servoPastThreshold);
  
  if(mc->channel)
  {
    if(limitReg &  DCOR1RLS) *sMask |= servoRLS;  //each must be atomic!
    if(limitReg & DCOR1FLS) *sMask |=servoFLS;
    if(limitReg & DCOR1HOME) *sMask |=servoHomeFlag;
    if(ifg & DCOR1PASSED) *sMask |=servoPastThreshold;
  }
  else
  {
    if(limitReg &  DCOR0RLS) *sMask |= servoRLS;  //each must be atomic!
    if(limitReg & DCOR0FLS) *sMask |=servoFLS;
    if(limitReg & DCOR0HOME) *sMask |=servoHomeFlag;
    if(ifg & DCOR0PASSED) *sMask |=servoPastThreshold;
  }
  
  return *sMask;
}


//start either relative or absolute move depending on cmd
void startServoMove(unsigned source, unsigned tag, unsigned cmd,
                    MotorChannel* mcChan, int32 offset)
{
 MASK_TIMER_INTERRUPTS();
 mcChan->cmdType = cmd;    //store away the cmd type for replies
 if(!mcChan->channel && MoveType0==ServoDoNothing ||
     mcChan->channel && MoveType1==ServoDoNothing || (
     cmd != servoRelMoveCmd && 
     mcChan->statusMask&servoLostHome && !mcChan->optionMask&servoOptionJog))
 {
     mcChan->reason = servoNotReady; 
     errorOut(mcChan);    
 } 
 else
 {
   byte iMask, cMask;
   _DINT();  //Mask ALL servo interrupts
   mcChan->tag = tag;
   mcChan->source = source;
   iMask = DWRFIEimage;
   disableDWRFirqs(DCOR1HOMED|DCOR1PASSED|DCOR0HOMED|DCOR0PASSED);
   
   //record new values
   //homePosition must be zero if servoLostHome!
   offset += cmd==servoRelMoveCmd ? mcChan->intermediate : mcChan->homePosition;
   mcChan->goalPosition = offset;
   _EINT();
   
   mcChan->maxSpeed=mcChan->defaultMaxSpeed;
   mcChan->reason=0;
   mcChan->settleCounter = mcChan->maxSettling;
   //need to clear out just in case previous move caused this error
   mcChan->flags &= ~(limited | pressureErrorSent);
   if(mcChan->channel == 0) {
     iMask &= DCOR1HOMED|DCOR1PASSED;
     cMask = DCOR0HOMED|DCOR0PASSED;
     if (mcChan->optionMask & servoOptionJog) {  //homing?
     
       MoveType0 = jogServo;
       _DINT();
       iMask |= DCOR0HOMED;
       encctlImage |= DCOR0HMLATCH;
       
     }else{  //normal move
     
       if (mcChan->optionMask & servoOptionThreshold)
         iMask |= DCOR0PASSED;
       _DINT();
       MoveType0 = trapServoLoop;
       encctlImage &= ~DCOR0HMLATCH;
     }
     
   }else{  //channel 1
   
     iMask &= DCOR0HOMED|DCOR0PASSED;
     cMask = DCOR1HOMED|DCOR1PASSED;
     if (mcChan->optionMask & servoOptionJog) {  //homing?
     
       MoveType1 = jogServo;
       _DINT();
       iMask |= DCOR1HOMED;
       encctlImage |= DCOR1HMLATCH;
       
     }else{  //normal move
     
       if (mcChan->optionMask & servoOptionThreshold)
         iMask |= DCOR1PASSED;
       _DINT();
       MoveType1 = trapServoLoop;
       encctlImage &= ~DCOR1HMLATCH;
     }    
   }
   writeDWRF (DCORENCCTL,encctlImage);
   writeDWRF (DCORIFG, cMask); //clear pending servo interrupts for this channel
   enableDWRFirqs (iMask);     //enable ints and restore other channel's
   _EINT();
 }
 UNMASK_TIMER_INTERRUPTS();
}