#include <Arduino.h>
#include "TcsBus.h"

#define TCS_OVERSAMPLING    8
#define TCS_BAUDRATE      475
#define TCS_IDLE_TIMEOUT   64 //8 bits

TcsBus::TcsBus(int sendPin, int receivePin) :
  sendPin { sendPin },
  receivePin { receivePin },
  state { SamplingState::IDLE },
  rxBits { 0 },
  oversamplingStep { 0 },
  defaultRxEvent { 0 },
  txEvent { 0 }
{

}
static hw_timer_t* timer = NULL;
static TcsBus* tcsInstance = NULL;
static void onTimer(void)
{
  tcsInstance->TimeBase();
}

static bool TimerSetup(TcsBus* inst)
{
  if(inst) {
    if(timer != NULL) return false; //already set up

    tcsInstance = inst;
    //time base is 80 MHz, which will be divided by the value from timerBegin, and then
    //a counter increments until it reaches the value from timerAlarmWrite. onTimer will then be fired as a callback.
    //80000000 / 50 / 421 = 3800.47 Hz = 8 * 475.06
    timer = timerBegin(0, 50, true);
    timerAttachInterrupt(timer, &onTimer, true);
    timerAlarmWrite(timer, 421, true);
    timerAlarmEnable(timer);
  } else {
    if(timer == NULL) return false; //not set up
    timerAlarmDisable(timer);
    timerDetachInterrupt(timer);
    timerEnd(timer);
    timer = NULL;
    tcsInstance = NULL;
  }
  return true;
}

void TcsBus::begin()
{
  //Pin setup
  pinMode(receivePin, INPUT);
  pinMode(sendPin, OUTPUT);
  digitalWrite(sendPin, LOW); //disable sending for now
//pinMode(0, OUTPUT);
  TimerSetup(this);
}

void TcsBus::end()
{
  pinMode(sendPin, INPUT);
  TimerSetup(NULL);
}

void TcsBus::TimeBase()
{
  bool sample = digitalRead(receivePin);

  do {
    if(state != SamplingState::IDLE) {
      if(sample) {
        oversamplingSum++;
      }
      if(oversamplingStep) oversamplingStep--;
      if(oversamplingStep) {
        return;
      }
    }

    switch(state) {
      case SamplingState::IDLE: {
        if(!sample) {
          state = SamplingState::RECEIVE;
          oversamplingStep = TCS_OVERSAMPLING;
          oversamplingSum = 0;
          currentBitIndex = 0;
          memset(rxBits, 0x00, (sizeof(rxBits) / sizeof(uint8_t)));
          continue;
        }
      } break;

      case SamplingState::RECEIVE: {
        if(oversamplingSum <= TCS_OVERSAMPLING / 4) {
          ReceiveBit(0);
        } else if(oversamplingSum >= (TCS_OVERSAMPLING * 3) / 4) {
          ReceiveBit(1);
        } else {
          //If received bit is invalid, go to idle wait
          EnterWaitIdle();
        }
      } break;
      case SamplingState::TRANSMIT: {
        TransmitBit();
      } break;
      case SamplingState::WAIT_IDLE: {
        WaitIdle();
      }
    }

  } while(0);
}


void TcsBus::RegisterDefaultRxEvent(EventData_t* evt)
{
  defaultRxEvent = evt;
}

void TcsBus::RegisterEvent(const EventData_t* evt)
{
  events.push_back(evt);
}
void TcsBus::UnRegisterEvent(const EventData_t* evt)
{
  for(int i = 0; i < events.size(); i++) {
    if(evt == events[i]) {
      //events.erase(evt); //no, needs iterators

      return;
    }
  }
}

void TcsBus::ReceiveBit(uint8_t currentBit)
{
  //push received bit to buffer.
  rxBits[currentBitIndex++] = currentBit;

  //check against event list
  const EventData_t* found = 0;
  for(const EventData_t* evt: events) {
    //std::cout << value << "\n";
    if(currentBitIndex >= evt->bitCount) {
      if(!memcmp(rxBits, evt->bits, evt->bitCount)) {
        if(found != 0) {
          //duplicate, skip
          found = 0;
          break;
        } else {
          found = evt;
        }
      }
    }
  }
  if(found) {
    found->func(found);
  }
  
  if(found || (currentBitIndex >= (sizeof(rxBits) / sizeof(uint8_t)))) {
    //if done or buffer is full, switch to idle wait state
    EnterWaitIdle();
  }
}
void TcsBus::EnterWaitIdle()
{
  state = SamplingState::WAIT_IDLE;
  oversamplingStep = TCS_IDLE_TIMEOUT;
  oversamplingSum = 0;
}

void TcsBus::TransmitBit()
{
  if(currentBitIndex < txEvent->bitCount) {
    digitalWrite(sendPin, !txEvent->bits[currentBitIndex++]);
  } else {
    //disable if done and return to WAIT_IDLE state
    digitalWrite(sendPin, 0); //disable tx mosfet
    EnterWaitIdle();
  }
}

void TcsBus::WaitIdle()
{

  //At this point, the error condition was triggered and the timout is over. If the bus is in idle state, we can
  //go over to IDLE, otherwise, we must reset the timeout to ensure an idle time on the bus
  if(oversamplingSum >= TCS_IDLE_TIMEOUT) {
    state = SamplingState::IDLE;
  } else {
    oversamplingStep = TCS_IDLE_TIMEOUT;
    oversamplingSum = 0;
  }
}