#include "rotate_servo.h"


#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V

static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
static BSP_OS_SEM ptz_vert_stop_mutex;


float ptz_vert_break_angle()
{
    //云台水平轴加速度(°/s²)
    const float a = 1000.0f * 6.0f 
        / ((float)(PTZ_VERT_MOTOR_DecelerationTimeConstant / 1000.0f)) 
        / PTZ_VERT_RATIO;

    const float tmp = a / 2.0f;

    //平稳运行角度，以最低转速运行1秒
    const float s1 = PTZ_VERT_BREAK_SPEED * 1;

    //当前云台水平轴转速(°/s)
    float Vnow = g_ptz.hori_speed_actual * 6.0f;

    return (Vnow - PTZ_VERT_BREAK_SPEED) * (Vnow + PTZ_VERT_BREAK_SPEED) / tmp + s1;
}
float ptz_hori_break_angle()
{
    //云台水平轴加速度(°/s²)
    const float a = 1000.0f * 6.0f 
        / ((float)(PTZ_HORI_MOTOR_DecelerationTimeConstant / 1000.0f)) 
        / PTZ_HORI_RATIO;

    const float tmp = a / 2.0f;

    //平稳运行角度，以最低转速运行1秒
    const float s1 = PTZ_HORI_BREAK_SPEED * 1;

    //当前云台水平轴转速(°/s)
    float Vnow = g_ptz.hori_speed_actual * 6.0f;

    return (Vnow - PTZ_HORI_BREAK_SPEED) * (Vnow + PTZ_HORI_BREAK_SPEED) / tmp + s1;
}

void ptz_sem_post_stop_mutex()
{
    BSP_OS_SemPost(&ptz_hori_stop_mutex);
    BSP_OS_SemPost(&ptz_vert_stop_mutex);
}

void ptz_hori_start(char direction, float speed)
{
    BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);

    BSP_OS_SemPost(&ptz_hori_stop_mutex);
}

void ptz_hori_stop(unsigned short int time)
{
    BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);

    BSP_OS_SemPost(&ptz_hori_stop_mutex);
}

void ptz_vert_start(char direction, float speed)
{
    BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);

    BSP_OS_SemPost(&ptz_vert_stop_mutex);
}

void ptz_vert_stop(unsigned short int time)
{
    BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);

    BSP_OS_SemPost(&ptz_vert_stop_mutex);
}

static void ptz_hori_rotate_monitor_task()
{

}

static void ptz_vert_rotate_monitor_task()
{

}


static void ptz_hori_rotate_task()
{
    while(1)
    {
        ptz_hori_rotate_monitor_task();
        OSTimeDlyHMSM(0u, 0u, 0u, 1u);
    }
}


static void ptz_vert_rotate_task()
{
    while(1)
    {
        ptz_vert_rotate_monitor_task();
        OSTimeDlyHMSM(0u, 0u, 0u, 1u);
    }
}

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



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

void init_rotate_monitor_module(void)
{
    BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex");
    BSP_OS_SemCreate(&ptz_vert_stop_mutex,1u,"ptz_vert_stop_mutex");

    creat_task_hori_rotate();
    creat_task_vert_rotate();
}


#endif