#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



uint8_t  EEMEM NonVolatileChar;
uint16_t EEMEM NonVolatileInt;
uint8_t  EEMEM NonVolatileString[10];


struct Servo{
	uint8_t actual_pos;
	uint8_t default_pos;
	uint8_t max_pos;
	uint8_t min_pos;
	uint8_t offset_pos;
	uint8_t port_mask;
} servos[18];




struct EEPROM_Servo{
	uint8_t dummy;      //to occupy EEPROM zero address
	uint8_t default_pos;
	uint8_t max_pos;
	uint8_t min_pos;
	uint8_t offset_pos;
} EEPROM_servo[18] EEMEM = {
	/* SERVO1  */ {0,0x27,125,121, 0},
	/* SERVO2  */ {0,0x27,125,121, 0},
	/* SERVO3  */ {0,0x27,125,121, 0},

	/* SERVO4  */ {0,0x55,125,121, 0},
	/* SERVO5  */ {0,0x55,125,121, 0},
	/* SERVO6  */ {0,0x55,125,121, 0},

	/* SERVO7  */ {0,0xD1,125,121, 0},
	/* SERVO8  */ {0,0xD1,125,121, 0},
	/* SERVO9  */ {0,0xD1,125,121, 0},

	/* SERVO10 */ {0,0xAA,125,121, 0},
	/* SERVO11 */ {0,0xAA,125,121, 0},
	/* SERVO12 */ {0,0xAA,125,121, 30},

	/* SERVO13 */ {0,255,125,121, 0},
	/* SERVO14 */ {0,255,125,121, 0},
	/* SERVO15 */ {0,255,125,121, 0},
	/* SERVO16 */ {0,255,125,121, 0},
	/* SERVO17 */ {0,255,125,121, 0},
	/* SERVO18 */ {0,255,255,121, 0}
};



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_pos = eeprom_read_byte(&EEPROM_servo[i].max_pos);
		servos[i].min_pos = eeprom_read_byte(&EEPROM_servo[i].min_pos);
		servos[i].offset_pos = eeprom_read_byte(&EEPROM_servo[i].offset_pos);
	}


	// 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;


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 (selected_servo<8){
			servo1 = servos[selected_servo].actual_pos+servos[selected_servo].offset_pos;
			servo2 = servos[selected_servo+8].actual_pos+servos[selected_servo+8].offset_pos;
		}
		else {
			servo1 = servos[selected_servo+8].actual_pos+servos[selected_servo+8].offset_pos;
			servo2 = servos[selected_servo+9].actual_pos+servos[selected_servo+9].offset_pos;
		}

		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;
		}
	}
	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;
	}
}

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);
	// Mode 4, CTC on OCR1A

	TIMSK1 |= (1 << OCIE1A);
	//Set interrupt on compare match

	TCCR1B |= (1 << CS11);
	// set prescaler to 8 and start the timer


	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[6].actual_pos);
				uart_putc(servos[3].actual_pos);
				uart_putc(servos[9].actual_pos);
			}
			else if (a == '6'){
				servos[0].actual_pos += 1;
				servos[1].actual_pos += 1;
				servos[2].actual_pos += 1;

				servos[6].actual_pos -= 1;
				servos[7].actual_pos -= 1;
				servos[8].actual_pos -= 1;

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

				servos[6].actual_pos += 1;
				servos[7].actual_pos += 1;
				servos[8].actual_pos += 1;
			}
			else if (a == '+'){
				servos[3].actual_pos += 1;
				servos[4].actual_pos += 1;
				servos[5].actual_pos += 1;

				servos[9].actual_pos -= 1;
				servos[10].actual_pos -= 1;
				servos[11].actual_pos -= 1;
			}
			else if (a == '-'){
				servos[3].actual_pos -= 1;
				servos[4].actual_pos -= 1;
				servos[5].actual_pos -= 1;

				servos[9].actual_pos += 1;
				servos[10].actual_pos += 1;
				servos[11].actual_pos += 1;
			}
			else if (a == 'q') {
				servos[11].offset_pos += 1;
				uart_putc(servos[11].offset_pos);
			}
			else if (a == 'a') {
				servos[11].offset_pos += 1;
				uart_putc(servos[11].offset_pos);
			}
		}
		//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);
	}
}

*/
