/*
 * @Author: ookk303
 * @Date: 2024-08-23 17:46:53
 * @LastEditors: Do not edit
 * @LastEditTime: 2024-08-28 15:26:17
 * @Description: 
 * @FilePath: \JZSDK_Linux\Module\Gimbal\Gimbal.c
 */

#include <stdio.h>
#include <pthread.h>
#include <math.h>
#include "JZsdkLib.h"
#include "Gimbal/Gimbal_Param.h"
#include "Gimbal/Gimbal.h"
#include "Gimbal/Gimbal_DataDeal/Gimbal_DataDeal.h"
#include "Gimbal/Gimbal_MotorFineTuning/MotorFineTuning.h"
#include "JZsdk_uart/JZsdk_Uart_Input.h"

#include "BaseConfig.h"


//数值角度记录
static int Gimbal_PitchAngle = 0; //俯仰角度  //控制云台上相机相对于水平面的上下移动。
static int Gimbal_YawAngle = 0;  //偏航角度   //个角度控制云台上相机相对于水平面的上下移动。
static int Gimbal_RollAngle = 0; //滚转角度   //控制云台上相机围绕其前后轴（即与相机镜头方向平行的轴）的旋转。

//微调角度
static int g_MotorFineTuningPitch = 0; //pitch微调值
static int g_MotorFineTuningYaw = 0; //yaw微调值
static int g_MotorFineTuningRoll = 0; //roll微调值

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

//实际角度 (数值角度+飞机自身角度+微调角度)
static int Gimbal_RealPitchAngle = 0;

static int g_Gimbal_Linkage; //云台联动标志位

static int g_JZsdk_GimbalStatusFlag = JZ_FLAGCODE_OFF; //云台状态，用于判断云台是否已经初始化完毕

static T_JZsdkReturnCode Gimbal_Set_RealPitchAngle(int pitch, int FineTuningP, int UAV_pitch);

/**************************
 * 
 * 
 *  云台角度发送线程
 * 
 * 
 * ***************************/
static void *Gimbal_SendAngleTask(void *arg)
{
    int Old_angle = 0;      //上一次变化的角度
    int Old_UAV_self_angle; //上一次飞机的角度
    int Old_PitchFineTuning;    //上一次微调的角度

    JZSDK_LOG_INFO("云台角度发送初始化完成");

    int test_value =0;

    while (1)
    {
        if (Gimbal_PitchAngle != Old_angle //云台角度发生了变化
        || Gimbal_UavSelfPitch != Old_UAV_self_angle //飞机自身角度发生了变化
        || g_MotorFineTuningPitch != Old_PitchFineTuning  //微调角度发生了变化
        )
        {
            Old_angle = Gimbal_PitchAngle;
            Old_UAV_self_angle = Gimbal_UavSelfPitch;
            Old_PitchFineTuning = g_MotorFineTuningPitch;
            Gimbal_Set_RealPitchAngle(Old_angle, Old_PitchFineTuning, Old_UAV_self_angle);
        }

        //1s检测200次舵机角度
        delayMs(1000/200);
    }
}

//发送线程初始化
static T_JZsdkReturnCode Gimbal_SendAngleTask_Init()
{
	pthread_t WriteDataTask;
	pthread_attr_t task_attribute; //线程属性
	pthread_attr_init(&task_attribute);  //初始化线程属性
	pthread_attr_setdetachstate(&task_attribute, PTHREAD_CREATE_DETACHED);      //设置线程分离属性
	int opus_Protection = pthread_create(&WriteDataTask,&task_attribute,Gimbal_SendAngleTask,NULL);		//线程
	if(opus_Protection != 0)
	{
		JZSDK_LOG_ERROR("角度发送线程初始化失败！");
		return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
	}

    JZSDK_LOG_INFO("MODULE_GIMBAL_INIT_COMPLETE");
}

//初始化舵机部分
static T_JZsdkReturnCode Gimal_MotorInit()
{
    //角度发送线程初始化
	Gimbal_SendAngleTask_Init();

    //数据处理初始化
    Gimbal_DataDeal_init();

}

//初始化云台模块
T_JZsdkReturnCode Gimbal_Init()
{
    //舵机初始化
    Gimal_MotorInit();

    //云台微调初始化
    MotorFineTuning_Init(&g_MotorFineTuningPitch, &g_MotorFineTuningYaw, &g_MotorFineTuningRoll);

    //云台flag打开
    g_JZsdk_GimbalStatusFlag = JZ_FLAGCODE_ON;

    JZSDK_LOG_INFO("JZsdk_Gimbal_Module_init");
}

//设置云台俯仰角度
T_JZsdkReturnCode Gimbal_Set_PitchAngle(int angle)
{
    //禁止打印
    //printf("设置云台俯仰角度\n");

    //云台未初始化
    if (g_JZsdk_GimbalStatusFlag != JZ_FLAGCODE_ON)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }
    
    if (angle < MIN_PITCH || angle > MAX_PITCH || (MIN_PITCH == MAX_PITCH))
    {
        printf("云台俯仰输入值范围出错:%d\n", angle);
        return JZ_ERRORCODE_GIMBAL_INVALID_PITCH;
    }
    
    Gimbal_PitchAngle = angle;

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;     
}

//设置云台航向角度
T_JZsdkReturnCode Gimbal_Set_YawAngle(int angle)
{
    printf("没有航向角度\n");
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//设置云台转向角度
T_JZsdkReturnCode Gimbal_Set_RollAngle(int angle)
{
    printf("没有转向角度\n");
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

/*******************
 * 
 *  设置Pitch_云台范围的最大最小值
 *  flag 0 pitch 1 yaw 2 roll
 * 
 * *************************/
T_JZsdkReturnCode Gimbal_SetGimbalRange(int flag, int value)
{
    JZSDK_LOG_INFO("设置云台范围最大最小值 flag:%d 0x%x", flag, value);

    switch (flag)
    {
        case 0:
            return Gimbal_DataDeal_SetPitchRange(value);     
        break;
        
        default:
            break;
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}

//查询云台微调值
T_JZsdkReturnCode Gimbal_CheckStatus_GimbalFineTuning(int *FineTuningPitch, int *FineTuningYaw, int *FineTuningRoll)
{
    JZSDK_LOG_DEBUG("查询云台微调值");

//组合机需要去获取另外一个设备的值
#if DEVICE_VERSION == JZ_H1T
    //1、刷新微调值
    JZsdk_Uart_Send_CheckStatus_GimbalFineTuningPitch(UART_DEV_1, 0);

    delayMs(10);
#endif

    if (FineTuningPitch != NULL)
    {
        *FineTuningPitch = g_MotorFineTuningPitch;
    }
    
    if (FineTuningYaw != NULL)
    {
        *FineTuningYaw = g_MotorFineTuningYaw;
    }

    if (FineTuningRoll != NULL)
    {
        *FineTuningRoll = g_MotorFineTuningRoll;
    }

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

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

//组合机需要记录另外一个设备的值
#if DEVICE_VERSION == JZ_H1T
    g_MotorFineTuningPitch = pitch;
#else
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
#endif

    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}


//设置pitch云台微调值
T_JZsdkReturnCode Gimbal_Set_PitchFineTuning(int pitch)
{
    JZSDK_LOG_INFO("设置云台pitch云台微调值\n");
    T_JZsdkReturnCode ret;

    if (pitch == 0)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    //设置本地微调值
    g_MotorFineTuningPitch = g_MotorFineTuningPitch + pitch;

    //组合机是发到另外一个设备
#if DEVICE_VERSION == JZ_H1T
    JZsdk_Uart_Send_SetGimbalFineTuningPitch(UART_DEV_1, g_MotorFineTuningPitch);
#else
    //存储到文件
    MotorFineTuning_WritePitch(g_MotorFineTuningPitch);
#endif

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

//设置yaw云台微调值
T_JZsdkReturnCode Gimbal_Set_YawFineTuning(int yaw)
{
    JZSDK_LOG_INFO("设置云台yaw云台微调值\n");
    T_JZsdkReturnCode ret;

    if (yaw == 0)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    g_MotorFineTuningYaw = g_MotorFineTuningYaw + yaw;

    //微调后重设角度
}

//设置roll云台微调值
T_JZsdkReturnCode Gimbal_Set_RollFineTuning(int pitch, int yaw, int roll)
{
    JZSDK_LOG_INFO("设置云台pitch云台微调值\n");
    T_JZsdkReturnCode ret;


    if (roll == 0)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
    }
    
    g_MotorFineTuningRoll = g_MotorFineTuningRoll + roll;

    //微调后重设角度
}


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

    g_Gimbal_Linkage = value;

#if DEVICE_VERSION == JZ_H1T
    JZsdk_Uart_Send_Set_GimbalLinkageControl(UART_DEV_1, 0, value);
#else
    //暂时没做接口
#endif
  
    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;

}

//查询云台联动
T_JZsdkReturnCode Gimbal_CheckStatus_GimbalLinkage(int *value)
{
    JZSDK_LOG_INFO("查询云台联动值\n");

#if DEVICE_VERSION == JZ_H1T
    //1、刷新云台联动值
    JZsdk_Uart_Send_CheckStatus_GimbalLinkage(UART_DEV_1, 0);

    delayMs(10);

    //2、回复云台联动值
    *value = g_Gimbal_Linkage;
#else
    //暂时没做接口
#endif

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

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

    g_Gimbal_Linkage = GimbalLinkage;

#if DEVICE_VERSION == JZ_H1T
    //无操作
    //毕竟是t60s返回来的值
#else
    //暂时没做接口
#endif

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/*********
 * 
 *  查询云台角度
 * 
**********/
T_JZsdkReturnCode Gimbal_CheckStatus_Angle(int *PitchAngle, int *YawAngle, int *RollAngle)
{
//组合机刷新查询角度
#if DEVICE_VERSION == JZ_H1T
    JZsdk_Uart_Send_CheckStatus_GimbalAngle(UART_DEV_1, 0);

    //给点延时问题不大
    delayMs(5);
#else
    //直接获取即可
#endif

    //2、获取一次本地云台角度
    if (PitchAngle != NULL)
    {
        *PitchAngle = Gimbal_PitchAngle;
    }
    
    if (YawAngle != NULL)
    {    
        *YawAngle = Gimbal_YawAngle;
    }

    if (RollAngle != NULL)
    {
        *RollAngle = Gimbal_RollAngle;
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


//获得云台值
T_JZsdkReturnCode Gimbal_Obtain_Gimbal_Pitch(int Pitch)
{
    //目前 U3和 H1T会接受到云台返回值
    if (DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D)
    {
        //不做处理，避免出现什么问题
    }
    else if (DEVICE_VERSION == JZ_H1T)
    {
        //Gimbal_PitchAngle = Pitch;
        //这里暂时不处理，避免多次把5a帧返还给串口1设备
        //但是会导致4g接收不到最新的角度
    }
    else
    {
        //不处理，避免出现什么问题
    }
    
    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

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

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;

}

//2、滑动条 换算 云台角度
int Gimbal_PitchScaleMode(int value)
{
    int step = (abs(MAX_PITCH) + abs(MIN_PITCH)) / 100;

    int angle = 0;

    angle = value * step + MIN_PITCH;

    return angle;
}

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

    //JZSDK_LOG_DEBUG("拨轮控制");
    if (MAX_PITCH == MIN_PITCH)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }
    

    angle = Gimbal_PitchAngle + value;
    if (angle > MAX_PITCH)
    {
        angle = MAX_PITCH;
    }
    else if (angle < MIN_PITCH)
    {
        angle = MIN_PITCH;
    }
    
    if (angle == Gimbal_PitchAngle)
    {
        //printf("拨轮控制无效\n");
        return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;  
    }

    Gimbal_Set_PitchAngle(angle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;   
}

//输入角度，回复滑动条值
int Gimbal_ReplyPitchToUAVScale(int angle)
{
    int value;

    int step = (abs(MAX_PITCH) + abs(MIN_PITCH)) / 100;
    value = (angle - MIN_PITCH)/step;

    return value;
}


//设置云台俯仰角度
static T_JZsdkReturnCode Gimbal_Set_RealPitchAngle(int pitch, int FineTuningP, int UAV_pitch)
{   
    int temp_angle = 0;

//组合机不用堆叠角度
#if DEVICE_VERSION == JZ_H1T
    temp_angle = pitch;
    Gimbal_RealPitchAngle = temp_angle;
#else
    //叠加飞机角度和微调角度
    temp_angle = pitch - UAV_pitch + FineTuningP;

    if (temp_angle < MIN_PITCH)
    {
        temp_angle = MIN_PITCH;
    }

    if (temp_angle > MAX_PITCH)
    {
        temp_angle = MAX_PITCH;
    }

    Gimbal_RealPitchAngle = temp_angle;
#endif

    //printf("设置真实俯仰角度\n"); //禁止打印

    Gimbal_DataDeal_SetRealAngle(temp_angle);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}



T_JZsdkReturnCode Get_JZsdk_GimbalStatusFlag()
{
    return g_JZsdk_GimbalStatusFlag;
}


int Gimbal_Get_PitchAngle()
{
    return Gimbal_PitchAngle;
}

int Gimbal_Get_PitchFineTuning()
{
    return g_MotorFineTuningPitch;
}

int Gimbal_Get_PitchRealPitchAngle()
{
    return Gimbal_RealPitchAngle;
}