/***************************************************
*  TBIB_Slave.h
*
*  handles the Two-Bit-Interface-Bus protocol as a Slave
*
*  main program
*    in setup call
*      InitTBIBpins(); 
*
*    in main loop call
*      PollTBIB(); 
*      
*    to send a message with no data call
*      SendMsgToMaster(msg);
*
*    to send a message with data call
*      StartMsgToMaster(msg);
*      SendByteToMaster(data1);
*      SendByteToMaster(data2);
*      SendByteToMaster(data3); ...
*      EndMsgToMaster();
*
*  main program must include a declaration of
*    void recvTBIBmsg(byte msg, byte *data);
*  which will be called whenever a message is received
*  
*****************************************************
*  
*  in the TBIB protocol, a message is  
*    0xA5: start of message
*    message_kind: 1 byte
*    data: 0 to 19 bytes
*    ckecksum: 1 byte
*  
*  All the bytes of the message from 0xA5 to the checksum must total to 0xFF.
*  
*  The following message_kinds can be sent to the Nano:
*    0x00: initialise - the receiver ignores everything on the TBIB until this is received
*    0x01, N,pos: set servo N to position pos
*    0x02: query PWM values
*   
*  The following message_kinds can be sent to the ESP32cam:
*    0x00: initialise - the receiver ignores everything on the TBIB until this is received
*    0x01: the servos have reached their targets
*    0x02, lo0,hi0,lo1,hi1,lo2,hi2,lo3,hi3,lo4,hi4,lo5,hi5,lo6,hi6,lo7,hi7: PWM values
*  
*****************************************************/

#ifndef TBIB_Slave_h
#define TBIB_Slave_h

const int pinMISOa = 6;
const int pinMOSIa = 7;
const int pinMCLK = 4;
const int pinSCLK = 5;
byte bOut = 0;
byte nOut = 0;
byte TxChkSum;
const int msgStart = 0xA5;
const int msgInitialise = 0; // no data
const int msgAtTargets = 1; // Tx: no data; 
const int msgSetServo = 1; // Rx: 2 bytes data
const int msgPWMs = 2; // Tx: no data; Rx: 16 bytes data

void SendMsgToMaster(byte msg);
void StartMsgToMaster(byte msg);
void EndMsgToMaster();
void SendByteToMaster(byte b);
void SetTBIBpins();
void PollTBIB();
void recvTBIBmsg(byte msg, byte *data);

//-----------------------------------------------------------
// SendByteToMaster
//-----------------------------------------------------------
void SendByteToMaster(byte b) {
  Serial.print("TTx "); Serial.println(b,HEX); 
  TxChkSum += b;
  bOut = b;
  nOut = 9;
  while (nOut > 0)
    PollTBIB();
}

//-----------------------------------------------------------
// SendMsgToMaster
//   send a message to the master with no data
//-----------------------------------------------------------
void SendMsgToMaster(byte msg) {
  StartMsgToMaster(msg);
  EndMsgToMaster();
}

//-----------------------------------------------------------
// StartMsgToMaster
//   start sending a message to the master 
//-----------------------------------------------------------
void StartMsgToMaster(byte msg) {
  TxChkSum = 0;
  SendByteToMaster(0xA5);
  SendByteToMaster(msg);
}

//-----------------------------------------------------------
// EndMsgToMaster
//   send message checksum to the master 
//-----------------------------------------------------------
void EndMsgToMaster() {
  SendByteToMaster(0xFF-TxChkSum);
}

//-----------------------------------------------------------
// recvTBIB() 
//   state is
//     0: waiting for start
//     1: waiting for msg
//     2..20: waiting for data 
//     255: waiting for checksum
//-----------------------------------------------------------
void recvTBIB(byte b) {
  static byte msg;
  static byte data[19];
  static byte state = 0; 
  static byte dataReqd = 0; // number of data bytes required
  static byte sum = 0; 
  
  Serial.print("TRx "); Serial.print(b,HEX); Serial.print(" "); Serial.print(state); Serial.print(" "); Serial.println(dataReqd);  
  sum += b; 
  switch (state) {
    case 0: // waiting for start
      if (b == msgStart) {
        state = 1;
        sum = b; 
      }
      break;
    case 1: // waiting for msg
      msg = b;
      switch (msg) {
        case msgSetServo: 
          dataReqd = 2; 
          state = 2;
          break;
        default: // message has no data
          dataReqd = 0; 
          state = 255;
          break;
      }      
      break;
    case 255: // waiting for checksum
  Serial.print("sum "); Serial.println(sum,HEX);  
      if (sum == 255)
        recvTBIBmsg(msg, data);
      state = 0;
      break;
    default: // waiting for data 
      data[state-2] = b;
      if (dataReqd == 1) {
        state = 255;
      } else {
        state++;
        dataReqd--;
      }
      break;    
  }
}

//-----------------------------------------------------------
// SetTBIBpins() 
//-----------------------------------------------------------
void SetTBIBpins() {
  pinMode(pinMISOa, OUTPUT);    digitalWrite(pinMISOa, LOW);
  pinMode(pinSCLK, OUTPUT);     digitalWrite(pinSCLK, LOW);
  pinMode(pinMOSIa, INPUT);   
  pinMode(pinMCLK, INPUT);    
}

//-----------------------------------------------------------
// PollTBIB() 
//   on entry and exit, both CLKs are low 
//-----------------------------------------------------------
void PollTBIB() {
  static byte bIn = 0;
  static byte nIn = 0;
  bool bMOSI;
  static bool bWaitForCLKhigh = true;
  bool haveReceivedByte = false;

  if (bWaitForCLKhigh) {
    // wait for MCLK
    if (!digitalRead(pinMCLK)) 
      return;
  
    // can change MISO  
    if (nOut == 9) {
      digitalWrite(pinMISOa, true);
      nOut--;
    } else 
    if (nOut > 0) {
      digitalWrite(pinMISOa, bOut & 1);
      nOut--;
      bOut = (bOut >> 1) & 0x7F;
    } else 
      digitalWrite(pinMISOa, false);
    
    // read MOSI
    bMOSI = digitalRead(pinMOSIa);
    if ((nIn == 0) && bMOSI)
    { // start of incoming data
      nIn = 8;
    } else
    if (nIn > 0) {
      bIn = (bIn >> 1) & 0x7F;
      if (bMOSI)
        bIn += 0x80;
      nIn--;
      if (nIn == 0) 
        haveReceivedByte = true; 
    }
  
    // set SCLK = true
    digitalWrite(pinSCLK, true);

    bWaitForCLKhigh = false;
  } else {
    // wait for ! MCLK
    if (digitalRead(pinMCLK)) 
      return;
    
    // set SCLK = false  
    digitalWrite(pinSCLK, false);

    bWaitForCLKhigh = true;
  }

  if (haveReceivedByte) {
    haveReceivedByte = false;
    recvTBIB(bIn);
  }
}

#endif