#include "motorstat.h"
#include "timer.h"
#include "usart.h"
#include "main.h"
//extern struct StructMotorStat MotorStat;
struct StructMotorStat MotorStat;

//#include <math.h> // 包含数学库




int temp_speed;
void managespeed()
{
     if ((MotorStat.speed>MotorStat.Targetspeed)&&(MotorStat.decflag==1))//dec
    {
				temp_speed=MotorStat.dec*0.001;
        MotorStat.speed=MotorStat.speed-temp_speed;
        if (MotorStat.speed<MotorStat.Targetspeed)
        {
            MotorStat.speed=MotorStat.Targetspeed;  
        }
        setfreq(MotorStat.speed);
        //MotorStat.set_speed(MotorStat.speed);
        //printf("dec.speed=%d\r\n",MotorStat.speed);
        
     }


    if ((MotorStat.speed<MotorStat.Targetspeed)&&(MotorStat.accflag==1))
    {
        MotorStat.speed=MotorStat.speed+MotorStat.acc*1.0/1000.0;
        if (MotorStat.speed>MotorStat.Targetspeed)
        {
            MotorStat.speed=MotorStat.Targetspeed;  
        }
        MotorStat.set_speed(MotorStat.speed);
        //printf("acc.speed=%d\r\n",MotorStat.speed);
    }
    
   
}
	


//void motorloop(void)
//{
//    if(MotorStat.speedflag==1){
//        managespeed();
//    }
//    if((MotorStat.speed==MotorStat.Targetspeed)&&(MotorStat.speed!=0)){//当目标速度等于现在速度时
//        MotorStat.speedflag=0;
//        MotorStat.accflag=0;
//        MotorStat.decflag=0;
//        // MotorStat.set_speed(MotorStat.speed);
//        //printf("target =%d\r\n",MotorStat.Targetspeed);
//    }
//    
//}


// 中断频率为1kHz（根据实际TIM4中断配置调整）
float dt = 1.0 / 1000; // 时间间隔 = 1ms

void motorloop(void)
{
			if (MotorStat.moveflag==1)//如果正在转动
		{
						
			if (MotorStat.direction==0)//正转
			{
				MotorStat.NmberoNow++;
			}else if (MotorStat.direction==1)//反转
				{	
					MotorStat.NmberoNow--;
				}		
					//printf("NmberoNow = %d\r\n",MotorStat.NmberoNow);	
				//printf("Stats: %d, Pos: %d, Speed: %.0f\r\n", MotorStat.move_phase, MotorStat.NmberoNow, MotorStat.current_speed);
			}
		
			

			
			
	
	if(MotorStat.speedflag==1){
	
			if(!MotorStat.moveflag) return;
		
		//int	STEP_PER_REV = 200;            // 每转200步

    // 计算下一步延迟
    float delay;
		
    MotorStat.step_counter++;
		printf("step_counter = %d\r\n",MotorStat.step_counter);	
    
    switch(MotorStat.move_phase){
        case 0: // 加速阶段
            MotorStat.current_speed += MotorStat.accel * dt; // 修正单位：steps/s² * s = steps/s
            if(MotorStat.current_speed > MotorStat.max_speed){
                MotorStat.current_speed = MotorStat.max_speed;
                MotorStat.move_phase = 1;
            }
            if(MotorStat.step_counter >= MotorStat.accel_steps){//
                MotorStat.move_phase = 1;
            }
            break;
            
        case 1: // 匀速
            if(MotorStat.step_counter >= (MotorStat.total_steps - MotorStat.decel_steps)){//
                MotorStat.move_phase = 2;
            }
            break;
            
        case 2: // 减速
            MotorStat.current_speed -= MotorStat.decel * dt;
         //   if(MotorStat.current_speed < MotorStat.decel) MotorStat.current_speed = 0;
            break;
    }
		
	//	printf("mx: %d \r\n", MotorStat.max_speed);
    
    // 更新PWM频率（需转换为Hz）
		float steps_per_sec = MotorStat.current_speed ;
		//float freq = steps_per_sec / (MotorStat.step_angle / 360.0f * STEP_PER_REV);
    float freq = steps_per_sec * 1.0;
    setfreq((u32)freq);
	
		//printf("Mode : %d, Step: %u,\r\n", MotorStat.move_phase, MotorStat.step_counter);		//运动模式，步数
//		printf("acc = %d, dec = %d, Step: %u,\r\n", MotorStat.accel_steps,MotorStat.decel_steps,MotorStat.step_counter);
//						    printf("Step: %u, Pos: %d, Speed: %.	printf("Dir : %d\r\n", MotorStat.direction);
		
		
			    // 检查是否到达目标位置
    if (MotorStat.step_counter >= MotorStat.total_steps || 
        MotorStat.NmberoNow == MotorStat.target_position) {
        stopmotormove();
        MotorStat.NmberoNow = MotorStat.target_position; // 确保位置准确
					
					MotorStat.move_phase = -1;
    }
	}
    

				
}