#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"
#include "servobits.h"

#define abs(a) (((a)>=0)?(a):(-a))
//#define validTag(tag) ((unsigned)((tag)-1) < (unsigned)0x7f)

void ServoDoNothing(MotorChannel* mcChan)
{
 //at least make sure no PWM is going out!
 if(mcChan->channel==0)                       
  TBCCR1 = 0;   
 else
  TBCCR2 = 0;  
}

void servoOff(MotorChannel* mcChan) 
{
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();
    }
   
   mcChan->intermediate = mcChan->position;
   mcChan->goalPosition = mcChan->position;
   mcChan->cmdVel = 0;
   mcChan->sum = 0;
   mcChan->previousSum=0;

   
   if(mcChan->channel==0){
   MoveType0 = ServoDoNothing;
   if (MoveType1 == ServoDoNothing)  
     clear_cpld_bit(DCOROUT,DCORMOTORPWR);  
   }
   else{
   MoveType1 = ServoDoNothing;
    if (MoveType0 == ServoDoNothing)  
     clear_cpld_bit(DCOROUT,DCORMOTORPWR);   
   }
   clear8(mcChan->statusMask, servoOn);
UNMASK_TIMER_INTERRUPTS();

}

void errorOut(MotorChannel* mcChan)
{
 uint16 servoBit;
   MASK_TIMER_INTERRUPTS();
   if(mcChan->channel==0)
   {                       
     TBCCR1 = 0;   
     MoveType0 = ServoDoNothing;
   }
   else
   {
     TBCCR2 = 0;  
     MoveType1 = ServoDoNothing;
   }
   
   mcChan->intermediate = mcChan->position;
   mcChan->goalPosition = mcChan->position;
   clear8(mcChan->statusMask, servoOn);
   
   if(mcChan->channel==0){
   if (MoveType1 == ServoDoNothing)  
     clear_cpld_bit(DCOROUT,DCORMOTORPWR);  
   }
   else{
    if (MoveType0 == ServoDoNothing)  
     clear_cpld_bit(DCOROUT,DCORMOTORPWR);   
   }
   mcChan->sum=0;
   mcChan->previousSum=0;
   servoBit=servo0<<mcChan->channel;
   set16(opComplete,servoBit);//Signal not sent yet so send it!
   signalEV();
   
   UNMASK_TIMER_INTERRUPTS();

}
void MotorOverCurrent(MotorChannel* mcChan)
{
    mcChan->reason = servoOvercurrent;     
    errorOut(mcChan);
   
}
void MotorPositionError(MotorChannel* mcChan)
{
    //if (!(mcChan->optionMask & servoOptionJog))
    //{
      mcChan->reason = servoPositionErr;
      errorOut(mcChan);
   // }
  
}

void trapServoLoop(MotorChannel* mcChan)
{
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 derivTerm=0;
long int accel=2;
long int intergralTerm=0;
uint16 servoBit;
	dist_remain=mcChan->goalPosition-mcChan->intermediate-mcChan->cmdVel;
    current_velocity = mcChan->intermediate-mcChan->lastIntermediate;
 	dist_stop=(((long int)mcChan->cmdVel+(long int)mcChan->accel)*(long int)mcChan->cmdVel)/(2*(long int)mcChan->accel);
 	
   //and save for next time
  mcChan->lastIntermediate = mcChan->intermediate;
  if((mcChan->cmdVel * dist_remain)>=0)
  {
  	// 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
  
    if((abs(dist_remain)<2*mcChan->accel))
    {
     // if(abs(mcChan->cmdVel)<2*mcChan->accel)
  	 // {	
     mcChan->cmdVel=0;
  	 mcChan->intermediate=mcChan->goalPosition;
     servoBit=servo0<<mcChan->channel;
    
      if(((opComplete & servoBit)== 0) && (mcChan->tag < 128) && (mcChan->tag != 0) ) //MSB not set yet
      {
   	   set16(opComplete,servoBit);//Signal not sent yet so send it!
      	  signalEV();
      }
    
   //}
  }
  else
  {
  
   //adjust commanded position
   mcChan->intermediate += mcChan->cmdVel;
  }

 
  error = mcChan->intermediate - mcChan->position;
 
  if (abs(error) > mcChan->maxPositionErr)
   {
   reportErr("pos error");
   MotorPositionError(mcChan);     
   }

 //calc proportional term 
  PID = (mcChan->gain.P * error);
  //calc deriv term 
  PID += mcChan->gain.D * (error - mcChan->lastError);
  
   //add in error to form intergral
  //if I term == 0 then skip I
 
   if (mcChan->gain.I != 0)
   {
      if (abs(error) > mcChan->maxPositionErr/16)
              mcChan->sum = mcChan->sum +  error;
       //check it's limits
      PID +=  mcChan->sum * mcChan->gain.I ;
      if(PID>0)
      {
        if ((PID)  > ACT_LIMIT)
        {
        PID=ACT_LIMIT;
        mcChan->sum = mcChan->previousSum;
       }
        
      }
      else
      {
        if ((PID)  < -ACT_LIMIT)
        {
        PID=-ACT_LIMIT;
        mcChan->sum = mcChan->previousSum;
       }
        
      } 
     
      
     
  }
  else mcChan->sum = 0;
  
  mcChan->previousSum=mcChan->sum;
  //update errors
  mcChan->lastError = error;
  PID=PID/PIDSCALE;
  //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 * -1;   //flip the PID value to positive
  }
  else 
  { 
   clear8(DCORMOTOROUT,(DCOR0DIR|DCOR0BRAKE)<<mcChan->channel*2);  
  }
  
  if(PID>85)
  {
  mcChan->limitedFlag=true;
  if((mcChan->position-mcChan->lastPosition)<0)
  {
  mcChan->maxSpeed=-1*(mcChan->position-mcChan->lastPosition);
  }
  else{mcChan->maxSpeed=(mcChan->position-mcChan->lastPosition);}
  if((mcChan->maxSpeed<mcChan->minSpeed)&&(mcChan->goalPosition!=mcChan->intermediate))
  {
    mcChan->reason = servoSpeedError;     
    errorOut(mcChan);
  }
  }
  else if((mcChan->limitedFlag)&&(PID<75))
  {
  mcChan->limitedFlag=false;
  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;  
   } 
   
     mcChan->lastPosition=mcChan->position;
 
}

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);
}

void 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;
  mc->latchPosition = nextPos (mc->position, latchValue);
 reportErr ("Latch Pos %lx", mc->latchPosition);
 reportErr ("Pos %lx",mc->position);
}
void configMotorChannel(MotorChannel* mc,byte channel)
{
   	mc->lastIntermediate=mc->position;
	mc->lastPosition=mc->position;
	mc->goalPosition=mc->position;
	mc->intermediate=mc->position;
	mc->maxSpeed=mc->defaultMaxSpeed=0;
	mc->stopFlag=0;
	mc->cmdVel=0;
	mc->gain.P=0;
	mc->gain.D=0;
	mc->gain.I=0;
	mc->sum=0;
	mc->previousSum=0;
	mc->maxCurrent=4096;
	mc->lastError=0;
	mc->accel=0;
	mc->channel = channel;
	mc->statusMask=0;
	mc->optionMask = 0;
	mc->reason = 0;
	mc->pressure1=0;
	mc->pressure2=0;
	mc->pressureSenseSwitch=0;
	mc->servoTics=0;
    set8(mc->statusMask,servoLostHome); //have not homed yet
}
void initServo(void)
{
 // first turn everything off!
 //encoders
 clear_cpld_bit(DCORENCPWR,DCOR0ENC5 | DCOR1ENC5);
 //and motor power
 clear_cpld_bit(DCOROUT,DCORMOTORPWR);
 
  
 //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);

}

int32 CalculateDerivativeTerm(MotorChannel* mc, byte channel,int32 error)
{
    int32 deltaTnum=1;
    int32 delatTden=32;
    int32 lambdaNum=32;
    int32 lambdaDen=100;
    int32 CFnum=100;
    int32 CFden=1124;
    int32 newDerivativeTerm;
     newDerivativeTerm=0;
   
  // newDerivativeTerm= mc[channel].lastDerivTerm - (CFnum*mc[channel].lastDerivTerm/CFden)+ (CFnum*(error-mc[channel].lastError)/CFden);
    return newDerivativeTerm;

}
enum servoStatusBits updateStatus(MotorChannel* mc)
{
 byte limitReg;
 enum servoStatusBits tempMask = 0;
 
 
  _DINT();_NOP();
  limitReg = readDWRF(DCORLIMITS);
  _EINT();
  
  if(mc->channel)
  {
  if(limitReg &  (DCOR1RLS)) tempMask|=servoRLS;
  if(limitReg & (DCOR1FLS)) tempMask|=servoFLS;
  if(limitReg & (DCOR1HOME)) tempMask|=servoHomeFlag;
  }
  else
  {
  if(limitReg &  DCOR0RLS) tempMask|=servoRLS;
  if(limitReg & DCOR0FLS) tempMask|=servoFLS;
  if(limitReg & DCOR0HOME) tempMask|=servoHomeFlag;
  }
  
  return mc->statusMask |= tempMask;  //must be an atomic operation -- verify this
}

int32 getPosition (MotorChannel *mc)
{
  int32 pos;
  unsigned oldSR = _BIC_SR (GIE);  //ensure interrupts are disabled
  pos = mc->position;
  _BIS_SR (oldSR);    //restore GIE state
  return pos;
}

void startAbsMove(MotorChannel* mcChan, int32 newPosition,
                unsigned tag, unsigned source)
{
uint16 servoBit;

MASK_TIMER_INTERRUPTS();
 mcChan->cmdType = servoAbsMoveCmd;    //store away the cmd type for replies
 if((!mcChan->channel)&&(MoveType0==ServoDoNothing)|
  ((mcChan->channel)&&(MoveType1==ServoDoNothing)))
 {
     clear8(mcChan->statusMask, servoOn);
     //Flag the failure
     mcChan->reason = servoNotReady;
     servoBit=servo0<<mcChan->channel;
     set16(opComplete,servoBit);//Signal not sent yet so send it!
     signalEV(); 
 }
 else if((mcChan->statusMask&servoLostHome)&&(!(mcChan->optionMask & servoOptionJog)))
 {
     //Flag the failure
     mcChan->reason = servoNotReady;     
     servoBit=servo0<<mcChan->channel;
     set16(opComplete,servoBit);//Signal not sent yet so send it!
     signalEV();
  
 } 
 else
 {
   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();
   }
   
   //clear threshold and record new values
   clear8(mcChan->statusMask,servoPastThreshold);
   mcChan->tag = tag;
   mcChan->source = source;
   mcChan->goalPosition = newPosition; //mc->goalPosition + relative;	//calculate goal position
   mcChan->lastPosition=mcChan->position;
   mcChan->maxSpeed=mcChan->defaultMaxSpeed;
   mcChan->reason=0;
   mcChan->limitedFlag=false;
   if(mcChan->channel == 0)
    {
     clear_cpld_bit(DCORENCCTL,DCOR0HMZERO); 
     MoveType0 = trapServoLoop;
     disableDWRFirqs (DCOR0HOMED);
    }
   else
    {
     clear_cpld_bit(DCORENCCTL,DCOR1HMZERO); 
     MoveType1 = trapServoLoop;
     disableDWRFirqs (DCOR1HOMED);
 
    }
    
    //Are we homing???
   if (mcChan->optionMask & servoOptionJog)
   {
     if (mcChan->channel == 0)
     {
      writeDWRF(DWRFIFG,DCOR0HOMED); //try this to keep home functions from false triggering
      set_cpld_bit(DCORENCCTL,DCOR0HMZERO); 
      MoveType0 = jogServo;
      enableDWRFirqs (DCOR0HOMED);
      }
     else 
     {
      writeDWRF(DWRFIFG,DCOR1HOMED); 
      set_cpld_bit(DCORENCCTL,DCOR1HMZERO); 
      MoveType1 = jogServo;
      enableDWRFirqs (DCOR1HOMED);
      }
   }
   //Is there a threshold???
   if (mcChan->optionMask & servoOptionThreshold)
   {
     if (mcChan->channel == 0)
      enableDWRFirqs (DCOR0PASSED);
     
     else 
       enableDWRFirqs (DCOR1PASSED);   
   }
   
   UNMASK_TIMER_INTERRUPTS();
 }
}
void startRelMove(MotorChannel* mcChan, int32 newPosition,
                unsigned tag, unsigned source)
{
uint16 servoBit;
 MASK_TIMER_INTERRUPTS();
 mcChan->cmdType = servoRelMoveCmd;    //store away the cmd type for replies
 if((!mcChan->channel)&&(MoveType0==ServoDoNothing)|
  ((mcChan->channel)&&(MoveType1==ServoDoNothing)))
 {
     clear8(mcChan->statusMask, servoOn);
     //Flag the failure
     mcChan->reason = servoNotReady;
     servoBit=servo0<<mcChan->channel;
     set16(opComplete,servoBit);//Signal not sent yet so send it!
     signalEV(); 
 } 
 else
 {
   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();
   }
   //clear threshold and record new values
   clear8(mcChan->statusMask,servoPastThreshold);
   mcChan->tag = tag;
   mcChan->source = source;
   mcChan->goalPosition = mcChan->intermediate + newPosition; //mc->goalPosition + relative;	//calculate goal position
   mcChan->lastPosition=mcChan->position;
   mcChan->maxSpeed=mcChan->defaultMaxSpeed;
   mcChan->reason=0;
   mcChan->limitedFlag=false;
   
   if(mcChan->channel == 0)
    {
     clear_cpld_bit(DCORENCCTL,DCOR0HMZERO); 
     MoveType0 = trapServoLoop;
     disableDWRFirqs (DCOR0HOMED);
    }
   else
    {
     clear_cpld_bit(DCORENCCTL,DCOR1HMZERO); 
     MoveType1 = trapServoLoop;
     disableDWRFirqs (DCOR1HOMED);
    }
    
   UNMASK_TIMER_INTERRUPTS();
 }
}