#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include "uart.h"
#include <avr/eeprom.h>

#define UART_BAUD_RATE      57600



#define ADC_BATTERY				ADC6
#define RESERVED_PIN			PD2


#define SERVO_POWER_SOURCE_E	PB1
#define SERVO_POWER_SOURCE_PORT PORTB
#define SERVO_POWER_SOURCE_DDR DDRB


#define	PIN_SERVO17				PC4
#define	PIN_SERVO18				PC5
#define ADDITIONAL_SERVOS_PORT	PORTC
#define ADDITIONAL_SERVOS_DDR	DDRC


#define MUX1_DDR	DDRC
#define	MUX1_PORT	PORTC
#define MUX1_S1		PC2
#define MUX1_S2		PC1
#define MUX1_S3		PC0
#define	MUX1_E		PC3

#define MUX2_DDR	DDRD
#define	MUX2_PORT	PORTD
#define MUX2_S1		PD4
#define MUX2_S2		PD5
#define MUX2_S3		PD6
#define MUX2_E		PD7

#define timer0_OCR0 165


struct Servo{
	uint8_t actual_pos;
	uint8_t default_pos;
	uint8_t max;
	uint8_t min;
	uint8_t offset;
	uint8_t port_mask;
} servos[18];




struct EEPROM_Servo{
	uint8_t dummy;      //to occupy EEPROM zero address
	uint8_t default_pos;
	uint8_t max;
	uint8_t min;
	uint8_t offset;
} EEPROM_servo[18] EEMEM = {
	/* SERVO1  */ {0, 0xD1, 0xFF, 0x00, 0x00},
	/* SERVO2  */ {0, 0xAA, 0xFF, 0x00, 0x00},
	/* SERVO3  */ {0, 0xFF, 0xFF, 0x00, 0x00},

	/* SERVO4  */ {0, 0xD1, 0xFF, 0x00, 0xDD},
	/* SERVO5  */ {0, 0xAA, 0xFF, 0x00, 0xF3},
	/* SERVO6  */ {0, 0xFF, 0xFF, 0x00, 0x00},

	/* SERVO7  */ {0, 0xD1, 0xFF, 0x00, 0x12},
	/* SERVO8  */ {0, 0xAA, 0xFF, 0x00, 0xE9},
	/* SERVO9  */ {0, 0xFF, 0xFF, 0x00, 0x00},

	/* SERVO10 */ {0, 0x27, 0xFF, 0x00, 0x00},
	/* SERVO11 */ {0, 0x55, 0xFF, 0x00, 0xF4},
	/* SERVO12 */ {0, 0xFF, 0xFF, 0x00, 0x00},

	/* SERVO13 */ {0, 0x27, 0xFF, 0x00, 0x05},
	/* SERVO14 */ {0, 0x55, 0xFF, 0x00, 0x08},
	/* SERVO15 */ {0, 0xFF, 0xFF, 0x00, 0x00},

	/* SERVO16 */ {0, 0x27, 0xFF, 0x00, 0xFB},
	/* SERVO17 */ {0, 0x55, 0xFF, 0x00, 0x00},
	/* SERVO18 */ {0, 0xFF, 0xFF, 0x00, 0x00}
};



void servo_load_values(){

	for (int i = 0; i < 18; i++){
		servos[i].default_pos = eeprom_read_byte(&EEPROM_servo[i].default_pos);
		servos[i].actual_pos = servos[i].default_pos;
		servos[i].max = eeprom_read_byte(&EEPROM_servo[i].max);
		servos[i].min = eeprom_read_byte(&EEPROM_servo[i].min);
		servos[i].offset = eeprom_read_byte(&EEPROM_servo[i].offset);
	}


	// SERVA MUX 1
	//servos[0].actual_pos = 124;
	servos[0].port_mask = 0;
	servos[1].port_mask = (1<<MUX1_S1);
	servos[2].port_mask = (1<<MUX1_S2);
	servos[3].port_mask = (1<<MUX1_S1) | (1<<MUX1_S2);
	servos[4].port_mask = (1<<MUX1_S3);
	servos[5].port_mask = (1<<MUX1_S3) | (1<<MUX1_S1);
	servos[6].port_mask = (1<<MUX1_S3) | (1<<MUX1_S2);
	servos[7].port_mask = (1<<MUX1_S3) | (1<<MUX1_S2)| (1<<MUX1_S1);


	servos[8].port_mask = 0;
	servos[9].port_mask = (1<<MUX2_S1);
	servos[10].port_mask = (1<<MUX2_S2);
	servos[11].port_mask = (1<<MUX2_S1) | (1<<MUX2_S2);
	servos[12].port_mask = (1<<MUX2_S3);
	servos[13].port_mask = (1<<MUX2_S3) | (1<<MUX2_S1);
	servos[14].port_mask = (1<<MUX2_S3) | (1<<MUX2_S2);
	servos[15].port_mask = (1<<MUX2_S3) | (1<<MUX2_S2)| (1<<MUX2_S1);

	servos[16].port_mask = (1<<PIN_SERVO17);
	servos[17].port_mask = (1<<PIN_SERVO18);
}


uint8_t selected_servo = 0;
uint8_t servo_diff_id = 0;
uint16_t servo_diff = 0;
uint8_t servos_overflow = 0;

ISR (TIMER1_COMPA_vect)
{

	if (servo_diff == 0){

		uint8_t servo1, servo2;
		MUX1_PORT &= ~(1<<MUX1_E);
		MUX2_PORT &= ~(1<<MUX2_E);
		ADDITIONAL_SERVOS_PORT &= ~((1<<PIN_SERVO17) | (1<<PIN_SERVO18));

		if (servos_overflow==1){
			servos_overflow = 0;
			TCCR1B &= ~((1 << CS10) | (1 << CS11) | (1 << CS12)); // vypnutí servo čítače

		}
		else {
			if (selected_servo==0){

			}
			if (selected_servo<8){
				servo1 = servos[selected_servo].actual_pos+servos[selected_servo].offset;
				servo2 = servos[selected_servo+8].actual_pos+servos[selected_servo+8].offset;
			}
			else {
				servo1 = servos[selected_servo+8].actual_pos+servos[selected_servo+8].offset;
				servo2 = servos[selected_servo+9].actual_pos+servos[selected_servo+9].offset;
			}


			// DODĚLAT!!!! ošetřit když je rozdíl hodnot = 1
			if (servo1 < servo2){
				servo_diff = 12*(servo2 - servo1);
				servo_diff_id = 2;
				OCR1A = 12*(servo1 + 116);
			}
			else if (servo2 < servo1){
				servo_diff = 12*(servo1 - servo2);
				servo_diff_id = 1;
				OCR1A = 12*(servo2 + 116);
			}
			else {
				OCR1A = 12*(servo1 + 116);
				servo_diff = 0;
			}

			if (selected_servo<8){
				MUX1_PORT &= ~((1<<MUX1_S1) | (1<<MUX1_S2) | (1<<MUX1_S3));
				MUX1_PORT |= servos[selected_servo].port_mask;
				MUX1_PORT |= (1<<MUX1_E);

				MUX2_PORT &= ~((1<<MUX2_S1) | (1<<MUX2_S2) | (1<<MUX2_S3));
				MUX2_PORT |= servos[selected_servo+8].port_mask;
				MUX2_PORT |= (1<<MUX2_E);
			}
			else {
				ADDITIONAL_SERVOS_PORT |= (1<<PIN_SERVO17) | (1<<PIN_SERVO18);
			}

			selected_servo++;
			if (selected_servo>8){
				selected_servo = 0;
				servos_overflow = 1;
			}
		}
	}
	else {
		if (servo_diff_id == 1){
			MUX2_PORT &= ~(1<<MUX2_E);
			ADDITIONAL_SERVOS_PORT &= ~(1<<PIN_SERVO18);
		}
		else if(servo_diff_id == 2){
			MUX1_PORT &= ~(1<<MUX1_E);
			ADDITIONAL_SERVOS_PORT &= ~(1<<PIN_SERVO17);
		}
		OCR1A = servo_diff;
		servo_diff = 0;
	}
}

uint8_t counter0 = 0;

ISR (TIMER0_COMPA_vect)
{

	if (counter0 == 0){
		OCR0A = timer0_OCR0;
		counter0++;
	}
	else {
		OCR0A = timer0_OCR0;
		//TCCR0B &= ~((1 << CS00) | (1 << CS01) | (1 << CS02)); // 50Hz čítače
		PORTD ^= (1 << RESERVED_PIN);
		OCR1A = 50;
		TCNT1 = 0;
		TCCR1B |= (1 << CS11); // zapnutí servo čítače
		selected_servo=0;
		counter0=0;
	}
}

int main(){

	SERVO_POWER_SOURCE_DDR |= (1<<SERVO_POWER_SOURCE_E);
	//SERVO_POWER_SOURCE_PORT &= ~(1<<SERVO_POWER_SOURCE_E);
	SERVO_POWER_SOURCE_PORT |= (1<<SERVO_POWER_SOURCE_E);

	ADDITIONAL_SERVOS_DDR |= (1<<PIN_SERVO17) | (1<<PIN_SERVO18);
	ADDITIONAL_SERVOS_PORT &= ~((1<<PIN_SERVO17) | (1<<PIN_SERVO18));

	// Inicializace multiplexorů
	MUX1_DDR |= (1<<MUX1_S1) | (1<<MUX1_S2) | (1<<MUX1_S3) | (1<<MUX1_E);
	MUX2_DDR |= (1<<MUX2_S1) | (1<<MUX2_S2) | (1<<MUX2_S3) | (1<<MUX2_E);

	MUX1_PORT &= ~((1<<MUX1_S1) | (1<<MUX1_S2) | (1<<MUX1_S3) | (1<<MUX1_E));
	MUX2_PORT &= ~((1<<MUX2_S1) | (1<<MUX2_S2) | (1<<MUX2_S3) | (1<<MUX2_E));


	uart_init( UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU) );



	servo_load_values();

	OCR1A = 10000;
	TCCR1B |= (1 << WGM12);
	TIMSK1 |= (1 << OCIE1A);


    TCCR0A |= (1 << WGM01);
    TIMSK0 |= (1 << OCIE0A);

	TCNT0 = 0;
	OCR0A = timer0_OCR0;
	TCCR0B |= (1 << CS02) | (1 << CS00);			// zapnutí 50Hz čítače


	//TCCR1B |= (1 << CS11);



	sei();
	// enable interrupts

	uart_puts("ready");


	for (;;){
		int a = uart_getc();
		if (a != UART_NO_DATA){
			if (a == 'f'){
				uart_putc(servos[0].actual_pos);
				uart_putc(servos[1].actual_pos);
				uart_putc(servos[2].actual_pos);
			}
			else if (a == '6'){
				servos[0].actual_pos += 1;
				servos[3].actual_pos += 1;
				servos[6].actual_pos += 1;

				servos[9].actual_pos -= 1;
				servos[12].actual_pos -= 1;
				servos[15].actual_pos -= 1;

			}
			else if (a == '3'){
				servos[0].actual_pos -= 1;
				servos[3].actual_pos -= 1;
				servos[6].actual_pos -= 1;

				servos[9].actual_pos += 1;
				servos[12].actual_pos += 1;
				servos[15].actual_pos += 1;
			}
			else if (a == '+'){
				servos[1].actual_pos += 1;
				servos[4].actual_pos += 1;
				servos[7].actual_pos += 1;

				servos[10].actual_pos -= 1;
				servos[13].actual_pos -= 1;
				servos[16].actual_pos -= 1;

			}
			else if (a == '-'){
				servos[1].actual_pos -= 1;
				servos[4].actual_pos -= 1;
				servos[7].actual_pos -= 1;

				servos[10].actual_pos += 1;
				servos[13].actual_pos += 1;
				servos[16].actual_pos += 1;
			}
			else if (a == 'q') {
				servos[11].offset += 1;
				uart_putc(servos[11].offset);
			}
			else if (a == 'a') {
				servos[11].offset += 1;
				uart_putc(servos[11].offset);
			}
			else if (a == 'w'){
				servos[0].actual_pos += 1;
				servos[3].actual_pos += 1;
				servos[6].actual_pos += 1;

				servos[9].actual_pos -= 1;
				servos[12].actual_pos -= 1;
				servos[15].actual_pos -= 1;

				servos[1].actual_pos += 1;
				servos[4].actual_pos += 1;
				servos[7].actual_pos += 1;

				servos[10].actual_pos -= 1;
				servos[13].actual_pos -= 1;
				servos[16].actual_pos -= 1;
			}
			else if (a == 's'){
				servos[0].actual_pos -= 1;
				servos[3].actual_pos -= 1;
				servos[6].actual_pos -= 1;

				servos[9].actual_pos += 1;
				servos[12].actual_pos += 1;
				servos[15].actual_pos += 1;

				servos[1].actual_pos -= 1;
				servos[4].actual_pos -= 1;
				servos[7].actual_pos -= 1;

				servos[10].actual_pos += 1;
				servos[13].actual_pos += 1;
				servos[16].actual_pos += 1;
			}
			else if (a == '4'){
				servos[2].actual_pos += 1;
			}

			else if (a == 'K'){

				for (int i = 0; i < 18; i++){
					while (1){
						int a = uart_getc();
						if (a == '+'){
							servos[i].offset++;
						}
						else if (a == '-'){
							servos[i].offset--;
						}
						else if (a == 'x'){
							break;
						}
						else if (a == 'p'){
							uart_putc(servos[i].offset);
						}
					}
				}

			}

		}
		//select_next_servos();

		//_delay_ms(500);

	}

}




/*

void servo_rise(struct Servo servo){
	//uart_putc(servo.default_pos);
	if ((servo.port == &MUX1_PORT) && (servo.port_mask <= ((1<<MUX1_S1) | (1<<MUX1_S2) | (1<<MUX1_S3)))){
		MUX1_PORT &= ~((1<<MUX1_S1) | (1<<MUX1_S2) | (1<<MUX1_S3));
		*servo.port |= servo.port_mask;

		MUX1_PORT |= (1<<MUX1_E);
		//uart_puts("MUX1");
		//ADDITIONAL_SERVOS_PORT &= ~((1<<PIN_SERVO17) | (1<<PIN_SERVO18));
	}
	else if (servo.port == &MUX2_PORT && (servo.port_mask <= ((1<<MUX2_S1) | (1<<MUX2_S2) | (1<<MUX2_S3)))){
		MUX2_PORT &= ~((1<<MUX2_S1) | (1<<MUX2_S2) | (1<<MUX2_S3));
		*servo.port |= servo.port_mask;

		MUX2_PORT |= (1<<MUX2_E);
		//uart_puts("MUX2");
		//ADDITIONAL_SERVOS_PORT &= ~((1<<PIN_SERVO17) | (1<<PIN_SERVO18));
	}
	else {
		*servo.port |= servo.port_mask;
		//uart_puts("NO MUX");
	}
}



void servo_fall(struct Servo servo){
	if ((servo.port == &MUX1_PORT) && (servo.port_mask <= ((1<<MUX1_S1) | (1<<MUX1_S2) | (1<<MUX1_S3)))){
		MUX1_PORT |= (1<<MUX1_E);
		MUX1_PORT &= ~((1<<MUX1_S1) | (1<<MUX1_S2) | (1<<MUX1_S3));
		uart_puts("MUX1");
	}
	else if (servo.port == &MUX2_PORT && (servo.port_mask <= ((1<<MUX2_S1) | (1<<MUX2_S2) | (1<<MUX2_S3)))){
		MUX2_PORT |= (1<<MUX2_E);
		MUX2_PORT &= ~((1<<MUX2_S1) | (1<<MUX2_S2) | (1<<MUX2_S3));
		uart_puts("MUX2");
	}
	else {
		uart_puts("NO MUX");
		*servo.port &= ~(servo.port_mask);
	}
}

*/

