#include <Arduino.h>
#include "config.h"
#include "clockMsg.h"
#include "master.h"


static struct clockMsg_s clockMsg;
static unsigned long lastSentTimeTick = 0;
static unsigned long sendFastclockTimer_ms = 3000;
static unsigned long msPerModelSecond = 50; // 500 = real time
static unsigned long sendFailedCounter = 0, pauseTime = 0, stopTime = 0;
static struct clock_s {
  uint8_t day;
  uint8_t hour;
  uint8_t minute;
  uint8_t second;
  uint16_t millisecond;
} fastclock;

void masterInit() {
  clockMsg.hour = 0;
  clockMsg.minute = 0;
  clockMsg.second = 0;
  lastSentTimeTick = millis();
  Serial.print("*** Setting up master, init lastSentTimeTick="); Serial.println(lastSentTimeTick);
  fastclock.day = 0;
  fastclock.hour = 0;
  fastclock.minute = 0;
  fastclock.second = 0;
  fastclock.millisecond = 0;
}


static void incrementClockByMilliseconds(int amount) {
  fastclock.millisecond += amount;
  if (fastclock.millisecond >= 1000) {
    int carryover = fastclock.millisecond / 1000;
    fastclock.millisecond = fastclock.millisecond % 1000;
    fastclock.second += carryover;
    if (fastclock.second >= 60) {
      carryover = fastclock.second / 60;
      fastclock.second = fastclock.second % 60;
      fastclock.minute += carryover;
      if (fastclock.minute >= 60) {
        carryover = fastclock.minute / 60;
        fastclock.minute = fastclock.minute % 60;
        fastclock.hour += carryover;
        if (fastclock.hour >= 24) {
          carryover = fastclock.hour / 24;
          fastclock.hour = fastclock.hour % 24;
          fastclock.day += carryover;
          if (fastclock.day >= 7) {
            fastclock.day = fastclock.day % 7;
          }
        }
      }
    }
  }
  Serial.print("*** new clock: ");
  Serial.print(fastclock.hour); Serial.print(":");
  Serial.print(fastclock.minute); Serial.print(":");
  Serial.print(fastclock.second); Serial.print(".");
  Serial.print(fastclock.millisecond);
  Serial.print(" day "); Serial.print(fastclock.day);
  Serial.print(", incBy_ms="); Serial.print(amount);
}

static void timeTick(void) {
  long newTimeTick = millis();
  int fastclockTimeAdvance = (newTimeTick - lastSentTimeTick) * 1000 / msPerModelSecond;

  incrementClockByMilliseconds(fastclockTimeAdvance);
  lastSentTimeTick = newTimeTick;
  Serial.print("*** tick (adv="); Serial.print(fastclockTimeAdvance); Serial.println("%d)");
}


#ifdef FCRF_RadioHead
void masterLoop(RHDatagram Datagram)
{
  static unsigned long nextTimeTick_ms = millis();
  static unsigned long updateEvery_ms = 1000;
  static uint8_t hour=0, minute=0, second=0;

  timeTick();
  if (Datagram.available())
  {
    // Should be a message for us now
    uint8_t buf[RH_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 {
      clockMsg.msgType = msgType_Clock;
      clockMsg.hour = fastclock.hour;
      clockMsg.minute = fastclock.minute;
      clockMsg.second = fastclock.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");
      }
    }
  }
}
#endif

#ifdef FCRF_RF24
extern const byte *addresses[];
static bool TX=1,RX=0,role=RX;

static void switchToSenderRole(RF24 radio)
{
  Serial.println("*** master: CHANGING TO TRANSMIT ROLE");
  radio.openWritingPipe(addresses[1]);
  radio.openReadingPipe(1,addresses[0]);
  radio.stopListening();
  role = TX;                  // Become the primary transmitter (ping out)
}

/*
static void switchToReceiverRole(RF24 radio)
{
  Serial.println("*** master: CHANGING TO RECEIVER ROLE");
  radio.openWritingPipe(addresses[0]);
  radio.openReadingPipe(1,addresses[1]);
  radio.startListening();
  role = RX;                // Become the primary receiver (pong back)
}
*/

void masterLoop(RF24 radio) {
  timeTick();

  if (millis() - lastSentTimeTick > sendFastclockTimer_ms) {
    lastSentTimeTick = millis();
    clockMsg.msgType = msgType_Clock;
    clockMsg.hour = fastclock.hour;
    clockMsg.minute = fastclock.minute;
    clockMsg.second = fastclock.second;
    switchToSenderRole(radio);
    if (!radio.write(&clockMsg, sizeof(clockMsg), true /*multicast*/)) {
        sendFailedCounter++;
    }
    //This is only required when NO ACK ( enableAutoAck(0) ) payloads are used
    if (millis() - pauseTime > 3) {
      pauseTime = millis();
      radio.txStandBy();          // Need to drop out of TX mode every 4ms if sending a steady stream of multicast data
      //delayMicroseconds(130);     // This gives the PLL time to sync back up
    }
    stopTime = millis();
                                        //This should be called to wait for completion and put the radio in standby mode after transmission, returns 0 if data still in FIFO (timed out), 1 if success
    if (!radio.txStandBy()) { sendFailedCounter += 3; } //Standby, block only until FIFO empty or auto-retry timeout. Flush TX FIFO if failed
    //radio.txStandBy(1000);              //Standby, using extended timeout period of 1 second

    Serial.println("*** send finished");
  }
}
#endif