#include "stepmotor.h"
#include "math.h"
#include "gpio_devs.h"
#include "time.h"


static _stepmotor *root = NULL;

#define s_table_count  50
uint32_t curve_s_table[s_table_count] = {0};


_stepmotor motor1;
_stepmotor motor2;
_stepmotor motor3;
_stepmotor motor4;
_stepmotor motor5;



_gpio_group start_1;
_gpio_group end_1;
_gpio_group pul_1;
_gpio_group ena_1;
_gpio_group dir_1;

_gpio_group start_2;
_gpio_group end_2;
_gpio_group pul_2;
_gpio_group ena_2;
_gpio_group dir_2;

_gpio_group start_3;
_gpio_group end_3;
_gpio_group pul_3;
_gpio_group ena_3;
_gpio_group dir_3;

_gpio_group start_4;
_gpio_group end_4;
_gpio_group pul_4;
_gpio_group ena_4;
_gpio_group dir_4;

_gpio_group start_5;
_gpio_group end_5;
_gpio_group pul_5;
_gpio_group ena_5;
_gpio_group dir_5;


//buff_pa是速度表数组。count_va是加速次数。pan_right_va是右移量，代表了精度，即右移多少后，y强制为0，一般用7，最后一次速度在99.9%。
//count_va越大，S曲线越平缓，加速过程越平滑，加速时间越长；count_va越小，S曲线越陡峭，加速过程越急剧，加速时间越短。
void CurveS_init(uint32_t *buff, uint16_t count, uint8_t pan_right)
{

  for (uint16_t x = 0; x < count; x++)
	{
      buff[x] = 3.0 / (10.0 + exp(((float)pan_right * 2 / count * -x) + pan_right)) * 4000;
      //buff_pa[x] = 1.0 / (1.0 + pow(sqrt(m_e_v), (float)pan_right_va * 2 / count_va * -x + pan_right_va)); 
		  //这是通用计算方法，pow函数的第一个参数越接近1，该算式计算出的曲线越平缓，可以改的和线性加减速无异
	}
}


void stepmotor_init(void)
{
	CurveS_init(curve_s_table, s_table_count, 7);
	
	start_1.port = BSP_DI_1_GPIO_PORT;
	start_1.pin  = BSP_DI_1_PIN;
	end_1.port   = BSP_DI_2_GPIO_PORT;
	end_1.pin    = BSP_DI_2_PIN;
	
	pul_1.port = BSP_PUL_1_GPIO_PORT;
	pul_1.pin  = BSP_PUL_1_PIN;
	ena_1.port = BSP_ENA_1_GPIO_PORT;
	ena_1.pin  = BSP_ENA_1_PIN;
	dir_1.port = BSP_DIR_1_GPIO_PORT;
	dir_1.pin  = BSP_DIR_1_PIN;
	
	stepmotor_install(&motor1,"motor1",start_1,end_1,pul_1,ena_1,dir_1);
	

	
	start_2.port = BSP_DI_3_GPIO_PORT;
	start_2.pin  = BSP_DI_3_PIN;
	end_2.port   = BSP_DI_4_GPIO_PORT;
	end_2.pin    = BSP_DI_4_PIN;
	
	pul_2.port = BSP_PUL_2_GPIO_PORT;
	pul_2.pin  = BSP_PUL_2_PIN;
	ena_2.port = BSP_ENA_2_GPIO_PORT;
	ena_2.pin  = BSP_ENA_2_PIN;
	dir_2.port = BSP_DIR_2_GPIO_PORT;
	dir_2.pin  = BSP_DIR_2_PIN;
	
	
	stepmotor_install(&motor2,"motor2",start_2,end_2,pul_2,ena_2,dir_2);
	
	
	start_3.port = BSP_DI_5_GPIO_PORT;
	start_3.pin  = BSP_DI_5_PIN;
	end_3.port   = BSP_DI_6_GPIO_PORT;
	end_3.pin    = BSP_DI_6_PIN;
	
	pul_3.port = BSP_PUL_3_GPIO_PORT;
	pul_3.pin  = BSP_PUL_3_PIN;
	ena_3.port = BSP_ENA_3_GPIO_PORT;
	ena_3.pin  = BSP_ENA_3_PIN;
	dir_3.port = BSP_DIR_3_GPIO_PORT;
	dir_3.pin  = BSP_DIR_3_PIN;
	
	stepmotor_install(&motor3,"motor3",start_3,end_3,pul_3,ena_3,dir_3);
	
	
	start_4.port = BSP_DI_7_GPIO_PORT;
	start_4.pin  = BSP_DI_7_PIN;
	end_4.port   = BSP_DI_8_GPIO_PORT;
	end_4.pin    = BSP_DI_8_PIN;
	
	pul_4.port = BSP_PUL_4_GPIO_PORT;
	pul_4.pin  = BSP_PUL_4_PIN;
	ena_4.port = BSP_ENA_4_GPIO_PORT;
	ena_4.pin  = BSP_ENA_4_PIN;
	dir_4.port = BSP_DIR_4_GPIO_PORT;
	dir_4.pin  = BSP_DIR_4_PIN;
	
	stepmotor_install(&motor4,"motor4",start_4,end_4,pul_4,ena_4,dir_4);
	
	
	
  start_5.port = BSP_DI_9_GPIO_PORT;
	start_5.pin  = BSP_DI_9_PIN;
	end_5.port   = BSP_DI_10_GPIO_PORT;
	end_5.pin    = BSP_DI_10_PIN;
	
	pul_5.port = BSP_PUL_5_GPIO_PORT;
	pul_5.pin  = BSP_PUL_5_PIN;
	ena_5.port = BSP_ENA_5_GPIO_PORT;
	ena_5.pin  = BSP_ENA_5_PIN;
	dir_5.port = BSP_DIR_5_GPIO_PORT;
	dir_5.pin  = BSP_DIR_5_PIN;
	
	stepmotor_install(&motor5,"motor5",start_5,end_5,pul_5,ena_5,dir_5);
	
	
	
}


uint8_t stepmotor_install(_stepmotor *e,char *name,_gpio_group start,
	                                _gpio_group end,
                                  _gpio_group pul,
                                  _gpio_group ena,
                                  _gpio_group dir)
{	
		e->channel = 0;
	  e->name = name;
		e->next_ptr = NULL;
	  
	  e->step_index = s_table_count - 1;//获取最大的延迟，因为最开始电机肯定是停止的
	
	
	  e->step_delay = curve_s_table[e->step_index];
		e->running = 0;
		e->isGetPos = 0;
	  e->step_count = 0;
	
	  e->stateCount = 0;
		e->step_len = 50000;
	
    //had been initialized as input
	  e->start.port = start.port;
	  e->start.pin  = start.pin;
	  e->end.port = end.port;
	  e->end.pin  = end.pin;
	  //===========
	  e->pul.port = pul.port;
	  e->pul.pin  = pul.pin;
	  e->ena.port = ena.port;
	  e->ena.pin  = ena.pin;
	  e->dir.port = dir.port;
	  e->dir.pin  = dir.pin;
	
	  GPIO_enable_rcu(e->pul.port);
	  gpio_init(e->pul.port, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, e->pul.pin);
	
	  GPIO_enable_rcu(e->ena.port);
	  gpio_init(e->ena.port, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, e->ena.pin);
		
		GPIO_enable_rcu(e->dir.port);
	  gpio_init(e->dir.port, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, e->dir.pin);
	
	
	
		if (root)
		{
			_stepmotor *last = root;
			while (last->next_ptr)
			{
				last = last->next_ptr;
			}
			last->next_ptr = e;
			e->channel = last->channel + 1;
		}
		else
		{
			root = e;
		}

		return 0;
}

bool stepmotor_getStart(_stepmotor *e)
{
	return gpio_input_bit_get(e->start.port, e->start.pin) ? true : false;
}

bool stepmotor_getEnd(_stepmotor *e)
{
	return gpio_input_bit_get(e->end.port, e->end.pin) ? true : false;
}

void stepmotor_setPul(_stepmotor *e,bool state)
{
	if (state)
			gpio_bit_set(e->pul.port, e->pul.pin);
	else
			gpio_bit_reset(e->pul.port, e->pul.pin);
}

void stepmotor_setDir(_stepmotor *e,bool state)
{
	if (state)
			gpio_bit_set(e->dir.port, e->dir.pin);
	else
			gpio_bit_reset(e->dir.port, e->dir.pin);
}

void stepmotor_setEna(_stepmotor *e,bool state)
{
	if (state)
			gpio_bit_set(e->ena.port, e->ena.pin);
	else
			gpio_bit_reset(e->ena.port, e->ena.pin);
}


void stepmotor_getStepLen(_stepmotor *e)
{
	stepmotor_setPul(e,false);//拉低电平
	stepmotor_setDir(e,true);//正向运动
	stepmotor_setEna(e,true);//运动

	e->step_delay = 20;
	e->running = 0;
	e->isGetPos = 1;
}



void stepmotor_setMove(_stepmotor *e,int8_t state)//-1 0 1
{
	if(state != 0)
	{
		stepmotor_setPul(e,false);//拉低电平
		
		stepmotor_setEna(e,true);//运动

		e->step_delay = 20;
		e->step_current = 0;
		e->isGetPos = 0;
		e->step_len = 50000;
		e->state = motor_accelerate;
		
		if(state > 0)
		{
			e->running = 1;
			stepmotor_setDir(e,false);//正向运动
		}
		else
		{
			e->running = -1;
			stepmotor_setDir(e,true);//反向运动
		}
		
	}
	else//0，停止
	{
		stepmotor_setPul(e,false);//拉低电平
		stepmotor_setDir(e,false);//反向运动
		stepmotor_setEna(e,false);//停止
		e->step_delay = 20;
		e->running = 0;
		e->isGetPos = 0;
	}
}

void stepmotor_thread(void)//20kHz
{
	if (root)
	{
		_stepmotor *e = root;
		while (e)
		{
			if(e->running == 0)//停止运行，进入测试模式
			{
				if(e->isGetPos == 0)
				{	
					stepmotor_setEna(e,false);//停止
					stepmotor_setPul(e,false);
					stepmotor_setDir(e,false);
				}
				else if(e->isGetPos > 0)//正方向 获取结束
				{
					e->step_count ++;
					
					if(e->step_delay <= e->step_count)//超时之后开始下一个周期
					{
						e->step_count = 0;
					}
					else if((e->step_delay * 0.5) <= e->step_count)
					{
						stepmotor_setPul(e,false);//拉低电平
					}
					else if(1 <= e->step_count)
					{
						stepmotor_setPul(e,true);//抬高电平
					}
					
					if(stepmotor_getEnd(e) == true)//开始按键还没检测到，所以可以一直运行
					{
						e->stateCount ++;
						
						if(e->stateCount >= 3)//检测3个脉冲后如果还是高电平，说明没有误触发
						{
						  e->stateCount = 0;
							e->isGetPos = -1;
							e->step_count = 0;
							e->step_len = 0;
							stepmotor_setEna(e,false);//停止
							stepmotor_setDir(e,false);
							stepmotor_setEna(e,true);//停止
						}
					}
					else
					{
						e->stateCount = 0;
					}
					
				}
				else if(e->isGetPos < 0)//反方向 获取开始
				{
					 e->step_count ++;
					
					if(e->step_delay <= e->step_count)//超时之后开始下一个周期
					{
						e->step_count = 0;
					}
					else if((e->step_delay * 0.5) <= e->step_count)
					{
						stepmotor_setPul(e,false);//拉低电平
						e->step_len ++;
					}
					else if(1 <= e->step_count)
					{
						stepmotor_setPul(e,true);//抬高电平
					}
					
					if(stepmotor_getStart(e) == true)//开始按键还没检测到，所以可以一直运行
					{
						e->stateCount ++;
						
						if(e->stateCount >= 3)//检测3个脉冲后如果还是高电平，说明没有误触发
						{
						  e->stateCount = 0;
							e->isGetPos = 0;
							e->step_count = 0;
						}
						
					}
					else
					{
						e->stateCount = 0;
					}
				}
				
			}
			else 
			{
				//计算时间是否合适
				e->step_count++;//每个周期加一个
				if(e->step_delay <= e->step_count)//超时之后开始下一个周期
				{
					//计时归零
					e->step_count = 0;
					
					e->step_current++;

					
					//延迟长度读取，并且状态切换计算
					if(e->state == motor_accelerate)//加速
					{
						e->step_index --;
						if(e->step_index <= 0)
						{
							e->step_index = 0;
							e->state = motor_speed_max;//状态切换到最大速度
						}
						e->step_delay = curve_s_table[e->step_index];
					}
					else if(e->state == motor_speed_max)//最大速度
					{
						//如果计数大于一个加速和一个正常运行长度时，就开始减速
						if(e->step_current  > (e->step_len - s_table_count))
						{
							e->state = motor_decelerate;//状态切换到减速
						}
						
						e->step_delay = curve_s_table[0];
					}
					else if(e->state == motor_decelerate)//减速
					{
						e->step_index ++;
						if(e->step_index >= s_table_count)
						{
							e->step_index = s_table_count - 1;
							e->state = motor_stop;
						}
						e->step_delay = curve_s_table[e->step_index];
					}
					else//停止
					{
						//还在一直低速运行，直到碰到开关才结束
						e->step_index = s_table_count - 1;
						e->step_delay = curve_s_table[e->step_index];
						e->running = 0;
					}
					
					
					if(e->running < 0)//往后运行
					{
						stepmotor_setDir(e,true);
						
						if(stepmotor_getStart(e) != true)//开始按键还没检测到，所以可以一直运行
						{
								 //如果不处理，那么没有传感器的时候可能停不下来
								 e->stateCount = 0;
						}
						else//已经检测到
						{
							e->stateCount ++;
						
							if(e->stateCount >= 3)//检测3个脉冲后如果还是高电平，说明没有误触发
							{
								e->stateCount = 0;
								e->isGetPos = 0;
								e->step_count = 0;
								e->step_current = 0;
								//e->step_len = 0;
								e->running = 0;//设置为0，下一个周期时可以停止运行
							}
						}
					}
					else if(e->running > 0)//往前运行
					{
						stepmotor_setDir(e,false);
						
						if(stepmotor_getEnd(e) != true)//开始按键还没检测到，所以可以一直运行
						{
								 //如果不处理，那么没有传感器的时候可能停不下来
								 e->stateCount = 0;
						}
						else
						{
							e->stateCount ++;
						
							if(e->stateCount >= 3)//检测3个脉冲后如果还是高电平，说明没有误触发
							{
								e->stateCount = 0;
								e->isGetPos = 0;
								e->step_count = 0;
								e->step_current = 0;
								//e->step_len = 0;
								e->running = 0;//设置为0，下一个周期时可以停止运行
							}
						}
					}
			  }
				else if((e->step_delay * 0.5) <= e->step_count)
				{
					stepmotor_setPul(e,false);//拉低电平
				}
				else if(1 <= e->step_count)
				{
					stepmotor_setPul(e,true);//抬高电平
				}
				
				
			}
			e = e->next_ptr;
		}
	}
}




void stepmotor_getlen(uint16_t channel)
{
	  switch(channel)
		{
			case 0: stepmotor_getStepLen(&motor1);break;
			case 1: stepmotor_getStepLen(&motor2);break;
			case 2: stepmotor_getStepLen(&motor3);break;
			case 3: stepmotor_getStepLen(&motor4);break;
			case 4: stepmotor_getStepLen(&motor5);break;
		}
}


void stepmotor_act(uint16_t channel,int8_t state)
{
	switch(channel)
	{
		case 0: stepmotor_setMove(&motor1,state);break;//-1 0 1
		case 1: stepmotor_setMove(&motor2,state);break;//-1 0 1
		case 2: stepmotor_setMove(&motor3,state);break;//-1 0 1
		case 3: stepmotor_setMove(&motor4,state);break;//-1 0 1
		case 4: stepmotor_setMove(&motor5,state);break;//-1 0 1
		
	}
	
}