//
// Created by iflyt on 2025/3/2.
//
#include "button_manager.h"

#include <stm32f4xx_hal.h>
#include <base/apphardware.hpp>
#include <base/zbasic.h>
#include <status/motor_manager.h>

#include "led.h"
#include "can_control/can_protocol_factory.h"

#define TAG "EXTI_KEY"

const B_PinInfo ButtonManager::pinInfos[8] = {
    {SYSTEM_POWER_PORT, SYSTEM_POWER_PIN, GPIO_PIN_SET, 100},    // PAUSE_PIN 高电平有效
    {E_STOP_PORT, E_STOP_PIN, GPIO_PIN_RESET, 100},
    {X_PORT, X_ORIGIN_PIN, GPIO_PIN_RESET, 0},
    {X_PORT, X_LIMIT_PIN, GPIO_PIN_RESET, 0},
    {Y_PORT, Y_ORIGIN_PIN, GPIO_PIN_RESET, 0},
    {Y_PORT, Y_LIMIT_PIN, GPIO_PIN_RESET, 0},
    {Z_PORT, Z_ORIGIN_PIN, GPIO_PIN_RESET, 0},
    {Z_PORT, Z_LIMIT_PIN, GPIO_PIN_RESET, 0}
};

const uint16_t ButtonManager::pins[] = {SYSTEM_POWER_PIN, E_STOP_PIN, X_ORIGIN_PIN, X_LIMIT_PIN,
                       Y_ORIGIN_PIN, Y_LIMIT_PIN, Z_ORIGIN_PIN, Z_LIMIT_PIN};

ButtonManager* ButtonManager::instance = nullptr;

ButtonManager* ButtonManager::ins() {
    if (instance == nullptr) {
        instance = new ButtonManager();
    }
    return instance;
}

ButtonManager::ButtonManager() {
    for (int i = 0; i < PIN_COUNT; ++i) {
        uint32_t pin_t = pins[i];
        pinStates[i] = readPin(getPortByPin(pin_t), pin_t);
    }

    keyQueue = osMessageQueueNew(10, sizeof(uint16_t), nullptr);
    if (keyQueue == nullptr) {
        // 处理队列创建失败
    }
    osThreadNew(ButtonManager::handleKeyInterrupts, this, nullptr);
}

ButtonManager::~ButtonManager() {
    osMessageQueueDelete(keyQueue);
}

bool ButtonManager::isPinStateTriggered(uint16_t targetPin, bool state) {
    for (int i = 0; i < sizeof(pinInfos) / sizeof(pinInfos[0]); i++) {
        if (pinInfos[i].pin == targetPin) {
            GPIO_PinState currentLevel = state ? GPIO_PIN_SET : GPIO_PIN_RESET;
            // 检查当前电平是否等于有效电平，并且输入的电平也等于有效电平
            return (currentLevel == pinInfos[i].activeLevel);
        }
    }
    return false;  // 如果未找到对应的引脚，返回 false
}

void ButtonManager::handleInterrupt(uint16_t pin) {
    if (instance && instance->keyQueue) {
        osMessageQueuePut(instance->keyQueue, &pin, 0, 0);
    }
}

bool ButtonManager::readPin(GPIO_TypeDef* port, uint16_t pin) {
    return HAL_GPIO_ReadPin(port, pin) == GPIO_PIN_SET;
}

int ButtonManager::getPinDebounceTime(uint16_t targetPin) {
    for (int i = 0; i < sizeof(pinInfos) / sizeof(pinInfos[0]); i++) {
        if (pinInfos[i].pin == targetPin) {
            return pinInfos[i].debounceTime;
        }
    }
    return 0;  // 如果未找到对应的引脚，返回 false
}

bool ButtonManager::isPinTriggered(uint16_t targetPin) {
    for (int i = 0; i < sizeof(pinInfos) / sizeof(pinInfos[0]); i++) {
        if (pinInfos[i].pin == targetPin) {
            GPIO_PinState currentLevel = HAL_GPIO_ReadPin(pinInfos[i].port, pinInfos[i].pin);
            // 检查当前电平是否等于有效电平，并且输入的电平也等于有效电平
            return (currentLevel == pinInfos[i].activeLevel);
        }
    }
    return false;  // 如果未找到对应的引脚，返回 false
}

void ButtonManager::handleKeyInterrupts(void* arg) {
    ButtonManager* manager = static_cast<ButtonManager*>(arg);
    uint16_t pin;
    while (1) {
        if (osMessageQueueGet(manager->keyQueue, &pin, nullptr, osWaitForever) == osOK) {
            GPIO_TypeDef* port = getPortByPin(pin);
            GPIO_PinState pin_state = HAL_GPIO_ReadPin(port, pin);
            ZLOGI(TAG, "GPIO PIN %d pin_state %d", pin, pin_state);
            int debounceTime = getPinDebounceTime(pin);
            if(debounceTime != 0) {
                bool b_pin_state =  pin_state == GPIO_PIN_SET;
                manager->processKeyEvent(pin, b_pin_state);
            }else {
                bool b_pin_state =  pin_state == GPIO_PIN_SET;
                osDelay(manager->debounceTime);
                bool b_cur_pin_state = HAL_GPIO_ReadPin(port, pin) == GPIO_PIN_SET;
                if (port != nullptr && b_pin_state == b_cur_pin_state) {
                    manager->processKeyEvent(pin, b_cur_pin_state);
                }
                else {
                    ZLOGI(TAG, "[SOFT DEBOUNCE] GPIO PIN %d pin_state  %02x  ", pin, pin_state);
                }
            }
        }
    }
}

void ButtonManager::processKeyEvent(uint16_t pin, bool state) {
    const int temp_i = getPinIndex(pin);
    if(temp_i < 0)
        return;
    const uint32_t index = temp_i;
    switch (pin) {
        case E_STOP_PIN: {
            // 处理急停按键事件
            if (isPinStateTriggered(E_STOP_PIN, state)) {
                ZLOGI(TAG, "E_STOP_PIN Pressed");

                {
                    CanMessage msg = CanProtocolFactory::createUploadEvent(0xFF, SYSTEM_E_STOP_Pressed);
                    iflytop::AppHardware::ins()->can0Controller.sendMessage(msg);
                }
                iflytop::AppHardware::ins()->setE_Stop(true);
                iflytop::AppHardware::ins()->SystemPowerOff();
            } else {
                ZLOGI(TAG, "E_STOP_PIN Released");
                {
                    CanMessage msg = CanProtocolFactory::createUploadEvent(0xFF, SYSTEM_E_STOP_Released);
                    iflytop::AppHardware::ins()->can0Controller.sendMessage(msg);
                }
                iflytop::AppHardware::ins()->setE_Stop(false);
                bool isLaunched = iflytop::AppHardware::ins()->isLaunched();
                if(isLaunched) {
                    iflytop::AppHardware::ins()->SystemPowerOn();
                }
            }

            if (pinStates[index] != state) {
                pinStates[index] = state;
            }
            break;
        }
        case SYSTEM_POWER_PIN: {
            // 处理暂停按键事件
            if(isPinStateTriggered(SYSTEM_POWER_PIN, state)) {
                ZLOGI(TAG, "SYSTEM_POWER_PIN Pressed");

                {
                    CanMessage msg = CanProtocolFactory::createUploadEvent(0xFF, SYSTEM_Pause_Pressed);
                    iflytop::AppHardware::ins()->can0Controller.sendMessage(msg);
                }
                RK3588_POWER_TOGGLE;
                LED_KEY_TOGGLE;
                // bool
                const bool isLaunched = iflytop::AppHardware::ins()->isLaunched();
                if(!isLaunched) {
                    iflytop::AppHardware::ins()->SystemPowerOn();
                }
                else {
                    iflytop::AppHardware::ins()->SystemPowerOff();
                }
                iflytop::AppHardware::ins()->toggleLaunched();
            }
            else {
                {
                    CanMessage msg = CanProtocolFactory::createUploadEvent(0xFF, SYSTEM_Pause_Released);
                    iflytop::AppHardware::ins()->can0Controller.sendMessage(msg);
                }
                ZLOGI(TAG, "SYSTEM POWER PIN Released");
            }

            if (pinStates[index] != state) {
                pinStates[index] = state;
            }
            break;
        }
        case X_ORIGIN_PIN: {
            // 处理X轴原点传感器事件
            const bool isEnter = isPinStateTriggered(X_ORIGIN_PIN, state);
            if (isEnter) {
                ZLOGI(TAG, "X_ORIGIN_PIN ENTER");
            } else {
                ZLOGI(TAG, "X_ORIGIN_PIN LEAVE");
            }
            MotorManager::ins()->motors[0].runZeroLimit(isEnter);

            if (pinStates[index] != state) {
                pinStates[index] = state;
            }
            break;
        }
        case X_LIMIT_PIN: {
            // 处理X轴限位传感器事件
            const bool isEnter = isPinStateTriggered(X_LIMIT_PIN, state);
            if (isPinStateTriggered(X_LIMIT_PIN, state)) {
                ZLOGI(TAG, "X_LIMIT_PIN ENTER");
            } else {
                ZLOGI(TAG, "X_LIMIT_PIN LEAVE");
            }
            MotorManager::ins()->motors[0].runEndLimit(isEnter);

            if (pinStates[index] != state) {
                pinStates[index] = state;
            }
            break;
        }
        case Y_ORIGIN_PIN: {
            // 处理Y轴原点传感器事件
            const bool isEnter = isPinStateTriggered(Y_ORIGIN_PIN, state);
            if (isEnter) {
                ZLOGI(TAG, "Y_ORIGIN_PIN ENTER");

            } else {
                ZLOGI(TAG, "Y_ORIGIN_PIN LEAVE");
            }
            MotorManager::ins()->motors[1].runZeroLimit(isEnter);

            if (pinStates[index] != state) {
                pinStates[index] = state;
            }
            break;
        }
        case Y_LIMIT_PIN: {
            // 处理Y轴限位传感器事件
            const bool isEnter = isPinStateTriggered(Y_LIMIT_PIN, state);
            if (isEnter) {
                ZLOGI(TAG, "Y_LIMIT_PIN ENTER");
            } else {
                ZLOGI(TAG, "Y_LIMIT_PIN LEAVE");
            }
            MotorManager::ins()->motors[1].runEndLimit(isEnter);

            if (pinStates[index] != state) {
                pinStates[index] = state;
            }
            break;
        }
        case Z_ORIGIN_PIN: {
            // 处理Z轴原点传感器事件
            const bool isEnter = isPinStateTriggered(Z_ORIGIN_PIN, state);
            if (isEnter) {
                ZLOGI(TAG, "Z_ORIGIN_PIN ENTER");
            } else {
                ZLOGI(TAG, "Z_ORIGIN_PIN LEAVE");
            }
            MotorManager::ins()->motors[2].runZeroLimit(isEnter);
            if (pinStates[index] != state) {
                pinStates[index] = state;
            }
            break;
        }
        case Z_LIMIT_PIN: {
            // 处理Z轴限位传感器事件
            const bool isEnter = isPinStateTriggered(Z_LIMIT_PIN, state);
            if (isEnter) {
                ZLOGI(TAG, "Z_LIMIT_PIN ENTER");

            } else {
                ZLOGI(TAG, "Z_LIMIT_PIN LEAVE");
            }
            MotorManager::ins()->motors[2].runEndLimit(isEnter);

            if (pinStates[index] != state) {
                pinStates[index] = state;
            }
            break;
        }
        default:
            break;
    }
}

GPIO_TypeDef* ButtonManager::getPortByPin(uint16_t pin) {
    if (pin == SYSTEM_POWER_PIN) return SYSTEM_POWER_PORT;
    if (pin == E_STOP_PIN) return E_STOP_PORT;
    if (pin >= X_ORIGIN_PIN && pin <= X_LIMIT_PIN) return X_PORT;
    if (pin >= Y_ORIGIN_PIN && pin <= Y_LIMIT_PIN) return Y_PORT;
    if (pin >= Z_ORIGIN_PIN && pin <= Z_LIMIT_PIN) return Z_PORT;
    return nullptr;
}

int ButtonManager::getPinIndex(uint16_t pin) {
    for (int i = 0; i < PIN_COUNT; ++i) {
        if (pins[i] == pin) {
            return i;
        }
    }
    return -1;
}

// 初始化 GPIO 引脚
void EX_GPIO_Init(void) {
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    // 使能 GPIO 时钟
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOF_CLK_ENABLE();

    // 配置 KEY1 引脚
    GPIO_InitStruct.Pin = SYSTEM_POWER_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;  // 上升沿触发中断
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(SYSTEM_POWER_PORT, &GPIO_InitStruct);

    // 配置 JT 引脚
    GPIO_InitStruct.Pin = E_STOP_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(E_STOP_PORT, &GPIO_InitStruct);

    // 配置 X 轴传感器引脚
    GPIO_InitStruct.Pin = X_ORIGIN_PIN | X_LIMIT_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(X_PORT, &GPIO_InitStruct);

    // 配置 Y 轴传感器引脚
    GPIO_InitStruct.Pin = Y_ORIGIN_PIN | Y_LIMIT_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(Y_PORT, &GPIO_InitStruct);

    // 配置 Z 轴传感器引脚
    GPIO_InitStruct.Pin = Z_ORIGIN_PIN | Z_LIMIT_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(Z_PORT, &GPIO_InitStruct);

    // 配置中断优先级
    HAL_NVIC_SetPriority(EXTI0_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(EXTI0_IRQn);

    HAL_NVIC_SetPriority(EXTI15_10_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);

    HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
}

extern "C" void EXTI0_IRQHandler(void) {
    HAL_GPIO_EXTI_IRQHandler(SYSTEM_POWER_PIN);
}

extern "C" void EXTI9_5_IRQHandler(void) {
	HAL_GPIO_EXTI_IRQHandler(X_ORIGIN_PIN);
	HAL_GPIO_EXTI_IRQHandler(X_LIMIT_PIN);
	HAL_GPIO_EXTI_IRQHandler(Y_ORIGIN_PIN);
}

extern "C" void EXTI15_10_IRQHandler(void) {
	HAL_GPIO_EXTI_IRQHandler(Y_LIMIT_PIN);
	HAL_GPIO_EXTI_IRQHandler(Z_ORIGIN_PIN);
	HAL_GPIO_EXTI_IRQHandler(Z_LIMIT_PIN);
    HAL_GPIO_EXTI_IRQHandler(E_STOP_PIN);
}

extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
    ButtonManager::handleInterrupt(GPIO_Pin);
}

extern "C" uint16_t getORIGINPin(uint32_t motor_index) {
    switch (motor_index) {
        case 0:
            return X_ORIGIN_PIN;
        case 1:
            return Y_ORIGIN_PIN;
        case 2:
            return Z_ORIGIN_PIN;
        default:
            return 0xFFFF;
    }
}

#include "button_manager.h"

uint16_t getAxisLimitPin(uint32_t motor_index) {
    switch (motor_index) {
        case 0:
            return X_LIMIT_PIN;
        case 1:
            return Y_LIMIT_PIN;
        case 2:
            return Z_LIMIT_PIN;
        default:
            return 0xFFFF;
    }
}