#include "ptz_header_file.h"
#include "Timer.h"
#include "tmc2160.h"
#include "speed_to_step.h"
#include "rotate_step.h"

#ifdef PTZ_STEP_MOTOR

static BSP_OS_SEM ptz_ftm_mutex;//共享资源锁

//发送云台实际转速
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;
  }
}








 
#ifdef TMC2160
extern TMC2160Parameter tmc2160_parameter;

static void ptz_hori_step_speed_task()
{
  static float a,angle;
  while(1)
  {
    if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
    {
      g_ptz.hori_tmc2160.set_f = ptz_hori_step_v_to_f((g_ptz.hori_speed_set * PTZ_HORI_RATIO), g_ptz.hori_tmc2160.microstep_vlue);//g_ptz.hori_speed_set
      //加速
      if(g_ptz.hori_tmc2160.f < g_ptz.hori_tmc2160.set_f)
      {
        g_ptz.hori_tmc2160.f = 
          g_ptz.hori_tmc2160.f + ptz_hori_step_v_to_f((PTZ_HORI_MAX_SPEED * PTZ_HORI_RATIO * PTZ_HORI_ACC_DEC_INC_T / (float)tmc2160_parameter.hori_acc_time_max), g_ptz.hori_tmc2160.microstep_vlue);
        if(g_ptz.hori_tmc2160.f > g_ptz.hori_tmc2160.set_f)
        {
          g_ptz.hori_tmc2160.f = g_ptz.hori_tmc2160.set_f;
        }
        ptz_hori_timer_start((unsigned int)(g_ptz.hori_tmc2160.f + 0.5));
      }
      //减速
      if(g_ptz.hori_tmc2160.f > g_ptz.hori_tmc2160.set_f)
      {
        if(g_ptz.hori_dec_mode == PTZ_DEC_DEF)
        {
          g_ptz.hori_tmc2160.f  = 
            g_ptz.hori_tmc2160.f - ptz_hori_step_v_to_f((PTZ_HORI_MAX_SPEED * PTZ_HORI_RATIO * PTZ_HORI_ACC_DEC_INC_T / (float)PTZ_HORI_DEC_TIME_DEF), g_ptz.hori_tmc2160.microstep_vlue);
        }
        else
        {
          g_ptz.hori_tmc2160.f  = 
            g_ptz.hori_tmc2160.f - ptz_hori_step_v_to_f((PTZ_HORI_MAX_SPEED * PTZ_HORI_RATIO * PTZ_HORI_ACC_DEC_INC_T / (float)tmc2160_parameter.hori_dec_time_max), g_ptz.hori_tmc2160.microstep_vlue);
        }
        if(g_ptz.hori_tmc2160.f < g_ptz.hori_tmc2160.set_f)
        {
          g_ptz.hori_tmc2160.f = g_ptz.hori_tmc2160.set_f;
        }
        ptz_hori_timer_start((unsigned int)(g_ptz.hori_tmc2160.f + 0.5));
      }
      //计算转速
      g_ptz.hori_motor_speed_actual = ptz_hori_step_f_to_v(g_ptz.hori_tmc2160.f, g_ptz.hori_tmc2160.microstep_vlue);
      g_ptz.hori_speed_actual = g_ptz.hori_motor_speed_actual / PTZ_HORI_RATIO;
      
//      a = fabs(g_ptz.hori_as5048a.as5048a_ptz_angle_actual - angle);
//      g_ptz.hori_speed_actual = (((a / PTZ_HORI_ACC_DEC_INC_T) / 360.0) * 60 * 1000)/50.0;
//      angle = g_ptz.hori_as5048a.as5048a_ptz_angle_actual;
    }
    
    if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
    {
      g_ptz.hori_motor_speed_actual = 0;
      g_ptz.hori_speed_actual = 0;
      g_ptz.hori_tmc2160.f = 0;
    }
     OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_ACC_DEC_INC_T);
    
  }
}


static void ptz_vert_step_speed_task()
{
  static float a,angle;
  while(1)
  {
    if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
    {
      g_ptz.vert_tmc2160.set_f = ptz_vert_step_v_to_f((g_ptz.vert_speed_set * PTZ_VERT_RATIO), g_ptz.vert_tmc2160.microstep_vlue);
      //加速
      if(g_ptz.vert_tmc2160.f < g_ptz.vert_tmc2160.set_f)
      {
        g_ptz.vert_tmc2160.f = 
          g_ptz.vert_tmc2160.f + ptz_vert_step_v_to_f((PTZ_VERT_MAX_SPEED * PTZ_VERT_RATIO * PTZ_VERT_ACC_DEC_INC_T / (float)tmc2160_parameter.vert_acc_time_max), g_ptz.vert_tmc2160.microstep_vlue);
        
        if(g_ptz.vert_tmc2160.f > g_ptz.vert_tmc2160.set_f)
        {
          g_ptz.vert_tmc2160.f = g_ptz.vert_tmc2160.set_f;
        }
        ptz_vert_timer_start((unsigned int)(g_ptz.vert_tmc2160.f + 0.5));
      }
      //减速
      if(g_ptz.vert_tmc2160.f > g_ptz.vert_tmc2160.set_f)
      {
        if(g_ptz.vert_dec_mode == PTZ_DEC_DEF)
        {
          g_ptz.vert_tmc2160.f  = 
            g_ptz.vert_tmc2160.f - ptz_vert_step_v_to_f((PTZ_VERT_MAX_SPEED * PTZ_VERT_RATIO * PTZ_VERT_ACC_DEC_INC_T / (float)PTZ_VERT_DEC_TIME_DEF), g_ptz.vert_tmc2160.microstep_vlue);
        }
        else
        {
          g_ptz.vert_tmc2160.f  = 
            g_ptz.vert_tmc2160.f - ptz_vert_step_v_to_f((PTZ_VERT_MAX_SPEED * PTZ_VERT_RATIO * PTZ_VERT_ACC_DEC_INC_T / (float)tmc2160_parameter.vert_dec_time_max), g_ptz.vert_tmc2160.microstep_vlue);
        }
        if(g_ptz.vert_tmc2160.f < g_ptz.vert_tmc2160.set_f)
        {
          g_ptz.vert_tmc2160.f = g_ptz.vert_tmc2160.set_f;
        }
        ptz_vert_timer_start((unsigned int)(g_ptz.vert_tmc2160.f + 0.5));
      }
      //计算转速
      g_ptz.vert_motor_speed_actual = ptz_vert_step_f_to_v(g_ptz.vert_tmc2160.f, g_ptz.vert_tmc2160.microstep_vlue);
      g_ptz.vert_speed_actual = g_ptz.vert_motor_speed_actual / PTZ_VERT_RATIO;
      
//      a = fabs(g_ptz.vert_as5048a.as5048a_ptz_angle_actual - angle);
//      g_ptz.vert_speed_actual = (((a / PTZ_VERT_ACC_DEC_INC_T) / 360.0) * 60 * 1000)/50.0;
//      angle = g_ptz.vert_as5048a.as5048a_ptz_angle_actual;
    }
    
    if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
    {
      g_ptz.vert_motor_speed_actual = 0;
      g_ptz.vert_speed_actual = 0;
      g_ptz.vert_tmc2160.f = 0;
    }
    
     OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_ACC_DEC_INC_T);
  }

}




#endif



static OS_STK  task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE];
static  void  creat_task_hori_step_speed_task(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_step_speed_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE - 1],
		   (INT8U           ) TASK_HORI_PID_PRIO,                   
		   (INT16U          ) TASK_HORI_PID_PRIO,
		   (OS_STK         *)&task_hori_step_speed_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_step_speed_stk[TASK_VERT_PID_STK_SIZE];
static  void  creat_task_vert_step_speed_task(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_step_speed_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE - 1],
		   (INT8U           ) TASK_VERT_PID_PRIO,                   
		   (INT16U          ) TASK_VERT_PID_PRIO,
		   (OS_STK         *)&task_vert_step_speed_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_step_speed_task", &name_err);
    #endif
       
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_step_speed_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_step_speed_task  failed...\n\r");
	}
}





void init_speed_module(void)
{
  BSP_OS_SemCreate(&ptz_ftm_mutex,1u,"ptz_ftm_mutex");
  step_time_init();
  creat_task_hori_step_speed_task();
  creat_task_vert_step_speed_task();
}
#endif








