#include <Arduino.h>
#include <SPI.h>
#include <RH_NRF24.h>
#include <RHDatagram.h>
#include "config.h"
#include "clockMsg.h"
#include "master.h"

#if WITH_DISPLAY
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define OLED_RESET /*4*/
Adafruit_SSD1306 display(OLED_RESET);
#endif

// Singleton instance of the radio driver
RH_NRF24 nrf24(PIN_NRF24_CSN, PIN_NRF24_CE);

// Address RH_BROADCAST_ADDRESS  can be used for broadcasts as destination
RHDatagram Datagram(nrf24, THIS_ADRESS);

struct clockMsg_s clockMsg;

int masterConfigPin = PIN_MASTER_CLIENT_SELECT;
static boolean isMaster = true;

// relays for client's physical clock
int relay1pin = PIN_RELAY1;
int relay2pin = PIN_RELAY2;

int holdRelay_ms = DEFAULT_HOLD_RELAY_MS;
int minRelayOffTime_ms = DEFAULT_MIN_RELAY_OFF_TIME_MS;
boolean relayActiveLow = DEFAULT_RELAY_ACTIVE_LOW;

struct {
  uint8_t hour;
  uint8_t minute;
  uint8_t second;
} displayedTime;

void updateRelays(struct clockMsg_s currentTime) {
  // to move forward for one minute, one of the relays is turned
  // on for holdRelay_ms milliseconds, then turned off. Next minute,
  // the other relay is turned on for holdRelay_ms.
  static long lastChange_ms = 0;
  static boolean relay1WasActiveLast = false;
  static enum {relayIdle, relayOn, relayOff} relayStatus = relayIdle;
  long current_ms = millis();

  // Serial.print("currentTime="); Serial.print(currentTime.hour); Serial.print(":"); Serial.print(currentTime.minute); Serial.print(":"); Serial.println(currentTime.second);
  if (relayStatus == relayIdle) {
    if (!((currentTime.hour % 12) == displayedTime.hour && currentTime.minute == displayedTime.minute)) {
      // change updateRelays
      digitalWrite(relay1WasActiveLast ? relay2pin : relay1pin, relayActiveLow ? LOW : HIGH);
      digitalWrite(relay1WasActiveLast ? relay1pin : relay2pin, relayActiveLow ? HIGH : LOW);
      Serial.print("Relay "); Serial.print(relay1WasActiveLast ? 2 : 1); Serial.print(relayActiveLow ? ": LOW, " : ": HIGH, ");
      Serial.print("Relay "); Serial.print(relay1WasActiveLast ? 1 : 2); Serial.println(relayActiveLow ? ": HIGH" : ": LOW");
      Serial.print("last change: "); Serial.print(lastChange_ms); Serial.print(", current: "); Serial.print(current_ms);
      Serial.println(", new relay status: ON");
      relay1WasActiveLast = !relay1WasActiveLast;
      lastChange_ms = current_ms;
      relayStatus = relayOn;
      displayedTime.minute++;
      if (displayedTime.minute >= 60) {
        displayedTime.minute = 0;
        displayedTime.hour++;
        if (displayedTime.hour >= 12) {
          displayedTime.hour = 0;
        }
      }
      Serial.print("displayedTime="); Serial.print(displayedTime.hour); Serial.print(":"); Serial.println(displayedTime.minute);
    }
  } else if (relayStatus == relayOn && current_ms > lastChange_ms + holdRelay_ms) {
    digitalWrite(relay1pin, relayActiveLow ? HIGH : LOW);
    digitalWrite(relay2pin, relayActiveLow ? HIGH : LOW);
    Serial.print("Relay 1: "); Serial.print(relayActiveLow ? "HIGH, " : "LOW, ");
    Serial.print("Relay 2: "); Serial.println(relayActiveLow ? "HIGH" : "LOW");
    Serial.print("last change: "); Serial.print(lastChange_ms); Serial.print(", current: "); Serial.print(current_ms);
    Serial.println(", new relay status: OFF");
    lastChange_ms = current_ms;
    relayStatus = relayOff;
  } else if (relayStatus == relayOff && current_ms > lastChange_ms + minRelayOffTime_ms) {
    Serial.print("last change: "); Serial.print(lastChange_ms); Serial.print(", current: "); Serial.print(current_ms);
    Serial.println(", new relay status: IDLE");
    lastChange_ms = current_ms;
    relayStatus = relayIdle;
  }
}

void setup() {
  Serial.begin(9600);
  while (!Serial)
    ; // wait for serial port to connect. Needed for Leonardo only

  // what is our role?
  pinMode(masterConfigPin, INPUT);

  pinMode(relay1pin, OUTPUT);
  pinMode(relay2pin, OUTPUT);
  displayedTime.hour = 0;
  displayedTime.minute = 0;
  displayedTime.second = 0;
  if (!digitalRead(masterConfigPin)) {
    isMaster = true;
    masterInit();
    Serial.println("In Master-Mode");
  } else {
    isMaster = false;
    Serial.println("In Client-Mode");
  }

  if (!Datagram.init())
    Serial.println("Init datagram with nrf24 failed");

#if WITH_DISPLAY
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.display();
  delay(2000);

  // Clear the buffer.
  display.clearDisplay();

  // draw a single pixel
  display.drawPixel(10, 10, WHITE);
  // Show the display buffer on the hardware.
  // NOTE: You _must_ call display after making any drawing commands
  // to make them visible on the display hardware!
  display.display();
  delay(2000);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.println("Hello, world!");
#endif
}


void clientLoop()
{
  // if (nrf24.available())
  if (Datagram.available())
  {
    // Should be a message for us now
    uint8_t buf[RH_NRF24_MAX_MESSAGE_LEN];
    uint8_t len = sizeof(buf);
    uint8_t from, to, id, flags;
    if (Datagram.recvfrom(buf, &len, &from, &to, &id, &flags)) {
      if (len == sizeof(clockMsg) && buf[0]==msgType_Clock) {
        Serial.print("Clock Msg: ");
        memcpy(&clockMsg, buf, sizeof(clockMsg));
        Serial.print(" h:m:s="); Serial.print(clockMsg.hour); Serial.print(":"); Serial.print(clockMsg.minute); Serial.print(":"); Serial.println(clockMsg.second);
        updateRelays(clockMsg);
      } else {
        Serial.print("got request: ");
        Serial.println((char*)buf);
      }
    }
    else
    {
      Serial.println("*** Datagram.recvfrom failed");
    }
  }
}

void loop() {
  if (isMaster) { masterLoop(Datagram); }
  else { clientLoop(); }
}