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

// datatype definitions macros
typedef unsigned char u08;
typedef signed char s08;
typedef unsigned short u16;
typedef signed short s16;
typedef unsigned long u32;
typedef signed long s32;
typedef unsigned long long u64;
typedef signed long long s64;
#include "avr/iom32.h"

#define MaxServo 18		// Число сервомашинок
u08 servo_state=0;		// Состояние конечного автомата. 
u32 ServoPortState[MaxServo+1];	// Значение порта которое надо вывести
u08 ServoNextOCR[MaxServo+1];	// Время вывода значения
u08 counter=0;
u08 servo_need_update=0;
typedef struct 	{
		u08 Position;
		u32 Bit;
		}  SArray_def;
 
union shit{ 
        u32 someshit; 
        u08 anyshit[4]; 
        };
        union shit happens; 
SArray_def Servo[MaxServo];
SArray_def *Servo_sorted[MaxServo];

//Простейший алгоритм сортировки вставкой. Недалеко ушел от пузырька, но на столь малых количествах
// данных являетя наиболее эффективным. 
void Servo_sort(void) 
{
u08 i,k;
SArray_def *tmp;
 
// Сортируем массив указателей.
for(i=1;i<MaxServo;i++)
	{
	for(k=i;((k>0)&&(Servo_sorted[k]->Position < Servo_sorted[k-1]->Position));k--)
		{	
		tmp = Servo_sorted[k];					// Swap [k,k-1] 
		Servo_sorted[k]=Servo_sorted[k-1];
		Servo_sorted[k-1]=tmp;
		}
 
	}
}

void Servo_upd(void)
{
u08 i,j,k;
 
for(i=0,k=0;i<MaxServo;i++,k++)
{
	if(Servo_sorted[i]->Position!=Servo_sorted[i+1]->Position)	//Если значения уникальные
	{
	ServoNextOCR[k] = Servo_sorted[i]->Position;			// Записываем их как есть
	ServoPortState[k+1] = Servo_sorted[i]->Bit;			// И битмаску туда же
	}
	else								// Но если совпадает со следующим
	{
	ServoNextOCR[k] = Servo_sorted[i]->Position;			// Позицию записываем
	ServoPortState[k+1] = Servo_sorted[i]->Bit;			// Записываем битмаску
 
	// И в цикле ищем все аналогичные позиции, склеивая их битмаски в одну.
 
	for(j=1;(Servo_sorted[i]->Position == Servo_sorted[i+j]->Position)&&(i+j<MaxServo);j++)
		{
		ServoPortState[k+1] |= Servo_sorted[i+j]->Bit;
		}
	i+=j-1;						// Перед выходом корректируем индекс
	}						// На глубину зарывания в повторы
}	
ServoNextOCR[k] = 0xFF;					// В последний элемент вписываем заглушку FF.
}

//============================================================================
void usart_tx (unsigned char data) {
    while ( !(UCSRA & (1<<5)) );    //ждём очистки регистра данных USART       
    UDR=data;    //отправить
} 
 unsigned char usart_rx (void) { 
    while ( !(UCSRA & (1<<7)) );    //ждём очистки регистра данных USART      
    return UDR;    //читаем данные
} 
//============================================================================
void Accept (void) {  //Принятие посылки
static unsigned char curr_serv,usart_data,state;
u08 Num,Pos;


        if(UCSRA&_BV(RXC)) {  //есть данные в usart
                         usart_data = UDR;

    switch(state) { // автомат с 2 состояниями
        case 0: // принимаем коды команд
            switch(usart_data)
            {
                case 'E': Servo_sort(); Servo_upd(); break; // конец посылки, обновление серв
                case 'B': state = 1; break; // переход в другое состояние, будем принимать параментры команды B
                                case 0xFF:  
                                Num = usart_rx ();  
                                Pos = usart_rx (); 
                                Servo[Num].Position = Pos; 
                                state = 0;      
                                Servo_sort(); 
                                Servo_upd();    
                                break;
            }
            break;
        case 1: // обработка параметров команды B
            Servo[curr_serv].Position = usart_data; // кладем в массив аргумент команды
            curr_serv++;
                        if(curr_serv == 18) // повторять будем, сколько у нас серв
            {
                curr_serv = 0; // обнуление счетчика, для следующего раза
                state = 0; // будем ожидать команды
            }
            break;
                                }
}}
int main(void) {
// Присваиваем указателям адреса наших структур. 
Servo_sorted[0] = &Servo[0];
Servo_sorted[1] = &Servo[1];
Servo_sorted[2] = &Servo[2];
Servo_sorted[3] = &Servo[3];
Servo_sorted[4] = &Servo[4];
Servo_sorted[5] = &Servo[5];
Servo_sorted[6] = &Servo[6];
Servo_sorted[7] = &Servo[7];
Servo_sorted[8] = &Servo[8];
Servo_sorted[9] = &Servo[9];
Servo_sorted[10] = &Servo[10];
Servo_sorted[11] = &Servo[11];
Servo_sorted[12] = &Servo[12];
Servo_sorted[13] = &Servo[13];
Servo_sorted[14] = &Servo[14];
Servo_sorted[15] = &Servo[15];
Servo_sorted[16] = &Servo[16];
Servo_sorted[17] = &Servo[17];
// Заполняем битмаски
//PORT A
Servo[0].Bit  = 1<<0; // A0
Servo[1].Bit  = 1<<1; // A1
Servo[2].Bit  = 1<<2; // A2
Servo[3].Bit  = 1<<5; // A5
Servo[4].Bit  = 1<<6; // A6
Servo[5].Bit  = 1<<7; // A7
// PORT B
Servo[6].Bit  = 1<<(0+8); // B0
Servo[7].Bit  = 1<<(1+8); // B1
Servo[8].Bit  = 1<<(2+8); // B2
Servo[9].Bit  = 1<<(3+8); // B3
Servo[10].Bit = 1<<(4+8); // B4
Servo[11].Bit = 1<<(5+8); // B5
// PORT C
Servo[12].Bit = 1UL<<(0+16); // C0
Servo[13].Bit = 1UL<<(1+16); // C1
// PORT D
Servo[14].Bit = 1UL<<(4+24); // D4
Servo[15].Bit = 1UL<<(5+24); // D5
Servo[16].Bit = 1UL<<(6+24); // D6
Servo[17].Bit = 1UL<<(7+24); // D7
// Выставляем все положения на нейтраль -- точно посредине диапазона. 
Servo[0].Position = 188;
Servo[1].Position = 100;
Servo[2].Position = 188;
Servo[3].Position = 188;
Servo[4].Position = 188;
Servo[5].Position = 188;
Servo[6].Position = 188;
Servo[7].Position = 188;
Servo[8].Position = 188;
Servo[9].Position = 100;
Servo[10].Position = 188;
Servo[11].Position = 188;
Servo[12].Position = 188;
Servo[13].Position = 188;
Servo[14].Position = 188;
Servo[15].Position = 188;
Servo[16].Position = 188;
Servo[17].Position = 188;

DDRA = 0xFF;
DDRB = 0xFF;
DDRC = 0xFF;
DDRD = 0b11111100;
Servo_sort();
Servo_upd();
   // OCR0 = ServoNextOCR[servo_state];
TIMSK |= 1<<OCIE0;
TCCR0 |= 0x03;
// UART 9600
UBRRH = 0;
UBRRL = 0x33;
UCSRB = (1<<RXEN) | (1<<TXEN);
UCSRC = (1<<URSEL) | (1<<UCSZ1) | (1<<UCSZ0); 


sei();
for(;;)
	{
		//PORTB=0;
//		_delay_ms(1000);
		//PORTB=4;
//		if (Servo[1].Position < 250) {
//		Servo[1].Position += 10;
//		} else {
//		Servo[1].Position += -10;
//		};
//		Servo_sort();
//		servo_need_update=1;
		counter++;
			Accept();
	}

}





ISR(TIMER0_COMP_vect)				// Прерывание по совпадению
{
if (servo_state)				// Если не нулевое состояние то
	{
	OCR0 = ServoNextOCR[servo_state];	// В регистр сравнения кладем следующий интервал
	happens.someshit = ServoPortState[servo_state];
	PORTA &= ~happens.anyshit[0];	// Сбрасываем биты в порту, в соответствии с маской в массиве масок.
	PORTB &= ~happens.anyshit[1];	// Сбрасываем биты в порту, в соответствии с маской в массиве масок.
	PORTC &= ~happens.anyshit[2];	// Сбрасываем биты в порту, в соответствии с маской в массиве масок.
	PORTD &= ~happens.anyshit[3];	// Сбрасываем биты в порту, в соответствии с маской в массиве масок.
	servo_state++;				// Увеличиваем состояние автомата
 
	if (OCR0 == 0xFF)				// Если значение сравнения равно FF значит это заглушка
		{				// И мы достигли конца таблицы. И пора обнулить автомат
		servo_state = 0;			// Выставляем нулевое состояние.
 
		TCCR0 &= 0b11111000;		// Сбрасываем предделитель таймера
		TCNT0 = 98;			// Программируем задержку в 20мс (на предделителе 1024)
		TCCR0 |= 0x05;			// Устанавливаем предделитель на 1024
 
		if (servo_need_update)		// Если поступил приказ обновить таблицы автомата
			{
			Servo_upd();		// Обновляем таблицы.
			servo_need_update = 0;	// Сбрасываем сигнал обновления.
			}
		}
	}
else						// Нулевое состояние автомата. Новый цикл
	{
	OCR0 = ServoNextOCR[servo_state];		// Берем первую выдержку.
 
	TCCR0 &= 0b11111000;			// Сбрасываем предделитель
	TCNT0=0;
	TCCR0 |= 0x03;				// Предделитель на 256
	PORTA = 0xFF;				// Выставялем все сервоканалы в 1 - начало импульса
	PORTB = 0xFF;				// Выставялем все сервоканалы в 1 - начало импульса
	PORTC = 0xFF;				// Выставялем все сервоканалы в 1 - начало импульса
	PORTD = 0b11111100;				// Выставялем все сервоканалы в 1 - начало импульса
 
	servo_state++;				// Увеличиваем состояние конечного автомата. 
	}
}