#include "pwm_servos.h"
#include "AI8051U.h"
#include "string.h"
#include "global.h"

PWMServoHandleTypeDef pwm_servos[USE_SERVO_NUM];
	
static void pwm_servo_object_init(PWMServoHandleTypeDef* handle, uint8_t id)
{
	memset(handle, 0, sizeof(PWMServoHandleTypeDef));
	handle->id = id;
	switch(id)
	{
		case 1:
			handle->current_duty = PWM_SERVO1_RESET_DUTY;
			handle->write_duty = PWM_SERVO1_RESET_DUTY;
			break;

		case 2:
			handle->current_duty = PWM_SERVO2_RESET_DUTY;
			handle->write_duty = PWM_SERVO2_RESET_DUTY;			
			break;
		
		case 3:
			handle->current_duty = PWM_SERVO3_RESET_DUTY;
			handle->write_duty = PWM_SERVO3_RESET_DUTY;			
			break;
		
		case 4:
			handle->current_duty = PWM_SERVO4_RESET_DUTY;
			handle->write_duty = PWM_SERVO4_RESET_DUTY;			
			break;
		
		case 5:
			handle->current_duty = PWM_SERVO5_RESET_DUTY;
			handle->write_duty = PWM_SERVO5_RESET_DUTY;			
			break;
		
		case 6:
			handle->current_duty = PWM_SERVO6_RESET_DUTY;
			handle->write_duty = PWM_SERVO6_RESET_DUTY;			
			break;
	}

}


static void pwm_servo_handler(PWMServoHandleTypeDef* handle)
{
	if(handle->duty_changed_state)
	{
		handle->duty_changed_state = 0;
		handle->inc_times = handle->time / MIN_RUNNING_TIME;
		handle->duty_inc = handle->target_duty > handle->current_duty ? 		\
						   (float)(-(handle->target_duty - handle->current_duty)) :	\
						   (float)(handle->current_duty - handle->target_duty);
		handle->duty_inc /= (float)handle->inc_times;
		handle->running_state = 1;
	}

	if (handle->running_state)
	{
		 --handle->inc_times;
		if (handle->inc_times == 0)
		{
			handle->current_duty = handle->target_duty;
			handle->running_state = 0;
		}
		else
		{
			handle->current_duty = handle->target_duty + (int)(handle->duty_inc * handle->inc_times);
		}
	}
	handle->write_duty = handle->current_duty + handle->offset;
}

void pwm_servo_duty_set(PWMServoHandleTypeDef* handle, int target_duty, uint32_t time)
{
	target_duty = target_duty > MAX_DUTY ? MAX_DUTY : (target_duty < MIN_DUTY ? MIN_DUTY : target_duty);
	time = time < MIN_RUNNING_TIME ? MIN_RUNNING_TIME : (time > MAX_RUNNING_TIME ? MAX_RUNNING_TIME : time);

	handle->time = time;
	handle->duty_changed_state = 1;	
	handle->target_duty = target_duty;
	handle->last_target_duty = handle->target_duty;
}

void pwm_servos_init()
{
	uint8_t i;
	for(i = 0; i < USE_SERVO_NUM; i++)
	{
		pwm_servo_object_init(&pwm_servos[i], (uint8_t)(USE_SERVO_NUM - i));
	}

	/* servo1 io init */
	P0M0 |= 0x40; 
	P0M1 &= ~0x40; 
	P0SR &= ~0x40; 
	SERVO1 = 0;
	
	/* servo2 io init */
	P0M0 |= 0x80; 
	P0M1 &= ~0x80; 
	P0SR &= ~0x80; 
	SERVO2 = 0;

	/* servo3 io init */
    P4M0 |= 0x40; 
	P4M1 &= ~0x40; 
	P4SR &= ~0x40; 
	SERVO3 = 0;

	/* servo4 io init */
    P4M0 |= 0x02; 
	P4M1 &= ~0x02; 
	P4SR &= ~0x02; 
	SERVO4 = 0;

	/* servo5 io init */
    P4M0 |= 0x20; 
	P4M1 &= ~0x20; 
	P4SR &= ~0x20;  
	SERVO5 = 0;
	
	/* servo6 io init */
    P2M0 |= 0x80; 
	P2M1 &= ~0x80; 
	P2SR &= ~0x80; 
	SERVO6 = 0;
	
	PWMA_PSCRH = 0x00;	/* 预分频值设置为40 */
	PWMA_PSCRL = 0x28;
	PWMA_ARRH = 0x13;	/* 自动重装载值设置为20000 */
	PWMA_ARRL = 0x88;
	PWMA_CCR1H = 0x05;
	PWMA_CCR1L = 0xDC;
	PWMA_CCR2H = 0x05;
	PWMA_CCR2L = 0xDC;
	
	PWMA_IER |= 0x07;	/* 打开更新中断、CCR1和CCR2的比较中断 */
	PWMA_CCMR1 |= 0x00;	/* 禁止CCR1寄存器的预装载功能 */
	PWMA_CCMR2 |= 0x00; /* 禁止CCR2寄存器的预装载功能 */
	PWMA_CCER1 |= 0x11;	/* 2个通道的比较使能 */
	PWMA_CR1 |= 0x01;	/* 开始计时 */

}



void pwm_servo_angle_set(PWMServoHandleTypeDef* handle, float target_angle, uint32_t time)
{
	target_angle = target_angle > MAX_ANGLE ? MAX_ANGLE : (target_angle < MIN_ANGLE ? MIN_ANGLE : target_angle);
	time = time < MIN_RUNNING_TIME ? MIN_RUNNING_TIME : (time > MAX_RUNNING_TIME ? MAX_RUNNING_TIME : time);
	
	handle->time = time;
	handle->duty_changed_state = 1;	
	handle->target_angle = target_angle;
	handle->target_duty = 500 + (int)(target_angle * 2000 / MAX_ANGLE);
}

void pwm_servo_offset_set(PWMServoHandleTypeDef* handle, int offset)
{
	offset = offset < MIN_OFFSET_DUTY ? MIN_OFFSET_DUTY : (offset > MAX_OFFSET_DUTY ? MAX_OFFSET_DUTY : offset);
	handle->offset = offset;
}

void pwm_servos_handler()
{
	uint8_t i;
	for(i = 0; i < USE_SERVO_NUM; i++)
	{
		pwm_servo_handler(&pwm_servos[i]);
	}
}



void pwm_generate_isr(void) interrupt 26
{
	static uint8_t servo_ccr1_index = 0;
	static uint8_t servo_ccr2_index = 4;
	static uint8_t ccr_extern_index = 0;
	if((PWMA_SR1 & 0x01) == 1)
	{
		/* 清除更新中断标志位 */
		PWMA_SR1 &= 0xfe;
			
		switch(servo_ccr1_index)
		{
			case 0:
				SERVO6 = 1;
				break;
			
			case 1:
				SERVO5 = 1;
				break;
			
			case 2:
				SERVO4 = 1;
				break;
			
			case 3:
				SERVO3 = 1;
				break;
		}
		PWMA_CCR1H = (uint8_t)(pwm_servos[servo_ccr1_index].write_duty >> 8);
		PWMA_CCR1L = (uint8_t)pwm_servos[servo_ccr1_index].write_duty;
		PWMA_CCR2H = (uint8_t)(pwm_servos[servo_ccr2_index].write_duty >> 8);
		PWMA_CCR2L = (uint8_t)pwm_servos[servo_ccr2_index].write_duty;
		
		
		if(ccr_extern_index == 0)
		{
			switch(servo_ccr2_index)
			{
				case 4:
					SERVO2 = 1;
					break;

				case 5:
					SERVO1 = 1;
					break;				
			}
		}		
	}
	
	if(((PWMA_SR1 >> 1) & 0x01) == 1)
	{
		/* 清除CCR1比较中断标志位 */
		PWMA_SR1 &= 0xfd;

		switch(servo_ccr1_index)
		{
			case 0:
				SERVO6 = 0;
				break;
			
			case 1:
				SERVO5 = 0;
				break;
			
			case 2:
				SERVO4 = 0;
				break;
			
			case 3:
				SERVO3 = 0;
				break;
		}

		servo_ccr1_index = servo_ccr1_index == 3 ? 0 : servo_ccr1_index + 1;
	}
	
	if(((PWMA_SR1 >> 2) & 0x01) == 1)
	{
		/* 清除CCR2比较中断标志位 */
		PWMA_SR1 &= 0xfb;
		
		switch(servo_ccr2_index)
		{
			case 4:
				SERVO2 = 0;
				break;

			case 5:
				SERVO1 = 0;
				break;				
		}

		if(servo_ccr2_index == 5)
		{
			ccr_extern_index++;
			if(ccr_extern_index == 3)
			{
				ccr_extern_index = 0;
				servo_ccr2_index = 4;
			}
		}
		else
		{
			servo_ccr2_index++;
		}
	}
}
