#include "solenoid.h"


//byte gDrvValve0, gDrvValve1;
static uint16 gDrvValve;
uint16 super;

//SolStruct SS;


void configureSolenoid(SolStruct *SS)
{
 // gDrvValve0 = gDrvValve1 = 0;
  gDrvValve = 0;
  SS->cHold = SS->nHold = 0x0000;
  SS->cPulse = SS->nPulse = 0x0000;
  SS->Duration = 0;
  SS->tic = 0;
  SS->Pupdate = SS->Hupdate = false;
}

#macro doValveHold(ValveMask)
  masked = SS->nHold & ValveMask;
  if (masked != 0x0)
    {
       if ((masked < ValveMask))
       {
           if ((gDrvValve & ValveMask) ^ masked == ValveMask) 
           {
             gDrvValve &= ~(ValveMask);
             {
             byte temphigh = (gDrvValve >> 8);
             _DINT();
             writeDWRFfirst(DRVVALVE0,gDrvValve);
             writeDWRFlast(temphigh);
             _EINT();
             }
            
           }
          SS->cHold = (SS->cHold & ~(ValveMask)) | masked;
          SS->cPulse = (SS->cPulse & ~(ValveMask));
          gDrvValve = (gDrvValve & ~(ValveMask)) | masked;
          updateCPLD++;
       }
       else // if (masked == 0x03)
       {
            SS->cHold = (SS->cHold & ~(ValveMask));
            SS->cPulse = (SS->cPulse & ~(ValveMask));
            gDrvValve = (gDrvValve & ~(ValveMask));
            updateCPLD++;
       }
    }
#endm

#macro doValvePulse(ValveMask)
 masked = SS->nPulse & ValveMask;
  if (masked != 0x0)
    {
       if ((masked < ValveMask))
       {   
          if ((gDrvValve & ValveMask) ^ masked == ValveMask) 
          {
             gDrvValve &= ~(ValveMask);
             {
             byte temphigh = (gDrvValve >> 8);
             _DINT();
             writeDWRFfirst(DRVVALVE0,gDrvValve);
             writeDWRFlast(temphigh);
             _EINT();
             }
            
           }
          SS->cPulse = (SS->cPulse & ~(ValveMask)) | masked;
          SS->cHold = (SS->cHold & ~(ValveMask));
          gDrvValve = (gDrvValve & ~(ValveMask)) | masked;
          SS->tic = SS->Duration + 1;
          updateCPLD++;
       }
       else //if (masked == 0x03)
       {
          SS->cPulse = SS->cPulse & ~(ValveMask);
          SS->cHold = (SS->cHold & ~(ValveMask));
          gDrvValve = (gDrvValve & ~(ValveMask));
          updateCPLD++;
       }
    }

#endm

#macro doEndPulse(ValveMask)
        if ((SS->cPulse & ValveMask) != 0x0000)
         {
          SS->cPulse = SS->cPulse & ~(ValveMask);
          gDrvValve = (gDrvValve & ~(ValveMask));
          updateCPLD++;
         }
#endm

void PulseAndHoldISR(SolStruct *SS)
{
int masked, updateCPLD = 0x0;

 if (SS->Hupdate)
  {
  SS->Hupdate = false;
  doValveHold(0x0003);
  doValveHold(0x000C);
  doValveHold(0x0030);
  doValveHold(0x00C0);
  doValveHold(0x0300);
  doValveHold(0x0C00);
  doValveHold(0x3000);
  doValveHold(0xC000);
  }
  
 if (SS->Pupdate)
 {
  SS->Pupdate = false;
  doValvePulse(0x0003);
  doValvePulse(0x000C);
  doValvePulse(0x0030);
  doValvePulse(0x00C0);
  doValvePulse(0x0300);
  doValvePulse(0x0C00);
  doValvePulse(0x3000);
  doValvePulse(0xC000);
 }

 if (SS->tic != 0)
 {
 if (SS->cPulse == 0) SS->tic = 0;
 else 
  {
      SS->tic--;
       if (SS->tic == 0)
       {
        doEndPulse(0x0003);
        doEndPulse(0x000C);
        doEndPulse(0x0030);
        doEndPulse(0x00C0);
        doEndPulse(0x0300);
        doEndPulse(0x0C00);
        doEndPulse(0x3000);
        doEndPulse(0xC000);
       }
   }
 }

 if (updateCPLD > 0)
  {
     byte temphigh = (gDrvValve >> 8);
     _DINT();
     writeDWRFfirst(DRVVALVE0,gDrvValve);
     writeDWRFlast(temphigh);
     _EINT();
  }
}//End of PulseAndHoldISR