#include <Arduino.h>
#include <SPI.h>
#include <RH_NRF24.h>
#include <RHDatagram.h>
#include "clockMsg.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(8, 10); // (CSN, CE)
// RH_NRF24 nrf24(8, 7); // use this to be electrically compatible with Mirf
// RH_NRF24 nrf24(8, 10);// For Leonardo, need explicit SS pin
// RH_NRF24 nrf24(8, 7); // For RFM73 on Anarduino Mini

#define nRF_Channel 1
#define THIS_ADRESS 0 // uint8_t address of this node
// Address RH_BROADCAST_ADDRESS  can be used for broadcasts as destination
RHDatagram Datagram(nrf24, THIS_ADRESS);

struct clockMsg_s clockMsg;

int masterConfigPin = 2;
static boolean isMaster = true;

// relays for client's physical clock
int relay1pin = 5;
int relay2pin = 6;
// configs:
int holdRelay_ms = 150;
int minRelayOffTime_ms = 80;
boolean relayActiveLow = true;

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;
    Serial.println("In Master-Mode");
  } else {
    isMaster = false;
    Serial.println("In Client-Mode");
  }

  if (!Datagram.init())
    Serial.println("Init datagram with nrf24 failed");
  // Defaults after init are 2.402 GHz (channel 2), 2Mbps, 0dBm
  /*if (!nrf24.setChannel(nRF_Channel))
    Serial.println("setChannel failed");
  if (!nrf24.setRF(RH_NRF24::DataRate2Mbps, RH_NRF24::TransmitPower0dBm))
    Serial.println("setRF 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 masterLoop()
{
  static unsigned long nextTimeTick_ms = millis();
  static unsigned long updateEvery_ms = 1000;
  static uint8_t hour=0, minute=0, second=0;

  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)) {
      Serial.print("got request: ");
      Serial.println((char*)buf);
    }
    else
    {
      Serial.println("*** Datagram.recvfrom failed");
    }
  } else {
    // prepare clock info
    if (nextTimeTick_ms < millis()) {
      nextTimeTick_ms += updateEvery_ms;
      second++;
      if (second >= 60) {
        second -= 60;
        minute++;
        if (minute >= 60) {
          minute -= 60;
          hour++;
          if (hour >= 24) {
            hour -= 24;
          }
        }
      }
      clockMsg.msgType = msgType_Clock;
      clockMsg.hour = hour;
      clockMsg.minute = minute;
      clockMsg.second = second;

      // send clock info as a broadcast message

      if (Datagram.sendto((uint8_t *) &clockMsg, sizeof(clockMsg), RH_BROADCAST_ADDRESS)) {
        Serial.print(hour); Serial.print(":");
        Serial.print(minute); Serial.print(":");
        Serial.print(second); Serial.print(" - ");
        Serial.println("Sent new clock tick");
      }
    }
  }
}


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(); }
  else { clientLoop(); }
}