/*
 * motor.c
 *
 * Created: 8/19/2020 6:14:22 PM
 *  Author: Penguin
 */ 

#include "motor.h"
#include "p_adc.h"
#include "p_usart.h"
#define MOTOR_TOP_VAL	(2499)
#define FWD_VAL		(137)
#define STOP_VAL	(187)
#define REV_VAL		(237)
static volatile motor_state_t motor_state = MS_STOP;

static uint16_t target_val = 0;
static uint16_t current_val = 0;

struct tcc_module tcc_instance;
static void update_current_val(void)
{
	if(current_val == target_val)
	{
		// do nothing
	}
	else if(current_val > target_val)
	{
		current_val--;
	}
	else
	{
		current_val++;
	}
}
uint16_t match_val_from_pulse_width(uint64_t pw)
{
	volatile int temp = (int)((float)(((((float)pw) / 1000000.f)) * (float)MOTOR_TOP_VAL * 50.f ));
	
	asm volatile ("nop");
	
	
	return temp;
}
static uint16_t voltage_to_match_value()
{
	switch(motor_state)
	{
		case MS_STOP:
		{
			//do nothing
		}break;
		case MS_FORWARD:
		{
			float fwd_val = p_get_forward_pot_voltage();
			int hard_val = (int)(fwd_val * 100.f);
			if(hard_val > 165)
			{
				hard_val = 165;
			}
			
			// 1100 - 1499 with 1100 giving the most power and 1499 giving the least
			int min_fwd = 1100;
			int max_fwd = 1499;
			int span = max_fwd - min_fwd;
			int subspan = (int)(((float)hard_val * (float)span / (float)165) + min_fwd);
			volatile int pulse_width = (min_fwd + max_fwd - subspan); // in us
			printf("Pulse width: %d\n", pulse_width);
			uint16_t match_val = match_val_from_pulse_width((uint64_t)pulse_width);
			asm volatile("nop");
			return match_val;
		}break;
		case MS_REVERSE:
		{
			// 1100 - 1499 with 1100 giving the most power and 1499 giving the least
			float rev_val = p_get_reverse_pot_voltage();
			int hard_val = (int)(rev_val * 100.f);
			if(hard_val > 165)
			{
				hard_val = 165;
			}
			
			// 1501 - 1900 with 1501 giving the least power and 1900 giving the most
			int min_rev = 1501;
			int max_rev = 1900;
			int span = max_rev - min_rev;
			int pulse_width = (int)(((float)hard_val * (float)span / (float)165) + min_rev); // in us
			printf("Pulse width: %d\n", pulse_width);
			uint16_t match_val = match_val_from_pulse_width((uint64_t)pulse_width);
			return match_val;
		}break;
		default:
		{
			//do nothing
		};
	}
	return 0;
}
void motor_init(void)
{
	//! [setup_config]
	struct tcc_config config_tcc;
	//! [setup_config]
	//! [setup_config_defaults]
	tcc_get_config_defaults(&config_tcc, TCC0);
	//! [setup_config_defaults]

	//! [setup_change_config]
	config_tcc.counter.clock_prescaler = TCC_CLOCK_PRESCALER_DIV64;
	config_tcc.counter.period = MOTOR_TOP_VAL;
	config_tcc.compare.wave_generation = TCC_WAVE_GENERATION_SINGLE_SLOPE_PWM;
	config_tcc.compare.match[0] = match_val_from_pulse_width(1500);
	
	//! [setup_change_config]

	//! [setup_change_config_pwm]
	config_tcc.pins.enable_wave_out_pin[0] = true;
	config_tcc.pins.wave_out_pin[0]        = PIN_PA04E_TCC0_WO0;
	config_tcc.pins.wave_out_pin_mux[0]    = MUX_PA04E_TCC0_WO0;
	//! [setup_change_config_pwm]
	config_tcc.counter.clock_source = GCLK_CLKCTRL_GEN_GCLK0;
	//! [setup_set_config]
	tcc_init(&tcc_instance, TCC0, &config_tcc);
	//! [setup_set_config]

	//! [setup_enable]
	tcc_enable(&tcc_instance);
	//! [setup_enable]
	
	target_val = match_val_from_pulse_width(1500);
	current_val = match_val_from_pulse_width(1500);
}
void motor_run(void)
{
	switch(motor_state)
	{
		case MS_FORWARD:
		case MS_REVERSE:
		{
			target_val = voltage_to_match_value();
		}break;
		default:
		{
			// do nothing
		};
	}
	// update current val from target val
	update_current_val();
	// write current val
	tcc_set_compare_value(&tcc_instance, 0, current_val);
	//printf("Current Value: %ul\tTarget Value: %ul\n", current_val, target_val);
}
void motor_update_state(motor_state_t state)
{
	motor_state = state;
	// disable leds
	// Do init stuff
	switch(motor_state)
	{
		case MS_STOP:
		{
			// enable stop led
			// set to static 1500
			
			target_val = match_val_from_pulse_width((uint64_t)1500);
		}break;
		case MS_FORWARD:
		{
			// enable forward led
		}break;
		case MS_REVERSE:
		{
			// enable reverse led
		}break;
		default:
		{
			// do nothing
		};
	}
}

