// standard library
#include <stdio.h>

// Driverlib includes
#include <inc/hw_types.h>
#include <driverlib/interrupt.h>
#include <inc/hw_ints.h>
#include <inc/hw_apps_rcm.h>
#include <inc/hw_common_reg.h>
#include <driverlib/prcm.h>
#include <driverlib/rom.h>
#include <driverlib/rom_map.h>
#include <inc/hw_memmap.h>
#include <driverlib/timer.h>
#include <driverlib/utils.h>

// energia library
#include <SLFS.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include <Wire.h>
#include <BMA222.h>

// Common interface includes
#include "timer_if.h"
#include "gpio_if.h"
#include "List.h"
#include "Networks.h"
#include "crypto.h"
#include "rng.h"

#if defined(ccs)
extern void (* const g_pfnVectors[])(void);
#endif
#if defined(ewarm)
extern uVectorEntry __vector_table;
#endif

#define MLEN 32

using namespace std;

char choice = 0; // use choice
char sent = 0;
List * known_networks;
char total_networks = 0;
char send_data = 0;

BMA222 my_sensor;

uint16_t ID = 1050;  // Device ID
uint16_t port = 9000;
IPAddress server(82,36,88,190);
WiFiClient wclient;

uint8_t aes_key[16] = {
        0x03, 0x02, 0x01, 0x00,
        0x07, 0x06, 0x05, 0x04,
        0x0b, 0x0a, 0x09, 0x08,
        0x0f, 0x0e, 0x0d, 0x0c
};

typedef struct sMessage {
  uint32_t acc;
  uint8_t light;
  uint8_t empty[12];
  
}Message;

typedef struct {
    uint16_t ID;
    uint16_t LEN;
} header;

uint8_t AESData[MLEN], *AESResult, AESResult2[MLEN];
uint32_t AESDataLength=MLEN;

uint8_t buffer[32] = {0};
uint16_t bufferPnt = 0;

static volatile unsigned long g_ulSysTickValue;
static volatile unsigned long g_ulBase;
static volatile unsigned long g_ulRefBase;
static volatile unsigned long g_ulRefTimerInts = 0;
static volatile unsigned long g_ulIntClearVector;
unsigned long g_ulTimerInts;

static void BoardInit(void)
{
    /* In case of TI-RTOS vector table is initialize by OS itself */
    #ifndef USE_TIRTOS
      //
      // Set vector table base
      //
    #if defined(ccs)
        MAP_IntVTableBaseSet((unsigned long)&g_pfnVectors[0]);
    #endif
    #if defined(ewarm)
        MAP_IntVTableBaseSet((unsigned long)&__vector_table);
    #endif
    #endif
    //
    // Enable Processor
    //
    MAP_IntMasterEnable();
    MAP_IntEnable(FAULT_SYSTICK);

    PRCMCC3200MCUInit();
}

void TimerBaseIntHandler(void)
{
    //
    // Clear the timer interrupt.
    //
    Timer_IF_InterruptClear(g_ulBase);

   send_data = 1;
}

void setup() {
  //Initialize serial and wait for port to open:
  Serial.begin(115200);
  
  SerFlash.begin();
  my_sensor.begin();

  g_ulBase = TIMERA0_BASE;
  Timer_IF_Init(PRCM_TIMERA0, g_ulBase, TIMER_CFG_PERIODIC, TIMER_A, 0);
  Timer_IF_IntSetup(g_ulBase, TIMER_A, TimerBaseIntHandler);
  
  WiFi.init();
  
  known_networks = new List();
  
  read_from_file();

  Serial.print("Setup RNG ..");
  RNGSetup();
  Serial.println("OK");

  Serial.print("Setup AES ..");
  AESSetup(AES_KEY_128BIT, aes_key);
  Serial.println("OK");

  Serial.print("Setup WiFi ..");
  auto_connect_networks();
  Serial.println("OK");

  Serial.print("Setup DHCP ..");
  while (WiFi.localIP() == INADDR_NONE) {delay(200);}
  Serial.print("OK ");
  Serial.println(WiFi.localIP());
    
  Serial.print("Setup Socket ..");
  while(wclient.connect(server, port) == false) {delay(200);}
  Serial.println("OK");
  pinMode(PIN_04, INPUT);
  Timer_IF_Start(g_ulBase, TIMER_A, 1000);
}

void loop() {
  // menu
  if(sent == 0)
  {
    Serial.println("****Menu****");
    Serial.println("1) Auto connect to Network");
    Serial.println("2) Send Data");
    Serial.println("3) Add Network");
    Serial.println("4) Print Network");
    sent = 1;
  }
  
  if(Serial.available() > 0)
  {
    choice = Serial.read();
    sent = 0;
    switch(choice)
    {
      case '1':
        // scan for existing networks:
        Serial.println("Scanning available networks...");
        auto_connect_networks();
        
      break;
      case '2':
        for(uint8_t i = 65; i <= 90; i++)
        {
          buffer[bufferPnt] = i;
          bufferPnt++;
        }
        // send();
      break;
      case '3':
        add_network();
      break;
      case '4':
        print_networks();
      break;
      default:
        Serial.println("Value has to be between 1 and 3");
      break;
    }
  }
  if(send_data == 1)
  {
    calc_acc();
  }
}

void print_networks()
{
  ListNode * current = known_networks->get_first();
  Networks * current_network;
  while(current != nullptr)
  {
    current_network = (Networks *)current->get_data();
    Serial.println(current_network->get_ssid());
    Serial.println(current_network->get_passwd());
    current = current->get_next();
  }
}

void auto_connect_networks() 
{
  int32_t rssi;
  ListNode * current; // pointer used to traverse the list
  Networks * current_network = nullptr; // used to see the data
  Networks * strongest_network = nullptr; // used to see the data
  bool connection = false;
  int num_ssid = 0;
  
  while(connection == false)
  {
    num_ssid = WiFi.scanNetworks();
    if (num_ssid == -1)
    {
      Serial.println("Couldn't get a wifi connection");
    }

    Serial.print("number of available networks:");
    Serial.println(num_ssid);
    
    rssi = -100;  // set the defualt rssi to -100
    
    for (int i = 0; i < num_ssid; i++) 
    {
      current = known_networks->get_first();
      while(current != nullptr)
      {
        current_network = (Networks *)current->get_data();  // type cast the current data to valid type
        if(strcmp(current_network->get_ssid(), WiFi.SSID(i)) == 0)  // compare current ssid in list with ssid scanned
        {
          if(WiFi.RSSI(i) > rssi) // check if this network strength is greater than   current strongest network
          {
            strongest_network = current_network; // set the id equal to the current number i
          }
          break;
        }
        else
        {
         strongest_network = nullptr; 
        }
        current = current->get_next();
      }
    }

    if(strongest_network != nullptr)
    {
      WiFi.begin(strongest_network->get_ssid(), strongest_network->get_passwd());
      delay(1000);  // wait 1 second to connect
    
      if( WiFi.status() == WL_CONNECTED)  // if connected exit
      {
        Serial.println("Connected");
        connection = true; 
      }
    }
    else
    {
      Serial.println("Known WiFi not available");
    }
  }
}

void add_network()
{
  char c_ssid[30] = "WannaSeeMyGates";
  
  char c_passwd[30] = "password";
  
  Networks * new_network = new Networks(c_ssid, c_passwd);
  known_networks->insert_at_front((void *) new_network);
  write_to_file();
}

void read_from_file(void)
{
  // open in read mode
  int32_t retval = SerFlash.open("/binfile/networks.bin", FS_MODE_OPEN_READ);
  // file does not exsit
  if (retval != SL_FS_OK) 
  {
    Serial.print("Error reading file /binfile/networks.bin, error code: ");
    Serial.println(SerFlash.lastErrorString());
    Serial.flush();  // flush pending serial output before entering suspend()
  }
  else
  {
    char ssid[30];
    char passwd[30];
    Networks * new_network;
    size_t read_len = SerFlash.size();
    Serial.print("File size: ");
    Serial.println(read_len);
    for(int i = 0; i < (read_len/60); i++)
    {
      retval = SerFlash.readBytes(ssid, sizeof(ssid));
      if(retval < 0) // if less than 0 break
      {
        break;
      }

      retval = SerFlash.readBytes(passwd, sizeof(passwd));
      if(retval < 0) // if less than 0 break
      {
        break; 
      }
      Serial.println(ssid);
      Serial.println(passwd);
      new_network = new Networks(ssid, passwd);
      known_networks->insert_at_front((void *) new_network);
      total_networks++;
    }
  }
  SerFlash.close();
}

void write_to_file(void)
{
  uint32_t retval;
  retval = SerFlash.open("/binfile/networks.bin", FS_MODE_OPEN_CREATE((total_networks * 60), _FS_FILE_OPEN_FLAG_COMMIT));
  if (retval != SL_FS_OK) 
  {
    Serial.print("Error creating file /binfile/networks.bin, error code: ");
    Serial.println(SerFlash.lastErrorString());
    Serial.flush(); 
  }

  ListNode * current = known_networks->get_first();
  Networks * current_network;
  while(current != nullptr)
  {
    current_network = (Networks *)current->get_data();
    Serial.println(current_network->get_ssid());
    Serial.println(current_network->get_passwd());
    SerFlash.write(current_network, sizeof(Networks));
    current = current->get_next();
  }
  SerFlash.close();
}

void send(Message * m) 
{
    uint32_t resLen = 0;
    Serial.print("encrypting..");
    uint8_t *res = AESEncrypt((uint8_t*)m, 16, &resLen);
    Serial.println(resLen);
    header h = {ID, resLen};
    wclient.write((uint8_t *) &h, sizeof(h)); 
    wclient.write(res, (uint16_t) resLen);
    free(res);
    memset(buffer, 0x00, 32);
    bufferPnt = 0;
}

void calc_acc(void)
{
  Message * m = (Message *)malloc(sizeof(Message));
  int minVal = 0;
  int maxVal = 65;
  double x;
  double y;
  double z;
  int32_t idatax = my_sensor.readXData();
  int ixAng = map(idatax, minVal, maxVal, -90, 90);
 
  int32_t idatay = my_sensor.readYData();
  int iyAng = map(idatay, minVal, maxVal, -90, 90);
 
  int32_t idataz = my_sensor.readZData();
  int izAng = map(idataz, minVal, maxVal, -90, 90);
 
  delay(100);
 
  int32_t fdatay = my_sensor.readYData();
  int fyAng = map(fdatay, minVal, maxVal, -90, 90);
 
  int32_t fdataz = my_sensor.readZData();
  int fzAng = map(fdataz, minVal, maxVal, -90, 90);
 
  int32_t fdatax = my_sensor.readXData();
  int fxAng = map(fdatax, minVal, maxVal, -90, 90);
 
  int xAng = fxAng - ixAng;
  int yAng = fyAng - iyAng;
  int zAng = fzAng - izAng;
 
  int32_t deltax = (fdatax - idatax)/0.1;
  int32_t deltay = (fdatay - idatay)/0.1;
  int32_t deltaz = (fdataz - idataz)/0.1;

  deltax = deltax * deltax;
  deltay = deltay * deltay;
  deltaz = deltaz * deltaz;

  m->acc = (uint32_t)sqrt(deltax + deltay + deltaz);
 
  x = RAD_TO_DEG*(atan2(-yAng, -zAng) + PI);
  y = RAD_TO_DEG*(atan2(-xAng, -zAng) + PI);
  z = RAD_TO_DEG*(atan2(-yAng, -xAng) + PI);
  
  m->light = digitalRead(PIN_04);
  while(wclient.connect(server, port) == true) 
  {
    Serial.println("Reconnecting");
    delay(200);
 }
  send(m);
  free(m);
  send_data = 0;
}