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

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

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

//if servoOptionHomeDir == 0 then leave direction but if one reverse
  tempSpeed = mcChan->maxSpeed;
  tempAccel = mcChan->accel;
 _DINT();
  if (mcChan->optionMask & servoOptionHomeDir)
   {
    tempSpeed = -tempSpeed;
    tempAccel = -tempAccel;
   }
 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;
  
 
  error = mcChan->intermediate - mcChan->position;
  
  //check pressure bounds has been removed
  
  //Add a condition for delta pressure in the future
  
  if (abs(error) > 2*mcChan->maxSpeed) {
    MotorPositionError(mcChan, servoTrajectoryErr);
    return;    
  } 
     

 //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) {
    mcChan->sum = mcChan->sum + error;
     //check its 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;
  
  //max speed control
  //calc averages  
    mcChan->PIDavg = ( mcChan->PIDavg*1+PID)/2;
    updateAvgSpeed(mcChan);
  //if the PID value has gotten larger than 85% need to limit speed
  //use average speed as new temp max speed in trajectory
  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);
      return;
    }
  }else //if PID is less that 75% then try back at full speed  
    if((mcChan->flags & limited)&&(PID<VEL_LIMITING_LOW)) {
      mcChan->flags &= ~limited;
      mcChan->maxSpeed=mcChan->defaultMaxSpeed;
    }

  //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;
    }
    PID = -PID;   //flip the PID value to positive
  }else{ 
   switch (mcChan->channel) {
    case 0:
     clear8(DCORMOTOROUT,DCOR0DIR|DCOR0BRAKE);
     break;
    case 1:
     clear8(DCORMOTOROUT,DCOR1DIR|DCOR1BRAKE);
     break;
   }
 }
  
 // encoders can't handle much faster than 700 for old demo board
 if (PID > MAXPWM) //must leave some off time for the
   PID = MAXPWM;   //the driver to recharge
 //update the PWM    

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

}//End of JogServo


void latchActive(MotorChannel* mc)
{
   byte passed = DCOR0PASSED;
    MASK_TIMER_INTERRUPTS();
   _DINT();
   if (mc->channel) passed = DCOR1PASSED;
   disableDWRFirqs (passed);
   _EINT();
   mc->latchPosition = getLatchPos(mc);
   mc->goalPosition = mc->latchPosition + mc->thresholdOffset;
   mc->settleCounter=mc->maxSettling;
    UNMASK_TIMER_INTERRUPTS();
   reportErr ("LATCHED ch#%01d @ %ld, new goal=%01ld", 
      mc->channel, mc->latchPosition, mc->goalPosition);
}//End of latchActive


void homed(MotorChannel* mc)
{
  byte iMask = 0;
  int32 oldHomePosition = mc->homePosition;
//unsigned tm;
MASK_TIMER_INTERRUPTS();
  mc->homePosition = getLatchPos(mc);    //record home base
  if (mc->cmdType != servoRelMoveCmd) {  //adjust goal for to new home position
    if (!(mc->statusMask & servoLostHome)) 
      mc->goalPosition -= oldHomePosition;
    mc->goalPosition += mc->homePosition;
  }
  clear8(mc->statusMask,servoLostHome);
  clear8(mc->optionMask,servoOptionJog);
  
  if(!mc->channel) {
    if (mc->optionMask & servoOptionThreshold)
      iMask = DCOR0PASSED;
    _DINT();
    MoveType0 = trapServoLoop;
//tm=TAR;
    DWRFIEimage &= ~DCOR0HOMED;  //disable the home interrupt
    encctlImage &= ~DCOR0HMLATCH; // and don't latch the count either
  }else{
    if (mc->optionMask & servoOptionThreshold)
      iMask = DCOR1PASSED;
    _DINT();
    MoveType1 = trapServoLoop;
//tm=TAR;
    DWRFIEimage &= ~DCOR1HOMED;  //disable the home interrupt
    encctlImage &= ~DCOR1HMLATCH; // and don't latch the count either
  }
  writeDWRF (DCORENCCTL,encctlImage);
  writeDWRF (DCORIFG, iMask);
  enableDWRFirqs (iMask);
  _EINT();
//tm=TAR-tm;
  UNMASK_TIMER_INTERRUPTS();
  reportErr ("HOMEd Ch#%01d @ %01ld, goal=%ld, pos = %01ld: status=%02x", 
    mc->channel, mc->homePosition, mc->goalPosition, mc->position, mc->statusMask);
//reportErr("SMClks=%d\n", tm);
}


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
   */
}