#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"

#include "Psdk_UI_io.h"

//数值角度记录
#if DEVICE_VERSION == TF_A1
    static int Gimbal_PitchAngle = 0;
    static int Gimbal_YawAngle = 0;
#else
    static int Gimbal_PitchAngle = 0;
    static int Gimbal_YawAngle = 0;
#endif


//微调角度
static int Gimbal_PitchFineTuning = 0;
static int Gimbal_YawFineTuning = 0;

//飞机自身角度
static int Gimbal_UavSelfPitch = 0;

//实际角度 (数值角度+飞机自身角度+微调角度)
static int Gimbal_RealPitchAngle = 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 if (DEVICE_VERSION == TF_A1)
    {
        Gimbal_V3S_TFA1_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 == TF_A1)
    {
        //1、判断输入范围是否正确
        if ((angle < -900) || (angle > 0))
        {
            printf("TFA1的云台俯仰输入值范围出错:%d\n", angle);
            return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
        }

        //2、记录全局云台值
        Gimbal_PitchAngle = angle;

        //3、叠加飞机角度和微调角度
        Gimbal_RealPitchAngle = angle - Gimbal_UavSelfPitch + Get_V3S_TFA1_motor_precise_adjustment_pitch();

        //5、修正angle值
        if (Gimbal_RealPitchAngle < -900)
        {
            angle = -900;
        }
        else if (Gimbal_RealPitchAngle > 0)
        {
            angle = 0;
        }
        else
        {
            angle = Gimbal_RealPitchAngle;
        }
        
        //6、发送到V3S_TFA1控制云台函数
        Gimbal_V3S_TFA1_set_angle(angle);

        //7、发送角度到飞机
        Gimbal_ReplyPitchToUAV(Gimbal_PitchAngle);

        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)
{

    printf("没有航向角度\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 == TF_A1)
    {
        return V3S_TFA1_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 == TF_A1)
    {   
        //1、获取微调值
        Gimbal_V3S_TFA1_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");
    T_JZsdkReturnCode ret;

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

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

    else if (DEVICE_VERSION == JZ_U3)
    {
        ret = Gimbal_V3S_U3_set_PitchFineTuning(pitch);
    }

    else if (DEVICE_VERSION == TF_A1)
    {
        ret = Gimbal_V3S_TFA1_set_PitchFineTuning(pitch);
    }

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

    //微调后重设角度
    Gimbal_Set_PitchAngle(Gimbal_PitchAngle);

    return ret;
}

//设置云台联动
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)
{

//目前只有A1修改了真实角度值
    if (DEVICE_VERSION == TF_A1)
    {
        //如果接受到的实际角度 - 飞机角度 + 微调角度 > 900 就不处理，避免角度左脚踩右脚上天
        if ( (Pitch - Gimbal_UavSelfPitch + Get_V3S_TFA1_motor_precise_adjustment_pitch()) >900)
        {
            //Gimbal_PitchAngle不变
            return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
        }
        else
        {
            Gimbal_PitchAngle = (Pitch + Gimbal_UavSelfPitch - Get_V3S_TFA1_motor_precise_adjustment_pitch());
        }
    }
    {
        Gimbal_PitchAngle = Pitch;
    }
}

/**************************************************************
 * 
 * 
 *         飞机特殊控制云台方法
 * 
 * 
***************************************************************/
//1、飞机自身角度控制云台
T_JZsdkReturnCode Gimbal_PitchSelfangleMode(int Pitch)
{
    if (Gimbal_UavSelfPitch  == Pitch)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    Gimbal_UavSelfPitch = Pitch;

    //设置云台角度
    Gimbal_Set_PitchAngle(Gimbal_PitchAngle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;

}

//2、滑动条控制云台
T_JZsdkReturnCode Gimbal_PitchScaleMode(int value)
{
    int angle;
    if (DEVICE_VERSION == TF_A1)
    {
        //0~100 转化为 -900~0
        angle = value*9 -900;
    }
    else
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    Gimbal_Set_PitchAngle(angle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

//3、拨轮控制云台
T_JZsdkReturnCode Gimbal_PitchRotationMode(int value)
{
    int angle;

    if (DEVICE_VERSION == TF_A1)
    {
        angle = Gimbal_PitchAngle + value;
        if (angle > 0)
        {
            angle = 0;
        }
        else if (angle < -900)
        {
            angle = -900;
        }
        
        if (angle == Gimbal_PitchAngle)
        {
            return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;  
        }
        
    }
    else
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    Gimbal_Set_PitchAngle(angle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;   
}

//回复角度到飞机
T_JZsdkReturnCode Gimbal_ReplyPitchToUAV(int angle)
{
    int value;

    if (DEVICE_VERSION == TF_A1)
    {
        //-900~0 转回 0~100
        value = (angle+ 900)/9;
    }
    else
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }

    JZsdk_Psdk_Ui_io_Gimbal_PitchPrint(value);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;   

}