#include <Wire.h>
#include "Pca9554.h"
#include "TcsBus.h"
#include "TcsBusMessages.h"

#define PIN_AUDIO_READ  0
#define PIN_API_SWITCH  4
#define PIN_TCS_READ   13
#define PIN_TCS_SEND   14
#define PIN_AUDIO_SEND 17

#define SREG_PIN_API_SWITCH    0x01u
#define SREG_PIN_LED1R         0x02u
#define SREG_PIN_REL1CTRL      0x04u
#define SREG_PIN_AUDIO_SEND_EN 0x08u
#define SREG_PIN_REL2CTRL      0x10u
#define SREG_PIN_LED1G         0x20u
#define SREG_PIN_LED2R         0x40u
#define SREG_PIN_LED2G         0x80u

#define LEDS 2

enum class LedColor : uint8_t {
  OFF          = 0,
  RED          = 1,
  GREEN        = 2,
  YELLOW       = 3,

  UBOUND
};

typedef struct LedState_s {
  enum LedColor color;
  bool blink;
} LedState_t;


const uint8_t LedBitmask[LEDS][(uint8_t) LedColor::UBOUND] = {
  {
    0, SREG_PIN_LED1R, SREG_PIN_LED1G, (SREG_PIN_LED1R | SREG_PIN_LED1G)
  },
  {
    0, SREG_PIN_LED2R, SREG_PIN_LED2G, (SREG_PIN_LED2R | SREG_PIN_LED2G)
  },
};

static void DoorbellEvt(const TcsBus::EventData_t* evt)
{

}

static const TcsBus::EventData_t doorbellEvt = {
  .func = DoorbellEvt,
  .bits = doorbell,
  .bitCount = sizeof(doorbell),
};

static LedState_t ledStates[LEDS] = {
  { LedColor::OFF, false },
  { LedColor::OFF, false }
};

static uint32_t ms_old = 0;

static TcsBus tcs = TcsBus(PIN_TCS_SEND, PIN_TCS_READ);
static Pca9554 sreg = Pca9554();
static bool blinkOn = false;

static void SetLed(uint8_t ledNr, LedState_t newState) {
  uint8_t ledBitmask = LedBitmask[ledNr][(uint8_t) newState.color];
  uint8_t ledAll     = LedBitmask[ledNr][(uint8_t) LedColor::YELLOW];

  if(newState.blink && !blinkOn) {
    ledBitmask = LedBitmask[ledNr][(uint8_t) LedColor::OFF];
  }
  sreg.outputValues = (sreg.outputValues & ~ledAll) | ledBitmask;
}

void setup() {
  // put your setup code here, to run once:
  Wire.begin();
  Serial.begin(115200);

  tcs.begin();

  tcs.RegisterEvent(&doorbellEvt);

  //const uint8_t sregOutputs = (uint8_t) (SREG_PIN_LED1R | SREG_PIN_REL1CTRL | SREG_PIN_AUDIO_SEND_EN | SREG_PIN_REL2CTRL | SREG_PIN_LED1G | SREG_PIN_LED2R | SREG_PIN_LED2G);
  const uint8_t sregInputs  = (uint8_t) (SREG_PIN_API_SWITCH | SREG_PIN_AUDIO_SEND_EN);

  sreg.begin(sregInputs);

  sreg.outputValues = 0;
  ms_old = millis();

  ledStates[1].color = LedColor::YELLOW;
  ledStates[0].blink = true;
  //ledStates[1].color = LedColor::YELLOW;
  //ledStates[1].blink = true;
}


void i2cScan() {
  for(uint8_t a = 0; a < 0x80; a++) {
    Wire.beginTransmission(a);
    byte error = Wire.endTransmission();
    if(!error) Serial.printf("i2cScan: found 0x%0x\n", a);
  }
}

void loop() {
  uint32_t ms = millis();
  if(ms_old != ms) {
    if(ms_old < ms) ms_old++; else ms_old = ms; //prevent overflow

    static uint32_t blinkCountdown = 500;
    blinkCountdown--;
    if(!blinkCountdown) {
      blinkCountdown = 500;
      blinkOn = !blinkOn;
      SetLed(0, ledStates[0]);
      SetLed(1, ledStates[1]);
      Serial.printf("0x%02x\r\n", sreg.outputValues);
      sreg.SendOutputValues();
    }    

    uint8_t inputs = sreg.ReadInputs();
    static uint8_t inputs_old = 0;
    if((inputs & SREG_PIN_API_SWITCH) != (inputs_old & SREG_PIN_API_SWITCH)) {
      Serial.printf("Switch=%u\n", inputs & SREG_PIN_API_SWITCH);
    }
    inputs_old = inputs;
  }
}