#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <math.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "pico/util/queue.h"
#include "hardware/timer.h"

// e-Paper library includes
#include "DEV_Config.h"
#include "GUI_Paint.h"
#include "fonts.h"
#include "EPD_4in2_V2.h"
#include "acme_5_outlines_font.h"

//Create a new image cache
UBYTE *BlackImage;
UBYTE *EditorImage;

queue_t q;

void process_command(char *cmd);
void core1_entry();

bool screen_on = false;
uint32_t screen_last_ts = 0;

int epaper_init()
{
    printf("EPD_4IN2_V2_test Demo\r\n");
    if(DEV_Module_Init()!=0){
        return -1;
    }

    printf("e-Paper Init and Clear...\r\n");
    EPD_4IN2_V2_Init();
    EPD_4IN2_V2_Clear();
    // DEV_Delay_ms(500);

    /* you have to edit the startup_stm32fxxx.s file and set a big enough heap size */
    UWORD Imagesize = ((EPD_4IN2_V2_WIDTH % 8 == 0)? (EPD_4IN2_V2_WIDTH / 8 ): (EPD_4IN2_V2_WIDTH / 8 + 1)) * EPD_4IN2_V2_HEIGHT;
    if((BlackImage = (UBYTE *)malloc(Imagesize)) == NULL) {
        printf("Failed to apply for black memory...\r\n");
        return -1;
    }
    if((EditorImage = (UBYTE *)malloc(Imagesize)) == NULL) {
        printf("Failed to apply for editor memory...\r\n");
        return -1;
    }
    printf("Paint_NewImage\r\n");
    Paint_NewImage(BlackImage, EPD_4IN2_V2_WIDTH, EPD_4IN2_V2_HEIGHT, 0, WHITE);
    screen_on = true;
    return 1;
}

void draw_sine_wave_line(int x_start, int y_center, int length, int amplitude, float cycles)
{
    if (length == 0) {
        return;
    }

    int step_count = abs(length);
    if (step_count < 40) {
        step_count = 40;
    } else if (step_count > 400) {
        step_count = 400;
    }

    double prev_x = x_start;
    double prev_y = y_center;

    for (int i = 1; i <= step_count; ++i) {
        double t = (double)i / step_count;
        double angle = 2.0 * M_PI * cycles * t;
        double next_x = x_start + length * t;
        double next_y = y_center + amplitude * sin(angle);

        Paint_DrawLine((int)round(prev_x), (int)round(prev_y),
                       (int)round(next_x), (int)round(next_y),
                       BLACK, DOT_PIXEL_1X1, LINE_STYLE_SOLID);

        prev_x = next_x;
        prev_y = next_y;
    }
}

bool editor_mode = false;
uint32_t editor_last_ts = 0;
char screen_log[2000];
int screen_log_i = 0;
sFONT *current_font = &Font12;
UBYTE editor_bg_color = WHITE;
UBYTE editor_fg_color = BLACK;

int epaper_close()
{
    DEV_Delay_ms(1000);
    //EPD_4IN2_V2_Init();
    //EPD_4IN2_V2_Clear();
    EPD_4IN2_V2_Display(BlackImage);
    DEV_Delay_ms(1000);
    printf("Goto Sleep...\r\n");
    EPD_4IN2_V2_Sleep();
    free(BlackImage);
    BlackImage = NULL;
    DEV_Delay_ms(2000);//important, at least 2s
    screen_on = false;
    return 0;
}

void process_command(char *cmd)
{
    if(!editor_mode) printf("Processing command: %s\n", cmd);
    Paint_NewImage(BlackImage, EPD_4IN2_V2_WIDTH, EPD_4IN2_V2_HEIGHT, 0, WHITE);
    if (strcmp(cmd, "init") == 0) {
        epaper_init();
        printf("e-Paper initialized\n");
    } else if (screen_on && strcmp(cmd, "clear") == 0 && (cmd[5] == ' ' || cmd[5] == '\0')) {
        if(editor_mode){
            screen_log_i = 0;
            screen_log[0] = '\0';
        } else {
            Paint_Clear(WHITE);
            printf("e-Paper cleared\n");
        }
    } else if (screen_on && strcmp(cmd, "clear_draw") == 0) {
        Paint_Clear(WHITE);
        printf("e-Paper cleared\n");
    } else if (screen_on && strcmp(cmd, "display") == 0) {
        EPD_4IN2_V2_Display(BlackImage);
        printf("Display updated\n");
    } else if (screen_on && strcmp(cmd, "draw_test") == 0) {
        Paint_Clear(WHITE);
        Paint_DrawString_EN(10, 0, "www.waveshare.com", &Font24, WHITE, BLACK);
        Paint_DrawString_EN(10, 35, "Pico-ePaper-4.2", &Font24, BLACK, WHITE);
        Paint_DrawString_CN(10, 70, "你好microPython", &Font24CN, WHITE, BLACK);
        Paint_DrawString_CN(10, 110, "微雪电子", &Font24CN, BLACK, WHITE);
        EPD_4IN2_V2_Display(BlackImage);
        printf("Draw test complete\n");
    } else if (screen_on && strncmp(cmd, "draw_text_white", 15) == 0) {
        int x, y;
        char text[200];
        if (sscanf(cmd, "draw_text_white %d %d %199[^\n]", &x, &y, text) == 3) {
            Paint_DrawString_EN(x, y, text, current_font, BLACK, WHITE);
            printf("Drew white text at %d, %d: %s\n", x, y, text);
        } else {
            printf("Invalid draw_text_white command format. Use: draw_text_white x y text\n");
        }
    } else if (screen_on && strncmp(cmd, "draw_text", 9) == 0) {
        int x, y;
        char text[200];
        if (sscanf(cmd, "draw_text %d %d %199[^\n]", &x, &y, text) == 3) {
            Paint_DrawString_EN(x, y, text, current_font, WHITE, BLACK);
            printf("Drew text at %d, %d: %s\n", x, y, text);
        } else {
            printf("Invalid draw_text command format. Use: draw_text x y text\n");
        }
    } else if (screen_on && strncmp(cmd, "draw_point", 10) == 0) {
        int x, y;
        if (sscanf(cmd, "draw_point %d %d", &x, &y) == 2) {
            Paint_DrawPoint(x, y, BLACK, DOT_PIXEL_1X1, DOT_STYLE_DFT);
            printf("Drew point at %d, %d\n", x, y);
        } else {
            printf("Invalid draw_point command format. Use: draw_point x y\n");
        }
    } else if (screen_on && strncmp(cmd, "draw_line", 9) == 0) {
        int x1, y1, x2, y2;
        if (sscanf(cmd, "draw_line %d %d %d %d", &x1, &y1, &x2, &y2) == 4) {
            Paint_DrawLine(x1, y1, x2, y2, BLACK, DOT_PIXEL_1X1, LINE_STYLE_SOLID);
            printf("Drew line from %d,%d to %d,%d\n", x1, y1, x2, y2);
        } else {
            printf("Invalid draw_line command format. Use: draw_line x1 y1 x2 y2\n");
        }
    } else if (screen_on && strncmp(cmd, "draw_sine", 9) == 0) {
        int x, y, length, amplitude;
        float frequency;
        if (sscanf(cmd, "draw_sine %d %d %d %d %f", &x, &y, &length, &amplitude, &frequency) == 5) {
            draw_sine_wave_line(x, y, length, amplitude, frequency);
            printf("Drew sine wave starting at %d,%d length %d amplitude %d frequency %.2f\n", x, y, length, amplitude, frequency);
        } else {
            printf("Invalid draw_sine command format. Use: draw_sine x y length amplitude frequency\n");
        }
    } else if (screen_on && strncmp(cmd, "draw_rectangle", 14) == 0 && (cmd[14] == ' ' || cmd[14] == '\0')) {
        int x1, y1, x2, y2;
        if (sscanf(cmd, "draw_rectangle %d %d %d %d", &x1, &y1, &x2, &y2) == 4) {
            Paint_DrawRectangle(x1, y1, x2, y2, BLACK, DOT_PIXEL_1X1, DRAW_FILL_EMPTY);
            printf("Drew rectangle from %d,%d to %d,%d\n", x1, y1, x2, y2);
        } else {
            printf("Invalid draw_rectangle command format. Use: draw_rectangle x1 y1 x2 y2\n");
        }
    } else if (screen_on && strncmp(cmd, "draw_rectangle_fill", 18) == 0) {
        int x1, y1, x2, y2;
        if (sscanf(cmd, "draw_rectangle_fill %d %d %d %d", &x1, &y1, &x2, &y2) == 4) {
            Paint_DrawRectangle(x1, y1, x2, y2, BLACK, DOT_PIXEL_1X1, DRAW_FILL_FULL);
            printf("Drew filled rectangle from %d,%d to %d,%d\n", x1, y1, x2, y2);
        } else {
            printf("Invalid draw_rectangle_fill command format. Use: draw_rectangle_fill x1 y1 x2 y2\n");
        }
    } else if (screen_on && strncmp(cmd, "draw_circle", 11) == 0 && (cmd[11] == ' ' || cmd[11] == '\0')) {
        int x, y, r;
        if (sscanf(cmd, "draw_circle %d %d %d", &x, &y, &r) == 3) {
            Paint_DrawCircle(x, y, r, BLACK, DOT_PIXEL_1X1, DRAW_FILL_EMPTY);
            printf("Drew circle at %d,%d with radius %d\n", x, y, r);
        } else {
            printf("Invalid draw_circle command format. Use: draw_circle x y r\n");
        }
    } else if (screen_on && strncmp(cmd, "draw_circle_fill", 16) == 0) {
        int x, y, r;
        if (sscanf(cmd, "draw_circle_fill %d %d %d", &x, &y, &r) == 3) {
            Paint_DrawCircle(x, y, r, BLACK, DOT_PIXEL_1X1, DRAW_FILL_FULL);
            printf("Drew filled circle at %d,%d with radius %d\n", x, y, r);
        } else {
            printf("Invalid draw_circle_fill command format. Use: draw_circle_fill x y r\n");
        }
    } else if (screen_on && strncmp(cmd, "draw_num", 8) == 0) {
        int x, y, num;
        if (sscanf(cmd, "draw_num %d %d %d", &x, &y, &num) == 3) {
            Paint_DrawNum(x, y, num, &Font16, BLACK, WHITE);
            printf("Drew number %d at %d,%d\n", num, x, y);
        } else {
            printf("Invalid draw_num command format. Use: draw_num x y num\n");
        }
    } else if (screen_on && strncmp(cmd, "set_pixel", 9) == 0) {
        int x, y, color;
        if (sscanf(cmd, "set_pixel %d %d %d", &x, &y, &color) == 3) {
            Paint_SetPixel(x, y, color);
            printf("Set pixel at %d,%d to color %d\n", x, y, color);
        } else {
            printf("Invalid set_pixel command format. Use: set_pixel x y color\n");
        }
    } else if (screen_on && strcmp(cmd, "close") == 0) {
        epaper_close();
        printf("e-Paper closed\n");
    } else if (screen_on && strncmp(cmd, "editor", 6) == 0 && (cmd[6] == '\0' || cmd[6] == ' ')) {
        int invert_flag = 0;
        if (cmd[6] == ' ') {
            if (sscanf(cmd, "editor %d", &invert_flag) != 1) {
                printf("Invalid editor command format. Use: editor [0|1]\n");
                return;
            }
        }
        if (invert_flag != 0) {
            editor_bg_color = BLACK;
            editor_fg_color = WHITE;
        } else {
            editor_bg_color = WHITE;
            editor_fg_color = BLACK;
        }
        if (!editor_mode) {
            screen_log_i = 0;
            screen_log[0] = '\0';
        }
        // editor_mode = true;
        // absolute_time_t time = get_absolute_time();
        // editor_last_ts = to_ms_since_boot(time);
        printf("Editor mode enabled (%s background)\n", invert_flag != 0 ? "black" : "white");
    } else {
        if(!editor_mode) printf("Unknown command: %s\n", cmd);
    }
    if(editor_mode) Paint_NewImage(EditorImage, EPD_4IN2_V2_WIDTH, EPD_4IN2_V2_HEIGHT, 0, WHITE);
    
}

void core1_entry() {
    epaper_init();
    
    EPD_4IN2_V2_Display(BlackImage);

    char cmd_buffer[256];
    int i = 0;
    bool display_image_on_editor = false;

    while (true) {
        bool newChar = false;
        while (!queue_is_empty(&q)) {
            int c;
            queue_remove_blocking(&q, &c);
            newChar = true;

            if(editor_mode){
                // absolute_time_t time = get_absolute_time();
                // editor_last_ts = to_ms_since_boot(time);
                display_image_on_editor = false;
                if (c == '\n' || c == '\r') {
                    screen_log[screen_log_i++] = '\n';
                } else if (c == '\b' || c == 127) { // Handle backspace
                    if (screen_log_i > 0) {
                        screen_log_i--;
                    }
                } else {
                    if (screen_log_i < (sizeof(screen_log) - 1)) {
                        screen_log[screen_log_i++] = c;
                    }
                }
            } 
            if (c == '\n' || c == '\r') {
                cmd_buffer[i] = '\0';
                process_command(cmd_buffer);
                i = 0;
                // Clear the command buffer display area
                if(!editor_mode){
                    Paint_ClearWindows(1, 281, 399, 299, WHITE);
                }
                break;
            } else if (c == '\b' || c == 127) { // Handle backspace
                if (i > 0) {
                    i--;
                }
            } else if (c == '\t'){
                editor_mode = !editor_mode;
                //display_image_on_editor = true;
            } else {
                if (i < (sizeof(cmd_buffer) - 1)) {
                    cmd_buffer[i++] = c;
                }
            }
        }
        if(newChar){
            if(!screen_on){
                epaper_init();
            }
            absolute_time_t time = get_absolute_time();
            screen_last_ts = to_ms_since_boot(time);
            if(editor_mode){
                bool overflow = false;
                Paint_Clear(editor_bg_color);
                screen_log[screen_log_i] = '\0';
                overflow = Paint_DrawString_EN(10, 10, screen_log, current_font, editor_bg_color, editor_fg_color);
                if(overflow){
                    while(screen_log[screen_log_i] != '\n' || screen_log_i>1){
                        screen_log_i--;
                    }
                    if(screen_log_i!=0){
                        int new_i = 0;
                        while(screen_log[screen_log_i] != '\0'){
                            screen_log[new_i++] = screen_log[screen_log_i++];
                        }
                        screen_log_i = new_i;
                    }
                    Paint_Clear(editor_bg_color);
                    // if overflow reset the screen_log buffer with only the last line
                    Paint_DrawString_EN(10, 10, screen_log, current_font, editor_bg_color, editor_fg_color);
                }
                EPD_4IN2_V2_PartialDisplay(EditorImage, 0, 0, 400, 300);
            } else {
                cmd_buffer[i] = '\0';
                // Update the command buffer display
                Paint_ClearWindows(2, 271, 399, 299, WHITE);
                Paint_DrawRectangle(1, 270, 400, 300, BLACK, DOT_PIXEL_1X1, DRAW_FILL_EMPTY);
                // printf("Current: %s, buff:%s\n", text, cmd_buffer);
                Paint_DrawString_EN(10, 275, cmd_buffer, &Font16, WHITE, BLACK);
                EPD_4IN2_V2_PartialDisplay(BlackImage, 0, 0, 400, 300);
            }
            
        }
        if(screen_on){
            absolute_time_t time = get_absolute_time();
            uint32_t ms = to_ms_since_boot(time);
            if(screen_last_ts + 30000 < ms){
                printf("Turning off screen due to inactivity (30s)");
                epaper_close();
            }
        }
    }
}


int main()
{
    stdio_init_all();
    
    printf("e-ink api ready\n");
    
    queue_init(&q, sizeof(int), 256);
    
    multicore_launch_core1(core1_entry);

    while (true) {
        int c = getchar_timeout_us(0);
        if (c != PICO_ERROR_TIMEOUT) {
            queue_add_blocking(&q, &c);
            if (c == '\n' || c == '\r') {
                 printf("\n");
            } else if (c == '\b' || c == 127) {
                 printf("\b \b");
            } else {
                printf("%c", c);
            }
        }
        absolute_time_t time = get_absolute_time();
        uint32_t ms = to_ms_since_boot(time);
        //if(editor_last_ts != 0 && editor_mode && (editor_last_ts + 10000 < ms)){
        //    printf("Disabling editor after 10s of inactivity");
        //    editor_mode = false;
        //    editor_bg_color = WHITE;
        //    editor_fg_color = BLACK;
        //}
    }

    epaper_close();
    return 0;
}