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

#include "version_choose.h"
#include "Gimbal_H3/Gimbal_H3.h"
#include "Gimbal_V3S/Gimbal_V3S.h"
#include "Gimbal_InAndOut.h"
#include "JZsdkLib.h"



//角度记录
static int Gimbal_PitchAngle = 0;
static int Gimbal_YawAngle = 0;

static int Gimbal_PitchFineTuning = 0;
static int Gimbal_YawFineTuning = 0;


static int Gimbal_LinkageNum;

/*
*     Gimbal模块初始化
*/

int Gimbal_Init()
{
    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        Gimbal_H3_H150ST_init_motor();
    }
    else if (DEVICE_VERSION == JZ_H10)
    {
        //注：h3_h10的电机引脚已在引脚初始化中完成
        Gimbal_H3_H10_init_motor();
    }
    else if (DEVICE_VERSION == JZ_H10T)
    {
        //注：h3_h10的电机引脚已在引脚初始化中完成
        Gimbal_V3S_H10T_init_motor();
    }
    else if (DEVICE_VERSION == JZ_U3)
    {
        Gimbal_V3S_U3_init_motor();    
    }
    else if (DEVICE_VERSION == JZ_H1T)
    {
        Gimbal_V3S_H1T_init_motor();
    }
    
    
    else
    {
        return -1;
    }

    printf("云台初始化完毕\n");

}  

//设置云台俯仰角度
T_JZsdkReturnCode Gimbal_Set_PitchAngle(int angle)
{   
    printf("设置云台俯仰角度\n");
    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //1、判断输入范围是否正确
        if ((angle < -900) || (angle > 0))
        {
            printf("H150s/H150t的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

        //2、发送到H3控制云台函数
        H3_H150ST_Gimbal_SetAngle(angle);

        //3、记录全球云台值
        Gimbal_PitchAngle = angle;

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    else if (DEVICE_VERSION == JZ_H10)
    {
        //1、判断输入范围是否正确
        if ((angle < -600) || (angle > 0))
        //if ((angle < -800) || (angle > 0))
        {
            printf("H10的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

        //2、发送到H3_H10控制云台函数
        Gimbal_H3_H10_set_angle(angle);

        //3、记录全球云台值
        Gimbal_PitchAngle = angle;

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    else if (DEVICE_VERSION == JZ_H10T)
    {
        //1、判断输入范围是否正确
        if ((angle < -600) || (angle > 0))
        {
            printf("H10T的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

        //2、发送到V3S_H10T控制云台函数
        Gimbal_V3S_H10T_set_angle(angle);

        //3、记录全球云台值
        Gimbal_PitchAngle = angle;

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    else if (DEVICE_VERSION == JZ_U3)
    {
        //1、判断输入范围是否正确
        if ((angle < -900) || (angle > 0))
        {
            printf("U3的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

        //2、发送到V3S_U3控制云台函数
        Gimbal_V3S_U3_set_angle(angle);

        //3、记录全球云台值
        Gimbal_PitchAngle = angle;

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    else if (DEVICE_VERSION == JZ_H1T)
    {   
        //1、判断输入范围是否正确
        if ((angle < -900) || (angle > 300))
        {
            printf("H1T的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

        //2、发送到V3S_U3控制云台函数
        Gimbal_V3S_H1T_set_PitchAngle(angle);

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    else
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }
}

//设置云台航向角度
int Gimbal_Set_YawAngle(int angle)
{
    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        printf("H150s没有航向角度\n");
        return -1;
    }
    
}

//设置云台范围的最大最小值
T_JZsdkReturnCode Gimbal_SetGimbalRange(int value)
{
    printf("设置云台范围最大最小值\n");

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        return H3_H150ST_SetGimbalRange(value);
    }
    

    if (DEVICE_VERSION == JZ_U3)
    {
        return V3S_U3_SetGimbalRange(value);
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        /* code */
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//查询云台微调值
T_JZsdkReturnCode Gimbal_CheckStatus_GimbalFineTuning(int *FineTunigPitch, int *FineTunigYaw)
{
    printf("查询云台微调值\n");

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //1、获取微调值
        Gimbal_H3_H150ST_CheckStatus_GimbalFineTuning(&Gimbal_PitchFineTuning);

        //2、回复微调值
        *FineTunigPitch = Gimbal_PitchFineTuning;
        *FineTunigYaw = Gimbal_YawFineTuning;        

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;        
    }
    

    if (DEVICE_VERSION == JZ_U3)
    {   
        //1、获取微调值
        Gimbal_V3S_U3_CheckStatus_GimbalFineTuning(&Gimbal_PitchFineTuning);

        //2、回复微调值
        *FineTunigPitch = Gimbal_PitchFineTuning;
        *FineTunigYaw = Gimbal_YawFineTuning;        

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        //1、刷新微调值
        Gimbal_V3S_H1T_CheckStatus_GimbalFineTuning();

        delayMs(10);

        //2、回复微调值
        *FineTunigPitch = Gimbal_PitchFineTuning;
        *FineTunigYaw = Gimbal_YawFineTuning;

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//收到云台pitch微调值
T_JZsdkReturnCode Gimbal_Obtain_GimbalFineTuning(int pitch)
{
    printf("收到云台pitch云台微调值\n");

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //无操作
    }
    

    if (DEVICE_VERSION == JZ_U3)
    {
        //无操作
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        Gimbal_PitchFineTuning = pitch;
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//设置云台pitch微调值
T_JZsdkReturnCode Gimbal_Set_PitchFineTuning(int DeviceName ,int pitch)
{
    printf("设置云台pitch云台微调值\n");

    //原值+修改值
    Gimbal_PitchFineTuning = Gimbal_PitchFineTuning + pitch;

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        return Gimbal_H3_H150ST_set_PitchFineTuning(pitch);
    }
    

    if (DEVICE_VERSION == JZ_U3)
    {
        return Gimbal_V3S_U3_set_PitchFineTuning(pitch);
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        return Gimbal_V3S_H1T_set_PitchFineTuning(pitch);
    }
}

//设置云台联动
T_JZsdkReturnCode Gimbal_Set_GimbalLinkageControl(int value)
{
    printf("设置云台联动\n");

    Gimbal_LinkageNum = value;

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {
        //没这玩意
    }

    if (DEVICE_VERSION == JZ_U3)
    {
        //没这玩意
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        Gimbal_V3S_H1T_Set_GimbalLinkageControl(Gimbal_LinkageNum);
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;

}

//查询云台联动
T_JZsdkReturnCode Gimbal_CheckStatus_GimbalLinkage(int *value)
{
    printf("查询云台联动值\n");
    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {

    }
    
    if (DEVICE_VERSION == JZ_U3)
    {
        
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        //1、刷新云台联动值
        Gimbal_V3S_H1T_CheckStatus_GimbalLinkage();

        delayMs(10);

        //2、回复云台联动值
        *value = Gimbal_LinkageNum;

        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//收到云台联动值
T_JZsdkReturnCode Gimbal_Obtain_GimbalLinkage(int GimbalLinkage)
{
    printf("收到云台联动值\n");

    Gimbal_LinkageNum = GimbalLinkage;

    if (DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T)
    {

    }
    

    if (DEVICE_VERSION == JZ_U3)
    {
        
    }

    if (DEVICE_VERSION == JZ_H1T)
    {
        //无操作
        //毕竟是t60s返回来的值
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}


//获取探警灯状态
T_JZsdkReturnCode Gimbal_Get_Angle(int *PitchAngle, int *YawAngle)
{
    *PitchAngle = Gimbal_PitchAngle;
    *YawAngle = Gimbal_YawAngle;

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/*********
 * 
 *  查询云台角度
 * 
**********/
T_JZsdkReturnCode Gimbal_CheckStatus_Angle(int *PitchAngle, int *YawAngle)
{
    //1、刷新查询角度
    if (DEVICE_VERSION == JZ_H1T)
    {
        Gimbal_V3S_H1T_CheckStatus_GimbalAngle();
    }

    //2、获取一次本地云台角度
    Gimbal_Get_Angle(PitchAngle, YawAngle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

int Gimbal_Get_PitchAngle()
{
    return Gimbal_PitchAngle;
}

T_JZsdkReturnCode Gimbal_Get_PitchFineTuning()
{
    return Gimbal_PitchFineTuning;
}

T_JZsdkReturnCode Gimbal_Obtain_Gimbal_Pitch(int Pitch)
{
    Gimbal_PitchAngle = Pitch;
}