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

#define abs(x) ((x)<0 ? -(x) : (x))

#define maxErr(rv) (rv->maxError & 0x7fff)

static byte drvCtlImage = 0;
static byte drvMtrCtlImage = 0;

static void setDrvCtl (byte mask)
{
  _DINT();
  drvCtlImage = mask;
  writeDWRF (DRVCTL, mask);
  _EINT();
}

static void setDrvMtrCtl (byte mask)
{
  _DINT();
  drvMtrCtlImage = mask;
  writeDWRF (DRVMTRCTL, mask);
  _EINT();
}


static void getShaftPos(RotaryValve* rv)
{
  register uint16 rotaryEnc0, rotaryEnc1, rotaryEnc2, rotaryEnc3;

  register uint16 *in = (uint16 *)&DWRFBUSIN;
  register byte mask = DWRFBUSRD;
  register uint16 temp;
  
#macro readNext(result)
  DWRFSTRBOUT &= ~mask;
/$
   mov.b \@@in, @result
$/
  DWRFSTRBOUT |= mask;
  DWRFSTRBOUT &= ~mask;
/$
   mov.b \@@in, @temp
$/
  DWRFSTRBOUT |= mask;
/$
   swpb  @temp
   bis.w @temp,@result
$/
#endm
  
  _DINT(); _NOP();
  setDWRFadr(DRV0LOW), floatDWRFbus();  
  readNext (rotaryEnc0);
  readNext (rotaryEnc1);
  readNext (rotaryEnc2);
  readNext (rotaryEnc3);
  freezeDWRFadr();
  _EINT();
  
  mask = drvCtlImage;
  if (mask & ENCPWR0)   rv[0].position = rotaryEnc0;
  if (mask & ENCPWR1)   rv[1].position = rotaryEnc1;
  if (mask & ENCPWR2)   rv[2].position = rotaryEnc2;
  if (mask & ENCPWR3)   rv[3].position = rotaryEnc3;
}


static const byte pwrOnx[NROTARY] = {ENCPWR0, ENCPWR1, ENCPWR2, ENCPWR3};
static const byte MxS[NROTARY] = {M0S, M1S, M2S, M3S};
static const byte MxB[NROTARY] = {M0B, M1B, M2B, M3B};
static const byte Mxx[2*NROTARY] = {M0R, M0F, M1R, M1F, M2R, M2F, M3R, M3F};


static int16 delta (RotaryValve *rv)
//return # of counts added modulo revCounts to current position to reach goal
//may be negative
{
  uint16 current = rv->position;
  uint16 goal = rv->lashGoal;  //goal offset for expected backlash
  uint16 revCounts = rv->revCounts;
  int16 halfRev = revCounts/2;
  int16 distance = goal - current;
  int16 offset = 0;
  if (distance > halfRev) offset = -revCounts;
  else if (distance < -halfRev) offset = revCounts;
  return distance + offset;
}


static uint16 desiredDirection (RotaryValve *rv, int16 delta)
// delta is the result from delta(rv)
// returns 0 for rotating Up (increasing counts), or 1 for rotating down
{
  uint16 direction = 0;
  if (delta < 0) {
    direction = 1;
    delta = -delta;
  }      //direction = the shortest direction toward goal 
  {
    int8 rvDir = rv->direction;
    if (rvDir) {    //a direction was specified so override nearest
      uint16 closest = direction; //remember the closest direction
      direction = rvDir < 0;  //map specified direction to a 0 or 1
      if (direction == closest && delta < rv->revCounts/4) rv->direction = 0;
        //all rotations specified eventually are in the "closest" direction
    }
  }
  return direction;
}


void idleShaft (RotaryValve *rv)
{
  byte mtrMask = drvCtlImage & ~pwrOnx[rv->Valve];
  if (!(mtrMask & (ENCPWR0|ENCPWR1|ENCPWR2|ENCPWR3)))
    mtrMask &= ~MTRPWR;
  setDrvCtl(mtrMask);   //power off motor too with last encoder
  rv->State = SSMoff;
}


static void ShaftStateMachine(RotaryValve *rv)
{
//this routine runs a state machine for one of the shafts
//it assume that the current position has been read alread
//and the structure updated.
  switch (rv->State) { 
    case SSMread:    //update position without moving
      if (--rv->brakeTimer) break;
      setDrvCtl (drvCtlImage | pwrOnx[rv->Valve]);
      rv->State = SSMreading;
      break;

    case SSMstart:  // start a move to specified goal angle
      //this valve has been asked to seek a postion
      //first turn on the encoder and motor power
      setDrvCtl (drvCtlImage | pwrOnx[rv->Valve] | MTRPWR);
      rv->lashGoal = rv->goal;
      rv->State = SSMcheck;
      break;   

    case SSMcheck:
    {
      int16 distance = delta(rv);
      if (abs(distance) > maxErr(rv)) {
        uint16 direction = desiredDirection(rv, distance);
        uint16 lash = rv->lash;
        uint16 lashGoal;
        if (direction) {    //offset goal for expected backlash
          lashGoal = rv->goal - lash;
          if (lashGoal >= rv->revCounts) lashGoal += rv->revCounts;
          rv->direction = -1;  //avoid ambiguity caused oscillation on half rotations
        }else{
          lashGoal = rv->goal + lash;
          if (lashGoal >= rv->revCounts) lashGoal -= rv->revCounts;
          rv->direction = 1;   //avoid ambiguity caused oscillation on half rotations
        }
        rv->lashGoal = lashGoal;
        rv->State = SSMrotatingUp + direction;
        if (rv->maxError & 0x8000) direction ^= 1;
        {
          unsigned v = rv->Valve;
          setDrvMtrCtl (drvMtrCtlImage & MxS[v] | Mxx[(v<<1)|direction]);
        }
        break;
      }  //else, fall into SSMreading case to report final position
    }
      
    case SSMreading:
      idleShaft(rv);
      set16(opComplete,rvalve0 << rv->Valve);//Signal not sent yet so send it!
      signalEV();
    case SSMoff:         //channel inactive
      break;    


    case SSMrotatingUp:
    case SSMrotatingDown:
    {  //continue while not at goal AND not wanting to change direction...
      int16 distance = delta(rv);  
      if (abs(distance) > maxErr(rv) && 
          desiredDirection(rv,distance) == rv->State-SSMrotatingUp)
        break;
      setDrvMtrCtl (drvMtrCtlImage | MxB[rv->Valve]);  //else, put on the brakes...
      rv->State = SSMbraking;
      rv->brakeTimer = rv->brakeTics;
    }
    case SSMbraking: //wait for motor to stop
      if (--rv->brakeTimer) break;
      rv->State = SSMcheck;
    default:
      setDrvMtrCtl (drvMtrCtlImage & MxS[rv->Valve]);
  }//end of state switch


}//End of ShaftStateMachine


void valveISR(RotaryValve* rv, char debugFlag)
{
//start the reading of the flags
  _DINT(); _NOP();
  writeDWRF (DRVCTL, drvCtlImage | START);
  _EINT();
  
//wait for completion
  MCLKsleep(35);  //approx 10 microseconds @4MHz
  
//now read the results
  getShaftPos(rv);
  ShaftStateMachine(rv);
  ShaftStateMachine(++rv);
  ShaftStateMachine(++rv);
  ShaftStateMachine(++rv);
}



void configRotaryValves(RotaryValve* rv)
{
  unsigned valveNum = 0;
  while (valveNum < NROTARY) {
    rv->Valve=valveNum;	 	
    rv->revCounts = 512;
    rv->maxError = 2;
    rv->brakeTics = 4;
    rv->lash = 0;
    rv->brakeTimer = 1;
    rv->State = SSMread;    //do an initial read of position
    rv++;
    valveNum++;
  }
}