#include <ArduinoLowPower.h>
#include <RHReliableDatagram.h>
#include <RH_RF95.h>
#include <SPI.h>
#include <TemperatureZero.h>´


#define CLIENT_ADDRESS 1
#define SERVER_ADDRESS 2
#define MODEM_CONFIG RH_RF95::Bw125Cr45Sf128
#define MODEM_POWER 13
#define MODEM_FREQ 868.0

const int RF_SS_PIN = 1;
const int RF_IRQ_PIN = 0;
const int BATMON_PIN = A5;
const int BOOSTEN_PIN = 27;

const float DIVIDER_mA = 183.677;
const float DIVIDER_V = 408.5;


byte LoopState = 0;
char inByte;
unsigned long nextTick;
unsigned int msTick = 500;


// RFM95 connected to Pin 1 for SS an Pin 0 for interrupt
RH_RF95 rfm95(RF_SS_PIN, RF_IRQ_PIN);

// Class to manage message delivery and receipt, using the rfm95 declared above
RHReliableDatagram rfManager(rfm95, CLIENT_ADDRESS);

// Internal on-chip Temperature sensor
TemperatureZero TempZero = TemperatureZero();


void setup()
{
  // Pins with internal functions
  pinMode(BOOSTEN_PIN, OUTPUT);
  digitalWrite(BOOSTEN_PIN, LOW);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);

  /*// Acivating Motor and Digital Outputs, but at LOW level
    pinMode(10, OUTPUT);
    digitalWrite(10, LOW);
    pinMode(12, OUTPUT);
    digitalWrite(12, LOW);

    pinMode(6, OUTPUT);
    digitalWrite(6, LOW);
    pinMode(7, OUTPUT);
    digitalWrite(7, LOW);

    // explcitly activate inputs, to avoid conflicts
    pinMode(38, INPUT);
    pinMode(2, INPUT);
    pinMode(5, INPUT);
    pinMode(11, INPUT);

    // Configuring ADC
    analogReference(AR_INTERNAL2V23);
    analogReadResolution(12);


    SerialUSB.begin(9600);
    //while (!SerialUSB) ; // Wait for serial port to be available


    if (!rfManager.init())
    SerialUSB.println("init failed");

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

    // 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:
    rfm95.setTxPower(MODEM_POWER, false);
    rfm95.setFrequency(MODEM_FREQ);

    // 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:
    // rfm95.setCADTimeout(10000);


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

    // activate all five "outputs" for a short period during startup (left to right)
    digitalWrite(BOOSTEN_PIN, HIGH);
    delay(1000);
    digitalWrite(BOOSTEN_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);

    // Print Status Report
    PrintSR();

    // switch of LED after startup
    digitalWrite(LED_BUILTIN, LOW);
    rfm95.sleep();*/
  digitalWrite(LED_BUILTIN, LOW);
  for (int i = 0; i < 10; i++)
  {
    digitalWrite(LED_BUILTIN, HIGH);
    delay(100);
    digitalWrite(LED_BUILTIN, LOW);
    delay(100);
  }
}


// from RadioHead sample
uint8_t data[] = "Hello World!";
// Dont put this on the stack:
uint8_t buf[RH_RF95_MAX_MESSAGE_LEN];



void loop()
{
  while (!USBDevice.connected())
  {
    LowPower.sleep(1000);
    USBDevice.init();
  }
  for (int i = 0; i < 10; i++)
  {
    digitalWrite(LED_BUILTIN, HIGH);
    delay(100);
    digitalWrite(LED_BUILTIN, LOW);
    delay(100);
  }
  /*if (USBDevice.connected())
    digitalWrite(LED_BUILTIN, HIGH);
    else
    digitalWrite(LED_BUILTIN, LOW);*/
  return;
  digitalWrite(BOOSTEN_PIN, HIGH);
  delay(2000);
  digitalWrite(BOOSTEN_PIN, LOW);
  delay(2000);
  LowPower.deepSleep(4000);
  return;

  /*
    SerialUSB.println("Sending to rf95_reliable_datagram_server");

    // Send a message to manager_server
    if (rfManager.sendtoWait(data, sizeof(data), SERVER_ADDRESS))
    {
     SerialUSB.println("...got an ACK");
    // Now wait for a reply from the server
    uint8_t len = sizeof(buf);
    uint8_t from;
    if (rfManager.recvfromAckTimeout(buf, &len, 2000, &from))
    {
      SerialUSB.print("got reply from : 0x");
      SerialUSB.print(from, HEX);
      SerialUSB.print(": ");
      SerialUSB.println((char*)buf);
    }
    else
    {
      SerialUSB.println("No reply, is rf95_reliable_datagram_server running?");
    }
    }
    else
    SerialUSB.println("sendtoWait failed");
  */

  // TICK-ROUTINE
  if (LoopState && millis() > nextTick)
  {
    switch (LoopState) {
      case 1:   // Read and print Currents
        SerialUSB.print(float(analogRead(A1)) / DIVIDER_mA, 3);
        SerialUSB.print("\t");
        SerialUSB.print(float(analogRead(A2)) / DIVIDER_mA, 3);
        SerialUSB.print("\t");
        SerialUSB.println(float(analogRead(A3)) / DIVIDER_mA, 3);
        break;

      case 2:   // Read and print Voltages
        SerialUSB.print(float(analogRead(A1)) / DIVIDER_V, 3);
        SerialUSB.print("\t");
        SerialUSB.print(float(analogRead(A2)) / DIVIDER_V, 3);
        SerialUSB.print("\t");
        SerialUSB.println(float(analogRead(A3)) / DIVIDER_V, 3);
        break;

      case 3:   // Read and print all GPIO and DigINPUTs
        SerialUSB.print(digitalRead(8));
        SerialUSB.print("\t");
        SerialUSB.print(digitalRead(9));
        SerialUSB.print("\t \t");
        SerialUSB.print(digitalRead(4));
        SerialUSB.print("\t");
        SerialUSB.print(digitalRead(3));
        SerialUSB.print("\t \t \t");
        SerialUSB.print(digitalRead(38));
        SerialUSB.print("\t");
        SerialUSB.print(digitalRead(2));
        SerialUSB.print("\t \t");
        SerialUSB.print(digitalRead(5));
        SerialUSB.print("\t");
        SerialUSB.println(digitalRead(11));
        break;

      case 4:   // Read and Print Battery Voltage
        SerialUSB.println(float(analogRead(BATMON_PIN)) / 918, 3);
        break;

      case 5:   // Read and Print internal Temperature
        SerialUSB.println(TempZero.readInternalTemperature());
        break;
    }

    nextTick = millis() + msTick;
  }

  // SERIAL CLI
  if (SerialUSB.available() > 0) {

    inByte = SerialUSB.read();
    SerialUSB.read();

    if (LoopState == 1)
      digitalWrite(BOOSTEN_PIN, LOW);
    //else if(LoopState == 4)
    //  msTick = 500;

    switch (inByte) {
      case 'c':
        SerialUSB.println("Currents in mA (switches in lower pos)");
        LoopState = 1;
        digitalWrite(BOOSTEN_PIN, HIGH);
        break;
      case 'v':
        SerialUSB.println("Voltages in V (switches in upper pos)");
        LoopState = 2;
        break;
      case 'd':
        SerialUSB.println("GPIO and Digital INPUTS");
        LoopState = 3;
        break;
      case 'b':
        SerialUSB.println("Battery Voltage in V");
        //msTick = 5000;
        LoopState = 4;
        break;
      case 't':
        SerialUSB.println("Searching for DS18B20-Sensors, connected to the four 1-Wire pins...");

        SerialUSB.println("On-Chip Temperature in °C");
        LoopState = 5;
        break;
      case 's':
        SerialUSB.println("Setting Radio Module and MCU in sleep mode for 10 seconds...");
        rfm95.sleep();
        LowPower.deepSleep(10000);
        break;
      default:
        LoopState = 0;
        PrintSR();
        break;
    }
  }

  /*
    delay(5000);
    rfm95.sleep();
    delay(5000);
    LowPower.deepSleep(10000);
  */

}


void PrintSR() {
  // 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(rfManager.thisAddress());
  SerialUSB.print("\tServer-ID: ");  SerialUSB.print(SERVER_ADDRESS);
  SerialUSB.print("\tRFM-Version: ");  SerialUSB.println(rfm95.getDeviceVersion());
  SerialUSB.print("\tRFM-Message Length: ");  SerialUSB.println(rfm95.maxMessageLength());
  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(rfm95.lastRssi());
  SerialUSB.print("\tFreq-Error: ");  SerialUSB.print(rfm95.frequencyError());
  SerialUSB.print("\tLast SNR: ");  SerialUSB.println(rfm95.lastSNR());
  SerialUSB.println();

  SerialUSB.println("********** ATSAMD21G18A MCU *********");
  SerialUSB.print("Internal Temperature (°C): ");
  SerialUSB.println(TempZero.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(BOOSTEN_PIN))
    SerialUSB.println("LoopSupply ENABLED (12...15 V)");
  else
    SerialUSB.println("LoopSupply DISABLED");

  SerialUSB.println();
  SerialUSB.println("Send c, v, d, b or t to plot Analog and Digital readings.");
  SerialUSB.println("Send s for 10 seconds of sleep mode.");

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