#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 "types.h"
#include "servobits.h"
#define abs(a) (((a)>=0)?(a):(-a))

void jogServo(MotorChannel* mcChan)
{
int home,servoBit;
long int error;
long int PID;
int16 tempSpeed, tempAccel;
long int derivTerm;

 //this routine will move the motor at a continous speed using the channels minspeed number
 //the direction is determined by the state of the home flag.

//if servoOptionHomeDir == 0 then leave direction but if one reverse
  if (mcChan->optionMask & servoOptionHomeDir)
   {
    tempSpeed = -mcChan->minSpeed;
    tempAccel = -mcChan->accel;
   }
  else 
   {
    tempSpeed = mcChan->minSpeed;
    tempAccel = mcChan->accel;
   }
 _DINT();_NOP();
 home = readDWRF(DCORLIMITS);_EINT();
 if (mcChan->channel == 0)
  {
   if (home & (DCOR0HOME)) 
        mcChan->cmdVel += tempAccel;
   else 
        mcChan->cmdVel -= tempAccel;
  }
 else
 {
   if (home & (DCOR1HOME)) 
        mcChan->cmdVel += tempAccel;
   else 
        mcChan->cmdVel -= tempAccel;
 }
 if (abs(mcChan->cmdVel) > abs(tempSpeed)) 
  {
   if (mcChan->cmdVel < 0)  
    {
     if (tempSpeed < 0) mcChan->cmdVel = tempSpeed;
     else mcChan->cmdVel = -tempSpeed; 
    }
   else 
    {
     if (tempSpeed < 0) mcChan->cmdVel = -tempSpeed;
     else mcChan->cmdVel = tempSpeed; 
    }
  }
   //adjust commanded position
   mcChan->intermediate += mcChan->cmdVel;
   mcChan->lastIntermediate = mcChan->intermediate;
  
 
  error = mcChan->intermediate - mcChan->position;
  
  if (abs(error) > mcChan->maxPositionErr)
   {
    MotorPositionError(mcChan);     
     } 
     

 //calc proportional term 
  PID = (mcChan->gain.P * error);
  
  //add in error to form intergral
  //if I term == 0 then skip I
  if (mcChan->gain.I != 0)
   {
       mcChan->sum = mcChan->sum + error;
       //check it's limits
  
      if (mcChan->sum > INT_LIMIT) mcChan->sum = INT_LIMIT;
       else if ( mcChan->sum < -INT_LIMIT) mcChan->sum = -INT_LIMIT;
       //calc the integral portion
 	     PID += mcChan->sum * mcChan->gain.I;
  }
  else mcChan->sum = 0;
  PID += mcChan->gain.D * (error - mcChan->lastError);
  
 
  //update errors
  mcChan->lastError = error;

  PID=PID/PIDSCALE;
  //now output the result
  //if negative then negate result but set dir
  if (PID < 0 )
  {
    switch (mcChan->channel)
    {
    case 0:
    set8(DCORMOTOROUT,DCOR0DIR);
    clear8(DCORMOTOROUT,DCOR0BRAKE);
    break;
    case 1:
    set8(DCORMOTOROUT,DCOR1DIR);
    clear8(DCORMOTOROUT,DCOR1BRAKE);
    break;
    default:break;
    }
  
  PID = PID * -1;   //flip the PID value to positive
  }
  else 
  { 
   switch (mcChan->channel)
    {
    case 0:
    clear8(DCORMOTOROUT,DCOR0DIR|DCOR0BRAKE);
    break;
    case 1:
    clear8(DCORMOTOROUT,DCOR1DIR|DCOR1BRAKE);
    break;
    default:break;
   }
  }
  
 // 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
	//maintain small dead band to eliminate unnecessary vibration  
	/*                            
 if(((abs((long)(mc->position-mc->goal_position))<8)&(PID<60))) 
 {	
 	PID=0;	
 }
 */
 //update the PWM    

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

	mcChan->lastPosition=mcChan->position;

}//End of JogServo

void latchActive1(MotorChannel* mc)
{
 _DINT();
  writeDWRF(DWRFIFG,DCOR1PASSED); //reset the interrupt flag in the CPLD
 _EINT();
 //Need to stop the motor!
 getLatchPos(mc);

 mc->goalPosition = mc->latchPosition + mc->thresholdOffset;//change this to latch position+offset
 mc->intermediate = mc->position;
 mc->lastIntermediate = mc->position;
 mc->cmdVel = 0;
 set8(mc->statusMask,servoPastThreshold);
 if (mc->channel == 0) 
 {
 disableDWRFirqs (DCOR0PASSED);    
 } 
 else  
 {
 disableDWRFirqs (DCOR1PASSED);
 }
 reportErr ("\nLatch Pos %ld\t chan%x\n", mc->latchPosition,mc->channel);
  
  reportErr ("Goal Pos %ld\n", mc->goalPosition);	

}//End of latchActive
void homed(MotorChannel* mc)
{
    //_DINT();
    clear_cpld_bit(DCORENCCTL,DCOR0HMZERO<<(mc->channel*4)); 
    _DINT();_NOP();
   // writeDWRF(DWRFIFG,DCOR0HOMED<<mc->channel); //reset the interrupt flag in the CPLD
    disableDWRFirqs (DCOR0HOMED<<mc->channel);
    _EINT();
  mc->position = 0;
  //mc[channel].intermediate = mc[channel].cmdVel;
  mc->intermediate =0;
  mc->lastIntermediate = 0;  
  mc->sum=0;
  //mc[channel].cmdVel=0;
  mc->lastError = 0;
  clear8(mc->statusMask,servoLostHome); 
  
  reportErr ("Servo status %x\n", mc->statusMask);	

  if(!mc->channel) {MoveType0 = trapServoLoop;reportErr ("InHOMED.New Move= Trap Ch=0");}
  else {
  MoveType1 = trapServoLoop;reportErr ("InHOMED.New Move= Trap Ch=1");}
}
void limitReached(MotorChannel* mc0, MotorChannel* mc1)
{
  byte limitReg;
 
  _DINT();_NOP();
  limitReg = readDWRF(DCORLIMITS);
  writeDWRF(DWRFIFG,DCORCRASHED); //reset the interrupt flag in the CPLD
  _EINT();
  
   if(limitReg & (DCOR0FLS|DCOR0RLS))
   {
    //set gains= 0  
    mc0->gain.P=0;
	mc0->gain.D=0;
	mc0->gain.I=0;
    TBCCR1 = 0;   
   } 
   else if (limitReg & (DCOR1FLS|DCOR1RLS))
   {
   mc1->gain.P=0;
   mc1->gain.D=0;
   mc1->gain.I=0;
   TBCCR2 = 0;  
   }
   /*
   initial plan for this function is to do the following things:
   1)read the limits reg
   2)reset interrupt flag
   3)set gains of motor to 0
   4)set PWM reg to 0
   5)toggle brake line (no delay between setting and resetting lines)
   6.0) set bit in servo reason mask
   6)send a servo reply- move aborted
   */
}