#include "JZsdkLib.h"
#include "BaseConfig.h"
#include <pthread.h>


#include "JZsdk_Hal.h"
#include "Megaphone/Megaphone.h"
#include "AudioDeal/AudioDeal.h"
#include "Gimbal/Gimbal.h"
#include "JZsdk_haldata_deal/JZsdk_data_transmisson.h"
#include "DeviceInfo/DeviceInfo.h"

#include "SearchLight/SearchLight.h"
#include "WarnLight/WarnLight.h"

#include "IRCUT/ircut.h"
#include "Hal_Send/HalSend.h"
#include "SideLaser/SideLaser.h"

static T_JZsdkReturnCode JZ_T40_Synchronize(void);

static int T40_SubConnectFlag = JZ_FLAGCODE_OFF;

void *JZ_T40_SubConnectTask(void *arg)
{
    static int LeftGimbalConnectFlag = JZ_FLAGCODE_OFF;
    static int RightGimbalConnectFlag = JZ_FLAGCODE_OFF;

    static int LeftLightConnectFlag = JZ_FLAGCODE_OFF;
    static int RightLightConnectFlag = JZ_FLAGCODE_OFF;

    static int DelayTime = 0;   //最多尝试连接设备30s

    while ( 
        (RightGimbalConnectFlag == JZ_FLAGCODE_OFF || 
        LeftGimbalConnectFlag == JZ_FLAGCODE_OFF || 
        RightLightConnectFlag == JZ_FLAGCODE_OFF || 
        LeftLightConnectFlag == JZ_FLAGCODE_OFF ) &&
        DelayTime <= 20000)
    {
        s_JZsdk_Hal_Info *HalInfo = JZsdk_Hal_GetHalInfo();

        //遍历hal信息
        for (int i = 0; i < HalInfo->HalPort[UART_DEV_2].SubNum; i++)
        {
            if(HalInfo->HalPort[UART_DEV_2].SubId[i] == 0x1c11)
            {
                LeftGimbalConnectFlag = JZ_FLAGCODE_ON;
                continue;
            }

            if(HalInfo->HalPort[UART_DEV_2].SubId[i] == 0x1c12)
            {
                RightGimbalConnectFlag = JZ_FLAGCODE_ON;
                continue;
            }

            if(HalInfo->HalPort[UART_DEV_2].SubId[i] == 0x1c21)
            {
                LeftLightConnectFlag = JZ_FLAGCODE_ON;
                continue;
            }

            if(HalInfo->HalPort[UART_DEV_2].SubId[i] == 0x1c22)
            {
                RightLightConnectFlag = JZ_FLAGCODE_ON;
                continue;
            }
        }

        //左云台未连接
        if (LeftGimbalConnectFlag == JZ_FLAGCODE_OFF)
        {
            //向左云台发送连接指令
            HalSend_type1Send_ConnectFrame(UART_DEV_2, 0x11);
            delayMs(500);
            DelayTime+=500;
        }

        //右云台未连接
        if (RightGimbalConnectFlag == JZ_FLAGCODE_OFF)
        {
            //向右云台发送连接指令
            HalSend_type1Send_ConnectFrame(UART_DEV_2, 0x12);
            delayMs(500);
            DelayTime+=500;
        }
        
        //左探照灯未连接
        if (LeftLightConnectFlag == JZ_FLAGCODE_OFF)
        {
            //向左探照灯发送连接指令
            HalSend_type1Send_ConnectFrame(UART_DEV_2, 0x21);
            delayMs(500);
            DelayTime+=500;
        }

        //右探照灯未连接
        if (RightLightConnectFlag == JZ_FLAGCODE_OFF)
        {
            //向右探照灯发送连接指令
            HalSend_type1Send_ConnectFrame(UART_DEV_2, 0x22);
            delayMs(500);
            DelayTime+=500;
        }
    }

    if (LeftGimbalConnectFlag != JZ_FLAGCODE_ON)
    {
        JZSDK_LOG_ERROR("LeftGimbalConnectFlag is OFF");
    }

    if (RightGimbalConnectFlag != JZ_FLAGCODE_ON)
    {
        JZSDK_LOG_ERROR("RightGimbalConnectFlag is OFF");
    }

    if (LeftLightConnectFlag != JZ_FLAGCODE_ON)
    {
        JZSDK_LOG_ERROR("LeftLightConnectFlag is OFF");
    }

    if (RightLightConnectFlag != JZ_FLAGCODE_ON)
    {
        JZSDK_LOG_ERROR("RightLightConnectFlag is OFF");
    }

    T40_SubConnectFlag = JZ_FLAGCODE_ON;    
    JZ_T40_Synchronize();
    JZSDK_LOG_INFO("T40 SubDevice Connect Success");
}

/*****************************
 * 
 *  
 *  子设备连接
 *  两个探照灯，两个云台
 * 
 * ***************************/
static T_JZsdkReturnCode JZ_T40_SubDevice_Init(void)
{
    T_JZsdkOsalHandler *osalHandle = JZsdk_Platform_GetOsalHandler();
    if (osalHandle == NULL)
    {
        JZSDK_LOG_ERROR("osalHandle is NULL");
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    T_JZTaskHandle JZ_T40_SubTaskHandle;
    
    osalHandle->TaskCreate("T40_SubDevice_Init", JZ_T40_SubConnectTask, 2048, NULL, &JZ_T40_SubTaskHandle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

T_JZsdkReturnCode JZ_T40_Ircut_Init()
{
    //引脚初始化
    IrcutInfo ircut_info;
    ircut_info.Amplifiter_Pin.Enable = JZ_FLAGCODE_ON;
    ircut_info.Amplifiter_Pin.Port = JZ_IRC_PORT_PG;
    ircut_info.Amplifiter_Pin.index = 5;
    ircut_info.Amplifiter_Pin.Group = 0;
    ircut_info.Amplifiter_Pin.mode = JZ_IRC_PIN_MODE_OUT;

    Ircut_Init(ircut_info);
}

void *JZ_T40_CheckLightTemperatureTask(void *arg)
{
    //等候子设备连接成功
    while (T40_SubConnectFlag == JZ_FLAGCODE_OFF)
    {
        delayMs(100);
    }

    while (1)
    {
        //检查左灯温度
        HalSend_type1Send_CheckStatus_SearchLight_Temperture(UART_DEV_2, 0x21);
        
        delayMs(500);

        //检查右灯温度
        HalSend_type1Send_CheckStatus_SearchLight_Temperture(UART_DEV_2, 0x22);

        delayMs(500);
    }
}

static T_JZsdkReturnCode JZ_T40_CheckLightTemperature(void)
{
    T_JZsdkOsalHandler *osalHandle = JZsdk_Platform_GetOsalHandler();
    if (osalHandle == NULL)
    {
        JZSDK_LOG_ERROR("osalHandle is NULL");
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    T_JZTaskHandle JZ_T40_CheckLightTemperatureTaskHandle;
    osalHandle->TaskCreate("T40_CheckLightTemperature", JZ_T40_CheckLightTemperatureTask, 2048, NULL, &JZ_T40_CheckLightTemperatureTaskHandle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/********************
 * 
 * 
 *  T40 基础参数设置
 * 
 * 
 * ****************/
static T_JZsdkReturnCode JZ_T40_BaseParamSet(void)
{
    //设置探照灯亮度

    //设置探照灯模式

    //设置激光模式

    //设置激光开关

    //设置警灯颜色

    //设置警灯模式

    //设置云台角度

    //设置喊话器音量
    

    //监测飞机信号，设定为对应的功率值

    E_JZ_SPECIAL_PLANE_INFO g_PlaneInfo;
    JZsdk_PlaneInfo_Get(&g_PlaneInfo);

    if (g_PlaneInfo == E_JZ_SPECIAL_PLANE_INFO_DJI_M4E)
    {
        //将总功率设置为无限制
        HalSend_type1Send_SearchLight_SetLumenPowerLimit(UART_DEV_2, 0x00, 0x64, 0x20, 0x00);
    }
    else if (g_PlaneInfo == E_JZ_SPECIAL_PLANE_INFO_DJI_M4TD || g_PlaneInfo == E_JZ_SPECIAL_PLANE_INFO_DJI_M4D)
    {
        //将总功率设置为31%
        //HalSend_type1Send_SearchLight_SetLumenPowerLimit(UART_DEV_2, 0x00, 0x1F, 0x20, 0x00);
        HalSend_type1Send_SearchLight_SetLumenPowerLimit(UART_DEV_2, 0x00, 0x64, 0x20, 0x00);
    }
    else
    {

        HalSend_type1Send_SearchLight_SetLumenPowerLimit(UART_DEV_2, 0x00, 0x64, 0x20, 0x00);
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

void* JZ_T40_SynchronizeTask(void* arg)
{
    while (1)
    {
        //发送同步帧
        HalSend_type1Send_SynchronizeMissionTime(UART_DEV_2, 0x20);

        delayMs(1000);
    }
}

static T_JZsdkReturnCode JZ_T40_Synchronize(void)
{
    T_JZsdkOsalHandler* osalHandle = JZsdk_Platform_GetOsalHandler();
    if (osalHandle == NULL)
    {
        JZSDK_LOG_ERROR("osalHandle is NULL");
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    T_JZTaskHandle JZ_T40_SynchronizeHandle;
    osalHandle->TaskCreate("JZ_T40_SynchronizeHandle", JZ_T40_SynchronizeTask, 2048, NULL, &JZ_T40_SynchronizeHandle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}



/****************************
 * 
 *  T40 初始化
 *  
 *  mode psdk为psdk模式 uart为串口模式
 * 
 * ****************************/
T_JZsdkReturnCode JZ_T40_init(int mode)
{        
    //引脚初始化
    JZ_T40_Ircut_Init();

    if (mode == APP_ATTENTION)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    if (mode == APP_UART)
    {
        //串口设备1初始化
        s_JZ_SerialPortHalRegInfo SerialInfo = {
            .BitRate = UART_DEV1_BITRATE,
            .ConnectType = PORT_CONNECT_TYPE_AS_SUB
        };  

        JZsdk_HalPort_Init(UART_DEV_1, (void *)&SerialInfo);
    }

    if (mode == APP_PSDK)
    {
        //外接传输模块，用于与飞机的数据通信
        JZsdk_data_transmisson_Receive_Init();
    }
    
    //串口设备2初始化
    s_JZ_SerialPortHalRegInfo SerialInfo2 = {
        .BitRate = UART_DEV2_BITRATE,
        .ConnectType = PORT_CONNECT_TYPE_AS_MAIN,
    };  

    JZsdk_HalPort_Init(UART_DEV_2, (void *)&SerialInfo2);

    //4G初始化
    s_JZ_SerialPortHalRegInfo SerialInfo3 = {
        .BitRate = COMM_4G_UART_BITRATE,
        .ConnectType = PORT_CONNECT_TYPE_AS_SUB,
    };
    JZsdk_HalPort_Init(UART_4G, (void *)&SerialInfo3);  

    //子设备连接
    JZ_T40_SubDevice_Init();

    //音频库初始化
    AudioDeal_Init();

    //喊话器初始化
    Megaphone_Init();

    //云台初始化
    Gimbal_Init(JZ_MODULE_CONTROL_WAY_INDEPENDENT);

    //探照灯初始化
    T_JzSearchLightInfo SearchLightInfo = {
        .Attribute.Frequency = 100,
        .Attribute.Mode = E_JZ_SEARCHLIGHTMODE_OFF,
        .Attribute.LeftBrightness = 30,
        .Attribute.RightBrightness = 30,
        .Attribute.LeftTemperature = 0,
        .Attribute.RightTemperature = 0,

        .ControlMode = JZ_MODULE_CONTROL_WAY_INDEPENDENT,
        .Device = UART_DEV_2,
        .FrameSequence = 0x20,
    };
    SearchLight_Init(SearchLightInfo);  

    //设置爆闪频率
    HalSend_type1Send_Set_SearchLightFrequency(UART_DEV_2, 50, 0x20);

    //警灯初始化
    T_JzWarnLightInfo WarnLightInfo = {
        .Attribute.Color1 = E_JZ_WARNLIGHT_COLOR_RED,
        .Attribute.Color2 = E_JZ_WARNLIGHT_COLOR_BLUE,
        .Attribute.mode = E_JZ_WARNLIGHT_MODE_SIMULTANEOUS_FAST_FALSHING,
        .Attribute.status = JZ_FLAGCODE_OFF,
        
        .ControlMode = JZ_MODULE_CONTROL_WAY_INDEPENDENT,
        .Device = UART_DEV_2,
        .FrameSequence = 0x20,
        .mode_version = 0,
    };
    //警灯模块初始化
    WarnLight_Init(WarnLightInfo);

    //激光初始化
    T_JzSideLaserInfo LaserInfo = {
        .attribute.Frequency = 1,
        .attribute.mode = SIDE_LASER_GREEN_ADD_RED_TOGETHER_FLICKER,
        .attribute.status = JZ_FLAGCODE_OFF,
        .attribute.Uimode = JZ_FLAGCODE_OFF,
        .ControlMode = JZ_MODULE_CONTROL_WAY_INDEPENDENT,
        .Device = UART_DEV_2,
        .FrameSequence = 0x20,
    };

    SideLaser_Init(LaserInfo);

    //温度检测
    JZ_T40_CheckLightTemperature();

    //基础参数设置
    JZ_T40_BaseParamSet();

    JZSDK_LOG_INFO("T40 Init Success");

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}