/**
 * @file referee.C
 * @author kidneygood (you@domain.com)
 * @brief
 * @version 0.1
 * @date 2022-11-18
 *
 * @copyright Copyright (c) 2022
 *
 */
#include "referee_task.h"
#include "robot_def.h"
#include "rm_referee.h"
#include "referee_UI.h"
#include "string.h"
#include "cmsis_os.h"
#include "chassis.h"
#include "super_cap.h"

extern SuperCapInstance *cap;                        // 超级电容
static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
static referee_info_t *referee_recv_info;            // 接收到的裁判系统数据
Referee_Interactive_info_t ui_data;
uint8_t UI_Seq;                                      // 包序号，供整个referee文件使用
// @todo 不应该使用全局变量

/**
 * @brief  判断各种ID，选择客户端ID
 * @param  referee_info_t *referee_recv_info
 * @retval none
 * @attention
 */
static void DeterminRobotID()
{
    // id小于7是红色,大于7是蓝色   #define Robot_Red 0    #define Robot_Blue 1
    referee_recv_info->referee_id.Robot_Color = referee_recv_info->GameRobotState.robot_id > 7 ? Robot_Blue : Robot_Red;
    referee_recv_info->referee_id.Robot_ID = referee_recv_info->GameRobotState.robot_id;
    referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID
    referee_recv_info->referee_id.Receiver_Robot_ID = 0;
}

static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data);
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化
static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data);

referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data)
{
    referee_recv_info = RefereeInit(referee_usart_handle); // 初始化裁判系统的串口,并返回裁判系统反馈数据指针
    Interactive_data = UI_data;                            // 获取UI绘制需要的机器人状态数据
    referee_recv_info->init_flag = 1;
    return referee_recv_info;
}

void UITask()
{
    MyUIRefresh(referee_recv_info, Interactive_data);
    xyz_refresh(referee_recv_info, Interactive_data);
}

static String_Data_t UI_State_sta[7];  // 机器人状态,静态只需画一次
static String_Data_t UI_State_dyn[7];  // 机器人状态,动态先add才能change
static Graph_Data_t UI_State_dyn_graph[2];
static Graph_Data_t UI_angle[4];

#define AXIS_LEN 250
#define AXIS_X0 550
#define AXIS_Y0 80
static Graph_Data_t xyz_axis[6];    // 抬升 横移位置

void MyUIInit()
{
    //if (!referee_recv_info->init_flag)
        //vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
    while (referee_recv_info->GameRobotState.robot_id == 0)
        osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查

    DeterminRobotID();                                            // 确定ui要发送到的目标客户端
    UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI

    // 绘制车辆状态标志指示
    UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 750, "G.AIR_PUMP:");
    UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 700, "ARM_MODE:");

    UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 650, "PITCH:");
    UICharDraw(&UI_State_sta[3], "ss2", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 600, "BIG_PITCH:");


    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]);

    // xyz坐标系 静态
    UILineDraw(&xyz_axis[0], "x_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0);
    UILineDraw(&xyz_axis[1], "y_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0+AXIS_LEN);
    UILineDraw(&xyz_axis[2], "z_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+AXIS_LEN);
    UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[0]);
    UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[1],xyz_axis[2]);

    // xyz坐标系 动态
    UILineDraw(&xyz_axis[3], "x_axis_d", UI_Graph_ADD, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN*0.5 , AXIS_Y0);
    UILineDraw(&xyz_axis[4], "y_axis_d", UI_Graph_ADD, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN*0.5 , AXIS_Y0+AXIS_LEN*0.5);
    UILineDraw(&xyz_axis[5], "z_axis_d", UI_Graph_ADD, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+AXIS_LEN*0.5);
    UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[3]);
    UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[4],xyz_axis[5]);


    // 绘制车辆状态标志，动态
    // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新

    UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_ADD, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);

    UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 350, 800, 10);
    UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]);

    // 关节角度显示,动态
    UIFloatDraw(&UI_angle[0], "pitch_angle", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 2, 230, 650, Interactive_data->diff_pitch_angle * 1000);
    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_angle[0]);
    UIFloatDraw(&UI_angle[1], "big_pitch_angle", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 2, 230, 600, Interactive_data->big_pitch_angle * 1000);
    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_angle[1]);

}

static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
{
    UIChangeCheck(_Interactive_data);
    // 显示 pitch 轴编码器位置
    UIFloatDraw(&UI_angle[0], "pitch_angle", UI_Graph_Change, 8, UI_Color_Green, 20, 2, 2, 230, 650, Interactive_data->diff_pitch_angle * 1000);
    UIFloatDraw(&UI_angle[1], "big_pitch_angle", UI_Graph_Change, 8, UI_Color_Green, 20, 2, 2, 230, 600, Interactive_data->big_pitch_angle * 1000);
    UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_angle[0],UI_angle[1]);
    // arm
    if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
    {
        switch (_Interactive_data->gimbal_mode)
        {
        case GIMBAL_ZERO_FORCE:
        {
            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
            break;
        }
        case ARM_MANUAL_MODE:
        {
            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "MANUAL    ");
            break;
        }
        case GIMBAL_SILVER_MODE:
        {
            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "SILVER    ");
            break;
        }
        case GIMBAL_FLOOR_MODE:
        {
            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "FLOOR     ");
            break;
        }
        case GIMBAL_IKINE_MODE:
        {
            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "CUSTOM    ");
            break;
        }
        case GIMBAL_STORAGE_MODE:
        {
            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "STORAGE    ");
            break;
        }
        }
        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
    }
    // pump
    if (_Interactive_data->Referee_Interactive_Flag.pump_flag == 1)
    {
        switch (_Interactive_data->pump_mode) {
            case PUMP_STOP:
                //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743);
                UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 350, 800, 10);
                break;
            case PUMP_OPEN:
                //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 0, 330, 695, 330, 695);
                UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Green, 10, 350, 800, 25);
                break;
        }
        UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]);

        _Interactive_data->Referee_Interactive_Flag.pump_flag = 0;
    }
}

/**
 * @brief  模式切换检测,模式发生切换时，对flag置位
 * @param  Referee_Interactive_info_t *_Interactive_data
 * @retval none
 * @attention
 */
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
{
    if (_Interactive_data->chassis_mode != _Interactive_data->chassis_last_mode)
    {
        _Interactive_data->Referee_Interactive_Flag.chassis_flag = 1;
        _Interactive_data->chassis_last_mode = _Interactive_data->chassis_mode;
    }

    if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode)
    {
        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1;
        _Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode;
    }

    if (_Interactive_data->pump_mode != _Interactive_data->pump_last_mode)
    {
        _Interactive_data->Referee_Interactive_Flag.pump_flag = 1;
        _Interactive_data->pump_last_mode = _Interactive_data->pump_mode;
    }
}

static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
{
    const float y_max = 0;//单位mm
    const float z_max = 0;//单位mm

    float y_ratio = _Interactive_data->xyz[1]/y_max;
    float z_ratio = _Interactive_data->xyz[2]/z_max;
    // xyz坐标系 动态
    UILineDraw(&xyz_axis[3], "x_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0);
    UILineDraw(&xyz_axis[4], "y_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+(AXIS_LEN*y_ratio) , AXIS_Y0+(AXIS_LEN*y_ratio));
    UILineDraw(&xyz_axis[5], "z_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+(AXIS_LEN*z_ratio));
    UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[3],xyz_axis[4]);
    UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[5]);

}