#include "MNLib.h"
#define FLASH_DEBUG 0
#define EEPROM_EMULATION_SIZE 1024
#include <FlashStorage_SAMD.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];

//void init_mn(bool is_client = true)
void init_mn()
{
  //_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();
}

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()
{
  radio.setFrequency(configuration.modem_frequency);

  // 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);

  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); // Does sizeof() work here?
  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?
  return success;
}

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

DeviceBase* getDevice(uint8_t pointer)
{
  uint8_t class_type = configuration_memory[pointer];
  if (class_type == 0)
  {
    DeviceBase* device = new Device;
    device->class_type = 0;
    device->sensor_type = 0xFF;
    device->pin = 0xFF;
    return device;
  }
  if (class_type == 1)
  {
    DeviceBase* device = new AnalogInput;
    //memcpy(device + 4, &configuration_memory[pointer], sizeof(AnalogInput) - 4); // There is some 4 bytes extra at the beginning
    memcpy(reinterpret_cast<uint8_t*>(device) + 4, &configuration_memory[pointer], sizeof(AnalogInput) - 4); // There is some 4 bytes extra at the beginning
    return device;
  }

  return new Device;
}

void loopSensors()
{
  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));
}