#include "rotate_servo.h"
#include "speed_to_servoMotor.h"

#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V

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

static char ptz_hori_stop_count;//水平停止计数，防止多次停止多次延时
static char ptz_vert_stop_count;//垂直停止计数，防止多次停止多次延时

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);
}

/*
      ///云台水平右转
      #define PTZ_HORI_DIR_RIGHT     1
      ///云台水平左转
      #define PTZ_HORI_DIR_LEFT      3//0
      ///云台处于停止状态
      #define PTZ_HORI_DIR_STOP      2
*/
void ptz_hori_start(char direction, float speed)
{
    BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
//    switch ( direction )
//    {
//        case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
//        case PTZ_HORI_DIR_STOP:
//            break;    
//        case PTZ_HORI_DIR_LEFT:
//            speed*=-1;
//        default:
//            break;
//    }
//    //设定转动速度
//    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度
//                              , WRITE_ONE_REG_FRAME_NUM, lowPriority);
//    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
//                              , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
//    g_ptz.hori_speed_set = speed;
//    //设定转动方向
//    g_ptz.hori_direction_last = g_ptz.hori_direction_set;
//    g_ptz.hori_direction_set = direction;
//    g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
//    //启动电机
//    g_ptz.hori_start_stop_set = PTZ_HORI_START;
//    g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
//        
//    ptz_vert_stop_count = 0;
    BSP_OS_SemPost(&ptz_hori_stop_mutex);
}

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

//    //停止电机
//    g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
//    g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
//
//    g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP;
//  
//    
//    //设定转动速度
//    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
//                              , WRITE_ONE_REG_FRAME_NUM, lowPriority);
//    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
//                              , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机 
//    g_ptz.hori_speed_set = 0;
//    g_ptz.hori_speed_actual = 0;
//  
//    if(ptz_hori_stop_count <= 0)
//    {
//        OSTimeDlyHMSM(0u, 0u, 0u, time);
//        ptz_hori_stop_count = 0;
//    }
//    ptz_hori_stop_count ++;
//  //电子稳定
//#ifdef PTZ_ELECTRIC_STABLE_L6235D
//  ptz_hori_electric_stable_init();
//#endif
    
    BSP_OS_SemPost(&ptz_hori_stop_mutex);
}

/*
    ///云台垂直向上
    #define PTZ_VERT_DIR_UP        3//0
    ///云台垂直向下
    #define PTZ_VERT_DIR_DOWN      1
    ///云台处于停止状态
    #define PTZ_VERT_DIR_STOP      2
*/
void ptz_vert_start(char direction, float speed)
{
    BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);

//    switch ( direction )
//    {
//        case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
//        case PTZ_HORI_DIR_STOP:
//            break;    
//        case PTZ_HORI_DIR_LEFT:
//            speed*=-1;
//        default:
//            break;
//    }
//    //设定转动速度
//    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度
//                              , WRITE_ONE_REG_FRAME_NUM, lowPriority);
//    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
//                              , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
//    g_ptz.hori_speed_set = speed;
//    //设定转动方向
//    g_ptz.hori_direction_last = g_ptz.hori_direction_set;
//    g_ptz.hori_direction_set = direction;
//    g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
//    //启动电机
//    g_ptz.hori_start_stop_set = PTZ_HORI_START;
//    g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
//        
//    ptz_vert_stop_count = 0;
    
    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 uint8_t status;
   static int16_t speed = 20;
    switch( status )
    {
        case 0://切换为速度控制模式
                
                if (servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                status = 1;
                
        break;
        
        case 1://设置加速度时间常数
                if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                status = 2;
        break;
        
        case 2://设置减速度时间常数
                if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                status = 3;       
        break;
        
        case 3://保存设定的参数并运行
                if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                status = 4;       
        break;
        
        case 4://读取速度
                if ( servoSendData(horiMotorType, ReadMotorOneReg(H_MOTOR, READ_MOTOR_SPEED_NOW)
                              , READ_ONE_REG_FRAME_NUM, lowPriority) )
                status = 5;       
        break;  
        
       case 5://设置运行速度
                if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                speed+=10, speed%=100, 
                status = 4;        
        break;
        
        default:
          break;
    }
}

static void ptz_vert_rotate_monitor_task()
{
    static uint8_t status;
   static int16_t speed = 20;
    switch( status )
    {
        case 0://切换为速度控制模式
                
                if (servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H02_CONTR_MODE_SELEC, 0)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                status = 1;
                
        break;
        
        case 1://设置加速度时间常数
                if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                status = 2;
        break;
        
        case 2://设置减速度时间常数
                if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                status = 3;       
        break;
              
        case 3://保存设定的参数并运行
                if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                status = 4;       
        break;
        
        case 4://读速度
                if ( servoSendData(vertMotorType, ReadMotorOneReg(V_MOTOR, READ_MOTOR_SPEED_NOW)
                              , READ_ONE_REG_FRAME_NUM, lowPriority) )
                status = 5;       
        break;  
        
        case 5://设置运行速度
                if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
                speed+=10, speed%=100, 
                status = 4;       
        break; 
        
        default:
          break;
    }
}


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


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

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