/**
	* 	\file HCS.ino
	*	\brief main file for Handheld Controller Station
	*	\author Uav4africa Pty Ltd
	*	\date December 2023
*/

//#include "HCS_Sim.h"
#include "HCS_functions.h" 
//#include "HCS_OLED.h"
//#include "HCS_mission_control.h"
//#include "HCS_manual_control.h"
#include <WiFi.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display(128, 64, &Wire, -1);
#include <Wire.h>
#include <Encoder.h>
#include <Adafruit_GFX.h>


// global variables
RF_struct_comms RF_data;          ///< RF data packet
selector_struct selector;         ///< selector object

float current_time, last_time;  

//-------------------- START OF HCS_encoder.cpp----------------------------------

/// global variables
bool button = false; //select

uint8_t buttonPin=25;

uint8_t pinA=35; //clk
uint8_t pinB=34; //dt
uint8_t pinAcurrentState;
uint8_t pinAlastState;
uint8_t pinBcurrentState;
uint8_t pinBlastState;
bool press_hold = false;
uint8_t hold_counter = 0;
bool CW_turn_request = false;
bool CCW_turn_request = false;
bool enteringPassword = false;  // Tracks if the user is in password entry mode
int selectedKeyCol = 0;  // Tracks the currently selected column on the keyboard
int selectedKeyRow = 0;  // Tracks the currently selected row on the keyboard
String passwordInput = "";  // To hold the password input

char keys[4][4] = {
    {'1', '2', '3', 'A'},
    {'4', '5', '6', 'B'},
    {'7', '8', '9', 'C'},
    {'*', '0', '#', 'D'}
};

#define BUTTON_PIN 25

bool buttonPressed() {
    // Read the state of the button and return whether it is pressed
    return digitalRead(BUTTON_PIN) == LOW;  // assuming LOW means pressed
}


menu_struct PageMenu[5];		 ///< Front Page menu
int8_t display_cursor = 0;		 ///< cursor to be changed with encoder selector
uint8_t current_page = 0;		 ///< Current page selected
bool custom_page_enable = false; ///< enable the display of custom page
uint8_t custom_page_index = 0;	 ///< index allocated to a custom page

void initEncoder(selector_struct *selector)
{
	// configure pins on rotary encoder
    pinMode(buttonPin,INPUT_PULLUP);
    pinMode(pinA, INPUT); 
    pinMode(pinB, INPUT);
    //attachInterrupt(digitalPinToInterrupt(pinA), rotaryEncoderCheck, CHANGE); //the interrupt is triggered when there's a change at B

	pinAlastState = digitalRead(pinA);
	pinBlastState = digitalRead(pinB);

	pinAcurrentState = pinAlastState;
	pinBcurrentState = pinBlastState;

	selector->CW_turn = false;
	selector->CCW_turn = false;
	selector->button_press = false;
	selector->updated = false; 
}

void exeEncoder(selector_struct *selector)
{
	if (selector->updated == false)
	{
		rotaryEncoderCheck(selector);
		buttonCheck(selector);	
	}
	else
	{
		// re-initialize the encoder readings
		pinAcurrentState = digitalRead(pinA);
		pinBcurrentState = digitalRead(pinB); 

		pinAlastState = pinAcurrentState;
		pinBlastState = pinBcurrentState;
	}
}

void rotaryEncoderCheck(selector_struct *selector)
{

	//checking rotation direction
	pinAcurrentState = digitalRead(pinA);
	pinBcurrentState = digitalRead(pinB); 

	// If last and current state of pinA are different, then pulse occurred
	// React to only 1 state change to avoid double count
	if((pinAlastState != pinAcurrentState) && (pinBcurrentState==pinBlastState))
	{
		if (pinAcurrentState != pinBcurrentState)  
		{
			selector->CW_turn = true; //CW rotation
			selector->CCW_turn = false;
		}
		else if (pinAcurrentState == pinBcurrentState)
		{
      selector->CCW_turn = true;
			selector->CW_turn = false; //CCW rotation
		}

        // confirm turn trigger has occurred
		if ((selector->CW_turn) || (selector->CCW_turn))
		{
            selector->updated = true;
		}
	}
	else
	{
		selector->CW_turn = false;
		selector->CCW_turn = false;
	}

	pinAlastState = pinAcurrentState;
	pinBlastState = pinBcurrentState;

	// Put in a slight delay to help debounce the reading
	// delay(1);
}


/// check button for single press - ignore press and hold
void buttonCheck(selector_struct *selector) {
    static unsigned long lastDebounceTime = 0;  // Last debounce time
    unsigned long debounceDelay = 50;           // Debounce delay in milliseconds
    
    // Get the current time
    unsigned long currentMillis = millis();
    
    // Check if the button is pressed
    if ((digitalRead(buttonPin) == LOW) && !press_hold && (currentMillis - lastDebounceTime > debounceDelay)) {
        lastDebounceTime = currentMillis;  // Update last debounce time
        
        selector->button_press = true;
        selector->updated = true;
    }
    // Handle press and hold logic
    else if ((digitalRead(buttonPin) == LOW) && (selector->button_press)) {
        selector->button_press = false;
        press_hold = true;
    }
    
    // Handle long press scenario
    if (press_hold) {
        hold_counter++;
    }
    if (hold_counter > 100) {  // Long press threshold (2 sec for 50 Hz refresh rate)
        selector->button_long_press = true;  
        selector->updated = true;
    }
    
    // Reset when the button is released
    if (digitalRead(buttonPin) == HIGH) {
        hold_counter = 0;
        press_hold = false;
        selector->button_press = false;
        selector->button_long_press = false;
    }
}

//---------------------END OF HCS_encoder.h ------------------------------------


//--------------------- START OF HCS_OLED.h ------------------------------------

void initDisplay(RF_struct_comms *RF_data_comms)
{
	display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);

	//getRFdata_OLED_display(RF_data_comms);

	definePages();							  // set the pages structure
	drawHeader();

  print_display();

}

void drawHeader(void)
{
	display.setTextSize(1);
	display.clearDisplay();
	display.setTextColor(WHITE, BLACK);
	display.setCursor(5, 0);
	display.print("LOCUS - HCS v1.0.1");
	display.drawFastHLine(0, 10, 120, WHITE);
}

void print_display(void)
{
	display.display();
}

void definePages(void)
{
	// Front page
	PageMenu[0].numOfentries = 3;
	PageMenu[0].parent_page = -1; // exception
	strcpy(PageMenu[0].menulist[0], "Mission Control");
	strcpy(PageMenu[0].menulist[1], "Remote Control");
	strcpy(PageMenu[0].menulist[2], "Settings");
	strcpy(PageMenu[0].menu_name, "Home");

	// Mission Control page
	PageMenu[1].numOfentries = 5;
	PageMenu[1].parent_page = 0; // top level page
	strcpy(PageMenu[1].menulist[0], "...");
	strcpy(PageMenu[1].menulist[1], "Create/Edit Mission");
	strcpy(PageMenu[1].menulist[2], "Save/Delete Mission");
	strcpy(PageMenu[1].menulist[3], "Start Mission");
	strcpy(PageMenu[1].menulist[4], "View Mission");
	strcpy(PageMenu[1].menu_name, "Mission Control");

	// Remote Control page
	PageMenu[2].numOfentries = 5;
	PageMenu[2].parent_page = 0; // top level page
	strcpy(PageMenu[2].menulist[0], "...");
	strcpy(PageMenu[2].menulist[1], "Range Check"); // not sure how this is done
	strcpy(PageMenu[2].menulist[2], "ALT/HDG Control");
	strcpy(PageMenu[2].menulist[3], "Flare Control");
	strcpy(PageMenu[2].menulist[4], "Return To Base");
	strcpy(PageMenu[2].menu_name, "Remote Control");

	// Settings page
	PageMenu[3].numOfentries = 5;
	PageMenu[3].parent_page = 0; // top level page
	strcpy(PageMenu[3].menulist[0], "...");
	strcpy(PageMenu[3].menulist[1], "WiFi Settings"); // include GPS location and
	strcpy(PageMenu[3].menulist[2], "Pre-flight checks"); // include GPS location and
	strcpy(PageMenu[3].menulist[3], "Display");
	strcpy(PageMenu[3].menulist[4], "About");
	strcpy(PageMenu[3].menu_name, "Settings");

  // WiFi Settings Page
  PageMenu[4].numOfentries = 4; // Updated to include "Testing"
  PageMenu[4].parent_page = 3; // Link to Settings page
  strcpy(PageMenu[4].menulist[0], "Scan Networks");
  strcpy(PageMenu[4].menulist[1], "Network Info");
  strcpy(PageMenu[4].menulist[2], "Testing");  // Added Testing option
  strcpy(PageMenu[4].menulist[3], "Back");
  strcpy(PageMenu[4].menu_name, "WiFi Settings");

  // Scanned Networks
  PageMenu[5].numOfentries = 4; // One for back + 3 networks  PageMenu[5].parent_page = 4; 
  strcpy(PageMenu[5].menulist[0], "Back");
  strcpy(PageMenu[5].menulist[1], "Network 1");
  strcpy(PageMenu[5].menulist[2], "Network 2");
  strcpy(PageMenu[5].menulist[3], "Network 3");
  strcpy(PageMenu[5].menu_name, "Scanned Networks");

}

	// Network Info page


	
	// Turn-On WiFi page

void drawMenu(selector_struct *selector) {
    int i;
    int x_pos = 15;
    bool selected = false;

    updateCursor(selector);  // Update cursor position

    // Check if the button is pressed to navigate or select options
    if (selector->button_press) {
        if (current_page == 0) {  // Home page
            current_page = (uint8_t)display_cursor + 1;
        } else if (display_cursor == 0) {  // Back option
            current_page = PageMenu[current_page].parent_page;
        } else if (strcmp(PageMenu[current_page].menu_name, "WiFi Settings") == 0) {
            if (display_cursor == 1) {  // "Scan Networks"
                scanNetworks();
                current_page = 5;  // Go to available networks page
            }
        } else if (strcmp(PageMenu[current_page].menu_name, "Available Networks") == 0) {
            if (display_cursor > 0) {  // Select a network
                connectToNetwork(display_cursor - 1);
            }
        } else if (strcmp(PageMenu[current_page].menu_name, "WiFi Settings") == 0) {
            if (display_cursor == 2) {  // Enter Password option
                enteringPassword = true;
                current_page = 6;  // Go to password entry page
            }
        }

        display_cursor = 0;  // Reset cursor after action
        selector->button_press = false;  // Reset button press flag
    }

    // Draw menu items
    for (i = 0; i < PageMenu[current_page].numOfentries; i++) {
        selected = (display_cursor == i);  // Highlight selected item
        displayMenuItem(PageMenu[current_page].menulist[i], x_pos, selected);
        x_pos += 10;
    }
}


void exeDisplay(selector_struct *selector) {
    drawHeader();       // Draw the header for the current page
    drawMenu(selector); // Handle menu navigation and actions
    print_display();    // Update the display
    drawInputText();  // Draw the input text at the top
    drawKeyboard();   // Draw the keyboard layout
    updateKeyboardCursor(selector);  // Update the cursor position
    handleButtonPress(selector);  // Handle button press for typing
    print_display();
}



void scanNetworks() {
    int numNetworks = WiFi.scanNetworks();  // Scan for available networks

    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("Scanning WiFi...");
    display.display();
    delay(1000); // Allow time for scanning

    // Clear previous network list
    for (int i = 1; i < 4; i++) { // Only 3 network slots to be filled
        memset(PageMenu[5].menulist[i], 0, sizeof(PageMenu[5].menulist[i]));
    }

    PageMenu[5].numOfentries = 1; // Start with "Back" option

    if (numNetworks == 0) {
        strcpy(PageMenu[5].menulist[1], "No networks found.");
        PageMenu[5].numOfentries++;
    } else {
        for (int i = 0; i < numNetworks && i < 3; i++) {  // Max 3 networks
            snprintf(PageMenu[5].menulist[i + 1], sizeof(PageMenu[5].menulist[i + 1]), "%d: %s", i + 1, WiFi.SSID(i).c_str());
            PageMenu[5].numOfentries++;
        }
    }

    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("WiFi Networks:");
    display.display();
}

void connectToNetwork(int selectedNetwork) {
    // Check if a valid network is selected
    if (selectedNetwork < 1 || selectedNetwork > 3 || strlen(PageMenu[5].menulist[selectedNetwork]) == 0) {
        return; // Invalid selection, do nothing
    }

    // Get the network SSID from the selected menu item
    String selectedSSID = WiFi.SSID(selectedNetwork - 1);  // Index is adjusted to match menu selection

    // Prompt the user to enter the password
    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("Enter password for:");
    display.setCursor(0, 10);
    display.print(selectedSSID);
    display.display();

    // Wait for the user to input the password 
    // Assume we have a variable `enteredPassword` that holds the user input
    String enteredPassword = getUserPasswordInput();  // Implement this function for getting the input

    // Attempt to connect to the selected network
    WiFi.begin(selectedSSID.c_str(), enteredPassword.c_str());

    // Wait for connection
    int attempts = 0;
    while (WiFi.status() != WL_CONNECTED && attempts < 10) {
        delay(1000);
        attempts++;
        display.clearDisplay();
        display.setCursor(0, 0);
        display.print("Connecting to ");
        display.setCursor(0, 10);
        display.print(selectedSSID);
        display.setCursor(0, 20);
        display.print("Attempts: ");
        display.print(attempts);
        display.display();
    }

    // Check if connection was successful
    if (WiFi.status() == WL_CONNECTED) {
        display.clearDisplay();
        display.setCursor(0, 0);
        display.print("Connected to ");
        display.setCursor(0, 10);
        display.print(selectedSSID);
        display.setCursor(0, 20);
        display.print("IP: ");
        display.print(WiFi.localIP());
        display.display();
    } else {
        display.clearDisplay();
        display.setCursor(0, 0);
        display.print("Failed to connect.");
        display.setCursor(0, 10);
        display.print("Try again.");
        display.display();
    }
}

String getUserPasswordInput() {
    // This function will be responsible for taking user input for the password.
    // For simplicity, we will just return a placeholder password here.
    return "your_password_here";
}


void displayMenuItem(char *item, uint8_t position, bool selected)
{
	int offset = 0;

	if (selected)
	{
		display.setTextColor(BLACK, WHITE);
	}
	else
	{
		display.setTextColor(WHITE, BLACK);
	}
	display.setCursor(0, position);

	while (*(item + offset) != '\0')
	{
		display.print(*(item + offset));
		++offset;
	}
}

void updateCursor(selector_struct *selector)
{	
	// only update display cursor if turn trigger is HIGH
	if (selector->CW_turn && selector->updated)
	{
		display_cursor += (uint8_t)selector->CW_turn;

		if (display_cursor > PageMenu[current_page].numOfentries - 1)
		{
			display_cursor = 0;
		} // reset
	}
	else if (selector->CCW_turn && selector->updated)
	{
		display_cursor -= 1;

		if (display_cursor < 0)
		{
			display_cursor = PageMenu[current_page].numOfentries - 1;
		} // reset
	}
}

//--------------------- END OF HCS_OLED.h ------------------------------------

void setup() {
    initEncoder(&selector);
    initDisplay(&RF_data);

    Serial.begin(115200);

    // Initialize WiFi in station mode
    WiFi.mode(WIFI_STA);
    Serial.println("WiFi initialized in station mode.");
    last_time = 0;
  
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  // Draw the initial keyboard layout
  drawKeyboard();
}


void loop () {
  checkKeySelection();
  current_time = millis()/1000.0f;
  RF_data.var1_uint8 = 0;

	exeEncoder(&selector);

	if ((current_time - last_time) > 0.5f)
  {
    exeDisplay(&selector);

    last_time = current_time;

		// request a new update of selector
    selector.updated = false;
  }
  // Placeholder for logic to change the selected key using rotary encoder
  // Here, we simulate it by cycling through keys (replace with encoder logic)
  delay(200);  // Simulate button press delay for demonstration
  selectedKeyCol = (selectedKeyCol + 1) % 10;  // Move to the next column
  if (selectedKeyCol == 0) {
    selectedKeyRow = (selectedKeyRow + 1) % 4;  // Move to the next row
  }
  
  // Redraw the keyboard with the updated selection
  display.clearDisplay();
  drawKeyboard();
}

// Key layout (4 rows, 10 columns)
char keyboard[4][10] = {
    {'1', '2', '3', '4', '5', '6', '7', '8', '9', '0'},
    {'q', 'w', 'e', 'r', 't', 'y', 'u', 'i', 'o', 'p'},
    {'a', 's', 'd', 'f', 'g', 'h', 'j', 'k', 'l', ';'},
    {'z', 'x', 'c', 'v', 'b', 'n', 'm', ',', '.', '/'}

};


// Key dimensions
int keyWidth = 18;
int keyHeight = 18;
int x_pos = 0;   // Horizontal start position
int y_pos = 16;  // Vertical start position

void drawKeyboard() {
  x_pos = 0;   // Reset horizontal position for the row
  y_pos = 16;  // Reset vertical position for the first row
  
  // Loop through each row and column to display the keys
  for (int row = 0; row < 4; row++) {
    for (int col = 0; col < 10; col++) {
      if (keys[row][col] != '\0') {  // Ensure the key is not empty
        // Draw the key at its position
        display.setCursor(x_pos, y_pos);
        
        // Highlight the selected key
        if (selectedKeyRow == row && selectedKeyCol == col) {
          display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);  // Highlight selected key
        } else {
          display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);  // Regular key
        }
        
        // Display the key character
        display.print(keys[row][col]);
        
        // Move to the next key position horizontally
        x_pos += keyWidth;
      }
    }
    
    // Move to the next row vertically
    y_pos += keyHeight;
    x_pos = 0;  // Reset x position for the next row
  }
  display.display();  // Update the screen with the new content
}
void updateKeyboardCursor(selector_struct *selector) {
    // Update the selected key position based on the encoder's direction
    if (selector->CW_turn && selector->updated) {
        selectedKeyCol++;  // Move to the right
        if (selectedKeyCol >= 10) {
            selectedKeyCol = 0;  // Wrap around to the first column
            selectedKeyRow++;
            if (selectedKeyRow >= 5) {
                selectedKeyRow = 0;  // Wrap around to the first row
            }
        }
    } else if (selector->CCW_turn && selector->updated) {
        selectedKeyCol--;  // Move to the left
        if (selectedKeyCol < 0) {
            selectedKeyCol = 9;  // Wrap around to the last column
            selectedKeyRow--;
            if (selectedKeyRow < 0) {
                selectedKeyRow = 4;  // Wrap around to the last row
            }
        }
    }
}

void handleButtonPress(selector_struct *selector) {
    if (selector->button_press) {
        if (enteringPassword) {
            int row = display_cursor / 10;  // Row number
            int col = display_cursor % 10;  // Column number
            passwordInput += keyboard[row][col];  // Add selected character to password input
        }
    }
}


void drawInputText() {
    display.setCursor(0, 0);
    display.print("Enter Password: ");
    display.print(passwordInput);  // Show the entered password
}


bool passwordEntered = false;  // Flag to indicate if password is entered

void checkPassword() {
    if (passwordInput == "expectedPassword") {
        Serial.println("Password correct!");
        // Proceed with the next action
    } else {
        Serial.println("Incorrect password.");
        passwordInput = "";  // Reset the input
    }
}


bool keySelected = false;  // Flag to track key selection

void checkKeySelection() {

    if (buttonPressed()) {  // Check if the encoder button is pressed
        char selectedKey = getSelectedKey();
        // Handle selected key (e.g., add to input string, display on screen, etc.)
        Serial.println(selectedKey);
        keySelected = true;  // Flag that the key has been selected
    }
}

char getSelectedKey() {
    char keyboard[5][10] = {
        {'1', '2', '3', '4', '5', '6', '7', '8', '9', '0'},
        {'Q', 'W', 'E', 'R', 'T', 'Y', 'U', 'I', 'O', 'P'},
        {'A', 'S', 'D', 'F', 'G', 'H', 'J', 'K', 'L', ' '},
        {'Z', 'X', 'C', 'V', 'B', 'N', 'M', '.', ',', '?'},
        {' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' '}
    };
    
    return keyboard[selectedKeyRow][selectedKeyCol];
}