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

/*!
    \brief      设置伺服电机速度
    \param[in]  data：储存发送数据的链表
    \param[out] none 
    \retval     none
*/
void set_speed_to_servoMotor(uint8_t motorType, float speed)
{

    /*水平电机*/
    if ( motorType == horiMotorType )//水平电机
    {
        //转换为电机端的r/min
        speed = speed * PTZ_HORI_RATIO;
        //设置速度，电机的r/min范围-6000~6000
        servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed)
                                , WRITE_ONE_REG_FRAME_NUM, lowPriority);  
    }
    /*垂直电机*/
    else
    {
        //转换为电机端的r/min
        speed = speed * PTZ_VERT_RATIO;
        //设置速度，电机的r/min范围-6000~6000
        servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed)
                                , WRITE_ONE_REG_FRAME_NUM, lowPriority);
    }
  
   
//    //转换为电机端的r/min
//    static float speedTemp;
//    speed = speed * PTZ_HORI_RATIO + 0.5;
//    speedTemp = speed;
//    speedTemp < 0 ? speedTemp *= -1 : speedTemp; //变为正数
//    
//    if (motorType == horiMotorType) {
//        if (speedTemp > PTZ_HORI_MOTOR_MAX_SPEED) {
//            speedTemp = PTZ_HORI_MOTOR_MAX_SPEED;
//        }
//        else if (speedTemp < PTZ_HORI_MIN_SPEED) {
//            speedTemp = PTZ_HORI_MOTOR_MIN_SPEED;
//        }
//        //设置速度，电机的r/min范围-6000~6000
//        servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
//                                , WRITE_ONE_REG_FRAME_NUM, lowPriority);
//    }
//    else if (motorType == vertMotorType) {
//        if (speedTemp > PTZ_VERT_MOTOR_MAX_SPEED) {
//            speedTemp = PTZ_VERT_MOTOR_MAX_SPEED;
//        }
//        else if (speedTemp < PTZ_VERT_MOTOR_MIN_SPEED) {
//            speedTemp = PTZ_VERT_MOTOR_MIN_SPEED;
//        }
//   
//        //设置速度，电机的r/min范围-6000~6000
//        servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
//                                , WRITE_ONE_REG_FRAME_NUM, lowPriority);
//    }
    
}

/*
      ///云台水平右转
      #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)//输入参数的speed是云台末端的r/min
{
    BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
    static float speedTemp;
    switch ( direction )
    {
        case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
             speedTemp = speed;
             break;
        case PTZ_HORI_DIR_LEFT:
            speedTemp = -speed;
            break;
        default:
            break;
    }
    /*
        -------------------------------------add speed change to here--------------------------------------
    */
    //设定转动速度
    // servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
    //                           , WRITE_ONE_REG_FRAME_NUM, lowPriority);
    set_speed_to_servoMotor(horiMotorType, speedTemp);
//    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
    g_ptz.hori_speed_set = speedTemp;
    //设定转动方向
    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_hori_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);

    set_speed_to_servoMotor(horiMotorType, 0);

    //停止电机
    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;
  
    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;
        servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
                                  , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机 
    }
    ptz_hori_stop_count ++;
  //电子稳定
#ifdef PTZ_ELECTRIC_STABLE_L6235D
  ptz_hori_electric_stable_init();
#endif
    
    BSP_OS_SemPost(&ptz_hori_stop_mutex);
}

void ptz_vert_start(char direction, float speed)//输入参数的speed是云台末端的r/min
{
    BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);

    switch ( direction )
    {
        case PTZ_VERT_DIR_UP://伺服电机默认速度为正使云台向上
            break;
        case PTZ_VERT_DIR_DOWN:
            speed = -speed;
            break;    
        case PTZ_VERT_DIR_STOP:
            break;
        default:
            break;
    }
    /*
        -------------------------------------add speed change to here--------------------------------------
    */
    //设定转动速度
    set_speed_to_servoMotor(vertMotorType, speed);
    servoSendData(vertMotorType, 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);

    set_speed_to_servoMotor(vertMotorType, 0);
    //停止电机
    g_ptz.vert_start_stop_set = PTZ_VERT_STOP;
    g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;

    g_ptz.vert_direction_actual = PTZ_VERT_DIR_STOP;
  
    g_ptz.vert_speed_set = 0;
    g_ptz.vert_speed_actual = 0;
  
    if(ptz_vert_stop_count <= 0)
    {
        OSTimeDlyHMSM(0u, 0u, 0u, time);
        ptz_vert_stop_count = 0;
        servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
                          , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机 
    }
    ptz_vert_stop_count ++;
  //电子稳定
#ifdef PTZ_ELECTRIC_STABLE_L6235D
  ptz_hori_electric_stable_init();
#endif
    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, 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