#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);
         _DINT();_NOP();
         encpwrImage|= DCOR1HOME3;
         updateEncpwrImage();
 
        if(grp->goalPosition)
        {
        //set_cpld_io(DCOROUT,(get_cpld_io(DCOROUT)|DCOR1OUT));
        _DINT();_NOP();
         outImage|= DCOR1OUT;
         updateOutImage();
        }
        else
        {
        //clear_cpld_bit(DCOROUT,DCOR1OUT);
         _DINT();_NOP();
         outImage&= ~ DCOR1OUT;
         updateOutImage();
        }
       }
       else
       {
      // set_cpld_bit(DCORENCPWR,DCOR0HOME3); 
         _DINT();_NOP();
         encpwrImage|= DCOR0HOME3;
         updateEncpwrImage();
 
         if(grp->goalPosition)
         {
         //set_cpld_io(DCOROUT,(get_cpld_io(DCOROUT)|DCOR0OUT));
          _DINT();_NOP();
          outImage|= DCOR0OUT;
          updateOutImage();
         }
         else
         {
         //clear_cpld_bit(DCOROUT,DCOR0OUT);
          _DINT();_NOP();
          outImage&= ~ DCOR0OUT;
          updateOutImage();
         }
       }
       
      grp->State=SWait;
      break;
     }
     case SWait:
     {
     _DINT();_NOP();
     dcorReg=readDWRF(DCORIN);
     _EINT();
      if(grp->channel)
       {
        if(dcorReg&(DCOR1IN))
        { 
        // clear_cpld_bit(DCORENCPWR,DCOR1HOME3);
         _DINT();
         grp->State=SReply;
         encpwrImage&= ~DCOR1HOME3;
         updateEncpwrImage();
 		 grp->inPositionFlag=1;
         break;}
         else break;
       }
       else
       {
          if(dcorReg&(DCOR0IN))
         {
         //turn latch power off
         //clear_cpld_bit(DCORENCPWR,DCOR0HOME3);
          _DINT();
         grp->State=SReply;
         encpwrImage&= ~DCOR0HOME3;
         updateEncpwrImage();
         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

}