#include "MNLib.h"
#define FLASH_DEBUG 0
#define EEPROM_EMULATION_SIZE 1024
#include <FlashStorage_SAMD.h>
#include <ArduinoLowPower.h>

MNConfiguration configuration;
RH_RF95 radio(RF_SS_PIN, RF_IRQ_PIN);
TemperatureZero temperature_sensor = TemperatureZero();
BLAKE2s hash_generator;
uint32_t message_id = 0;
bool _is_client;
uint8_t configuration_memory[CFGMEM];
DeviceBase* devices[N_DEVICES];
uint32_t last_server_message_id = 0;
uint32_t rtc_offset = 0;

//void init_mn(bool is_client = true)
void initMN()
{
  //_is_client = is_client;

  EEPROM.setCommitASAP(false); // Don't unnecessarily write to the EEPROM

  pinMode(LOOPEN_PIN, OUTPUT);
  digitalWrite(LOOPEN_PIN, LOW);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);

  pinMode(MOTOR10, OUTPUT);
  digitalWrite(MOTOR10, LOW);
  pinMode(MOTOR12, OUTPUT);
  digitalWrite(MOTOR12, LOW);

  pinMode(DOUT6, OUTPUT);
  digitalWrite(DOUT6, LOW);
  pinMode(DOUT7, OUTPUT);
  digitalWrite(DOUT7, LOW);

  // explicitly activate inputs, to avoid conflicts
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(DIGIN38, INPUT);
  pinMode(DIGIN2, INPUT);
  pinMode(DIGIN5, INPUT);
  pinMode(DIGIN11, INPUT);
  pinMode(GPIO8, INPUT);
  pinMode(GPIO9, INPUT);
  pinMode(GPIO4, INPUT);
  pinMode(GPIO3, INPUT);
  pinMode(BATMON_PIN, INPUT);

  // Activate LED output
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, OFF);

  // Configuring ADC
  analogReference(AR_INTERNAL2V23);
  analogReadResolution(12);
  while (!radio.init())
  {
    SerialUSB.println("Radio init failed");
    errorBlink(5);
  }

  loadMemory();

  /*if (configuration.client_address == 0xFFFF) // Check for uninitialized flash
    {
    configuration.modem_frequency = 868.0;
    configuration.modem_power = 10;
    configuration.client_address = 0x1FFF;
    configuration.server_address = 0x0001;
    configuration.client_secret_key = 0;
    saveMemory();
    loadMemory();
    }*/

  // Defaults after init are 868.0MHz, 13dBm, Bw = 125 kHz, Cr = 4/5, Sf = 128chips/symbol, CRC on
  radio.setModemConfig(MODEM_CONFIG);

  // You can optionally require this module to wait until Channel Activity
  // Detection shows no activity on the channel before transmitting by setting
  // the CAD timeout to non-zero:
  radio.setCADTimeout(1000);

  //radio.setHeaderFrom(1);
  //radio.setHeaderTo(255);

  // on-chip teperature sensor
  temperature_sensor.init();

  initializeDevices();
  initRTC();

  if (batteryVoltage() < 3.5 && !USBDevice.connected()) // Shut off below this voltage, down to 3.1 V should be OK if the regulator has a dropout of 400 mV and conducts fully if its output is below 3.3 V
    LowPower.deepSleep();
}

void test()
{
  digitalWrite(LED_BUILTIN, ON);

  // activate all five "outputs" for a short period during startup (left to right)
  digitalWrite(LOOPEN_PIN, HIGH);
  delay(1000);
  digitalWrite(LOOPEN_PIN, LOW);
  digitalWrite(10, HIGH);
  delay(1000);
  digitalWrite(10, LOW);
  digitalWrite(12, HIGH);
  delay(1000);
  digitalWrite(12, LOW);
  digitalWrite(6, HIGH);
  delay(1000);
  digitalWrite(6, LOW);
  digitalWrite(7, HIGH);
  delay(1000);
  digitalWrite(7, LOW);
  // switch of LED after startup
  digitalWrite(LED_BUILTIN, OFF);

  // Print Status Report
  printStatusReport();
}

void printStatusReport()
{
  // Status Report: Dumps a bunch of usefull information to SerialUSB

  // Toggle LED
  digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));

  SerialUSB.println("**************************************");
  SerialUSB.println("********** NetMon MultiNode **********");
  SerialUSB.println("**************************************");
  SerialUSB.println();
  SerialUSB.println(__FILE__);
  SerialUSB.print(__DATE__);
  SerialUSB.print(" / ");
  SerialUSB.println(__TIME__);
  SerialUSB.println();
  SerialUSB.println("********** RFM95 LoRa Module *********");
  SerialUSB.print("Client-ID: ");    SerialUSB.print(configuration.client_address);
  SerialUSB.print("\tServer-ID: ");  SerialUSB.print(configuration.server_address);
  //SerialUSB.print("\tRFM-Version: ");  SerialUSB.println(radio.getDeviceVersion());
  SerialUSB.print("Modem-Config: ");  SerialUSB.print(MODEM_CONFIG);
  SerialUSB.print("\tFrequency: ");  SerialUSB.print(MODEM_FREQ);
  SerialUSB.print("\tPower: ");  SerialUSB.println(MODEM_POWER);
  SerialUSB.print("Last RSSI: ");  SerialUSB.print(radio.lastRssi());
  SerialUSB.print("\tFreq-Error: ");  SerialUSB.print(radio.frequencyError());
  SerialUSB.print("\tLast SNR: ");  SerialUSB.println(radio.lastSNR());
  SerialUSB.println();

  SerialUSB.println("********** ATSAMD21G18A MCU *********");
  SerialUSB.print("Internal Temperature (°C): ");
  SerialUSB.println(temperature_sensor.readInternalTemperature());
  SerialUSB.print("Battery Voltage (V):       ");
  SerialUSB.println(float(analogRead(BATMON_PIN)) / 918);

  if (digitalRead(LED_BUILTIN))
    SerialUSB.println("Built-In LED ON");
  else
    SerialUSB.println("Built-In LED OFF");

  if (digitalRead(LOOPEN_PIN))
    SerialUSB.println("LoopSupply ENABLED (12...15 V)");
  else
    SerialUSB.println("LoopSupply DISABLED");

  SerialUSB.println();

  // Toggle LED back to previous state
  digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
}

void setLoopPower(bool state)
{
  digitalWrite(LOOPEN_PIN, state);
}

void saveMemory() // Use sparingly https://github.com/khoih-prog/FlashStorage_SAMD
{
  EEPROM.put(CONFIGURATION_ADDRESS, configuration);
  EEPROM.put(CONFIGURATION_MEMORY_ADDRESS, configuration_memory);
  EEPROM.commit();
}

void loadMemory()
{
  EEPROM.get(CONFIGURATION_ADDRESS, configuration);
  EEPROM.get(CONFIGURATION_MEMORY_ADDRESS, configuration_memory);
  refreshConfig();
}

void refreshConfig()
{
  float frequency = configuration.modem_frequency;
  
  if (MODEM_FREQ_LOWER < value && value < MODEM_FREQ_UPPER)
    radio.setFrequency(configuration.modem_frequency);
    
  else
    radio.setFrequency(0);

  // The default transmitter power is 13dBm, using PA_BOOST.
  // If you are using RFM95/96/97/98 modules which uses the PA_BOOST transmitter pin, then
  // you can set transmitter powers from 2 to 20 dBm:
  radio.setTxPower(configuration.modem_power, false);

  randomSeed(configuration.client_address);

  initializeDevices();
}

float batteryVoltage()
{
  return float(analogRead(BATMON_PIN)) * 2.0 * 2.23 / 4096.0;
}

void errorBlink(int n)
{
  for (int i = 0; i < n; i++)
  {
    delay(ERROR_BLINK_HALF_INTERVAL);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(ERROR_BLINK_HALF_INTERVAL);
    digitalWrite(LED_BUILTIN, LOW);
  }
}

bool send(uint8_t data[], uint8_t len)
{
  if (2 + 2 + 4 + 1 + len + HASH_LENGTH > RH_RF95_MAX_MESSAGE_LEN)
    return false;

  // Full data composed of RX ID, TX ID, Message ID, Length, Data, Hash
  uint8_t full_data[2 + 2 + 4 + 1 + len + HASH_LENGTH];
  uint8_t hash[HASH_LENGTH];

  //if (_is_client)
  //{
  full_data[0] =  configuration.server_address       & 0xFF;
  full_data[1] = (configuration.server_address >> 8) & 0xFF;
  full_data[2] =  configuration.client_address       & 0xFF;
  full_data[3] = (configuration.client_address >> 8) & 0xFF;
  /*} else {
    full_data[0] =  configuration.client_address       & 0xFF;
    full_data[1] = (configuration.client_address >> 8) & 0xFF;
    full_data[2] =  configuration.server_address       & 0xFF;
    full_data[3] = (configuration.server_address >> 8) & 0xFF;
    }*/

  full_data[4] =  message_id        & 0xFF;
  full_data[5] = (message_id >>  8) & 0xFF;
  full_data[6] = (message_id >> 16) & 0xFF;
  full_data[7] = (message_id >> 24) & 0xFF;

  full_data[8] = len;

  for (size_t i = 0; i < len; i++)
  {
    full_data[i + 2 + 2 + 4 + 1] = data[i];
  }

  hash_generator.reset(&configuration.client_secret_key, sizeof(configuration.client_secret_key), HASH_LENGTH);
  hash_generator.update(full_data, 2 + 2 + 4 + 1 + len);
  hash_generator.finalize(hash, HASH_LENGTH);

  for (size_t i = 0; i < HASH_LENGTH; i++)
  {
    full_data[i + 2 + 2 + 4 + 1 + len] = hash[i];
  }

  message_id++;

  bool success = true;
  success = success & radio.send(full_data, 2 + 2 + 4 + 1 + len + HASH_LENGTH);
  radio.waitPacketSent(); // What does the returned value here indicate?
  radio.setModeIdle();
  radio.sleep();
  return success;
}

bool receive()
{
  uint8_t buffer[RH_RF95_MAX_MESSAGE_LEN];
  uint8_t len = sizeof(buffer);

  if (radio.recv(buffer, &len))
  {
    radio.setModeIdle();
    radio.sleep();
    uint16_t target_address = buffer[0] + ((uint16_t)buffer[1] << 8);
    if ((target_address == configuration.client_address || target_address == 0xFFFF) && buffer[2] == (configuration.server_address & 0xFF) && buffer[3] == (configuration.server_address >> 8 & 0xFF))
    {

      hash_generator.reset(&configuration.server_secret_key, sizeof(configuration.server_secret_key), HASH_LENGTH);
      hash_generator.update(buffer, len - HASH_LENGTH);
      uint8_t hash[HASH_LENGTH];
      hash_generator.finalize(hash, HASH_LENGTH);
      for (size_t i = 0; i < HASH_LENGTH; i++)
        if (buffer[len - HASH_LENGTH + i] != hash[i])
          return false;

      uint32_t server_message_id = buffer[4] | ((uint32_t)buffer[5] << 8) | ((uint32_t)buffer[6] << 16) | ((uint32_t)buffer[7] << 24);
      if (server_message_id > last_server_message_id)
      {
        uint8_t packet_length = buffer[8];
        MessageType message_type = (MessageType)buffer[9];

        switch (message_type)
        {
          case MT_Time:
            rtc_offset = server_message_id - RTC->MODE1.COUNT.reg;

            if (server_message_id > message_id) // Ensure that IDs are sequential
              message_id = server_message_id;

            return true;
            break;
        }
      }
    }
  }
  return false;
}

void initializeDevices()
{
  for (int i = 0; i < N_DEVICES; i++)
  {
    devices[i] = getDevice(configuration.devices[i]);
    devices[i]->initialize();
  }
}

DeviceBase* getDeviceFromClass(uint8_t class_type)
{
  switch (class_type)
  {
    case 0:
      return new Device;
      break;
    case 1:
      return new AnalogInput;
      break;
    default:
      return new Device;
      break;
  }
}

DeviceBase* getDevice(uint8_t pointer)
{
  uint8_t class_type = configuration_memory[pointer];
  DeviceBase* device = getDeviceFromClass(class_type);
  if (class_type == 0)
  {
    device->class_type = 0;
    device->sensor_type = 0xFF;
    device->pin = 0xFF;
  }
  else
    memcpy(reinterpret_cast<uint8_t*>(device) + 4, &configuration_memory[pointer], device->size());

  return device;
}

void storeDevice(uint8_t pointer, DeviceBase* device)
{
  memcpy(&configuration_memory[pointer], reinterpret_cast<uint8_t*>(device) + 4, device->size());
}

void loopMN()
{
  receive();

  for (int i = 0; i < N_DEVICES; i++)
    devices[i]->loop();
}

void sendSensorData()
{
  uint8_t buffer[N_DEVICES * (sizeof(float) + sizeof(uint8_t) * 2)];
  uint16_t send_register = 0;
  static_assert(N_DEVICES <= 16, "N_DEVICES too large");
  int cnt = 0;

  for (int i = 0; i < N_DEVICES; i++)
  {
    if (devices[i]->doSend())
    {
      send_register |= 1 << i;
      float value = devices[i]->getValue();
      memcpy(&buffer[cnt * (sizeof(float) + sizeof(uint8_t) * 2)], &devices[i]->sensor_type, sizeof(uint8_t));
      memcpy(&buffer[cnt * (sizeof(float) + sizeof(uint8_t) * 2) + sizeof(uint8_t)], &devices[i]->pin, sizeof(uint8_t));
      memcpy(&buffer[cnt * (sizeof(float) + sizeof(uint8_t) * 2) + sizeof(uint8_t) * 2], &value, sizeof(float));
      cnt++;
    }
  }

  uint8_t data[N_DEVICES * (sizeof(float) + sizeof(uint8_t) * 2) + sizeof(uint16_t) + sizeof(uint8_t)];
  data[0] = MT_SensorStatus;
  memcpy(&data[1], &send_register, sizeof(send_register));
  memcpy(&data[3], &buffer, cnt * (sizeof(float) + sizeof(uint8_t) * 2));
  send(data, sizeof(uint8_t) + sizeof(uint16_t) + cnt * (sizeof(float) + sizeof(uint8_t) * 2));
}

void sendDeviceData()
{
  float temperature = temperature_sensor.readInternalTemperature();
  float voltage = batteryVoltage();
  uint8_t data[9];
  data[0] = MT_DeviceStatus;
  memcpy(&data[1], &voltage, 4);
  memcpy(&data[5], &temperature, 4);
  send(data, 9);
}

void initRTC() // https://github.com/arduino-libraries/RTCZero/blob/master/src/RTCZero.cpp
{
  PM->APBAMASK.reg |= PM_APBAMASK_RTC;
#ifndef CRYSTALLESS
  SYSCTRL->XOSC32K.reg = SYSCTRL_XOSC32K_ONDEMAND |
                         SYSCTRL_XOSC32K_RUNSTDBY |
                         SYSCTRL_XOSC32K_EN32K |
                         SYSCTRL_XOSC32K_XTALEN |
                         SYSCTRL_XOSC32K_STARTUP(6) |
                         SYSCTRL_XOSC32K_ENABLE;
#endif

  GCLK->GENDIV.reg = GCLK_GENDIV_ID(2) | GCLK_GENDIV_DIV(4);
  while (GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY);
#ifdef CRYSTALLESS
  GCLK->GENCTRL.reg = (GCLK_GENCTRL_GENEN | GCLK_GENCTRL_SRC_OSCULP32K | GCLK_GENCTRL_ID(2) | GCLK_GENCTRL_DIVSEL );
#else
  GCLK->GENCTRL.reg = (GCLK_GENCTRL_GENEN | GCLK_GENCTRL_SRC_XOSC32K | GCLK_GENCTRL_ID(2) | GCLK_GENCTRL_DIVSEL );
#endif
  while (GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY);
  GCLK->CLKCTRL.reg = (uint32_t)((GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK2 | (RTC_GCLK_ID << GCLK_CLKCTRL_ID_Pos)));
  while (GCLK->STATUS.bit.SYNCBUSY);

  RTC->MODE0.CTRL.reg &= ~RTC_MODE0_CTRL_ENABLE; // disable RTC

  RTC->MODE0.CTRL.reg |= RTC_MODE0_CTRL_SWRST; // software reset

  RTC->MODE0.CTRL.reg = RTC_MODE0_CTRL_PRESCALER_DIV1024 | RTC_MODE0_CTRL_MODE_COUNT32;
  RTC->MODE0.CTRL.reg &= ~RTC_MODE0_CTRL_MATCHCLR; // disable clear on match

  RTC->MODE0.CTRL.reg |= RTC_MODE0_CTRL_ENABLE; // enable RTC
}

uint32_t getRTC()
{
  return RTC->MODE1.COUNT.reg + rtc_offset;
}