#include <Adafruit_EEPROM_I2C.h>
#include <RHReliableDatagram.h>
#include <RH_RF95.h>
#include <Adafruit_EEPROM_I2C.h>

#define BROADCAST_ADDRESS 255
#define MASTER_NODE_ID 0
#define RFM95_CS    8
#define RFM95_INT   3
#define RFM95_RST   4
#define RF95_FREQ 915.0
#define EEPROM_START_ADDRESS 0
Adafruit_EEPROM_I2C EEPROM;

#define EEPROM_ADDR 0x50  


#define MAX_NEIGHBORS 254
#define MAX_HOPS 20


#define MESSAGE_TYPE_BROADCAST 0
#define MESSAGE_TYPE_COMMAND 1
#define MESSAGE_TYPE_SENSOR_DATA 2
#define MESSAGE_TYPE_ACK 4
#define MESSAGE_TYPE_NETWORK_ADDITION_PROPOSAL 10
#define MESSAGE_TYPE_NETWORK_NEIGHBOUR_UPDATE 11
#define MESSAGE_TYPE_NETWORK_ROUTE_REQUEST 12




#define MESSAGE_CONSUMED 5
#define MESSAGE_NOT_CONSUMED 6



#define MAX_NODES 75
#define MAX_NODE_NEIGHBORS 15
#define MAX_SAVED_MSGS 100

struct Node {
    int nodeId;
    int neighbors[MAX_NODE_NEIGHBORS];
    int neighborsCount;
};

struct Message {
    uint8_t type;
    uint8_t senderID;
    uint8_t lastRelayID;
    uint8_t targetID;
    char id[10]; 
    uint8_t hops;
    char route[10]; 
    char masterRoute[10]; 
    char data[20];  
    uint8_t sensorType;
    uint16_t sensorValue;
};
struct Neighbor {
    uint8_t nodeID;
    unsigned long lastSeen;
};

Node graph[MAX_NODES];
Neighbor neighbors[MAX_NEIGHBORS];
uint8_t neighborCount = 0;

RH_RF95 rf95(RFM95_CS, RFM95_INT);
RHReliableDatagram manager(rf95, MASTER_NODE_ID);
Message incomingMsg;
uint8_t masterIdList[MAX_NEIGHBORS]; 
int masterIdListLength = 0;
char consumedMessageIds[MAX_SAVED_MSGS][10];
char relayedMessageIds[MAX_SAVED_MSGS][10];
int consumedMsgCounter = 0;
int relayedMsgCounter = 0;
void updateGraph(Node graph[], int nodeId, int neighbors[], int neighborsCount, bool toRemove);

//TODO MAster should Broadcast prescence
void setup() {

    Serial.begin(4800);
    delay(10);
    Serial.println("LoRa MASTER NODE");
    if (EEPROM.begin(EEPROM_ADDR)) {  // you can stick the new i2c addr in here, e.g. begin(0x51);
        Serial.println("Found I2C EEPROM");
    } else {
        Serial.println("I2C EEPROM not identified ... check your connections?\r\n");
        while (1) delay(10);
    }

    pinMode(RFM95_RST, OUTPUT);
    digitalWrite(RFM95_RST, HIGH);
 

    digitalWrite(RFM95_RST, LOW);
    delay(10);
    digitalWrite(RFM95_RST, HIGH);
    delay(10);

    while (!rf95.init()) {
        while (1);
    }
    // Defaults after init are 434.0MHz, modulation GFSK_Rb250Fd250, +13dbM
    if (!rf95.setFrequency(RF95_FREQ)) {
        while (1);
    }
    // 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 5 to 23 dBm:
    rf95.setTxPower(20, false);

    // Initialize EEPROM
    if (EEPROM.read(EEPROM_START_ADDRESS) != 5) {
        EEPROM.write(EEPROM_START_ADDRESS, 5);
        EEPROM.write(EEPROM_START_ADDRESS + 1, masterIdListLength); 
    } else {
        masterIdListLength = EEPROM.read(EEPROM_START_ADDRESS + 1);
        for(int i = 0; i < masterIdListLength; i++) {
            masterIdList[i] = EEPROM.read(EEPROM_START_ADDRESS + 3 + i); 
        }
    }
    initializeGraph(graph);
}

void loop() { 
    if (manager.available()) {
        Message msg;
        uint8_t len = sizeof(Message);
        uint8_t from;
        if (manager.recvfrom((uint8_t*)&msg, &len, &from)) {
            Serial.println("Msg Recv");
            handleIncomingMessage(msg);
        }
    }
    // Broadcast presence
    static signed long lastBroadcast = -1800000; // 30 mins 
    if (accurateMillis() - lastBroadcast >= 1800000) { 
        Serial.println("BroadCasting Presence");
        broadcastPresence();
        lastBroadcast = accurateMillis();
    }
    // Check and prune neighbors list
    static signed long lastPrune = 0;
    if (accurateMillis() - lastPrune >= 7200000) { // 2 hours
        Serial.println("Prune Neighbours");
        pruneNeighbors();
        lastPrune = accurateMillis();
    }
}

void handleIncomingMessage(Message &msg) {
    // Check if message ID is in consumed or relayed lists
    if (isMessageIDPresent(consumedMessageIds, msg.id) || isMessageIDPresent(relayedMessageIds, msg.id)) {
        return;  // Ignore duplicate messages
    }
 
    msg.hops++;
    // Handle different message types
    switch (msg.type) {
        case MESSAGE_TYPE_NETWORK_ADDITION_PROPOSAL:
            handleNetworkAdditionProposal(msg);
            break;
        case MESSAGE_TYPE_BROADCAST: 
            addNeighbor(msg.senderID);
            Message ackMsg;
            ackMsg.type = MESSAGE_TYPE_ACK;
            ackMsg.targetID = msg.senderID;
            ackMsg.senderID = MASTER_NODE_ID;
            ackMsg.sensorValue = MESSAGE_CONSUMED;
            ackMsg.hops = 0;
            sendToTargetOrBroadcast(ackMsg);
            break;
        case MESSAGE_TYPE_NETWORK_NEIGHBOUR_UPDATE:
            handleNetworkNeighbourUpdate(msg);
            break;
        case MESSAGE_TYPE_NETWORK_ROUTE_REQUEST:
            handleNetworkRouteRequest(msg);
            break;
        default:
            if (msg.targetID == MASTER_NODE_ID) {
                consumeMessage(msg);
            } else {
                relayMessage(msg);
            }
            break;
    }
}


void handleNetworkRouteRequest(Message receivedMsg) {
    char* sp = shortestPath(graph, receivedMsg.senderID, receivedMsg.sensorValue);
    char* rPath = redundantPath(graph, receivedMsg.senderID, receivedMsg.sensorValue);

    // Constructing response
    Message response;
    response.type = MESSAGE_TYPE_NETWORK_ROUTE_REQUEST;
    response.senderID = MASTER_NODE_ID;
    response.targetID = receivedMsg.senderID;
    response.hops = 0;

    // Copy shortest path to data
    for (int i = 0; i < 10; i++) {
        response.data[i] = sp[i];
    }

    // Insert 100 as a separator
    response.data[10] = 100;

    // Copy redundant path to data
    for (int i = 0; i < 10; i++) {
        response.data[11 + i] = rPath[i];
    }

    // Getting the path from master to the sender
    char* pathToSender = shortestPath(graph, MASTER_NODE_ID, receivedMsg.senderID);
    for (int i = 0; i < 10; i++) {
        response.route[i] = pathToSender[i];
    }

    // Send the message using pathed route
    manager.sendto((uint8_t*)&response, sizeof(Message), response.route[response.hops]);
    const uint16_t ACK_TIMEOUT = 2500;  // 500 milliseconds
    // Wait for an acknowledgment with specific values
    unsigned long startTime = accurateMillis();
    while (accurateMillis() - startTime < ACK_TIMEOUT) {// 5 secs = 5000 milliseconds
        if (manager.available()) {
            // Read the incoming message
            Message incomingMsg;
            uint8_t len = sizeof(Message);
            uint8_t from;
            manager.recvfrom((uint8_t*)&incomingMsg, &len, &from);


            if (incomingMsg.senderID != receivedMsg.senderID && incomingMsg.targetID == MASTER_NODE_ID && strcmp(incomingMsg.data, "300")) {
                return;                        
            }
        }
    }
    
}

void handleNetworkNeighbourUpdate(Message &msg) {
    // Assuming neighbors are sent as comma-separated values in data
    int neighbors[MAX_NODE_NEIGHBORS];
    int neighborsCount = 0;

    char *token = strtok(msg.data, ",");
    while(token != nullptr && neighborsCount < MAX_NEIGHBORS) {
        int nodeId;
        if(sscanf(token, "%d", &nodeId) == 1) {
            neighbors[neighborsCount++] = nodeId;
        }
        token = strtok(nullptr, ",");
    }
    updateGraph(graph, msg.senderID, neighbors, neighborsCount, false);
}




void handleNetworkAdditionProposal(Message &msg) {
    int proposedID = atoi(msg.data);
    bool idExists = false;

    for (int i = 0; i < masterIdListLength; i++) {
        if (masterIdList[i] == proposedID) {
            idExists = true;
            break;
        }
    }

    Message responseMsg;
    responseMsg.senderID = MASTER_NODE_ID;
    responseMsg.targetID = msg.senderID;
    responseMsg.type = MESSAGE_TYPE_NETWORK_ADDITION_PROPOSAL;

    if (idExists || masterIdListLength >= 255) {
        sprintf(responseMsg.data, "500%d", proposedID);
    } else {
        masterIdList[masterIdListLength++] = proposedID;
        updateEEPROM();
        sprintf(responseMsg.data, "300%d", proposedID);
        updateGraph(graph, proposedID, {}, 0, false);
        //, int nodeId, int neighbors[], int neighborsCount, bool toRemove = false
    }
    sendToTargetOrBroadcast(responseMsg);
}

void consumeMessage(Message &msg) {
    // Cache the message ID in the consumed list
    cacheMessageID(consumedMessageIds, msg.id); 
    // TODO
}

void relayMessage(Message msg) {
    // Cache the message ID in the relayed list
    cacheMessageID(relayedMessageIds, msg.id);
    msg.lastRelayID = MASTER_NODE_ID;


    if (msg.targetID != BROADCAST_ADDRESS) {
        // Send the message using pathed route
        manager.sendto((uint8_t*)&msg, sizeof(Message), msg.route[msg.hops]);
        const uint16_t ACK_TIMEOUT = 2500;  // 500 milliseconds
        // Wait for an acknowledgment with specific values
        unsigned long startTime = accurateMillis();
        while (accurateMillis() - startTime < ACK_TIMEOUT) {// 5 secs = 5000 milliseconds
            if (manager.available()) {
                // Read the incoming message
                Message incomingMsg;
                uint8_t len = sizeof(Message);
                uint8_t from;
                manager.recvfrom((uint8_t*)&incomingMsg, &len, &from);


                if (incomingMsg.senderID != msg.senderID && incomingMsg.targetID == MASTER_NODE_ID && strcmp(incomingMsg.data, "300")) {
                    return;                        
                }
            }
        }



        // bool isClose = isNeighbor(msg.targetID);
        // const uint16_t ACK_TIMEOUT = 2500;  // 500 milliseconds
        // uint64_t startTime = accurateMillis();
        // if(isClose){
        //     manager.sendto((uint8_t *)&msg, sizeof(Message), msg.targetID);

        //     unsigned long startTime = accurateMillis();
        //     while (accurateMillis() - startTime < ACK_TIMEOUT) {// 5 secs = 5000 milliseconds
        //         if (manager.available()) {
        //             // Read the incoming message
        //             Message incomingMsg;
        //             uint8_t len = sizeof(Message);
        //             uint8_t from;
        //             manager.recvfrom((uint8_t*)&incomingMsg, &len, &from);
        

        //             if (incomingMsg.type == MESSAGE_TYPE_ACK && incomingMsg.targetID == MASTER_NODE_ID) {
        //                 return;                        
        //             }
        //         }
        //     }
        // }else {
            
        //     for (uint8_t i = 0; i < neighborCount; i++) {
        //         manager.sendto((uint8_t *)&msg, sizeof(Message), neighbors[i].nodeID);

        //         unsigned long startTime = accurateMillis();
        //         while (accurateMillis() - startTime < ACK_TIMEOUT) {// 5 secs = 5000 milliseconds
        //             if (manager.available()) {
        //                 // Read the incoming message
        //                 Message incomingMsg;
        //                 uint8_t len = sizeof(Message);
        //                 uint8_t from;
        //                 manager.recvfrom((uint8_t*)&incomingMsg, &len, &from);
        //                 if (incomingMsg.type == MESSAGE_TYPE_ACK && incomingMsg.targetID == MASTER_NODE_ID) {
        //                     break;// continue for loop to send to neighbours                     
        //                 }
        //             }
        //         }
        //     }
        // }
    }
}

void cacheMessageID(char list[MAX_SAVED_MSGS][10], const char* id) {
    for (int i = MAX_SAVED_MSGS - 1; i > 0; i--) {
        strcpy(list[i], list[i - 1]);
    }
    strcpy(list[0], id);
}

bool isMessageIDPresent(char list[100][10], const char* id) {
    for (int i = 0; i < MAX_SAVED_MSGS; i++) {
        if (strcmp(list[i], id) == 0) {
            return true;
        }
    }
    return false;
}

void updateEEPROM() {
    EEPROM.write(1, masterIdListLength);
    for (int i = 0; i < masterIdListLength; i++) {
        EEPROM.write(3 + i, masterIdList[i]);
    }
}

uint64_t accurateMillis() {
    const uint64_t maxMillisValue = 0xFFFFFFFF; // max val of unsigned long
    const uint64_t overflowIntervalMillis = maxMillisValue + 1;
    static uint64_t lastMillisValue = 0;
    static short numOverflows = 0; // 7 overflows a year, 140 in 20 years, if this is still running jesus
    uint64_t currentMillisValue = millis();
    
    // rip overflowed
    if (currentMillisValue < lastMillisValue) {
        numOverflows++;
    }
    
    lastMillisValue = currentMillisValue;
    
    return (numOverflows * overflowIntervalMillis) + currentMillisValue;
}


bool isNeighbor(uint8_t nodeId) {
    for (uint8_t i = 0; i < neighborCount; i++) {
        if (neighbors[i].nodeID == nodeId) {
            return true;
        }
    }
    return false;
}

void broadcastPresence() {
    Message msg;
    msg.type = MESSAGE_TYPE_BROADCAST;
    msg.senderID = MASTER_NODE_ID;
    msg.targetID = BROADCAST_ADDRESS;
    strncpy(msg.id, generateMessageID(), sizeof(msg.id));
    msg.hops = 0;
    // Fill other data fields if necessary
    
    manager.sendto((uint8_t*)&msg, sizeof(Message), BROADCAST_ADDRESS);
}
void addNeighbor(uint8_t id) {
    for (uint8_t i = 0; i < neighborCount; i++) {
        if (neighbors[i].nodeID == id) {
            neighbors[i].lastSeen = accurateMillis();
            return;
        }
    }

    if (neighborCount < MAX_NEIGHBORS) {
        neighbors[neighborCount].nodeID = id;
        neighbors[neighborCount].lastSeen = accurateMillis();
        neighborCount++;
    }
}

void pruneNeighbors() {
    for (uint8_t i = 0; i < neighborCount; i++) {
        if (accurateMillis() - neighbors[i].lastSeen >= 3600000 / 4 * 3) { // 1 hours / 4 * 3 = 45 mins
            for (uint8_t j = i; j < neighborCount - 1; j++) {
                neighbors[j] = neighbors[j + 1];
            }
            neighborCount--;
            i--;  // Check the same index again
        }
    }
}

void sendToTargetOrBroadcast(Message& message) {
    if (isNeighbor(message.targetID)) {
        manager.sendto((uint8_t*)&message, sizeof(Message), message.targetID);
    } else {
        manager.sendto((uint8_t*)&message, sizeof(Message), BROADCAST_ADDRESS);
    }
}
 




void initializeGraph(Node graph[]) {
    for (int i = 0; i < MAX_NODES; i++) {
        graph[i].nodeId = -1;
        graph[i].neighborsCount = 0;
    }
    updateGraph(graph, MASTER_NODE_ID, {}, 0, false); // Initilize master Node
}

int findNodeIndex(Node graph[], int nodeId) {
    for(int i = 0; i < MAX_NODES; i++) {
        if(graph[i].nodeId == nodeId) {
            return i;
        }
    }
    return -1; // Node not found
}

void enqueue(int queue[], int val, int &rear) {
    queue[rear++] = val;
}

int dequeue(int queue[], int &front) {
    return queue[front++];
}

bool isEmpty(int front, int rear) {
    return front == rear;
}

char* shortestPath(Node graph[], int start, int end) {
    int queue[MAX_NODES];
    int front = 0, rear = 0;
    int pathLength = 0;
    char prevNode[MAX_NODES];
    for(int i = 0; i < MAX_NODES; i++) {
        prevNode[i] = -1;
    }

    char* path = (char*) malloc(MAX_NODES * sizeof(char));
    for(int i = 0; i < MAX_NODES; i++) {
        path[i] = 100;
    }

    enqueue(queue, start, rear);
    while(!isEmpty(front, rear)) {
        char node = (char)dequeue(queue, front);
        int index = findNodeIndex(graph, node);
        for(int i = 0; i < graph[index].neighborsCount; i++) {
            int neighbor = graph[index].neighbors[i];
            if(prevNode[neighbor] == -1 && neighbor != start) {
                enqueue(queue, neighbor, rear);
                prevNode[neighbor] = node;
            }
        }
    }

    pathLength = 0;
    for(char at = end; at != start; at = prevNode[at]) {
        path[pathLength++] = at;
    }
    path[pathLength++] = start;

    // Reversing the path
    for(int i = 0; i < pathLength / 2; i++) {
        char temp = path[i];
        path[i] = path[pathLength - 1 - i];
        path[pathLength - 1 - i] = temp;
    }

    return path;
}

int getNeighborsCount(Node graph[], int nodeId) {
    int index = findNodeIndex(graph, nodeId);
    if(index == -1) {
        // Handle error
        return 0;
    }
    return graph[index].neighborsCount;
}

void resetVisited(bool visited[]) {
    for(int i = 0; i < MAX_NODES; i++) {
        visited[i] = false;
    }
}

int mostConnectedNeighbor(Node graph[], bool visited[], int nodeId) {
    int index = findNodeIndex(graph, nodeId);
    int maxNeighbors = -1;
    int chosenNode = -1;
    for(int i = 0; i < graph[index].neighborsCount; i++) {
        int neighborId = graph[index].neighbors[i];
        int count = getNeighborsCount(graph, neighborId);
        if(!visited[neighborId] && count > maxNeighbors) {
            maxNeighbors = count;
            chosenNode = neighborId;
        }
    }
    visited[chosenNode] = true;
    return chosenNode;
}

bool redundantPathDFS(Node graph[], bool visited[], int start, int end, char path[], int &pathLength) {
    if(start == end) {
        path[pathLength++] = end;
        return true;
    }

    visited[start] = true;
    int nextNode = mostConnectedNeighbor(graph, visited, start);
    while(nextNode != -1) {
        if(redundantPathDFS(graph, visited, nextNode, end, path, pathLength)) {
            path[pathLength++] = start;
            return true;
        }
        nextNode = mostConnectedNeighbor(graph, visited, start);
    }
    return false;
}

char* redundantPath(Node graph[], int start, int end ) {
    bool visited[MAX_NODES];
    resetVisited(visited); 

    int pathLength = 0; 

    char* path = (char*) malloc(MAX_NODES * sizeof(char));
    for(int i = 0; i < MAX_NODES; i++) {
        path[i] = 100;
    }

    if(!redundantPathDFS(graph, visited, start, end, path, pathLength)) {
        // Handle case where no path exists
        pathLength = -1;
    } else {
        // Reversing the path to get it from start to end
        for(int i = 0; i < pathLength / 2; i++) {
            int temp = path[i];
            path[i] = path[pathLength - 1 - i];
            path[pathLength - 1 - i] = temp;
        }
    }
    return path;
}
char* generateMessageID() {
    static char id[10];  // 9 characters + null terminator
    const char* charset = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz";

    for(int i = 0; i < 9; i++) {
        id[i] = charset[random(0, 62)];  // 62 possible alphanumeric characters
    }
    id[9] = '\0';  // Null terminate the string

    return id;
}
void updateGraph(Node graph[], int nodeId, int neighbors[], int neighborsCount, bool toRemove) {
    int index = findNodeIndex(graph, nodeId);
    
    if(toRemove) {
        if(index == -1) {
            // Handle error: Node not found to remove
            return;
        }
        // Remove nodeId from all its neighboring nodes' neighbors list
        for(int i = 0; i < graph[index].neighborsCount; i++) {
            int neighborIndex = findNodeIndex(graph, graph[index].neighbors[i]);
            for(int j = 0; j < graph[neighborIndex].neighborsCount; j++) {
                if(graph[neighborIndex].neighbors[j] == nodeId) {
                    for(int k = j; k < graph[neighborIndex].neighborsCount - 1; k++) {
                        graph[neighborIndex].neighbors[k] = graph[neighborIndex].neighbors[k + 1];
                    }
                    graph[neighborIndex].neighborsCount--;
                    break;
                }
            }
        }
        // Reset the node's data
        graph[index].nodeId = -1;
        graph[index].neighborsCount = 0;
    } else {
        if(index == -1) {
            // Add the new node
            for(index = 0; index < MAX_NODES; index++) {
                if(graph[index].nodeId == -1) {
                    graph[index].nodeId = nodeId;
                    break;
                }
            }
        }
        // Update the neighbors
        for(int i = 0; i < neighborsCount; i++) {
            graph[index].neighbors[i] = neighbors[i];
        }
        graph[index].neighborsCount = neighborsCount;
    }
}