#include <SPI.h>
#include <RH_RF95.h>
#include <Process.h>
#include <EEPROM.h>
#include <FileIO.h>
#include <Console.h>

// Comment to disable verbose debugging.
#define DEBUG_VERBOSE

// Comment to disable encryption.
//#define ENCRYPT_ENABLE

// Comment to disable the LED.
#define LED_ENABLE

#ifdef LED_ENABLE
    // Define pin of the LED.
    #define LED_PIN A2
#endif

// Define the LoRa RF95_FREQ use for this client.
#define RFM95_RST 9
#define RF95_FREQ 868.00
#define RFM95_BAND 500000
#define RFM95_SPREAD 12

// Gateway ID
#define ID_GATEWAY 15

// Gateway ID address in EEPROM.
//#define ID_ADDRESS 0x00

// Max clients to support, more clients will increase time to establish network and polling.
#define MAX_CLIENT 3

// Time to refresh the network. Will add new clients after network refresh.
#define REFRESH_TIME 30000

// The no response retry count before it stops the broadcast message and finishes network setup.
#define MAX_BROADCAST_RETRY 15

// How many retries after which to kick a client if there is no response.
#define MAX_REMOVE_RETRY 5

#ifdef ENCRYPT_ENABLE
    #include "encrypt.h"
    ENCRYPT  encrypt_decrypt;

    // Define the encrypt key, so different group of LoRa devices won't communicate with each other.
    unsigned char encryptkey[16] = {0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF};
#endif


RH_RF95 rf95;

String sensor_data = "Start";

int gateway_id = 0;

int network_setup_finish = 0 ; // Whether network has finished setting up.
int network_loop_count = 2;
int b = 0; // Network set up counter

char clients[MAX_CLIENT] = {0}; // Client ID array
int client_numbers = 0;// Client count
int broadcast_retry = 0 ; // Broadcast retry counter
int no_response_count = 0; // Counter for no response from client

long poll_start_time = 0;
long total_poll_time = 0;

int CrcFlag = 0;



void setup() {
    manualReset();

    Bridge.begin(115200);
    Console.begin();
    delay(100);

    while (!Console)
        delay(1);
    Console.print("Bridge ready!\n");

    delay(100);
    setupRadio();
    #ifdef LED_ENABLE
        pinMode(LED_PIN, OUTPUT);
    #endif
    store_data_linux(true); // Store the data on Linux
}

uint16_t calcByte(uint16_t crc, uint8_t b) {
    uint32_t i = 0;
    crc = crc ^ (uint32_t)b << 8;

    for (i = 0; i < 8; i++) {
        if ((crc & 0x8000) == 0x8000)
            crc = crc << 1 ^ 0x1021;
        else
            crc = crc << 1;
    }
    return crc & 0xffff;
}

uint16_t CRC16(uint8_t *pBuffer, uint32_t length) {
    uint16_t wCRC16 = 0;
    uint32_t i = 0;

    if ((pBuffer == 0) || (length == 0))
        return 0;

    for (i = 0; i < length; i++)
        wCRC16 = calcByte(wCRC16, pBuffer[i]);

    return wCRC16;
}

uint16_t recdata(unsigned char* recbuf, int Length) {
    uint16_t crcdata = 0;
    uint16_t recCRCData = 0;

    crcdata = CRC16(recbuf, Length - 2); // Calculate the CRC for the received message
    recCRCData = recbuf[Length - 1]; // Get the CRC high byte
    recCRCData = recCRCData << 8; // Get the CRC low byte
    recCRCData |= recbuf[Length - 2]; // Get the receive CRC

    if (crcdata == recCRCData) {
        CrcFlag = 1;
        crcdata = 0;
        recCRCData = 0;
        #ifdef DEBUG_VERBOSE
            Console.println("CRC++++");
        #endif
    }
    else {
        CrcFlag = 0 ;
        crcdata = 0;
        recCRCData = 0;
    }
}

void set_up_network(void) {
    uint8_t broadcast[4] = {0}; // Broadcast Message
    broadcast[0] = 'B';
    broadcast[1] = 'C';
    broadcast[2] = ':';
    broadcast[3] = gateway_id;
    int length = sizeof(broadcast); // Get broadcast message length

    #ifdef ENCRYPT_ENABLE
        encrypt_decrypt.btea(broadcast, length, encryptkey); // Encrypt the broadcast message
    #endif
    for (b = 0; b < network_loop_count; b++) { // Loop to set up network
        #ifdef DEBUG_VERBOSE
            Console.println("Send broadcast message");
        #endif
        rf95.send(broadcast , sizeof(broadcast));
        rf95.waitPacketSent();
        #ifdef DEBUG_VERBOSE
            Console.println("Packet sent");
        #endif

        if (rf95.waitAvailableTimeout(1000)) { // Check the incoming message
            Console.println("Get an incoming message");
            uint8_t buf[RH_RF95_MAX_MESSAGE_LEN];
            uint8_t len = sizeof(buf);
            if (rf95.recv(buf, &len)) {
                #ifdef ENCRYPT_ENABLE
                    encrypt_decrypt.btea(buf, -len, encryptkey); // Decode the message
                #endif

                /*------JOIN REQUEST-------*/

                if (buf[0] == 'J' && buf[1] == 'R' && buf[2] == ':') { // Get a join request, store the client ID
                    #ifdef DEBUG_VERBOSE
                        Console.println(buf[0]);
                        Console.println(buf[1]);
                        Console.println(buf[2]);
                        Console.println(buf[3]);
                    #endif

                    if (buf[3] == 0) { // Error Message
                        Console.println("SET_UP_NETWORK: Error Message");
                    }
                    else {
                        for (int b = 0; b < 3; b++) { // Send 3 Join ACK to the client, make sure it arrives

                            /*------JOIN ACCEPT-------*/

                            uint8_t JoinAck[4] = {0}; // Join ACK message
                            JoinAck[0] = 'J';
                            JoinAck[1] = 'A';
                            JoinAck[2] = ':';
                            JoinAck[3] = buf[3]; // Put client ID
                            Console.println("SET_UP_NETWORK: Sending Join Accept: ");

                            int length = sizeof(JoinAck); // Get data length
                            #ifdef ENCRYPT_ENABLE
                                encrypt_decrypt.btea(JoinAck, length, encryptkey); // Encode
                            #endif
                            Console.print("Send Join ACK to client:");
                            Console.println(buf[3]);
                            rf95.send(JoinAck, sizeof(JoinAck));
                            rf95.waitPacketSent();
                            broadcast_retry = 0;
                            delay(500);
                        }
                        int pos ;
                        for (pos = 0; pos < sizeof(clients); pos++) { // Check if this client is already stored
                            if (clients[pos] == buf[3]) {
                                Console.println("SET_UP_NETWORK: Client already registered: " + buf[3]);
                                break;
                            }
                        }
                        if (pos == sizeof(clients)) {
                            Console.println("SET_UP_NETWORK: Registering Client: " + buf[3]);

                            clients[client_numbers] = buf[3]; // Store the client id
                            client_numbers++;
                            broadcast_retry = 0; // Get a new client, reset retry count
                        }
                    }
                }
                else {
                    Console.println("SET_UP_NETWORK: Join Request Message Error");
                }
            }
        }
        else {
            broadcast_retry++;
            #ifdef DEBUG_VERBOSE
                Console.println("SET_UP_NETWORK: Broadcast retry");
            #endif

            if (broadcast_retry > MAX_BROADCAST_RETRY) { // Check if retry count exceed the max retry, if yes, network set up done
                network_setup_finish = 1;
                broadcast_retry = 0;
                break;
            }
        }
    }
    delay(100);
}

void polling_clients(void) {
    int polling_count = 0;
    poll_start_time = millis();

    for (polling_count = 0; polling_count < 2; polling_count++) { // Send data requst to every client one by one
        uint8_t query[4] = {0}; // Data request message
        query[3] = clients[polling_count];
        for (int poll_cnt = 0; poll_cnt < MAX_REMOVE_RETRY; poll_cnt++) { // Send a data requst message

            /*------DATA REQUEST SEND---------*/

            uint8_t query[4] = {0}; // Data request message
            query[0] = 'D';
            query[1] = 'R';
            query[2] = ':';
            query[3] = clients[polling_count] ; // Client ID to be polled

            #ifdef DEBUG_VERBOSE
                Console.print("POLLING -> Request Data from client: ");
                Console.print(query[3]);
                Console.print(" ;count: ");
                Console.println(poll_cnt);
            #endif

            int length = sizeof(query); // Get length
            #ifdef ENCRYPT_ENABLE
                encrypt_decrypt.btea(query, length, encryptkey); // Encode
            #endif
            rf95.send(query, sizeof(query)); // Send poll message
            rf95.waitPacketSent();

            /*------DATA REQUEST RECEIVE---------*/

            if (rf95.waitAvailableTimeout(3000)) {
                Console.println("POLLING -> Get incoming message");
                uint8_t buf[RH_RF95_MAX_MESSAGE_LEN] = {0};
                uint8_t len = sizeof(buf);
                if (rf95.recv(buf, &len)) {
                    no_response_count = 0;
                    #ifdef ENCRYPT_ENABLE
                        encrypt_decrypt.btea(buf, - len, encryptkey); // Decode incoming message
                    #endif
                    recdata(buf, len); // Calculate CRC
                    sensor_data = "";
                    for (int a = 0; a < len - 2; a++) // Store data
                        if (a == 3 || a == 5)
                            sensor_data += buf[a];
                        else
                            sensor_data += (char)buf[a];

                    #ifdef LED_ENABLE
                        digitalWrite(LED_PIN, HIGH);
                    #endif

                    no_response_count = 0;
                    if (CrcFlag == 1) { // CRC calculation successful, match
                        #ifdef DEBUG_VERBOSE
                            Console.println("POLLING -> CRC succesful");
                        #endif
                        Console.print("POLLING -> Received data: ");
                        Console.println(sensor_data);
                        store_data_linux(false); // Store the data on Linux

                        #ifdef LED_ENABLE
                            digitalWrite(LED_PIN, LOW);
                        #endif

                        CrcFlag = 0;
                        if (buf[0] == 'D' && buf[1] == 'S' && buf[3] == query[3]) {
                            Console.print("POLLING -> Received data: ");
                            Console.println(sensor_data);
                            store_data_linux(false); // Store the data on Linux
                            no_response_count = 0;

                            break;
                        }
                        else {
                            no_response_count ++;
                        }
                    }
                    else {
                        Console.println("CRC Error");

                        #ifdef LED_ENABLE
                            digitalWrite(LED_PIN, LOW);
                        #endif
                    }
                }
            }
            else {
                no_response_count ++;
                if (no_response_count == MAX_REMOVE_RETRY) { // No response from client, remove it after max retry
                    no_response_count = 0;
                    clients[polling_count] = 0;
                    Console.print("Client ID:");
                    Console.println(clients[polling_count]);
                }
            }
        }
    }
    total_poll_time += millis() - poll_start_time; // Get total poll time
    Console.println(total_poll_time);
    if (total_poll_time > REFRESH_TIME) { // Refresh the network
        Console.println("Refresh network after timeout");
        network_setup_finish = 0;
        total_poll_time = 0;
    }
}

void loop() {
    #ifdef DEBUG_VERBOSE
        Console.println("Start concurrent client: ");
        #ifdef ENCRYPT_ENABLE
            Console.print("Encrypt encryptkey :");
            Console.println((char*)encryptkey);
        #endif
        Console.print("Frequency :");
        Console.println(RF95_FREQ);
        Console.print("Gateway ID :");
        Console.println(gateway_id);
    #endif
    if (network_setup_finish == 0) {
        Console.println("Network setup...");
        set_up_network(); // Set up the network
    }
    else {
        Console.println("Polling...");
        polling_clients(); // Request data from clients
    }
}

void store_data_linux(bool first) {
    Console.print("Writing data...");

    /*
    File script = FileSystem.open("/root/sensor_data/test.sh", FILE_WRITE);
    // Shell script header
    script.print("#!/bin/sh\n");
    // Make the script executable
    Process chmod;
    chmod.begin("chmod");      // chmod: change mode
    chmod.addParameter("+x");  // x stays for executable
    chmod.addParameter("/root/sensor_data/test.sh");  // path to the file to make it executable
    chmod.run();

    //  Process p;   // Create a process and call it "p"
    //  p.begin("/root/sensor_data"); // Store data in Linux file /root/test.sh
    //  p.addParameter(sensor_data); // add the data
    //  Console.println("Writing to file.");
    //
    //  p.run();    // Run the process and wait for its termination
    */

    File data = FileSystem.open("/var/iot/data", (first)?FILE_WRITE:FILE_APPEND);
    data.println(sensor_data);
    data.close();

    Console.print(" Done.\n");
}

void setupRadio() {
    while (!rf95.init()) {
        Console.println("LoRa radio init failed");
        Console.println("Uncomment '#define Console_DEBUG' in RH_RF95.cpp for detailed debug info");
        while (1);
    }
    Console.println("LoRa radio init OK!");
    if (!rf95.setFrequency(RF95_FREQ)) {
        Console.println("setFrequency failed");
        while (1);
    }
    rf95.setTxPower(13, false);
    rf95.setSignalBandwidth(RFM95_BAND);
    rf95.setSpreadingFactor(RFM95_SPREAD);
    gateway_id = ID_GATEWAY;
    //gateway_id = EEPROM.read(ID_ADDRESS); // Get gateway ID
    FileSystem.begin();
}

void manualReset() {
    pinMode(RFM95_RST, OUTPUT);
    delay(10);
    digitalWrite(RFM95_RST, HIGH);
    delay(10);
}