#include "msp430x16x.h"  //register and I/O definitions
#include "dwarfcore.h"
#include "gripper.h"
#include "parser.h"
#include "debugport.h"
#include "cpld.h"
#include "espdwarf.h"
#include "dwarfcore.h"
#include "servobits.h"

void gripperISR(Gripper* grp)
{
 unsigned int servoBit;
 byte dcorReg;

   switch(grp->State)
   {
     case SDoNothing:break;
  
     case SWrite:
     {
      //write position
      //turn latch power on
       if(grp->channel)
       {
        set_cpld_bit(DCORENCPWR,DCOR1HOME3);
        if(grp->goalPosition)
        set_cpld_io(DCOROUT,(get_cpld_io(DCOROUT)|DCOR1OUT));
        else
        clear_cpld_bit(DCOROUT,DCOR1OUT);
       }
       else
       {
       set_cpld_bit(DCORENCPWR,DCOR0HOME3); 
       if(grp->goalPosition)
       set_cpld_io(DCOROUT,(get_cpld_io(DCOROUT)|DCOR0OUT));
       else
       clear_cpld_bit(DCOROUT,DCOR0OUT);
       }
       
      grp->State=SWait;
     }
     case SWait:
     {
     _DINT();_NOP();
     dcorReg=readDWRF(DCORIN);
     _EINT();
       if(grp->channel)
       {
          if(dcorReg&(DCOR1IN))
         { 
         clear_cpld_bit(DCORENCPWR,DCOR1HOME3);
         grp->State=SReply; 
         grp->inPositionFlag=1;
         break;}
         else break;
       }
       else
       {
         if(dcorReg&(DCOR0IN))
         {
         //turn latch power off
         clear_cpld_bit(DCORENCPWR,DCOR0HOME3);
         grp->State=SReply;
         grp->inPositionFlag=1;
         break;}
         else break;
       }
      }
      case SReply:
      {
      grp->State=SDoNothing;
      grp->sendDoneReply=1;
      if(grp->goalPosition==0){
      _NOP();}
      servoBit=grip;
      set16(opComplete,servoBit<<grp->channel);//Signal not sent yet so send it!
      signalEV();
      break;
      }

   }
     //check if in position, if yes go to S3 else stay, s=s3

}
void initGripper(Gripper* grp,byte channel)
{
 grp->channel=channel;
 grp->State=SDoNothing;
 grp->goalPosition=0;
 grp->inPositionFlag=0;        //high if in pos, low if out of pos
 grp->sendDoneReply=0;         //set as true to signal reply ready
 grp->source=0;    // add these to hold who reguested the current move
 grp->tag=0;         // if this field is 0 then no move in progress
 grp->cmdType=0;   //rel or absolute
 grp->reason=0;  //the information sent back at the completion of a move


}