#include <stdint.h>
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>
#include "stepper.h"
#include "writepin.h"
#include "comperator.h"
#include "buttons.h"
#include "eeprom.h"

#define LED_2_PIN PB3
#define LED_1_PIN PB4

#define BUTTON_1_PIN PD5
#define BUTTON_2_PIN PD6

bool setup = false;
bool direction = false;
static bool buttonLongpressed[2];
uint16_t lockedSpeed = 0xFF00;
Stepper stepper(&PORTD, PD0, PD1, PD2, PD3);

ISR(TIMER0_COMPA_vect) 
{
		stepper.tick();
}

void blink(bool fast = true, bool red = false)
{
	_delay_ms(100);
	if(!fast) _delay_ms(200); 
	PORTB |= 1 << red ? LED_2_PIN : LED_1_PIN;
	_delay_ms(100);
	if(!fast) _delay_ms(200); 
	PORTB &= ~(1 << red ? LED_2_PIN : LED_1_PIN);
}

void lowBattery(Stepper* stepper)
{
	stepper->setEndlesMove(false);
	stepper->shutdown();
	writePin(&PORTB, LED_1_PIN, false);
	while(true)
	{
		blink(true, true);
	}
}

void buttonHandler(uint8_t index, uint8_t type, void* data)
{
	Stepper* stepper = reinterpret_cast<Stepper*>(data);
	if(setup == false)
	{
		if(type == Buttons::PRESSED)
		{
			stepper->setHalfCurrent(false);
			if(index == 0) 
			{
				stepper->setSpeed(0xFFEA);
				stepper->setEndlesMove(true, true);
			}
			else
			{
				stepper->setSpeed(0xFFEA);
				stepper->setEndlesMove(true, false);
			}
		}
		else if(type == Buttons::RELEASED || type == Buttons::LONG_RELEASED)
		{
			stepper->setSpeed(lockedSpeed);
			stepper->setEndlesMove(true, direction);
			stepper->setHalfCurrent(true);
			buttonLongpressed[0] = false;
			buttonLongpressed[1] = false;
		}
		else if(type == Buttons::LONG_PRESSED)
		{
			buttonLongpressed[index] = true;
			if(buttonLongpressed[0] && buttonLongpressed[1])
			{
				stepper->setHalfCurrent(false);
				writePin(&PORTB, LED_2_PIN, true);
				writePin(&PORTB, LED_1_PIN, false);
				setup = true;
				buttonLongpressed[0] = false;
				buttonLongpressed[1] = false;
				stepper->setSpeed(lockedSpeed);
				stepper->setEndlesMove(true, direction);
				blink();
				blink();
				blink();
			}
		}
	}
	else
	{
		if(type == Buttons::PRESSED) writePin(&PORTB, LED_1_PIN, true);
		if(type == Buttons::RELEASED)
		{
			writePin(&PORTB, LED_1_PIN, false);
			if(index == 0 && lockedSpeed < 0xFF00) lockedSpeed+=80; 
			else if(lockedSpeed > 0xEF00) lockedSpeed-=80;
			cli();
			EEPROM_write_char(6, lockedSpeed>>8);
			EEPROM_write_char(5, lockedSpeed & 0x00FF);
			sei();
			stepper->setSpeed(lockedSpeed);
		}
		else if(type == Buttons::LONG_PRESSED)
		{
			if(index == 0)
			{
				writePin(&PORTB, LED_2_PIN, false);
				writePin(&PORTB, LED_1_PIN, true);
				setup = false;
				buttonLongpressed[0] = false;
				buttonLongpressed[1] = false;
				stepper->setSpeed(lockedSpeed);
				stepper->setEndlesMove(true, direction);
			}
			else
			{
				direction = !direction;
				cli();
				EEPROM_write_char(2, direction);
				sei();
				blink();
				blink();
				stepper->setEndlesMove(true, direction);
			}
		}
	}
}

int main()
{
	DDRB  = 1 << LED_1_PIN | 1 << LED_2_PIN;
	DDRD  = 1 << PD0 | 1 << PD1 | 1 << PD2 | 1 << PD3;
	PORTD = 1 << BUTTON_1_PIN | 1 << BUTTON_2_PIN;
	
	if(EEPROM_read_char(6) != 0) 
	{
		lockedSpeed = (uint16_t)EEPROM_read_char(5) | ((uint16_t)EEPROM_read_char(6) << 8);
		direction   = EEPROM_read_char(2);
	}
	
	if(lockedSpeed > 0xFF00 || lockedSpeed < 0xEF00) 
	{
		lockedSpeed = 0xFF00;
		blink(true, true);
	}

	
	TCCR0A = (1<<WGM01); //CTC timer mode
	TIMSK = (1<<OCIE0A); //OCIE0A compeare interrupt enable
	OCR0A  = 255;
    TCCR0B = (1<<CS00);  //start with no prescaleing
	sei();
	
	blink(false);
	blink(false);
	
	writePin(&PORTB, LED_1_PIN, true);
	stepper.setSpeed(lockedSpeed);
	stepper.setEndlesMove(true, direction);
	stepper.setHalfCurrent(true);
	
	Comperator comperator;
	comperator.on();
	
	Buttons buttons(&buttonHandler, reinterpret_cast<void*>(&stepper));
	
	while(true)
	{
		if(comperator.compare()) lowBattery(&stepper);
		buttons.tick();
		_delay_ms(2);
	}
}