/***************************************************
*  TBIB_Master.h
*
*  handles the Two-Bit-Interface-Bus protocol as a master
*
*  main program
*    in setup call
*      InitTBIBpins(); 
*
*    in main loop call
*      PollTBIB(); 
*      returns true is both clocks are low
*        main program should only use SPI when both clocks are low
*      
*    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
*    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_Master_h
#define TBIB_Master_h

void InitTBIBpins(); // sets pins to input/output
bool PollTBIB(); // call in main loop
void recvTBIBmsg(byte msg, byte *data);
void SendMsgToSlave(byte msg);
void StartMsgToSlave(byte msg);
void EndMsgToSlave();
void SendByteToSlave(byte b);

//-----------------------------------------------------------

const int pinMISOa = 15;
const int pinMOSIa = 13;
const int pinMCLK = 2;
const int pinSCLK = 14;

byte bOut = 0;
byte nOut = 0;
byte TxChkSum; // checksum for transmitted characters
const int msgStart = 0xA5;
const int msgInitialise = 0;
const int msgAtTargets = 1;
const int msgSetServo = 1;
const int msgPWMs = 2;

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

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

//-----------------------------------------------------------
// SendMsgToSlave
//   send a message to the Slave with no data
//-----------------------------------------------------------
void SendMsgToSlave(byte msg) {
  StartMsgToSlave(msg);
  EndMsgToSlave();
}

//-----------------------------------------------------------
// StartMsgToSlave
//   start sending a message to the Slave 
//-----------------------------------------------------------
void StartMsgToSlave(byte msg) {
  TxChkSum = 0;
  SendByteToSlave(0xA5);
  SendByteToSlave(msg);
}

//-----------------------------------------------------------
// EndMsgToSlave
//   send message checksum to the Slave 
//-----------------------------------------------------------
void EndMsgToSlave() {
  SendByteToSlave(0xFF-TxChkSum);
}

//-----------------------------------------------------------
// recvTBIB() 
//   receive byte over TBIB
//   decode the message protocol
//
//   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.println(b,HEX);  
  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 msgPWMs: 
          dataReqd = 16; 
          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; 
//Serial.print("data["); Serial.print(state-2); Serial.print("] = "); Serial.println(b,HEX); 

      if (dataReqd == 1) {
        state = 255; 
      } else {
        state++;
        dataReqd--;
      }
      break;    
  }
}

//-----------------------------------------------------------
// PollTBIB() 
//   on entry, both CLKs are low 
//-----------------------------------------------------------
bool PollTBIB() {
  static byte bIn = 0;
  static byte nIn = 0;
  bool bMISO;
  static bool bWaitForCLKhigh = true;

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

    bWaitForCLKhigh = false;
    return false;
  } else {
    // wait for !SCLK
    if (digitalRead(pinSCLK)) 
      return false;

    bWaitForCLKhigh = true;
    return true;
  }
}

#endif