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

#include "JZsdkLib.h"
#include "BaseConfig.h"

#include "JZsdk_TaskManagement/TaskManagement.h"
#include "Gimbal_SpecialUart/Gimbal_SpecialUart.h"
#include "../Gimbal_Param.h"
#include "Gimbal/Gimbal.h"

#include "Hal_Send/HalSend.h"
#include "JZsdk_uart/JZsdk_Uart.h"
#include "Ircut/H3_ircut/H3_ircut.h"

#ifdef WIRINGPI_STATUS_ON
	#include <wiringPi.h>
#endif

#define PITCH_OFFSET_FILE_PATH "/root/Gimbal_Pitch_BaseOffset"

static T_JZsdkReturnCode Gimbal_DataDeal_Read_PitchBaseOffset(int *offset);
static T_JZsdkReturnCode Gimbal_DataDeal_Write_PitchBaseOffset(int offset);


static int g_Gimbal_Pitch_BaseOffset = 0; //云台俯仰基准偏移量

/******************
 * 
 * 初始化  
 * 
 * **************/
T_JZsdkReturnCode Gimbal_DataDeal_init()
{    
//由于150s 150t 开发时还没有帧协议,所以是一个独特的控制类型
#if DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T

    int Uart_fd = 0;

    //1、串口初始化
    JZsdk_Uart_UartEnabled(GIMBAL_UART_NUM, GIMBAL_UART_BITRATE, &Uart_fd);

    //2、串口接收初始化
    Gimbal_SpecialUart_UartReceive_Init(Uart_fd);

//不需特地初始化
#elif DEVICE_VERSION == JZ_H1T || DEVICE_VERSION == JZ_C1
    // 不用去特地初始化t60的云台
#elif DEVICE_VERSION == JZ_H10 ||  DEVICE_VERSION == JZ_H10T

    //H10 H10T需要去读取云台偏移量
    Gimbal_DataDeal_Read_PitchBaseOffset(&g_Gimbal_Pitch_BaseOffset);

#elif DEVICE_VERSION == JZ_U3 \
    || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D || DEVICE_VERSION == JZ_U30 || DEVICE_VERSION == TF_A1
    //设置0度
    
#else
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
#endif

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

T_JZsdkReturnCode Gimbal_DataDeal_SetRealAngle(int angle)
{
    int angle_PWM = 0;

#if DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T
    //调整为PWM值 1000~2000

    //完整计算公式
    //angle_PWM= PITCH_PWM_MIN +(-angle)*(PITCH_PWM_MAX-PITCH_PWM_MIN)/(abs(MAX_PITCH)+abs(MIN_PITCH));
    //简化为
    angle_PWM= PITCH_PWM_MIN +(angle)*(PITCH_PWM_MAX-PITCH_PWM_MIN)/MIN_PITCH;

	unsigned char send_angle[]={0x5a,0x00,0x00,0xa5};
	send_angle[1]=(unsigned char )(angle_PWM);
	send_angle[2]=(unsigned char )(angle_PWM>>8);
	
    Gimbal_SpecialUart_UartSend(send_angle,   4);
#elif DEVICE_VERSION == JZ_H10
    //因为PWM写的时候是按90度写的，所以分母是900，不是设定的600
    int base_offset = g_Gimbal_Pitch_BaseOffset; 
                            //最大最小值的偏移量 //正整数时，则总体角度向0偏移 为负数时，向-900便宜
                         //计算方法 设置最小角度时 为 PwM - 195 - 200  //设置最大角度时 为 PwM - 0 - 200
    int base = 200;  //0度

    //每43.5对饮20度

    //完整计算公式
    //angle_PWM = base + angle*195/((abs(MAX_PITCH) + abs(MIN_PITCH)))
    //上调15度
    //angle_PWM = 200 -angle*195/900 -150*195/900; //上调15度

    //简化为
    //angle_PWM =base + base_offset +angle*195/MIN_PITCH; 
    angle_PWM =base + base_offset +angle*195/(-900);  

#ifdef WIRINGPI_STATUS_ON
    H3_ircut_pwm(angle_PWM);
#endif

#elif DEVICE_VERSION == JZ_H10T
    int base_offset = g_Gimbal_Pitch_BaseOffset;
    int base = 820;

    //因为PWM写的时候是按60度写的
    angle_PWM = base + base_offset + ( angle * (120) /(MAX_PITCH - MIN_PITCH) ); 

    Ircut_PWM_control(angle_PWM);

#elif DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D || DEVICE_VERSION == JZ_U30 \
    || DEVICE_VERSION == TF_A1

    //发送角度给串口2
    HalSend_type1Send_Set_GimbalPitchAngle(UART_DEV_2,  angle);

#elif DEVICE_VERSION == JZ_H1T
    
    //发送角度给串口1
    HalSend_type1Send_Set_GimbalPitchAngle(UART_DEV_1,  angle);

#else
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
#endif
    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/******************************
 * 
 * 
 *  设置云台最大最小值
 * 
 * 
 * *******************************/
T_JZsdkReturnCode Gimbal_DataDeal_SetPitchRange(int Range)
{
    if (Range != 0xFF && Range != 0x00)
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }
    
#if DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T
    uint8_t set_min_motor[]={0x4A,0x00,0x00,0xA4};
    uint8_t set_max_motor[]={0x6A,0x00,0x00,0xA6};

    if (Range == 0xFF)
    {
        Gimbal_SpecialUart_UartSend(set_max_motor,   4);
    }
    else if (Range == 0x00)
    {
        Gimbal_SpecialUart_UartSend(set_min_motor,   4);
    }
#elif DEVICE_VERSION == JZ_H10
    //因为PWM写的时候是按90度写的，所以分母是900，不是设定的600
    int base_offset = g_Gimbal_Pitch_BaseOffset;

    //在最大的角度弄的
    if (Range == 0xFF)
    {
        //先通过角度 得出目前的PWM值
        int angle = Gimbal_Get_PitchRealPitchAngle();
        //int angle_PWM =200 + base_offset +angle*195/MIN_PITCH; 
        int angle_PWM =200 + base_offset +angle*195/(-900); 

        //通过最大值的计算方式 得出offset值
        //g_Gimbal_Pitch_BaseOffset = angle_PWM - ((MAX_PITCH*195)/(MAX_PITCH - MIN_PITCH)) - base_offset;
        g_Gimbal_Pitch_BaseOffset = angle_PWM - ((MAX_PITCH*195)/(MAX_PITCH - (-900))) - base_offset;
        if (g_Gimbal_Pitch_BaseOffset <= - 100)
        {
            g_Gimbal_Pitch_BaseOffset = - 100;
        }
        else if (g_Gimbal_Pitch_BaseOffset >= 100)
        {
            g_Gimbal_Pitch_BaseOffset = 100;
        }
    
    }

    //在最小的角度弄的
    else if (Range == 0x00)
    {
        //先通过角度 得出目前的PWM值
        int angle = Gimbal_Get_PitchRealPitchAngle();
        int angle_PWM =200 + base_offset +angle*195/(-900); 

        //通过最小值的计算方式 得出offset值
        g_Gimbal_Pitch_BaseOffset = angle_PWM - (((MAX_PITCH - (-900))*195)/(MAX_PITCH - (-900)) ) - base_offset;
        if (g_Gimbal_Pitch_BaseOffset <= - 100) //最小160
        {
            g_Gimbal_Pitch_BaseOffset = - 100;
        }
        else if (g_Gimbal_Pitch_BaseOffset >= 100) //最大420
        {
            g_Gimbal_Pitch_BaseOffset = 100;
        }
    
    }

    JZSDK_LOG_INFO("设置pitch的offset值:%d",g_Gimbal_Pitch_BaseOffset);
    
    //写入偏移量
    Gimbal_DataDeal_Write_PitchBaseOffset(g_Gimbal_Pitch_BaseOffset);

    //刷新角度
    Gimbal_Flush_Pitch();

#elif DEVICE_VERSION == JZ_H10T

    //因为PWM写的时候是按90度写的，所以分母是900，不是设定的600
    int base_offset = g_Gimbal_Pitch_BaseOffset;

    //先通过角度 得出目前的PWM值
    int angle = Gimbal_Get_PitchRealPitchAngle();

    //在最大的角度弄的
    if (Range == 0xFF)
    {
        g_Gimbal_Pitch_BaseOffset++;
    }
    //在最小的角度弄的
    else if (Range == 0x00)
    {
        g_Gimbal_Pitch_BaseOffset--;
    }


    // //因为PWM写的时候是按90度写的，所以分母是900，不是设定的600
    // int base_offset = g_Gimbal_Pitch_BaseOffset;
    
    // //先通过角度 得出目前的PWM值
    // int angle = Gimbal_Get_PitchRealPitchAngle();

    // //在最大的角度弄的
    // if (Range == 0xFF)
    // {
    //     //先得出目前pwm值
    //     int angle_PWM = 820 + base_offset + (angle * (120)/(MAX_PITCH - MIN_PITCH));   //820是0度 

    //     //通过最大值的计算方式 得出新的offset值
    //     //g_Gimbal_Pitch_BaseOffset = angle_PWM - ((MAX_PITCH*195)/(MAX_PITCH - MIN_PITCH)) - base_offset;
    //     g_Gimbal_Pitch_BaseOffset = angle_PWM - 820 - ((MAX_PITCH*120)/(MAX_PITCH - MIN_PITCH)) - base_offset;

    //     //下限600，上限830
    //     if (g_Gimbal_Pitch_BaseOffset <= - 100)
    //     {
    //         g_Gimbal_Pitch_BaseOffset = - 100;
    //     }
    //     else if (g_Gimbal_Pitch_BaseOffset >= 20)
    //     {
    //         g_Gimbal_Pitch_BaseOffset = 20;
    //     }
    // }

    // //在最小的角度弄的
    // else if (Range == 0x00)
    // {
    //     //先通过角度 得出目前的PWM值
    //     int angle = Gimbal_Get_PitchRealPitchAngle();
    //     int angle_PWM = 820 + base_offset + (angle * (120)/(900));   //820是0度 

    //     //通过最小值的计算方式 得出offset值
    //     g_Gimbal_Pitch_BaseOffset = angle_PWM - 820 - ((MAX_PITCH*120)/(MAX_PITCH - MIN_PITCH)) - base_offset;

    //     //下限600，上限830
    //     if (g_Gimbal_Pitch_BaseOffset <= - 100)
    //     {
    //         g_Gimbal_Pitch_BaseOffset = - 100;
    //     }
    //     else if (g_Gimbal_Pitch_BaseOffset >= 20)
    //     {
    //         g_Gimbal_Pitch_BaseOffset = 20;
    //     }
    // }

    JZSDK_LOG_INFO("设置pitch的offset值:%d",g_Gimbal_Pitch_BaseOffset);
    
    //写入偏移量
    Gimbal_DataDeal_Write_PitchBaseOffset(g_Gimbal_Pitch_BaseOffset);

    //刷新角度
    Gimbal_Flush_Pitch();
    
#elif DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D || DEVICE_VERSION == JZ_U30  \
    || DEVICE_VERSION == TF_A1

    if (Range == 0xFF)
    {
        HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0, Range);
    }
    else if (Range == 0x00)
    {
        HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0, Range);
    }

#elif DEVICE_VERSION == JZ_H1T

    //好像没必要弄

#else
    return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
#endif

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

static T_JZsdkReturnCode Gimbal_DataDeal_Read_PitchBaseOffset(int *offset)
{
    FILE *fp = NULL;
    
	fp = fopen(PITCH_OFFSET_FILE_PATH, "r+b");
    if (fp == NULL) 
    {
        fp = fopen(PITCH_OFFSET_FILE_PATH, "w+b");
        if (fp == NULL) 
        {
            *offset = 0;
            return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
        }
    } 
    else 
    {
        int ret = fseek(fp, 0, SEEK_SET);
        if (ret != 0) 
        {
            JZSDK_LOG_ERROR("Seek count file error, ret: %d.", ret);
        }

        ret = fread((int *) offset, 1, sizeof(int), fp);
        if (ret != sizeof(int)) 
        {
            JZSDK_LOG_ERROR("Read Pitch BaseOffset error.");
            *offset = 0;
            return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
        }
		else
        {
			JZSDK_LOG_INFO("Pitch BaseOffset=%d",*offset);
            return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
		}
    }

    if (fp != NULL)
    {
        fclose(fp);
    }
    	
	return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

static T_JZsdkReturnCode Gimbal_DataDeal_Write_PitchBaseOffset(int offset)
{
    FILE *fp = NULL;

    fp = fopen(PITCH_OFFSET_FILE_PATH, "w+b");
    if (fp == NULL) 
    {
        return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
    }
    
	int ret = fwrite((int *) &offset, 1, sizeof(unsigned int),fp);
    if (ret != sizeof(unsigned int)) 
    {
        JZSDK_LOG_ERROR("Pitch BaseOffset error.");
    }
	else
    {
		JZSDK_LOG_DEBUG("Pitch BaseOffset=%d\n",offset);
	}

    fflush(fp);

    fclose(fp);

    return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}