#include <ArduinoLowPower.h>
#include <SPI.h>
#include "MNLib.h"

//#define IS_CLIENT
//#define IS_SERVER

unsigned long tick_tracker_sensors = 0;
unsigned long tick_tracker_device = 0;
unsigned long next_tick_sensors = 0;
unsigned long next_tick_device = 0;
unsigned long offset = 0;

void setup()
{
  SerialUSB.begin(115200);
  //while (!SerialUSB);
  /*for (int i = 0; i < 5; i++)
    {
    delay(1000);
    SerialUSB.println("owo");
    }*/
  initMN();
  /*for (int i = 0; i < 2; i++)
    {
    delay(1000);
    SerialUSB.println("awa");
    }
    printStatusReport();*/
  tick_tracker_sensors = millis();
  tick_tracker_device = millis();
}

void loop()
{
  if (SerialUSB.available())
  {
    size_t config_size = sizeof(MNConfiguration) - sizeof(configuration.client_secret_key) - sizeof(configuration.server_secret_key); // Don't leak the secret key
    char command = SerialUSB.read();

    switch (command)
    {
      case 'C': // Print configuration for in the field debugging
        SerialUSB.println("Configuration:");
        SerialUSB.print("Frequency: ");
        SerialUSB.print(configuration.modem_frequency);
        SerialUSB.println(" MHz");
        SerialUSB.print("Power: ");
        SerialUSB.print(configuration.modem_power);
        SerialUSB.println(" dBm");
        SerialUSB.print("Client address: ");
        SerialUSB.println(configuration.client_address);
        SerialUSB.print("Server address: ");
        SerialUSB.println(configuration.server_address);

        for (int i = 0; i < N_DEVICES; i++)
        {
          SerialUSB.print("Pointer ");
          SerialUSB.print(i);
          SerialUSB.print(": ");
          SerialUSB.println(configuration.devices[i]);
          if (configuration.devices[i] != 255)
          {
            for (int j = 0; j < sizeof(AnalogInput); j++)
            {
              SerialUSB.print(reinterpret_cast<uint8_t*>(devices[i])[j]);
              SerialUSB.print(" ");
            }
            SerialUSB.println();
            devices[i]->printStatus();
          }
        }

        SerialUSB.println();
        SerialUSB.print("Battery voltage: ");
        SerialUSB.println(batteryVoltage());

        SerialUSB.print("RTC time: ");
        SerialUSB.println(RTC->MODE1.COUNT.reg);
        SerialUSB.println("END");
        break;

      case 'c':
        SerialUSB.print("N_DEVICES: ");
        SerialUSB.println(N_DEVICES);
        SerialUSB.print("CFGMEM: ");
        SerialUSB.println(CFGMEM);
        SerialUSB.print("MNConfiguration: ");
        SerialUSB.println(sizeof(MNConfiguration) - sizeof(configuration.client_secret_key) - sizeof(configuration.server_secret_key));
        SerialUSB.print("Device: ");
        SerialUSB.println(sizeof(Device) - 4);
        SerialUSB.print("AnalogInput: ");
        SerialUSB.println(sizeof(AnalogInput) - 4);
        SerialUSB.println("END");
        break;

      case 'r': // Read configuration (Frequency, which sensors etc.)
        SerialUSB.write(reinterpret_cast<char*>(&configuration), config_size);
        break;

      case 'w':
        SerialUSB.readBytes(reinterpret_cast<char*>(&configuration), config_size);
        refreshConfig();
        tick_tracker_sensors = millis();
        tick_tracker_device = millis();
        break;

      case 'R': // Read configuration memory (extended configuration for each sensor)
        SerialUSB.write(reinterpret_cast<char*>(&configuration_memory), sizeof(configuration_memory));
        break;

      case 'W':
        SerialUSB.readBytes(reinterpret_cast<char*>(&configuration_memory), sizeof(configuration_memory));
        refreshConfig();
        break;

      case 'k':
        char data[sizeof(MNConfiguration)];
        SerialUSB.readBytes(reinterpret_cast<char*>(&configuration) + config_size, sizeof(configuration.client_secret_key) + sizeof(configuration.server_secret_key));
        SerialUSB.write(reinterpret_cast<char*>(&configuration) + config_size, sizeof(configuration.client_secret_key) + sizeof(configuration.server_secret_key));

        refreshConfig();
        break;

      case 's':
        saveMemory();
    }
  }

  loopMN();

  long now_time = millis() + offset;

  if (now_time >= next_tick_sensors)
  {
    sendSensorData();
    tick_tracker_sensors = tick_tracker_sensors + configuration.sensor_update_interval;
    next_tick_sensors = tick_tracker_sensors + random(configuration.jitter);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(2);
    digitalWrite(LED_BUILTIN, LOW);
  }

  if (now_time >= next_tick_device)
  {
    sendDeviceData();
    tick_tracker_device = tick_tracker_device + configuration.device_update_interval;
    next_tick_device = tick_tracker_device + random(configuration.jitter);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(2);
    digitalWrite(LED_BUILTIN, LOW);
  }

  long next_tick_device_dt = next_tick_device - now_time;
  long next_tick_sensors_dt = next_tick_sensors - now_time;
  long min_delay = min(next_tick_device_dt, next_tick_sensors_dt);
  
  if (min_delay > 1000 && !USBDevice.connected())
  {
    power_sleep(min_delay);
    offset += min_delay;
    //USBDevice.init()
  }
}

void power_sleep(uint32_t milliseconds)
{
  radio.setModeIdle();
  //radio.sleep(); Doesn't work
  setLoopPower(OFF);
  LowPower.sleep(milliseconds);
  radio.setModeIdle();
  delay(100);
}