//AVR
#include <avr/sleep.h>
#include <avr/wdt.h>

//Arduino
#include <EEPROM.h>

//Third party
#include <SerialCommand.h>

#include <U8g2lib.h>             // Display
#include <ClosedCube_HDC1080.h>  // Temperature, humidity
#include <RFM69.h>               // Wireless
#include <Adafruit_TSL2561_U.h>  // Light Sensor


#define SERIAL_BAUD 9600
#define BUTTON_INTERRUPT_PIN 3
#define LED_PIN 4
const char ENCRYPTKEY[] PROGMEM = "TajneHeslo123456";

volatile bool powerDownEnabled = true;
volatile bool wdtInterrupted = true; // To send data after power up
uint8_t lastSource = 0;
int16_t lastRssi = 0;
long int prepis = 0;


struct Settings
{
  byte networkid;
  byte nodeid;
};

Settings settings;

struct Packet
{
  float temperature;
  float humidity;
  uint32_t light;
};

struct Hal
{
  bool tsl;
  bool hdc;
};

Hal hal;

SerialCommand SCmd;

// Senzor teploty a vlhkosti
ClosedCube_HDC1080 hdc;

// RF modul
RFM69 radio;

// Oled display
U8X8_SSD1306_128X64_NONAME_HW_I2C oled(U8G2_R0);

// Senzor svetla
Adafruit_TSL2561_Unified tsl(TSL2561_ADDR_FLOAT, 12345);


void setup(void)
{
  Serial.begin(SERIAL_BAUD);

  Serial.println(F("[CMD]"));
  SCmd.addCommand("sleep", cmdSleep);
  SCmd.addCommand("gNetworkId", cmdGetNetowrkId);
  SCmd.addCommand("sNetworkId", cmdSetNetworkId);
  SCmd.addCommand("gNodeId", cmdGetNodeId);
  SCmd.addCommand("sNodeId", cmdSetNodeId);
  SCmd.addDefaultHandler(cmdUnrecognized);


  Serial.print(F("[EEPROM] "));
  EEPROM.get(0, settings);
  Serial.print(F("Network id: "));
  Serial.print(settings.networkid);
  Serial.print(F(" Node id: "));
  Serial.println(settings.nodeid);


  Serial.println(F("[PINS]"));
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);
  pinMode(BUTTON_INTERRUPT_PIN, INPUT_PULLUP);


  Serial.println(F("[OLED]"));
  oled.begin();
  oled.setFont(u8x8_font_chroma48medium8_r);
  oled.draw2x2String(0, 0, "Booting");


  Serial.print(F("[HDC1080] "));
  hdc.begin(0x40);
  hal.hdc = true;
  if( hal.hdc )
  {
    uint16_t manId = hdc.readManufacturerId();
    uint16_t devId = hdc.readDeviceId();
    Serial.print(F("Manufacture: "));
    Serial.print(manId, HEX);
    Serial.print(F(" Device: "));
    Serial.println(devId, HEX);
  }
  else
  {
    Serial.println(F(" Failed"));
  }


  Serial.print(F("[TSL2561]"));
  hal.tsl = tsl.begin();
  if( hal.tsl )
  {
    tsl.enableAutoRange(true);
    tsl.setIntegrationTime(TSL2561_INTEGRATIONTIME_13MS);
    sensor_t sensor;
    tsl.getSensor(&sensor);
    Serial.print(F(" Sensor: "));
    Serial.print(sensor.name);
    Serial.print(F(" Ver: "));
    Serial.print(sensor.version);
    Serial.print(F(" ID: "));
    Serial.println(sensor.sensor_id);
  }
  else
  {
    Serial.println(F(" Failed"));
  }

  Serial.print(F("[RFM69] "));
  bool init = radio.initialize(RF69_868MHZ, settings.nodeid, settings.networkid);
  if(init)
  {
    radio.setHighPower(true);
    radio.setPowerLevel(2); // 0-31
    radio.encrypt(ENCRYPTKEY);
    uint8_t temp = radio.readTemperature();
    Serial.print(F("Temperature: "));
    Serial.println(temp);
  }
  else
  {
    Serial.println(F(" Failed"));
  }

}


void loop(void)
{
  SCmd.readSerial();


  if (radio.receiveDone())
  {
    Serial.print(F("[RFM69] Incoming packet from: "));
    Serial.print(radio.SENDERID, DEC);
    Serial.print(F(" LEN: "));
    Serial.print(radio.DATALEN);
    Serial.print(F(" RSSI: "));
    Serial.println(radio.RSSI);

    if(settings.nodeid == 0)
    {
      masterPacket();
    }
    else
    {
      nodePacket();
    }
  }


  // If timed out set new interval
  if( wdtInterrupted )
  {
    if(settings.nodeid == 0)
    {
      masterLoop();
    }
    else
    {
      nodeLoop();
    }

    // Set new wdt timer
    wdtInterrupted = false;

    wdt_enable(WDTO_8S);
    WDTCSR |= (1 << WDIE);
  }

  attachInterrupt(digitalPinToInterrupt(BUTTON_INTERRUPT_PIN), ButtonInterrupt, LOW);

  if( powerDownEnabled )
  {
    powerDown();
  }
}

// Receive
void masterPacket()
{
  if(radio.DATALEN == sizeof(Packet))
  {
    Serial.print(F("[RFM69] DATA: "));

    Packet *p = (Packet*)radio.DATA;
    printPacket(p);

    lastSource = radio.SENDERID;
    lastRssi = radio.RSSI;
  }
  else
  {
    Serial.println(F("[RFM69] Invalid packet size"));
  }
}


void masterLoop()
{
  sensors_event_t event;
  tsl.getEvent(&event);
  uint32_t light = event.light;

  updateDisplay(hdc.readTemperature(), hdc.readHumidity(), light, lastSource, lastRssi);
}

// Receive
void nodePacket()
{

}


void nodeLoop()
{
  Packet p;

  Serial.print(F("[SENSORS] Reading"));

  if( hal.hdc )
  {
    p.temperature = hdc.readTemperature();
    p.humidity = hdc.readHumidity();
    Serial.print(F(" hdc1080"));
  }

  if( hal.tsl )
  {
    sensors_event_t event;
    tsl.getEvent(&event);
    p.light = event.light;
    Serial.println(F(" tsl2561"));
  }

  // Send to master (nodeid == 0)
  // 255 = broadcast
  Serial.print(F("[RFM69] Sending packet: "));
  printPacket(&p);
  radio.send(0, &p, sizeof(Packet));

  updateDisplay(p.temperature, p.humidity, p.light, lastSource, lastRssi);
}


void printPacket(Packet *p)
{
    Serial.print(F("S: "));
    Serial.print(radio.SENDERID, DEC);
    Serial.print(F(" R: "));
    Serial.print(radio.RSSI, DEC);
    Serial.print(F(" T: "));
    Serial.print(p->temperature);
    Serial.print(F(" H: "));
    Serial.print(p->humidity);
    Serial.print(F(" L: "));
    Serial.print(p->light);

    Serial.println(F(""));
}

void updateDisplay(float t, float h, uint32_t l, uint8_t source, int16_t rssi)
{
  Serial.println(F("[OLED] Updating"));
  const int MAX_LEN = 17;
  char line[MAX_LEN];

  ++prepis;

  // 1. zluty radek
  snprintf(line, MAX_LEN, "Net:%3d SIG:%4d", settings.networkid, rssi);
  oled.drawString(0, 0, line);

  // 2. zluty radek
  snprintf(line, MAX_LEN, "Node:%3d Src:%3d", settings.nodeid, source);
  oled.drawString(0, 1, line);

  // 3. modry radek
  dtostrf(t, 4, 2, line);
  snprintf(line, MAX_LEN, "%s C", line);
  //oled.draw2x2String(0, 2, line);
  oled.drawString(0, 2, line);

  // 4. modry radek
  dtostrf(h, 4, 2, line);
  snprintf(line, MAX_LEN, "%s %%", line);
  //oled.draw2x2String(0, 4, line);
  oled.drawString(0, 3, line);

  // 5. modry radek
  dtostrf(l, 4, 0, line);
  snprintf(line, MAX_LEN, "%s lux", line);
  oled.drawString(0, 4, line);

  // 8. Posledni radek
  snprintf(line, MAX_LEN, "%d x", prepis);
  oled.drawString(0, 7, line);
}


void cmdSleep()
{
  powerDownEnabled = true;
}

void cmdGetNetowrkId()
{
  Serial.print(F("Network ID: "));
  Serial.println(settings.networkid);
}

void cmdSetNetworkId()
{
  char *arg = SCmd.next();

  if(arg)
  {
    uint8_t nid = constrain(atoi(arg), 0, 255);
    Serial.print(F("new Network ID: "));
    Serial.println(nid);
    settings.networkid = nid;

    EEPROM.put(0, settings);
    radio.setNetwork(nid);
  }
  else
  {
    Serial.println(F("[CMD] Missing argument"));
  }
}


void cmdGetNodeId()
{
  Serial.print(F("Node ID: "));
  Serial.println(settings.nodeid);
}


void cmdSetNodeId()
{
  char *arg = SCmd.next();

  if(arg)
  {
    uint8_t nid = constrain(atoi(arg), 0, 255);
    Serial.print(F("new Node ID: "));
    Serial.println(nid);
    settings.nodeid = nid;

    EEPROM.put(0, settings);
    radio.setAddress(nid);
  }
  else
  {
    Serial.println(F("[CMD] Missing argument"));
  }
}


void cmdUnrecognized()
{
  Serial.println(F("[CMD] Uncrecognized commands"));
}


void ButtonInterrupt(void)
{
  detachInterrupt(digitalPinToInterrupt(BUTTON_INTERRUPT_PIN));
  powerDownEnabled = false;
}


ISR(WDT_vect)
{
  wdt_disable();

  wdtInterrupted = true;
}

void powerDown()
{
  digitalWrite(LED_PIN, LOW);
  Serial.flush();
  radio.receiveDone();  // Must be here to have working waking interrupt when packet is received
  //attachInterrupt(digitalPinToInterrupt(BUTTON_INTERRUPT_PIN), ButtonInterrupt, LOW);

  //Turn off ADC
  ADCSRA &= ~(1 << ADEN);

  //Do the sleep according to documentation
  set_sleep_mode(SLEEP_MODE_PWR_DOWN);
  cli();
  sleep_enable();
  sleep_bod_disable();
  sei();
  sleep_cpu();
  sleep_disable();
  sei();

  //Turn on ADC
  ADCSRA |= (1 << ADEN);

  digitalWrite(LED_PIN, HIGH);
}