#include "bsp_track_ir.h"

#define TRACK_IR_FILTER_SAMPLES 5U

static uint8_t track_ir_read_line_mask_once(void)
{
    uint8_t mask = 0;

    if (HAL_GPIO_ReadPin(H1_GPIO_Port, H1_Pin) == TRACK_IR_ACTIVE_LEVEL) {
        mask |= TRACK_IR_H1_BIT;
    }
    if (HAL_GPIO_ReadPin(H2_GPIO_Port, H2_Pin) == TRACK_IR_ACTIVE_LEVEL) {
        mask |= TRACK_IR_H2_BIT;
    }
    if (HAL_GPIO_ReadPin(H3_GPIO_Port, H3_Pin) == TRACK_IR_ACTIVE_LEVEL) {
        mask |= TRACK_IR_H3_BIT;
    }
    if (HAL_GPIO_ReadPin(H4_GPIO_Port, H4_Pin) == TRACK_IR_ACTIVE_LEVEL) {
        mask |= TRACK_IR_H4_BIT;
    }

    return mask;
}

void track_ir_init(void)
{
    /*
     * 引脚模式和上下拉由 CubeMX 配置。
     * 这里先保留初始化接口，后续若需要滤波/标定可直接扩展。
     */
}

// 读取原始电平状态（bit=1 表示 GPIO 为高电平）
uint8_t track_ir_get_raw_mask(void)
{
    uint8_t mask = 0;

    if (HAL_GPIO_ReadPin(H1_GPIO_Port, H1_Pin) == GPIO_PIN_SET) {
        mask |= TRACK_IR_H1_BIT;
    }
    if (HAL_GPIO_ReadPin(H2_GPIO_Port, H2_Pin) == GPIO_PIN_SET) {
        mask |= TRACK_IR_H2_BIT;
    }
    if (HAL_GPIO_ReadPin(H3_GPIO_Port, H3_Pin) == GPIO_PIN_SET) {
        mask |= TRACK_IR_H3_BIT;
    }
    if (HAL_GPIO_ReadPin(H4_GPIO_Port, H4_Pin) == GPIO_PIN_SET) {
        mask |= TRACK_IR_H4_BIT;
    }

    return mask;
}

// 读取线状态掩码（bit=1 表示对应通道检测到黑线）
uint8_t track_ir_get_line_mask(void)
{
    return track_ir_read_line_mask_once();
}

uint8_t track_ir_get_line_mask_filtered(void)
{
    uint8_t bit_hits[4] = {0};
    uint8_t filtered_mask = 0;

    for (uint8_t i = 0; i < TRACK_IR_FILTER_SAMPLES; i++) {
        uint8_t sample = track_ir_read_line_mask_once();
        if ((sample & TRACK_IR_H1_BIT) != 0U) {
            bit_hits[0]++;
        }
        if ((sample & TRACK_IR_H2_BIT) != 0U) {
            bit_hits[1]++;
        }
        if ((sample & TRACK_IR_H3_BIT) != 0U) {
            bit_hits[2]++;
        }
        if ((sample & TRACK_IR_H4_BIT) != 0U) {
            bit_hits[3]++;
        }
    }

    if (bit_hits[0] >= ((TRACK_IR_FILTER_SAMPLES / 2U) + 1U)) {
        filtered_mask |= TRACK_IR_H1_BIT;
    }
    if (bit_hits[1] >= ((TRACK_IR_FILTER_SAMPLES / 2U) + 1U)) {
        filtered_mask |= TRACK_IR_H2_BIT;
    }
    if (bit_hits[2] >= ((TRACK_IR_FILTER_SAMPLES / 2U) + 1U)) {
        filtered_mask |= TRACK_IR_H3_BIT;
    }
    if (bit_hits[3] >= ((TRACK_IR_FILTER_SAMPLES / 2U) + 1U)) {
        filtered_mask |= TRACK_IR_H4_BIT;
    }

    return filtered_mask;
}

track_ir_state_t track_ir_basic_judge(void)
{
    uint8_t line_mask = track_ir_get_line_mask_filtered();
    
    /* 
     * 根据用户描述：超出范围输出低电平（等同于黑线信号）。
     * 如果四路全为低电平（line_mask == 0x0F），则判定为“超出范围”或“丢线”。
     */
    if (line_mask == (TRACK_IR_H1_BIT | TRACK_IR_H2_BIT | TRACK_IR_H3_BIT | TRACK_IR_H4_BIT)) {
        return TRACK_IR_STATE_LOST;
    }

    /* 全高电平（未检测到黑线） */
    if (line_mask == 0U) {
        return TRACK_IR_STATE_CENTER; // 或者根据业务逻辑定义为其他状态
    }

    uint8_t left_mask = line_mask & (TRACK_IR_H4_BIT | TRACK_IR_H3_BIT);
    uint8_t right_mask = line_mask & (TRACK_IR_H2_BIT | TRACK_IR_H1_BIT);

    if ((left_mask != 0U) && (right_mask == 0U)) {
        return TRACK_IR_STATE_LEFT;
    }

    if ((right_mask != 0U) && (left_mask == 0U)) {
        return TRACK_IR_STATE_RIGHT;
    }

    /* 左右都有信号，视为十字或居中压线 */
    return TRACK_IR_STATE_CROSS;
}

const char *track_ir_state_to_string(track_ir_state_t state)
{
    switch (state) {
    case TRACK_IR_STATE_LOST:
        return "LOST";
    case TRACK_IR_STATE_LEFT:
        return "LEFT";
    case TRACK_IR_STATE_CENTER:
        return "CENTER";
    case TRACK_IR_STATE_RIGHT:
        return "RIGHT";
    case TRACK_IR_STATE_CROSS:
        return "CROSS";
    default:
        return "UNKNOWN";
    }
}