/*
 * task_display.c
 *
 *  Created on: Mar 14, 2025
 *      Author: gca76
* Handles all functionality related to the display screen
*
 */
#include "globals.h"

#include "task_display.h"
#include "ssd1306.h"
#include "ssd1306_fonts.h"
#include "task_joystick.h"
#include <stdio.h>
#include "usart.h"
#include <stdbool.h>
#include "buzzer.h"
#include "imu_accel.h"
#include "filter.h"
#include "step_detection.h"
#include "buttons.h"
#include "tim.h"

#define X_MIN 400   // Joystick value when fully Right
#define X_MAX 4070  // Joystick value when fully Left
#define Y_MIN 370   // Joystick value when fully down
#define Y_MAX 4090  // Joystick value when fully up

// Range of joystick values when at rest
#define X_REST_MIN 2050
#define X_REST_MAX 2300
#define Y_REST_MIN 2200
#define Y_REST_MAX 2400

// Minimum joystick displacement percentage in any direction to change the display 
#define MIN_PERCENTAGE 50


#define POT_MAX 4095 // Maximum potentiometer value
#define POT_MIN 120 // Minimum potentiometer value

// Possible range of step goal values
#define GOAL_MIN 500
#define GOAL_MAX 15000

static bool kilometers = true;
static bool yards = false;
static bool total_steps = true;
static bool percentage_goal = false;


static bool goal_buzzed = false;
static uint32_t buzz_on_time = 5000;

bool test_mode_active = false;

static uint16_t mapped_value;

void display_task_execute(void)
{
	// Read joystick position in x and y
	uint16_t x_val;
	x_val = get_x_val();
	uint16_t y_val;
	y_val = get_y_val();

	int x_percentage = 0, y_percentage = 0;
    char x_direction[16] = "REST ";
    char y_direction[16] = "REST ";

    // calculate x and y direction and percentage
    if (x_val > X_REST_MAX) {
        snprintf(x_direction, sizeof(x_direction), "%s", "LEFT ");
        x_percentage = ((x_val - X_REST_MAX) * 100) / (X_MAX - X_REST_MAX);
    } else if (x_val < X_REST_MIN) {
        snprintf(x_direction, sizeof(x_direction), "%s", "RIGHT");
        x_percentage = ((X_REST_MIN - x_val) * 100) / (X_REST_MIN - X_MIN);
    } else {
    	x_percentage = 0;
    }

    if (y_val < Y_REST_MIN) {
        snprintf(y_direction, sizeof(y_direction), "%s", "UP   ");
        y_percentage = ((Y_REST_MIN - y_val) * 100) / (Y_REST_MIN - Y_MIN);
    } else if (y_val > Y_REST_MAX) {
        snprintf(y_direction, sizeof(y_direction), "%s", "DOWN ");
        y_percentage = ((y_val - Y_REST_MAX) * 100) / (Y_MAX - Y_REST_MAX);
    } else {
    	y_percentage = 0;
    }

    if (y_percentage > 100) { // Make sure no percentage is above 100
    	y_percentage = 100;
    }
    if (x_percentage > 100) {
     	x_percentage = 100;
     }


    // TEST MODE
    if (test_mode_active) {
        static uint32_t last_step_update = 0;
        uint32_t now = HAL_GetTick();

        // Only update at fixed intervals, e.g., every 100 ms
        if (now - last_step_update >= 100) {
        	int rate = (GOAL / 60) * y_percentage / 100;
        	if (rate < 1) rate = 1;  // Ensure minimum increment if y_percentage > 0


            if (rate > 0) {
                if (y_val < Y_REST_MIN) {
                    // Joystick UP → increment
                    STEPCOUNT += rate;
                    if (STEPCOUNT > GOAL) STEPCOUNT = GOAL;
                } else if (y_val > Y_REST_MAX) {
                    // Joystick DOWN → decrement
                	if (STEPCOUNT >= rate) {
                	    STEPCOUNT -= rate;
                	} else {
                	    STEPCOUNT = 0;
                	}
                }
            }
            last_step_update = now;
        }
    }
    if (!GOALSTATE){ // turn off Joystick input when in goal state
		static bool joystick_moved_x = false;

		// Only allow one screen switch per joystick push
		if (!joystick_moved_x) {
			if (x_val > X_REST_MAX && x_percentage > MIN_PERCENTAGE) {
				current_screen = (current_screen + 1) % 3;  // LEFT = next screen
				joystick_moved_x = true;
			} else if (x_val < X_REST_MIN && x_percentage > MIN_PERCENTAGE) {
				current_screen = (current_screen + 2) % 3;  // RIGHT = previous screen (mod 3 wrap)
				joystick_moved_x = true;
			}
		} else {
			// Reset once joystick returns to rest zone
			if (x_val >= X_REST_MIN && x_val <= X_REST_MAX) {
				joystick_moved_x = false;
			}
		}

		static bool joystick_moved_y = false;

		if (!joystick_moved_y && !test_mode_active) {
			if (y_val < Y_REST_MIN && y_percentage > MIN_PERCENTAGE) { // UP
				
				// Toggle unit between kilometers and yards
				if (current_screen == DISTANCE_TRAVELLED) {
					if (kilometers) {
						kilometers = false;
						yards = true;
					} else if (yards) {
						kilometers = true;
						yards = false;
					}
				}
				// Toggle between current step count and percentage of goal
				if (current_screen == CURRENT_STEPS) {
					if (total_steps) {
						total_steps = false;
						percentage_goal = true;
					} else if (percentage_goal) {
						total_steps = true;
						percentage_goal = false;
					}
				}
				joystick_moved_y = true;  // block until joystick returns to rest
			}
		} else {
			if (y_val >= Y_REST_MIN && y_val <= Y_REST_MAX) {
				joystick_moved_y = false;
			}
		}
    }

    ssd1306_SetCursor(0, 30);

	// Display screen cases
    switch (current_screen) {
        case CURRENT_STEPS:
			{
				char str[32];
				ssd1306_SetCursor(0, 30);
				snprintf(str, sizeof(str), "Current Steps          ");
				ssd1306_WriteString(str, Font_7x10, White);
				ssd1306_SetCursor(0, 48);

				if (total_steps) {
					snprintf(str, sizeof(str), "%lu            ", STEPCOUNT);
				} else if (percentage_goal) {
					int step_percent = (GOAL > 0) ? (STEPCOUNT * 100) / GOAL : 0;

					snprintf(str, sizeof(str), "%3d%%           ", step_percent);
				}

				ssd1306_WriteString(str, Font_7x10, White);
			}
			break;

        case DISTANCE_TRAVELLED:
        {

            char str[32];
            ssd1306_SetCursor(0, 30);
            snprintf(str, sizeof(str), "Distance Travelled          ");
            ssd1306_WriteString(str, Font_7x10, White);
            ssd1306_SetCursor(0, 48);

            int distance_tenths = 0;  // holds distance ×10 (for 1dp)

            if (kilometers) {
                distance_tenths = (STEPCOUNT * 8) / 100;  // meters to km
            } else if (yards) {
                distance_tenths = (STEPCOUNT * 875) / 100;  // meters to yards
            }

            int whole = distance_tenths / 10;
            int tenths = distance_tenths % 10;

            if (kilometers) {
                snprintf(str, sizeof(str), "%d.%dkm           ", whole, tenths);
            } else if (yards) {
                snprintf(str, sizeof(str), "%d.%dyd           ", whole, tenths);
            }

            ssd1306_WriteString(str, Font_7x10, White);
        }
        break;

        case GOAL_PROGRESS:
			{
				if (!GOALSTATE){
					char str[32];
					ssd1306_SetCursor(0, 30);
					snprintf(str, sizeof(str), "Goal Progress          ");
					ssd1306_WriteString(str, Font_7x10, White);
					ssd1306_SetCursor(0, 48);

					snprintf(str, sizeof(str), "%-lu/%-lu          ", STEPCOUNT, GOAL);
					ssd1306_WriteString(str, Font_7x10, White);
				} else {
					// get potentiometer values
					uint16_t pot_val;
					pot_val = get_pot_val();

				    if (pot_val < POT_MIN) pot_val = POT_MIN;
				    if (pot_val > POT_MAX) pot_val = POT_MAX;
					mapped_value = GOAL_MIN + ((pot_val - POT_MIN) * (GOAL_MAX - GOAL_MIN)) / (POT_MAX - POT_MIN);
					SETGOAL = (mapped_value / 500) * 500;

					char str[32];
					ssd1306_SetCursor(0, 30);
					snprintf(str, sizeof(str), "Set Goal          ");
					ssd1306_WriteString(str, Font_7x10, White);
					ssd1306_SetCursor(0, 48);

					snprintf(str, sizeof(str), "%-lu/%-lu          ", STEPCOUNT, SETGOAL);
					ssd1306_WriteString(str, Font_7x10, White);
				}

			}
			break;
    }
    ssd1306_UpdateScreen();




    static uint32_t buzz_start_time = 0;
	//Turn on buzzer if reached goal
    if (STEPCOUNT >= GOAL && !goal_buzzed) {
        buzzer_on(50);
        buzz_start_time = HAL_GetTick();
        goal_buzzed = true;
    }
	// Turn off buzzer
    if (goal_buzzed && HAL_GetTick() - buzz_start_time > buzz_on_time) {
        buzzer_off();
        if (STEPCOUNT < GOAL){
        	goal_buzzed = false; // makes it keep buzzing comment out this if you only want it to buzz once then not buzz again
    	}
    }

    // TEST MODE
    if (test_mode_active) {
        ssd1306_SetCursor(0, 0);
        ssd1306_WriteString("TEST MODE          ", Font_6x8, White);
    } else {
        ssd1306_SetCursor(0, 0);
        ssd1306_WriteString("                     ", Font_6x8, White);
    }
}

//Reads imu values, prints to Serial Debugging if chosen 
void setSerialDebugging(uint8_t toggleSerialDebugging)
{
    int16_t imu_x_raw, imu_y_raw, imu_z_raw;
    imu_accel_read_all(&imu_x_raw, &imu_y_raw, &imu_z_raw);

    int16_t imu_x = filter_apply(&x_filter, imu_x_raw);
    int16_t imu_y = filter_apply(&y_filter, imu_y_raw);
    int16_t imu_z = filter_apply(&z_filter, imu_z_raw);
    uint32_t imu_mag = imu_magnitude(imu_x, imu_y, imu_z) / 1000000;

	// Proccess the imu data to detect steps
    process_step(imu_mag);

    if (toggleSerialDebugging) {
    	printf(">imu_x:%d,imu_y:%d,imu_z:%d\r\n", imu_x, imu_y, imu_z);
    	printf(">imu_mag:%lu,UPPER:%d,LOWER:%d\r\n", imu_mag,UPPER_THRESHOLD,LOWER_THRESHOLD);
    }
}


int __io_putchar(int ch)
{
HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, HAL_MAX_DELAY);
return ch;
}
