#include "speed_to_servoMotor.h"
#include "ptz_header_file.h"
#include "servoMotor.h"


#ifdef PTZ_SERVO_MOTOR

//电机数据解析任务互斥量
BSP_OS_SEM g_horiMotorMutex;
BSP_OS_SEM g_vertMotorMutex;

/**
* @brief    写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
* @param    motorNo：写垂直电机还是水平电机
* @return   false:错误，读出的内容与写入的内容不一致
*/
static bool MotorReplyForWrite(uint8_t motorNo)
{
    static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
    if (motorNo == horiMotorType )
    {
        if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
        {

            if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
            {
                /*提取数据失败*/
                if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
                {
                    return false;
                }
                if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
                {
                    return false;
                }
                servoLinkListMemPut(horiMotorType);  
            }
            else
            {
              return false;
            }
        }
        else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
        {

            if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
            {
                /*提取数据失败*/
                if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
                {
                    return false;
                }
                if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
                {
                    return false;
                }
                servoLinkListMemPut(horiMotorType);  
            }
            else
            {
              return false;
            }
        }
//        servoLinkListMemPut(horiMotorType);    
    }
    else if ( motorNo == vertMotorType )
    {
        if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
        {

            if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
            {
                /*提取数据失败*/
                if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
                {
                    return false;
                }
                if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
                {
                    return false;
                }
                servoLinkListMemPut(vertMotorType);  
            }
            else
            {
              return false;
            }
        }
        else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
        {

            if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
            {
                /*提取数据失败*/
                if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
                {
                    return false;
                }
                if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
                {
                    return false;
                }
                servoLinkListMemPut(vertMotorType);  
            }
            else
            {
              return false;
            }
        }
//        servoLinkListMemPut(vertMotorType);   
    }
    
    return true;
}

/**
* @brief    读单个寄存器成功后，电机会返回一帧7字节数据
* @param    motorNo：写垂直电机还是水平电机
* @param    speed：获取读取命令后，电机返回的数据内容
* @return   false:读取返回错误
*/

static bool MotorReplyForRead(uint8_t motorNo)
{
    int16_t speed = 0;
    static uint8_t motorReplybuff[READ_ONE_REG_FRAME_NUM];
    if (motorNo == horiMotorType )
    {
        if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
        {

            if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
            {
                /*提取数据失败*/
                if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
                {
                    return false;
                }
                if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
                if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
                if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
                speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
                if (speed < 0)
                {
                    speed = -speed;
                }
                g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;                
                servoLinkListMemPut(horiMotorType); 
            }
            else
            {
              return false;
            }
        }
        else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
        {

            if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
            {
                /*提取数据失败*/
                if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
                {
                    return false;
                }
                if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
                if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
                if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
                speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
                if (speed < 0)
                {
                    speed = -speed;
                }
                g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO; 
                servoLinkListMemPut(horiMotorType); 
            }
            else
            {
              return false;
            }
        }
//        servoLinkListMemPut(horiMotorType);    
    }
    else if ( motorNo == vertMotorType )
    {
        if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
        {

            if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
            {
                /*提取数据失败*/
                if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
                {
                    return false;
                }
                if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
                if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
                if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
                speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
                if (speed < 0)
                {
                    speed = -speed;
                }
                g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
                servoLinkListMemPut(vertMotorType); 
            }
            else
            {
              return false;
            }
        }
        else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
        {

            if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
            {
                /*提取数据失败*/
                if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
                {
                    return false;
                }
                if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
                if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
                if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
                speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
                if (speed < 0)
                {
                    speed = -speed;
                }
                g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
                servoLinkListMemPut(vertMotorType); 
            }
            else
            {
                return false;
            }
        }
//        servoLinkListMemPut(vertMotorType);   
    }
    
    return true;
}

/*!
    \brief      水平电机接收任务
    \param[in]  none
    \param[out] none 
    \retval     none
*/
static void ptz_recv_hori_servo_task()
{
    CPU_INT08U  err;
    while(1) {
        OSSemPend(g_horiMotorMutex, 0, &err);
        stopTimeOut(H_MOTOR);
        if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR) ) == false)
        {
            // H_MOTOR_STOP;
            continue;
        }
//        OSTimeDlyHMSM(0u, 0u, 0u, 5u);
        //释放信号量，通知能发送一次
        OSSemPost(g_horiSpeedMutex); 
    }
}

/*!
    \brief      俯仰电机接收任务
    \param[in]  none
    \param[out] none
    \retval     none
*/
static void ptz_recv_vert_servo_task()
{
    CPU_INT08U  err;
    while(1) {
        OSSemPend(g_vertMotorMutex, 0, &err);
        stopTimeOut(V_MOTOR);
        if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR) ) == false)
        {
            // V_MOTOR_STOP;
            continue;
        }
        
        //释放信号量，通知能发送一次
        OSSemPost(g_vertSpeedMutex);  
    }
}

static OS_STK  task_recv_hori_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE];
static OS_STK  task_recv_vert_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE];
static  void  creat_task_servo_recv_task(void)
{
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;

   task_err = OSTaskCreateExt((void (*)(void *)) ptz_recv_hori_servo_task,
		   (void           *) 0,
		   (OS_STK         *)&task_recv_hori_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE - 1],
		   (INT8U           ) TASK_RECV_HORI_SERVO_PRIO,                   
		   (INT16U          ) TASK_RECV_HORI_SERVO_PRIO,
		   (OS_STK         *)&task_recv_hori_servo_stk[0],
		   (INT32U          ) TASK_RECV_HORI_SERVO_STK_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_recv_hori_servo_task", &name_err);
    #endif
       
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_recv_hori_servo_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_recv_hori_servo_task  failed...\n\r");
	}

   task_err = OSTaskCreateExt((void (*)(void *)) ptz_recv_vert_servo_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_recv_vert_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE - 1],
		   (INT8U           ) TASK_RECV_VERT_SERVO_PRIO,                   
		   (INT16U          ) TASK_RECV_VERT_SERVO_PRIO,
		   (OS_STK         *)&task_recv_vert_servo_stk[0],
		   (INT32U          ) TASK_RECV_HORI_SERVO_STK_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_recv_vert_servo_task", &name_err);
    #endif
       
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_recv_vert_servo_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_recv_vert_servo_task  failed...\n\r");
	}
}


void Init_ServoMotorRecv(void)
{
    CPU_INT08U  err;
    g_horiMotorMutex = OSSemCreate(1);
    g_vertMotorMutex = OSSemCreate(1);
    OSSemPend(g_horiMotorMutex, 1, &err);
    OSSemPend(g_vertMotorMutex, 1, &err);

    creat_task_servo_recv_task();
}

#endif