#include "speed_to_servoMotor.h"

#ifdef PTZ_SERVO_MOTOR

//云台伺服电机通信信号量，增加发送的数据信号量+1，发送完成信号量-1
BSP_OS_SEM g_horiSpeedSem;
BSP_OS_SEM g_vertSpeedSem;
//云台伺服电机能否发生数据互斥量
BSP_OS_SEM g_horiSpeedMutex;
BSP_OS_SEM g_vertSpeedMutex;



uint16_t memBuffer[linkListNodeLen * sendDataBufNumber * 2] = {0};
OS_MEM *g_memPtr;

ptzServoLinkList g_servoMotorLinkList = {0};


//发送云台实际转速
void ptz_send_speed(char dev, char speed)
{
    unsigned char  Speed1[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
    unsigned char  Speed2[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
    unsigned short int HSpeed = 0;
    unsigned short int VSpeed = 0;
    Speed1[1] = g_ptz.address;
    Speed2[1] = g_ptz.address;
    
    HSpeed = (unsigned short int)(g_ptz.hori_speed_actual * 100 + 0.5);
    Speed1[3] = 0X03;//水平电机
    Speed1[4] = (unsigned char)(HSpeed >> 8);
    Speed1[5] = (unsigned char)(HSpeed & 0x00ff);
    Speed1[6] = MotorCalPelcoDSUM(Speed1,sizeof(Speed1));
    
    VSpeed = (unsigned short int)(g_ptz.vert_speed_actual * 100 + 0.5);
    Speed2[3] = 0X04;//垂直电机
    Speed2[4] = (unsigned char)(VSpeed >> 8);
    Speed2[5] = (unsigned char)(VSpeed & 0x00ff);
    Speed2[6] = MotorCalPelcoDSUM(Speed2,sizeof(Speed2));
    
    switch(speed)
    {
        case 1://只查询水平转速
        ptz_send_data(dev,Speed1,sizeof(Speed1));        
        break;
        
        case 2://只查询垂直转速
        ptz_send_data(dev,Speed2,sizeof(Speed2));        
        break;
        
        default://水平转速垂直转速都查询
        ptz_send_data(dev,Speed1,sizeof(Speed1));
        ptz_send_data(dev,Speed2,sizeof(Speed2));        
        break;
    }
}

/*!
    \brief      链表发送完成后用于释放内存
    \param[in]  data：储存发送数据的链表
    \param[out] none 
    \retval     none
*/
void servoLinkListMemPut(linkList *data)
{
    if (data == NULL) {
        return;
    }
    
    linkList *ptr;
    ptr = data;
    data = data->next;
    OSMemPut(g_memPtr, (uint8_t *)ptr);
}

/*!
    \brief      发送函数
    \param[in]  motor：电机
                            horiMotor 水平电机
                            vertMotor 俯仰电机
    \param[in]  *data：要发送的数据
    \param[in]  dataLen：指向要发送的数据
    \param[in]  priority：将要发送的数据填入优先级
    \param[out] none 
    \retval     none
*/
void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
{
    if ((motor != horiMotorType) && (motor!= vertMotorType)) {
        return;
    }
    if (data == NULL) {
        return;
    }
    if (dataLen > sendDataBufLen) {
        return;
    }
    if ((priority != highPriority) && (priority != lowPriority)) {
        return;
    }

    linkList *ptr = NULL;
    CPU_INT08U  err;

    //保存数据到链表节点
    ptr = OSMemGet(g_memPtr, &err);
    ptr->length = dataLen;
    memcpy(ptr->data, data, dataLen);

    //将节点添加进入链表中
    if (motor == horiMotorType) {
        if (g_servoMotorLinkList.horiMotor.linkListNum > sendDataBufNumber) {
            OSMemPut(g_memPtr, ptr);
            return;
        }

        if (priority == highPriority) {
            g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr;
            if (g_servoMotorLinkList.horiMotor.LinkListHead_H == NULL) {
                g_servoMotorLinkList.horiMotor.LinkListHead_H
                    = g_servoMotorLinkList.horiMotor.LinkListTail_H;
            }
        }
        else {
            g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr;
            if (g_servoMotorLinkList.horiMotor.LinkListHead_L == NULL) {
                g_servoMotorLinkList.horiMotor.LinkListHead_L
                    = g_servoMotorLinkList.horiMotor.LinkListTail_L;
            }            
        }

        //释放信号量，通知能发送一次
        OSSemPost(g_horiSpeedSem);
    }
    else {
        if (g_servoMotorLinkList.vertMotor.linkListNum > sendDataBufNumber) {
            OSMemPut(g_memPtr, ptr);
            return;
        }

        if (priority == highPriority) {
            g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr;
            if (g_servoMotorLinkList.vertMotor.LinkListHead_H == NULL) {
                g_servoMotorLinkList.vertMotor.LinkListHead_H
                    = g_servoMotorLinkList.vertMotor.LinkListTail_H;
            }
        }
        else {
            g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr;
            if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) {
                g_servoMotorLinkList.vertMotor.LinkListHead_L
                    = g_servoMotorLinkList.vertMotor.LinkListTail_L;
            }            
        }

        //释放信号量，通知能发送一次
        OSSemPost(g_vertSpeedSem);
    }
}

static void ptz_hori_servo_task()
{   
    CPU_INT08U  err;
    while(1) {
        // if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
        // {
        // }
        
        // if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
        // {
        // }
        OSSemPend(g_horiSpeedMutex, 0, &err);
        OSSemPend(g_horiSpeedSem, 0, &err);

        // 高优先级链表中数据先发送
        if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) {
            //发送数据



            g_servoMotorLinkList.horiMotor.linkListNum = highPriority;
            continue;
        }
        
        // 高优先级链表中无数据时，发送低优先级中的数据
        if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
            //发送数据



            g_servoMotorLinkList.horiMotor.linkListNum = lowPriority;
        }
        else {
            pdebug(DEBUG_LEVEL_FATAL,"ptz_hori_servo_task  no data...\n\r");
        }
    }
}


static void ptz_vert_servo_task()
{
    CPU_INT08U  err;
    while(1) {
        // if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
        // {
        // }
        // if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
        // {
        // } 
        OSSemPend(g_vertSpeedMutex, 0, &err);
        OSSemPend(g_vertSpeedSem, 0, &err);

        // 高优先级链表中数据先发送
        if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
            g_servoMotorLinkList.vertMotor.linkListNum = highPriority;


            
            continue;
        }
        
        // 高优先级链表中无数据时，发送低优先级中的数据
        if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
            g_servoMotorLinkList.vertMotor.linkListNum = highPriority;


            
        }
        else {
            pdebug(DEBUG_LEVEL_FATAL,"ptz_vert_servo_task  no data...\n\r");
        }
    }
}

static OS_STK  task_hori_servo_stk[TASK_HORI_PID_STK_SIZE];
static  void  creat_task_hori_servo_task(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_servo_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_hori_servo_stk[TASK_HORI_PID_STK_SIZE - 1],
		   (INT8U           ) TASK_HORI_PID_PRIO,                   
		   (INT16U          ) TASK_HORI_PID_PRIO,
		   (OS_STK         *)&task_hori_servo_stk[0],
		   (INT32U          ) TASK_HORI_PID_STK_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));     
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(TASK_HORI_PID_PRIO, "ptz_hori_step_speed_task", &name_err);
    #endif
       
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_step_speed_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_step_speed_task  failed...\n\r");
	}
}


static OS_STK  task_vert_servo_stk[TASK_VERT_PID_STK_SIZE];
static  void  creat_task_vert_servo_task(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_servo_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_vert_servo_stk[TASK_VERT_PID_STK_SIZE - 1],
		   (INT8U           ) TASK_VERT_PID_PRIO,                   
		   (INT16U          ) TASK_VERT_PID_PRIO,
		   (OS_STK         *)&task_vert_servo_stk[0],
		   (INT32U          ) TASK_VERT_PID_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_vert_servo_task", &name_err);
    #endif
       
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_servo_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_servo_task  failed...\n\r");
	}
}



void init_speed_module(void)
{
    g_horiSpeedSem = OSSemCreate(0);
    g_vertSpeedSem = OSSemCreate(0);
    g_horiSpeedMutex = OSSemCreate(1);
    g_vertSpeedMutex = OSSemCreate(1);

    OSSemPost(g_horiSpeedMutex);
    OSSemPost(g_vertSpeedMutex);

    CPU_INT08U  err;
    g_memPtr = OSMemCreate(memBuffer, sendDataBufNumber * 2, linkListNodeLen, &err);
    if (err != OS_ERR_NONE) {
        pdebug(DEBUG_LEVEL_FATAL, "Failed to create the motor memory pool\n\r");
    }

    //初始化链表头尾
    g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
    g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
    g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
    g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
    g_servoMotorLinkList.horiMotor.linkListPriority = nonePriority;
    g_servoMotorLinkList.horiMotor.linkListNum = 0;
    g_servoMotorLinkList.vertMotor.LinkListHead_H = NULL;
    g_servoMotorLinkList.vertMotor.LinkListHead_L = NULL;
    g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
    g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
    g_servoMotorLinkList.vertMotor.linkListPriority = nonePriority;
    g_servoMotorLinkList.vertMotor.linkListNum = 0;


    servoMotorInit();
    Init_ServoMotorRecv();

    creat_task_hori_servo_task();
    creat_task_vert_servo_task();
}




#endif