#include "sys.h"
#include "usart.h"
#include "led.h"
#include "thb6128.h"
#include "stdlib.h"
#include "stdio.h"
#include "string.h"
#include "main.h"
#include "motorstat.h"

#include <math.h>

extern char USART_RX_BUF[200];

int s1=0,s2=0;

int main(void)
{

	// 初始化参数
		
    u16 t;
    u16 len;
	int value;
    MotorStat.MoveMode     = 0;
    MotorStat.NmberoNow    = 0;
    MotorStat.NmberoTarget = 0;
    MotorStat.direction    = 0;
    MotorStat.speed        = 0;
    MotorStat.acc          = 0;
    MotorStat.dec          = 0;
	MotorStat.moveflag     = 0;
    MotorStat.backzero     = 0;
    MotorStat.speedflag    = 0;
//    MotorStat.accflag      = 0;
//    MotorStat.decflag      = 0;
    MotorStat.Nmbertargetflag  = 0;
    MotorStat.set_speed    = setfreq;
    MotorStat.stopmove     = stopmotormove;
		MotorStat.max_speed = 0;
		
		MotorStat.move_phase = -1;
		MotorStat.zheng_LimitTrigger = 0;
		MotorStat.fu_LimitTrigger = 0;
		MotorStat.target_position = 0;
		MotorStat.zero_flag = 0;
		
		MotorStat.max_speed = 2000;
		
		
    //clk应该接在有定时器的io接口上，PA8 TIM3用来捕获脉冲上升沿
    delay_init();                                   // 延时函数初始化
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 设置NVIC中断分组2:2位抢占优先级，2位响应优先级
    uart_init(115200);                              // 串口初始化为 115200,用的串口1，PA9->RX ,PA10->TX
    STEP_MOTOR_Init();
    LED_Init();               // LED端口初始化
		
		
TIM1_PWM_CH1_Init();
    //TIM1_PWM_Init(999, 3599); // 分频得到20hz 。PWM频率=72000000/9999/719=100hz 有震动

    TIM3_Int_Init(999, 71);  

	TIM2_Capture_Init();
//	TIM4_Capture_Init();
EXTI_Config(); //负限位
	
		//TIM4_Int_Init(999,7199);
    TIM_SetCompare1(TIM1, 500);
    Dac1_Init(); // DAC初始化
    // 关闭待机，拉高
    GPIO_SetBits(GPIOC, GPIO_Pin_0);
    // 设置enable引脚
    GPIO_ResetBits(GPIOA, GPIO_Pin_11);
    // 设置细分
    STEP_SUBDIVIDE(4);
    // 设置电流
    Dac1_Set_Vol(1500); //
	  printf("wellcom to my motor\r\n ");

    while (1) {
     
        if (USART_RX_STA & 0x8000) {
            len = USART_RX_STA & 0x3fff; // 得到此次接收到的数据长度
            for (t = 0; t < len; t++) {
                USART_SendData(USART1, USART_RX_BUF[t]); // 向串口1发送数据
                while (USART_GetFlagStatus(USART1, USART_FLAG_TC) != SET); // 等待发送结束
            }
            USART_RX_STA = 0;
            // 解析USART_RX_BUF 格式如下
            /*SET#OPEN           打开
            SET#OFF              脱机
            SET#EN                使能电机
            SET#MOVEING       转动
            SET#DRECTION#0 	 正向 1反向

            SET#CURRENT#1  	设置电流
            SET#ACC#1000	 设置加速度
            SET#DEC#1000 	设置减速度
            SET#ORIGIN	 设置当前位置为0
            SET#BAUD_RATE#9600 	设置波特率
            SET#MICROSTEPPING#1 	设置细分
            SET#SPEED#1000 	设置速度  freq=1000时，为50hz
            SET#TARGET#1000 	设置目标速度

            MOVE#TO##1000	旋转到1000这个位置
            MOVE#BY##1000 	旋转1000步
            MOVE#ZERO 	旋转到远点*/
            // 先提取第一个井号前的字符串
									
            char *p = strtok(USART_RX_BUF, "#");
            if (strcmp(p, "SET") == 0) {
                // 提取第二个井号前的字符串
                p = strtok(NULL, "#");
                if (strcmp(p, "OPEN") == 0) {
                    // 打开
                    STEP_MOTOR_OPEN();
                    printf("OPEN\r\n");
                }
                if (strcmp(p, "OFF") == 0) {
                    // 脱机
                    STEP_MOTOR_OFF();
                    printf("OFF\r\n");
                }
                if (strcmp(p, "EN") == 0) {
                    // 使能电机
                    STEP_MOTOR_EN();
                    printf("EN\r\n");
                }
                // if (strcmp(p, "SET#CLEAR") == 0) {
                //     // 使能电机
                //     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, DISABLE); //时钟使能
                //     printf("CLEAR\r\n");
                // }
                
                // if (strcmp(p, "MOVEING") == 0) {
                //     // 转动
                //     STEP_MOTOR_FORWARD();
                //     printf("MOVEING\r\n");
                // }
                if (strcmp(p, "ZERO") == 0) {
                    //设置当前位置为0					
                    MotorStat.NmberoNow=0;
                    stopmotormove();
                    printf("ZERO\r\n");
                }
                if (strcmp(p, "DRECTION") == 0) {
                    // 转动
                    p = strtok(NULL, "#");
                    if (strcmp(p, "0") == 0) {
                        // 正向
                        STEP_MOTOR_FORWARD(); 
                                     
                        printf("DRECTION FORWARD\r\n");
                    }
                    if (strcmp(p, "1") == 0) {
                        // 反向
                        STEP_MOTOR_BACKWARD();
                        
                        printf("DRECTION BACKWARD\r\n");
                    }
                }
                if (strcmp(p, "CURRENT") == 0) {
                    // 设置电流
                    p = strtok(NULL, "#");				
                    value = atoi(p);
									  if(value<=3000)
                    {
											set_CurrentValue(value);
										  printf("CURRENT %d\r\n", value);
										}
										else
										printf("CURRENT OVER");
                    
                }
												
								if (strcmp(p, "DEC") == 0) {
                    // 设置减速度
                    TIM5_Int_Init(999, 71);
                    p  = strtok(NULL, "#");
                    value = atoi(p);
									MotorStat.decel = value;
                    //MotorStat.dec=value;
//                    MotorStat.speedflag=1;
//                    MotorStat.decflag=1;
                    printf("DEC %d\r\n", value);
                }
                if (strcmp(p, "ACC") == 0) {
                    // 设置加速度
                    TIM5_Int_Init(999, 71);
                    p = strtok(NULL, "#");
                    value = atoi(p);
									MotorStat.accel = value;
                    //MotorStat.acc=value;
//                    MotorStat.speedflag=1;
//                    MotorStat.accflag=1;
                    printf("ACC %d\r\n", value);
                }
                
				 if (strcmp(p, "MICROSTEPPING") == 0) {
					p  = strtok(NULL, "#");
                    value = atoi(p);
					STEP_SUBDIVIDE(value);
					printf("MICROSTEPPING %d\r\n", value);
				 }
				 if (strcmp(p, "SPEED") == 0) {
					p  = strtok(NULL, "#");
					value = atoi(p);
					setfreq(value);
          MotorStat.speed=value;
				 }
          if (strcmp(p, "TARGET") == 0) {					//max_speed
					p  = strtok(NULL, "#");
					value = atoi(p);
							MotorStat.max_speed = value;     // 最大1000 steps/s
					//MotorStat.Targetspeed=value;
          printf("TARGET %d\r\n", value);
				 }
				  if (strcmp(p, "BAUD_RATE") == 0) {
					p  = strtok(NULL, "#");
					value = atoi(p);
					USART_Cmd(USART1, DISABLE); 
					uart_init(value);	
				  }
                


            } else if (strcmp(p, "MOVE") == 0) {
                // 提取第二个井号前的字符串
                p = strtok(NULL, "#");
                 if (strcmp(p, "TO") == 0) {
                     // 旋转到
                     p = strtok(NULL, "#");
                     value = atoi(p);
                     moveto(value);
                     printf("MOVE TO %d\r\n", value);
                 }
                if (strcmp(p, "BY") == 0) {
                    // 旋转了
                    p = strtok(NULL, "#");
                    value = atoi(p);
                    MotorStat.NmberoTarget=value;
                    moveby();
                    printf("MOVE BY %d\r\n", value);
                }
                if (strcmp(p, "ZERO") == 0) {
                    // 旋转到负极限，归零
                    movetozero();
                    printf("MOVE TO ZERO\r\n");
                }
								if (strcmp(p, "INFO") == 0) {
                    // 获取当前信息（状态，位置，速度）  
//									if ( MotorStat.zero_flag == 1 && MotorStat.fu_LimitTrigger == 1){
//										printf("Stats: %d, Pos: 0, Speed: %.1f\r\n", MotorStat.move_phase, MotorStat.current_speed);
//									}
//									else{
												printf(" Stats: %d, Pos: %d, Speed: %.1f\r\n", MotorStat.move_phase, MotorStat.NmberoNow, MotorStat.current_speed);
//									printf("fu : %d\r\n", MotorStat.fu_LimitTrigger);
//									printf("GPIO : %d\r\n", GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_6 ));
											//}
                }
            }

            memset(USART_RX_BUF, 0, sizeof(USART_RX_BUF));
        }
				
	
//        switch_fan();
//        switch_zheng();
        
    }
}
void moveby(void)
{
    MotorStat.MoveMode=1; 	
    MotorStat.moveflag = 1; 
	
    if(MotorStat.NmberoTarget>0){
        GPIO_ResetBits(GPIOA, GPIO_Pin_12);//正转NmberoTarget个脉冲
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
        GPIO_SetBits(GPIOA, GPIO_Pin_11);
        MotorStat.direction=0;
        printf("zhengzhuan\r\n");
    }
     if(MotorStat.NmberoTarget<0){
        GPIO_SetBits(GPIOA,GPIO_Pin_12);//反转
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
        GPIO_SetBits(GPIOA, GPIO_Pin_11);
        MotorStat.direction=1;
        printf("fan\r\n");
    }        

}

void moveto(int target)
{
	stopmotormove();
	
	MotorStat.target_position = target; // 保存目标位置到结构体
	
	
    if (MotorStat.NmberoNow == target && MotorStat.zero_flag == 0) {
        stopmotormove();
        printf("Already at target: %d\r\n", target);
        return;
    }

		
    int steps = target - MotorStat.NmberoNow;
    
    // 运动参数设置
    MotorStat.total_steps = abs(steps);
		MotorStat.direction = (steps > 0) ? 0 : 1;

    // 计算梯形曲线参数
    double max_reachable_speed = sqrt(1.0* (2 * MotorStat.accel * MotorStat.total_steps));
    MotorStat.max_speed = (max_reachable_speed < MotorStat.max_speed) ? 
                         max_reachable_speed : MotorStat.max_speed;

    MotorStat.accel_steps = (MotorStat.max_speed * MotorStat.max_speed) / (2 * MotorStat.accel);
    MotorStat.decel_steps = (MotorStat.max_speed * MotorStat.max_speed) / (2 * MotorStat.decel);
    
    if(MotorStat.accel_steps + MotorStat.decel_steps <= MotorStat.total_steps){
        MotorStat.const_steps = MotorStat.total_steps - MotorStat.accel_steps - MotorStat.decel_steps;
    } else {
        MotorStat.accel_steps = MotorStat.total_steps * MotorStat.decel / (MotorStat.accel + MotorStat.decel);
        MotorStat.decel_steps = MotorStat.total_steps - MotorStat.accel_steps;
        MotorStat.const_steps = 0;
    }
		

    // 初始化运动参数
		MotorStat.NmberoTarget = MotorStat.total_steps;
		MotorStat.MoveMode = 1;
    MotorStat.step_counter = 0;
    MotorStat.current_speed = 0;
    MotorStat.move_phase = 0;
    MotorStat.moveflag = 1;
		MotorStat.speedflag = 1;
    

        if (MotorStat.direction == 0)
    {
        GPIO_ResetBits(GPIOA, GPIO_Pin_12);
    }
    else
    {
        GPIO_SetBits(GPIOA, GPIO_Pin_12);
    }
			
    // 启动定时器和电机
		GPIO_SetBits(GPIOA, GPIO_Pin_11);
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    
		
}



//void moveto(void)
//{
//    if(MotorStat.NmberoNow>0)
//    {
//        GPIO_SetBits(GPIOA,GPIO_Pin_12);//反转
//        MotorStat.direction=1;
//    }
//    else if(MotorStat.NmberoNow<0)
//    {
//        GPIO_ResetBits(GPIOA,GPIO_Pin_12);
//        MotorStat.direction=0;
//    }
//    MotorStat.backzero = 1;
//    if(MotorStat.NmberoNow==0)
//    {
//        stopmotormove();
//        printf("already at zero\r\n");
//    }
//}
void stopmotormove(void)
{
    GPIO_ResetBits(GPIOA, GPIO_Pin_11);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, DISABLE);	
    MotorStat.moveflag = 0;
    MotorStat.step_counter = 0;          // 重置步数计数器
	
	MotorStat.current_speed = 0;
}



void setfreq(int freq)
{
    uint32_t timer_clk = 72000000; // TIM1 时钟频率
    uint32_t arr;
    uint16_t psc = 0; // 预分频值，默认 0（即不分频）

    if (freq <= 0)
    {
      TIM_Cmd(TIM1, DISABLE);  
			return;
    }

    // 计算初始 ARR 值（PSC = 0）
    arr = (timer_clk / freq) - 1;

    // 如果 ARR 超过 16 位（65535），增加预分频值，直到 ARR 适配
    while (arr > 65535 && psc < 65535)
    {
        psc++;
        arr = (timer_clk / (freq * (psc))) - 1;
    }

    if (arr > 65535)
    {
        return;
    }

    TIM_Cmd(TIM1, DISABLE);
    TIM_ARRPreloadConfig(TIM1, DISABLE);

    TIM1->PSC = psc; // 设置新的预分频值
    TIM1->ARR = arr;
    TIM1->CCR1 = arr / 2; // 50% 占空比

    TIM_ARRPreloadConfig(TIM1, ENABLE);
    TIM_Cmd(TIM1, ENABLE);

//    printf("Freq = %d Hz, ARR = %u, PSC = %u\r\n", freq, arr, psc);
	//	printf("Freq = %d Hz\r\n", freq);
}



void setgpiovalue(GPIO_TypeDef *GPIOx, int pinnumber, int vlue)
{

    if (vlue == 0) { 
        GPIO_ResetBits(GPIOx, pinnumber);

    } else {
        GPIO_SetBits(GPIOx, pinnumber);
    }
}

//void STEP_SUBDIVIDE(int divide)
//{ // 设置细分
//    setgpiovalue(GPIOB, GPIO_Pin_13, divide & 0x01);      ///   0
//    setgpiovalue(GPIOB, GPIO_Pin_14, divide >> 1 & 0x01); //	1
//    setgpiovalue(GPIOB, GPIO_Pin_15, divide >> 2 & 0x01); //    0
//}
void STEP_SUBDIVIDE(int divide)// 设置细分
{
    // 设置细分
    switch (divide) {
        case 1:
            setgpiovalue(GPIOB, GPIO_Pin_13, 0); // LLL
            setgpiovalue(GPIOB, GPIO_Pin_14, 0);
            setgpiovalue(GPIOB, GPIO_Pin_15, 0);
            break;
        case 2:
            setgpiovalue(GPIOB, GPIO_Pin_13, 1); // HLL
            setgpiovalue(GPIOB, GPIO_Pin_14, 0);
            setgpiovalue(GPIOB, GPIO_Pin_15, 0);
            break;
        case 4:
            setgpiovalue(GPIOB, GPIO_Pin_13, 0); // LHL
            setgpiovalue(GPIOB, GPIO_Pin_14, 1);
            setgpiovalue(GPIOB, GPIO_Pin_15, 0);
            break;
        case 8:
            setgpiovalue(GPIOB, GPIO_Pin_13, 1); // HHL
            setgpiovalue(GPIOB, GPIO_Pin_14, 1);
            setgpiovalue(GPIOB, GPIO_Pin_15, 0);
            break;
        case 16:
            setgpiovalue(GPIOB, GPIO_Pin_13, 0); // LLH
            setgpiovalue(GPIOB, GPIO_Pin_14, 0);
            setgpiovalue(GPIOB, GPIO_Pin_15, 1);
            break;
        case 32:
            setgpiovalue(GPIOB, GPIO_Pin_13, 1); // HLH
            setgpiovalue(GPIOB, GPIO_Pin_14, 1);
            setgpiovalue(GPIOB, GPIO_Pin_15, 0);
            break;
        case 64:
            setgpiovalue(GPIOB, GPIO_Pin_13, 0); // LHH
            setgpiovalue(GPIOB, GPIO_Pin_14, 1);
            setgpiovalue(GPIOB, GPIO_Pin_15, 1);
            break;
        case 128:
            setgpiovalue(GPIOB, GPIO_Pin_13, 1); // HHH
            setgpiovalue(GPIOB, GPIO_Pin_14, 1);
            setgpiovalue(GPIOB, GPIO_Pin_15, 1);
            break;
        default:
            printf("Error value: %d\r\n", divide);
            break;
    }
	//	printf("xifen = %d, %d %d %d \r\n", divide, GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_13), GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_14), GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_15));
}

void set_CurrentValue(int value) // 设置电流
{
    Dac1_Set_Vol(value);
}
void switch_zheng (void)  //脱机
{
	
	 if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_6)==1&&s1==0)		//PB6 1 -->极限
		 {
			   		
            GPIO_ResetBits(GPIOA,GPIO_Pin_12); //正转
			      MotorStat.direction=0;
            RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, DISABLE);
            printf("already close & zheng\r\n");
            GPIO_ResetBits(GPIOA,GPIO_Pin_11);			 
		 }
		if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_6)==1&&s1!=0)
		 {
            GPIO_ResetBits(GPIOA,GPIO_Pin_12); //,一定要注意初始化，将函数提前声明，将PA12拉低，正转	
			      MotorStat.direction=0;
            GPIO_SetBits(GPIOA,GPIO_Pin_11); 	 
            RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
            s1++;
            printf("s%d  \r\n",s1);
            if(s1==500)
            s1=0;
		 }
}
void switch_fan(void)  //脱机
{
	
	 if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_0)==1&&s2==0)    //PA0 1 -->极限
		 {
            GPIO_SetBits(GPIOA,GPIO_Pin_12);  //将PA12拉高，反转	
			      MotorStat.direction=1;
            RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, DISABLE);
            printf("already close & fan\r\n");
            GPIO_ResetBits(GPIOA,GPIO_Pin_11); 
		 }
	 if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_0)==1&&s2!=0)    
		 {
            GPIO_SetBits(GPIOA,GPIO_Pin_11); 
            GPIO_SetBits(GPIOA,GPIO_Pin_12);  //将PA12拉高，反转	
            MotorStat.direction=1;		 
            RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
            s2++;
			 
            printf(" s%d \r\n",s2);
            if(s2==500)
            s2=0;
		 }
}

//float pulse_to_distance(int pulse_count) {
//    const float pulse_distance = 0.0046; // 每个脉冲对应的位移 (mm)
//    return pulse_count * pulse_distance;
//}

//bool is_reached_target(int pulse_count, float target_distance) {
//    const float pulse_distance = 0.0046;
//    float current_distance = pulse_count * pulse_distance;
//    return fabs(current_distance - target_distance) < 0.001; // 允许一定的误差范围
//}


void movetozero(void) {

//	MotorStat.fu_LimitTrigger = 0;
    // 启动归零流程
    MotorStat.zero_flag = 1;
	
			if(GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_6 ) ==1) {		
						stopmotormove();
						MotorStat.NmberoNow = 0;
						}

    // 持续向负极限方向移动
		if(MotorStat.zero_flag == 1 && GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_6 ) ==0) {  
        moveto(-6666); // 目标位置设为极大值
    }
}