/*
 * Stairs lighting
 * Copyright 2017 (c) Mark van Renswoude
 *
 * https://git.x2software.net/pub/Stairs
*/
#include <Arduino.h>
#include <Wire.h>
#include "global.h"
#include "config.h"
#include "display.h"
#include "protocol.h"


void setup()
{
  settings.init();

  // Set up I2C devices: the SSD1306 OLED display and PCA9685 LED PWM driver
  Wire.begin();
  display.init();

  pwmDriver.begin();
  pwmDriver.setPWMFreq(PWMDriverFrequency);

  // At 16 Mhz we pick a baud rate with a very acceptable 0.2% error rate
  // Source: http://www.robotroom.com/Asynchronous-Serial-Communication-2.html
  Serial.begin(CommBaudRate);
  pinMode(CommWriteEnablePin, OUTPUT);
  comm.begin();

  pinMode(PinButton, INPUT_PULLUP);

  pinMode(5, OUTPUT);
}



// Forward declarations
void handleCommMessage();
void handlePing(uint8_t* data, uint8_t length);
void handleDisplayModuleIndex(uint8_t* data, uint8_t length);
void handleStartLink(uint8_t* data, uint8_t length);
void handleRequestLinkResponse(uint8_t* data, uint8_t length);
void handleStopLink(uint8_t* data, uint8_t length);
void handleSetPWM(uint8_t* data, uint8_t length);
void handleGetSensors(uint8_t* data, uint8_t length);

void checkButtonPress();


void loop()
{
  currentTime = millis();

  if (comm.update())
    handleCommMessage();

  checkButtonPress();
  display.update();
}



void sendCommMessage(const uint8_t* data, uint8_t size)
{
  digitalWrite(CommWriteEnablePin, HIGH);
  comm.sendMsg(data, size);


  // Wait for the hardware buffer to clear before turning
  // off the write enable pin, or we'll cut off the message too early
  // Straight from: http://www.gammon.com.au/forum/?id=11428

  while (!(UCSR0A & (1 << UDRE0)))  // Wait for empty transmit buffer
    UCSR0A |= 1 << TXC0;  // mark transmission not complete

  while (!(UCSR0A & (1 << TXC0)));   // Wait for the transmission to complete


  digitalWrite(CommWriteEnablePin, LOW);
}


void handleCommMessage()
{
  uint8_t* data = comm.getData();
  uint8_t length = comm.getLength();

  if (length == 0)
    return;

  if (state == State::WaitingForComm)
    state = State::DisplayModuleIndex;

  uint8_t command = *data; data++; length--;
  uint8_t moduleIndex = ModuleIndexUndefined;

  if ((command & MaskMessageType) == MaskModuleCommand)
  {
    if (!settings.hasModuleIndex())
      // We're not linked yet
      return;

    moduleIndex = *data; data++; length--;

    if (settings.getModuleIndex() != moduleIndex)
      // This message is meant for another module
      return;
  }

  switch (command)
  {
    case CommandPing: handlePing(data, length); break;
    case CommandDisplayModuleIndex: handleDisplayModuleIndex(data, length); break;
    case CommandStartLink: handleStartLink(data, length); break;
    case ResponseRequestLink: handleRequestLinkResponse(data, length); break;
    case CommandStopLink: handleStopLink(data, length); break;
    case CommandSetPWM: handleSetPWM(data, length); break;
    case CommandGetSensors: handleGetSensors(data, length); break;

    default:
      if ((command & MaskMessageType) == MaskModuleCommand)
      {
        // Sender expects a response from us
        const uint8_t msg[] = { ResponseUhmWhat, moduleIndex };
        sendCommMessage(msg, sizeof(msg));
      }
  }
}


void handlePing(uint8_t* data, uint8_t length)
{
  const uint8_t msg[] = { ResponsePing, settings.getModuleIndex() };
  sendCommMessage(msg, sizeof(msg));
}


void handleDisplayModuleIndex(uint8_t* data, uint8_t length)
{
  if (state == State::DisplayOff)
    state = State::DisplayModuleIndex;
}


void handleStartLink(uint8_t* data, uint8_t length)
{
  state = State::Linking;
}


void handleRequestLinkResponse(uint8_t* data, uint8_t length)
{
  if (length == 0)
    return;

  if (state != State::LinkingRequest)
    return;

  settings.setModuleIndex(*data);
  state = State::LinkingSet;
}


void handleStopLink(uint8_t* data, uint8_t length)
{
  if (state == State::Linking || state == State::LinkingRequest || state == State::LinkingSet)
    state = State::DisplayModuleIndex;
}


void handleSetPWM(uint8_t* data, uint8_t length)
{
  if (length < 5)
    return;

  uint8_t flags = *data; data++;
  uint16_t value1 = *(reinterpret_cast<uint16_t*>(data)); data += 2;
  uint16_t value2 = *(reinterpret_cast<uint16_t*>(data)); data += 2;

  pwmDriver.setPin(0, value1);
  pwmDriver.setPin(1, value2);

  if ((flags & SetPWMFlagModuleLEDs) == SetPWMFlagModuleLEDs)
  {
    pwmDriver.setPin(2, value1);
    pwmDriver.setPin(3, value2);
  }
  else
  {
    pwmDriver.setPin(2, 0);
    pwmDriver.setPin(3, 0);
  }

  const uint8_t msg[] = { ResponseSetPWM, settings.getModuleIndex() };
  sendCommMessage(msg, sizeof(msg));
}


void handleGetSensors(uint8_t* data, uint8_t length)
{
  uint16_t value1 = analogRead(PinSensor1);
  uint16_t value2 = analogRead(PinSensor2);

  const uint8_t msg[] = { ResponseGetSensors, settings.getModuleIndex(), lowByte(value1), highByte(value1), lowByte(value2), highByte(value2) };
  sendCommMessage(msg, sizeof(msg));
}


uint32_t lastButtonPress = 0;

void checkButtonPress()
{
  if ((state == State::Linking || state == State::LinkingRequest) &&
      digitalRead(PinButton) == HIGH &&
      currentTime - lastButtonPress >= ButtonDebounceTime)
  {
    state = State::LinkingRequest;

    const uint8_t msg[] = { CommandRequestLink };
    sendCommMessage(msg, sizeof(msg));

    lastButtonPress = currentTime;
  }
}