/*
 * Stairs lighting
 * Copyright 2017 (c) Mark van Renswoude
 *
 * https://git.x2software.net/pub/Stairs
*/
#include <Arduino.h>
#include <Wire.h>
#include "global.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.setAddress(0x40);

  // 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(76800);
  comm.begin();
}



// 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 loop()
{
  currentTime = millis();

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

  display.update();
}



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 & 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 & MaskModuleCommand)
      {
        // Sender expects a response from us
        const uint8_t msg[] = { ResponseUhmWhat, moduleIndex };
        comm.sendMsg(msg, sizeof(msg));
      }
  }
}


void handlePing(uint8_t* data, uint8_t length)
{
  const uint8_t msg[] = { ResponsePing, settings.getModuleIndex() };
  comm.sendMsg(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::Linking)
    return;

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


void handleStopLink(uint8_t* data, uint8_t length)
{
  if (state == State::Linking || 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.setPWM(0, value1);
  pwmDriver.setPWM(1, value2);

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

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


void handleGetSensors(uint8_t* data, uint8_t length)
{
  // TODO get sensor values

  uint8_t sensor1 = 0;
  uint8_t sensor2 = 0;

  const uint8_t msg[] = { ResponseSetPWM, settings.getModuleIndex(), sensor1, sensor2 };
  comm.sendMsg(msg, sizeof(msg));
}