#define F_CPU 20000000
#define BAUD 9600
#define BRC ((F_CPU/16/BAUD)-1)
#define MPU6050 0x68
#define interval 10
#define sensormotor 100

#include <avr/io.h>
#include <stdio.h> // for serial
#include <avr/interrupt.h>
#include <util/delay.h>
#include <util/setbaud.h>
#include <util/twi.h>
#include <inttypes.h> // for printf uint16_t
#include <avr/pgmspace.h>

void uart_init(void){
	UBRR0L = BRC;
	UBRR0H = (BRC >> 8); //Baud rate setting
	UCSR0B |= (1 << RXEN0) | (1 << TXEN0); //RxTx enable
	UCSR0C = (1 << UCSZ01) | (1 << UCSZ00); //8bit char
}

void uart_setchar(char c) {
    loop_until_bit_is_set(UCSR0A, UDRE0); /* Wait until data register empty. */
    UDR0 = c;
} 

char uart_getchar(void) {
    loop_until_bit_is_set(UCSR0A, RXC0); /* Wait until data exists. */
    return UDR0;
}

int usart_putchar_printf(char var, FILE *stream) {
    //if (var == '\n') usart_putchar('\r');
    uart_setchar(var);
    return 0;
}

static FILE mystdout = FDEV_SETUP_STREAM(usart_putchar_printf, NULL, _FDEV_SETUP_WRITE);//define stream

///////////////////////////////////////////////////////////////////////////////////////////////

void I2C_init(void){
	//TWSR = 0x00;
	TWBR = 0x5C;// TWI clock 100kHz, SCL frequency = F_CPU/(16 + 2(TWBR)*(TWSR prescale))
	TWCR = (1 << TWEN);
	//address 1101000
}

void I2C_start(void){
	TWCR = (1 << TWEN) | (1 << TWINT) |(1 << TWSTA);//twi enable/clear int flag/start (master)
	while ((TWCR & (1<<TWINT)) == 0);//? wait for TWINT to be 1?
	//if ((TWSR & 0xF8) != TW_START) return -1;//comfire start condition has been transmitted
}

void I2C_stop(void){
	TWCR = (1 << TWEN) | (1 << TWINT) |(1 << TWSTO);
}

void I2C_write(uint8_t data){
	TWDR = data;
	TWCR = (1 << TWINT) | (1 << TWEN);
	while ((TWCR & (1<<TWINT)) == 0);//? wait for TWINT to be 1
	//if ((TWSR & 0xF8) != TW_MT_SLA_ACK) return -1;//comfire start condition has been transmitted
}

uint8_t I2C_readACK(){
	TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA);//with acknowledge
    while ((TWCR & (1<<TWINT)) == 0);
    return TWDR;
}

uint8_t I2C_readNACK(){
	TWCR = (1<<TWINT)|(1<<TWEN);//without acknowledge
    while ((TWCR & (1<<TWINT)) == 0);
    return TWDR;
}

uint8_t I2C_status(void)
{
    uint8_t status;
    //mask status
    status = TWSR & 0xF8;
    return status;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////

uint8_t MPU_init(){
	I2C_init();
	I2C_start();
	if (I2C_status() != TW_START) return -1;
	I2C_write(((MPU6050 << 1) | TW_WRITE)); //TW_WRITE = 0 defined in twi.h
	if (I2C_status() != TW_MT_SLA_ACK) return -1; 
	I2C_write(0x6B);//PWR_MGMT_1 address
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_write(0);//set PWR_MGMT_1 to 0
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_stop();
	_delay_us(10);//allow time for stop to be sent
	return 0;
}

uint8_t MPU_write(uint8_t u8addr, uint8_t u8data){
	I2C_init();
	I2C_start();
	if (I2C_status() != TW_START) return -1;
	I2C_write(((MPU6050 << 1) | TW_WRITE)); //TW_WRITE = 0 defined in twi.h
	if (I2C_status() != TW_MT_SLA_ACK) return -1; 
	I2C_write(u8addr);//set address
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_write(u8data);//set value
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_stop();
	_delay_us(10);//allow time for stop to be sent
	return 0;
}

uint8_t MPU_read(uint8_t u8addr, uint16_t *u16data){
	I2C_start();
	if (I2C_status() != TW_START) return -1;
	I2C_write(((MPU6050 << 1) | TW_WRITE)); //TW_WRITE = 0 defined in twi.h
	if (I2C_status() != TW_MT_SLA_ACK) return -1; 
	I2C_write(u8addr); //send reading address of MPU-6050
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_stop();
	_delay_us(10);//wait for data
	
	I2C_start();
	if (I2C_status() != TW_START) return -1;
	I2C_write(((MPU6050 << 1) | TW_READ));//ask to read
	if (I2C_status() != TW_MR_SLA_ACK) return -1;
	
	*u16data = I2C_readACK() << 8; //significant 8 bits with acknowledge
	*u16data |= I2C_readNACK(); //no ACK after the last bytes.
	I2C_stop();
	_delay_us(10);//wait for data
	return 0;
}

///////////////////////////////////////////////////////////////////////////////////////////
//sinosoidal wave
/* const uint8_t M1[3][1024] PROGMEM = {
	{127,128,129,129,130,131,132,132,133,134,135,135,136,137,138,138,139,140,141,142,142,143,144,145,145,146,147,148,148,149,150,151,151,152,153,154,154,155,156,157,157,158,159,160,160,161,162,163,163,164,165,165,166,167,168,168,169,170,171,171,172,173,173,174,175,176,176,177,178,178,179,180,180,181,182,183,183,184,185,185,186,187,187,188,189,189,190,191,191,192,193,193,194,195,195,196,196,197,198,198,199,200,200,201,201,202,203,203,204,205,205,206,206,207,207,208,209,209,210,210,211,212,212,213,213,214,214,215,215,216,216,217,218,218,219,219,220,220,221,221,222,222,223,223,224,224,225,225,226,226,226,227,227,228,228,229,229,230,230,231,231,231,232,232,233,233,233,234,234,235,235,235,236,236,237,237,237,238,238,238,239,239,239,240,240,240,241,241,241,242,242,242,242,243,243,243,244,244,244,244,245,245,245,245,246,246,246,246,247,247,247,247,247,248,248,248,248,248,249,249,249,249,249,249,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,249,249,249,249,249,249,248,248,248,248,248,247,247,247,247,247,246,246,246,246,245,245,245,245,244,244,244,244,243,243,243,242,242,242,242,241,241,241,240,240,240,239,239,239,238,238,238,237,237,237,236,236,235,235,235,234,234,233,233,233,232,232,231,231,231,230,230,229,229,228,228,227,227,226,226,226,225,225,224,224,223,223,222,222,221,221,220,220,219,219,218,218,217,216,216,215,215,214,214,213,213,212,212,211,210,210,209,209,208,207,207,206,206,205,205,204,203,203,202,201,201,200,200,199,198,198,197,196,196,195,195,194,193,193,192,191,191,190,189,189,188,187,187,186,185,185,184,183,183,182,181,180,180,179,178,178,177,176,176,175,174,173,173,172,171,171,170,169,168,168,167,166,165,165,164,163,163,162,161,160,160,159,158,157,157,156,155,154,154,153,152,151,151,150,149,148,148,147,146,145,145,144,143,142,142,141,140,139,138,138,137,136,135,135,134,133,132,132,131,130,129,129,128,127,126,125,125,124,123,122,122,121,120,119,119,118,117,116,116,115,114,113,112,112,111,110,109,109,108,107,106,106,105,104,103,103,102,101,100,100,99,98,97,97,96,95,94,94,93,92,91,91,90,89,89,88,87,86,86,85,84,83,83,82,81,81,80,79,78,78,77,76,76,75,74,74,73,72,71,71,70,69,69,68,67,67,66,65,65,64,63,63,62,61,61,60,59,59,58,58,57,56,56,55,54,54,53,53,52,51,51,50,49,49,48,48,47,47,46,45,45,44,44,43,42,42,41,41,40,40,39,39,38,38,37,36,36,35,35,34,34,33,33,32,32,31,31,30,30,29,29,28,28,28,27,27,26,26,25,25,24,24,23,23,23,22,22,21,21,21,20,20,19,19,19,18,18,17,17,17,16,16,16,15,15,15,14,14,14,13,13,13,12,12,12,12,11,11,11,10,10,10,10,9,9,9,9,8,8,8,8,7,7,7,7,7,6,6,6,6,6,5,5,5,5,5,5,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,5,5,5,5,5,5,6,6,6,6,6,7,7,7,7,7,8,8,8,8,9,9,9,9,10,10,10,10,11,11,11,12,12,12,12,13,13,13,14,14,14,15,15,15,16,16,16,17,17,17,18,18,19,19,19,20,20,21,21,21,22,22,23,23,23,24,24,25,25,26,26,27,27,28,28,28,29,29,30,30,31,31,32,32,33,33,34,34,35,35,36,36,37,38,38,39,39,40,40,41,41,42,42,43,44,44,45,45,46,47,47,48,48,49,49,50,51,51,52,53,53,54,54,55,56,56,57,58,58,59,59,60,61,61,62,63,63,64,65,65,66,67,67,68,69,69,70,71,71,72,73,74,74,75,76,76,77,78,78,79,80,81,81,82,83,83,84,85,86,86,87,88,89,89,90,91,91,92,93,94,94,95,96,97,97,98,99,100,100,101,102,103,103,104,105,106,106,107,108,109,109,110,111,112,112,113,114,115,116,116,117,118,119,119,120,121,122,122,123,124,125,125,126},
	{235,235,234,234,234,233,233,232,232,232,231,231,230,230,229,229,229,228,228,227,227,226,226,225,225,224,224,223,223,222,222,221,221,220,220,219,219,218,218,217,217,216,216,215,215,214,214,213,212,212,211,211,210,210,209,208,208,207,207,206,206,205,204,204,203,202,202,201,201,200,199,199,198,198,197,196,196,195,194,194,193,192,192,191,190,190,189,188,188,187,186,186,185,184,184,183,182,182,181,180,180,179,178,177,177,176,175,175,174,173,172,172,171,170,170,169,168,167,167,166,165,165,164,163,162,162,161,160,159,159,158,157,156,156,155,154,153,153,152,151,150,150,149,148,147,147,146,145,144,144,143,142,141,141,140,139,138,137,137,136,135,134,134,133,132,131,131,130,129,128,128,127,126,125,124,124,123,122,121,121,120,119,118,118,117,116,115,114,114,113,112,111,111,110,109,108,108,107,106,105,105,104,103,102,102,101,100,99,99,98,97,96,96,95,94,93,93,92,91,90,90,89,88,88,87,86,85,85,84,83,82,82,81,80,80,79,78,78,77,76,75,75,74,73,73,72,71,71,70,69,69,68,67,67,66,65,65,64,63,63,62,61,61,60,59,59,58,57,57,56,55,55,54,54,53,52,52,51,50,50,49,49,48,48,47,46,46,45,45,44,43,43,42,42,41,41,40,40,39,38,38,37,37,36,36,35,35,34,34,33,33,32,32,31,31,30,30,29,29,28,28,27,27,26,26,26,25,25,24,24,23,23,23,22,22,21,21,20,20,20,19,19,18,18,18,17,17,17,16,16,16,15,15,15,14,14,14,13,13,13,12,12,12,11,11,11,11,10,10,10,9,9,9,9,8,8,8,8,8,7,7,7,7,6,6,6,6,6,6,5,5,5,5,5,5,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,5,5,5,5,5,5,6,6,6,6,6,7,7,7,7,7,8,8,8,8,9,9,9,9,10,10,10,10,11,11,11,12,12,12,13,13,13,13,14,14,14,15,15,15,16,16,17,17,17,18,18,18,19,19,20,20,20,21,21,22,22,22,23,23,24,24,25,25,25,26,26,27,27,28,28,29,29,30,30,31,31,32,32,33,33,34,34,35,35,36,36,37,37,38,38,39,39,40,40,41,42,42,43,43,44,44,45,46,46,47,47,48,48,49,50,50,51,52,52,53,53,54,55,55,56,56,57,58,58,59,60,60,61,62,62,63,64,64,65,66,66,67,68,68,69,70,70,71,72,72,73,74,74,75,76,77,77,78,79,79,80,81,82,82,83,84,84,85,86,87,87,88,89,89,90,91,92,92,93,94,95,95,96,97,98,98,99,100,101,101,102,103,104,104,105,106,107,107,108,109,110,110,111,112,113,113,114,115,116,117,117,118,119,120,120,121,122,123,123,124,125,126,126,127,128,129,130,130,131,132,133,133,134,135,136,136,137,138,139,140,140,141,142,143,143,144,145,146,146,147,148,149,149,150,151,152,152,153,154,155,155,156,157,158,158,159,160,161,161,162,163,164,164,165,166,166,167,168,169,169,170,171,172,172,173,174,174,175,176,176,177,178,179,179,180,181,181,182,183,183,184,185,185,186,187,187,188,189,190,190,191,191,192,193,193,194,195,195,196,197,197,198,199,199,200,200,201,202,202,203,204,204,205,205,206,206,207,208,208,209,209,210,211,211,212,212,213,213,214,214,215,216,216,217,217,218,218,219,219,220,220,221,221,222,222,223,223,224,224,225,225,226,226,227,227,228,228,228,229,229,230,230,231,231,231,232,232,233,233,234,234,234,235,235,236,236,236,237,237,237,238,238,238,239,239,239,240,240,240,241,241,241,242,242,242,243,243,243,243,244,244,244,245,245,245,245,246,246,246,246,246,247,247,247,247,248,248,248,248,248,248,249,249,249,249,249,249,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,250,249,249,249,249,249,249,248,248,248,248,248,247,247,247,247,247,246,246,246,246,245,245,245,245,244,244,244,244,243,243,243,242,242,242,241,241,241,241,240,240,240,239,239,239,238,238,237,237,237,236,236,236},
	{19,18,18,18,17,17,17,16,16,15,15,15,14,14,14,13,13,13,13,12,12,12,11,11,11,10,10,10,10,9,9,9,9,8,8,8,8,7,7,7,7,7,6,6,6,6,6,5,5,5,5,5,5,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,5,5,5,5,5,5,6,6,6,6,6,6,7,7,7,7,8,8,8,8,8,9,9,9,9,10,10,10,11,11,11,11,12,12,12,13,13,13,14,14,14,15,15,15,16,16,16,17,17,17,18,18,18,19,19,20,20,20,21,21,22,22,23,23,23,24,24,25,25,26,26,26,27,27,28,28,29,29,30,30,31,31,32,32,33,33,34,34,35,35,36,36,37,37,38,38,39,40,40,41,41,42,42,43,43,44,45,45,46,46,47,48,48,49,49,50,50,51,52,52,53,54,54,55,55,56,57,57,58,59,59,60,61,61,62,63,63,64,65,65,66,67,67,68,69,69,70,71,71,72,73,73,74,75,75,76,77,78,78,79,80,80,81,82,82,83,84,85,85,86,87,88,88,89,90,90,91,92,93,93,94,95,96,96,97,98,99,99,100,101,102,102,103,104,105,105,106,107,108,108,109,110,111,111,112,113,114,114,115,116,117,118,118,119,120,121,121,122,123,124,124,125,126,127,128,128,129,130,131,131,132,133,134,134,135,136,137,137,138,139,140,141,141,142,143,144,144,145,146,147,147,148,149,150,150,151,152,153,153,154,155,156,156,157,158,159,159,160,161,162,162,163,164,165,165,166,167,167,168,169,170,170,171,172,172,173,174,175,175,176,177,177,178,179,180,180,181,182,182,183,184,184,185,186,186,187,188,188,189,190,190,191,192,192,193,194,194,195,196,196,197,198,198,199,199,200,201,201,202,202,203,204,204,205,206,206,207,207,208,208,209,210,210,211,211,212,212,213,214,214,215,215,216,216,217,217,218,218,219,219,220,220,221,221,222,222,223,223,224,224,225,225,226,226,227,227,228,228,229,229,229,230,230,231,231,232,232,232,233,233,234,234,234,235,235,236,236,236,237,237,237,238,238,239,239,239,240,240,240,241,241,241,241,242,242,242,243,243,243,244,244,244,244,245,245,245,245,246,246,246,246,247,247,247,247,247,248,248,248,248,248,249,249,249,249,249,249,250,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,249,249,249,249,249,249,248,248,248,248,248,248,247,247,247,247,246,246,246,246,246,245,245,245,245,244,244,244,243,243,243,243,242,242,242,241,241,241,240,240,240,239,239,239,238,238,238,237,237,237,236,236,236,235,235,234,234,234,233,233,232,232,231,231,231,230,230,229,229,228,228,228,227,227,226,226,225,225,224,224,223,223,222,222,221,221,220,220,219,219,218,218,217,217,216,216,215,214,214,213,213,212,212,211,211,210,209,209,208,208,207,206,206,205,205,204,204,203,202,202,201,200,200,199,199,198,197,197,196,195,195,194,193,193,192,191,191,190,190,189,188,187,187,186,185,185,184,183,183,182,181,181,180,179,179,178,177,176,176,175,174,174,173,172,172,171,170,169,169,168,167,166,166,165,164,164,163,162,161,161,160,159,158,158,157,156,155,155,154,153,152,152,151,150,149,149,148,147,146,146,145,144,143,143,142,141,140,140,139,138,137,136,136,135,134,133,133,132,131,130,130,129,128,127,126,126,125,124,123,123,122,121,120,120,119,118,117,117,116,115,114,113,113,112,111,110,110,109,108,107,107,106,105,104,104,103,102,101,101,100,99,98,98,97,96,95,95,94,93,92,92,91,90,89,89,88,87,87,86,85,84,84,83,82,82,81,80,79,79,78,77,77,76,75,74,74,73,72,72,71,70,70,69,68,68,67,66,66,65,64,64,63,62,62,61,60,60,59,58,58,57,56,56,55,55,54,53,53,52,52,51,50,50,49,48,48,47,47,46,46,45,44,44,43,43,42,42,41,40,40,39,39,38,38,37,37,36,36,35,35,34,34,33,33,32,32,31,31,30,30,29,29,28,28,27,27,26,26,25,25,25,24,24,23,23,22,22,22,21,21,20,20,20,19},
}; */

const uint8_t M1[3][1024] PROGMEM = {
	{127,134,141,148,152,154,156,157,158,159,160,160,161,162,163,163,164,165,166,166,167,167,168,168,169,169,170,170,171,171,172,172,173,173,173,174,174,175,175,175,176,176,176,177,177,178,178,179,179,179,180,180,180,181,181,181,182,182,182,182,183,183,184,184,184,185,185,185,186,186,186,187,187,187,188,188,188,189,189,189,190,190,190,191,191,191,192,192,192,193,193,193,194,194,194,195,195,196,196,196,197,197,197,198,198,199,199,199,200,200,200,201,201,201,202,202,202,203,203,203,204,204,204,205,205,205,205,206,206,206,207,207,208,208,208,209,209,209,210,210,211,211,211,212,212,213,213,213,214,214,215,215,216,216,217,217,217,218,218,219,220,220,221,222,223,225,228,232,236,240,241,245,245,246,246,246,246,247,247,247,247,248,248,248,248,248,248,249,249,249,249,249,249,249,250,250,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,250,250,250,249,249,249,249,249,249,249,249,248,248,248,248,248,248,248,247,247,247,247,247,247,247,246,246,246,246,245,244,243,241,239,237,235,231,229,227,225,224,224,223,222,222,222,221,221,220,220,219,219,218,218,218,217,217,217,216,216,216,215,215,215,214,214,214,213,213,213,212,212,212,212,212,211,211,211,210,210,209,209,209,208,208,208,207,207,207,206,206,205,205,205,204,204,204,204,203,203,203,202,202,201,201,201,200,200,199,199,199,198,198,198,197,197,197,196,196,196,195,195,194,194,194,193,193,193,192,192,191,191,191,190,190,190,189,189,189,188,188,187,187,187,186,186,185,185,185,184,184,183,183,182,182,181,181,181,180,180,179,179,178,178,177,177,177,176,175,175,175,174,174,173,173,172,172,171,171,170,170,169,169,168,168,167,167,166,166,166,165,165,164,164,163,163,162,162,162,161,161,160,160,159,159,158,158,158,157,156,156,155,155,154,153,153,152,151,151,150,149,148,148,147,146,145,144,142,139,135,129,124,115,106,99,98,97,96,95,94,93,93,92,91,91,90,90,89,89,88,88,88,87,87,87,86,86,85,85,84,84,84,83,83,82,82,81,81,80,80,80,79,79,78,78,78,77,77,76,76,76,75,75,75,74,74,74,73,73,73,72,72,72,71,71,71,70,70,69,69,69,68,68,68,67,67,66,66,65,65,65,64,64,63,63,63,62,62,61,61,61,60,60,60,59,59,58,58,57,57,57,56,56,55,55,55,54,54,53,53,53,52,52,52,51,51,50,50,50,49,49,49,48,48,47,47,47,46,46,46,45,45,45,45,44,44,43,43,43,43,42,42,42,41,41,40,40,40,39,39,38,38,37,37,37,36,36,35,35,34,34,33,33,32,31,29,25,22,19,15,12,11,9,9,8,8,8,7,7,7,6,6,6,6,6,5,5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,6,6,6,6,6,6,7,7,7,7,8,8,9,10,11,13,16,19,23,26,29,29,30,30,31,31,32,32,32,33,33,34,34,34,35,35,36,36,36,37,37,37,38,38,38,39,39,40,40,40,41,41,42,42,42,43,43,43,44,44,45,45,46,46,46,47,47,47,48,49,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,58,58,58,58,59,59,59,60,60,60,61,61,61,62,62,62,62,63,63,63,64,64,64,65,65,65,66,66,67,67,67,68,68,69,69,69,70,70,71,71,72,72,72,73,73,73,74,74,75,75,76,76,77,77,78,78,79,79,79,80,80,81,81,81,82,82,83,83,84,84,85,85,86,86,87,87,88,88,88,89,90,90,91,92,92,93,93,93,94,95,95,96,96,97,97,98,99,99,100,100,101,101,102,103,103,104,105,105,106,107,108,108,110,111,114,121},
	{235,231,228,223,220,219,218,217,217,216,215,215,214,213,213,212,212,211,211,210,210,209,209,209,208,208,207,207,207,206,206,205,205,205,204,204,204,203,203,203,202,202,202,201,201,200,200,200,199,199,199,199,198,198,198,197,197,197,197,196,196,196,195,195,195,194,194,194,193,193,193,192,192,192,191,191,191,190,190,190,189,189,189,188,188,188,187,187,187,186,186,186,185,185,184,184,184,183,183,182,182,182,181,181,180,180,180,179,179,178,178,178,177,177,176,176,176,175,175,175,174,174,173,173,173,173,172,172,171,171,170,170,169,169,168,168,168,167,167,166,166,165,165,164,164,163,163,162,161,161,160,160,159,158,158,157,156,156,155,154,153,152,151,150,148,145,140,133,126,117,113,105,103,102,101,100,99,98,97,97,96,95,94,94,93,93,92,91,91,90,89,89,88,87,87,86,86,85,85,84,83,83,83,82,81,81,80,80,79,79,78,77,77,76,76,75,75,74,74,73,73,72,72,71,71,70,70,70,69,69,68,68,67,67,66,66,66,65,65,64,64,63,63,62,62,62,61,61,60,60,59,59,58,58,58,57,57,56,56,55,55,54,54,53,53,52,52,51,51,51,50,50,49,49,48,48,47,47,46,46,46,45,45,44,44,44,43,43,42,42,42,41,41,40,40,39,39,39,38,38,38,37,37,36,36,36,35,35,34,34,33,31,29,26,23,21,18,15,13,12,11,10,10,10,9,9,9,9,8,8,8,8,8,7,7,7,7,7,7,7,6,6,6,6,6,6,6,6,5,5,5,5,5,5,5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,5,5,6,6,6,6,6,6,6,6,7,7,7,7,7,7,7,8,8,8,8,8,9,9,9,9,10,10,10,11,11,11,12,13,15,18,20,25,31,36,36,37,38,38,39,39,40,41,41,41,42,42,43,43,43,44,44,44,44,45,45,45,46,46,47,47,47,48,48,48,49,49,49,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,58,58,58,59,59,59,60,60,60,61,61,62,62,62,63,63,64,64,64,65,65,66,66,66,67,67,68,68,68,69,69,70,70,70,71,71,72,72,73,73,74,74,75,75,75,76,76,77,77,78,78,78,79,79,80,80,81,81,81,82,82,83,83,84,84,85,85,85,86,86,86,87,87,88,88,89,89,89,90,90,91,91,92,92,93,93,94,94,95,96,96,97,97,98,99,99,100,101,102,102,103,104,105,108,115,120,126,136,141,146,149,152,153,154,155,156,157,158,159,160,160,161,162,162,163,164,164,165,166,166,167,167,168,168,169,170,170,171,171,172,172,173,173,174,174,175,175,176,176,177,177,178,178,179,179,179,180,180,181,181,181,182,182,182,183,183,184,184,184,185,185,185,186,186,187,187,187,188,188,188,189,189,190,190,191,191,192,192,192,193,193,194,194,194,195,195,196,196,197,197,198,198,199,199,200,200,201,201,201,202,202,203,203,203,204,204,205,205,206,206,207,207,207,207,208,208,209,209,209,210,210,210,211,211,212,212,213,213,214,214,214,215,215,216,216,217,217,218,219,220,221,223,225,228,232,235,239,241,243,243,244,244,244,245,245,245,245,245,246,246,246,246,246,246,247,247,247,247,247,247,247,248,248,248,248,248,248,248,248,249,249,249,249,249,249,249,249,250,250,250,250,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,250,250,250,250,249,249,249,249,249,249,249,249,248,248,248,248,248,248,248,247,247,247,247,247,247,246,246,246,246,246,246,245,245,245,245,244,244,244,244,243,243,242,241,238},
	{19,15,13,10,8,8,7,7,7,6,6,6,6,6,5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,6,6,6,6,6,6,7,7,7,7,7,7,8,8,8,9,9,10,11,13,16,19,24,26,31,33,33,34,35,35,36,37,37,38,39,39,39,40,40,41,41,42,42,42,43,43,44,45,45,45,46,46,47,47,48,48,49,49,50,50,51,51,51,52,53,53,53,54,54,55,55,56,56,57,57,57,58,58,59,59,60,60,60,61,61,62,62,63,63,63,64,64,65,65,66,66,67,67,68,68,68,69,69,70,70,71,71,72,72,73,73,74,74,75,75,76,76,77,77,78,79,79,80,80,81,81,82,82,83,84,84,84,85,85,86,87,87,88,88,89,89,90,90,91,91,92,92,93,94,94,95,95,96,96,97,97,98,98,99,99,100,101,101,103,106,110,114,119,124,128,134,139,143,145,146,147,148,149,150,150,151,152,152,153,154,155,155,156,156,157,157,158,158,159,159,159,160,160,161,161,162,162,163,163,163,164,164,164,165,165,165,166,166,167,167,168,168,169,169,169,170,170,171,171,172,172,172,173,173,174,174,174,175,175,175,176,176,177,177,178,178,179,179,179,180,180,180,181,181,182,182,182,183,183,184,184,185,185,185,186,186,186,187,187,188,188,188,189,189,189,190,190,190,191,191,192,192,192,193,193,193,194,194,195,195,196,196,196,197,197,198,198,198,199,199,200,200,200,201,201,202,202,203,203,203,204,204,204,205,205,206,206,206,207,207,208,208,208,209,209,210,210,210,211,211,211,212,212,212,213,213,213,214,214,214,215,215,215,216,216,216,217,217,217,218,218,219,219,220,220,221,221,222,222,223,223,223,224,225,225,226,227,229,231,234,237,241,244,247,247,247,247,248,248,248,248,248,249,249,249,249,249,249,249,249,249,250,250,250,250,250,250,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,250,250,250,250,250,250,249,249,249,249,249,249,249,249,249,249,248,248,248,248,248,248,247,247,247,247,247,247,246,246,246,246,245,245,245,245,243,241,239,236,231,227,225,222,221,220,219,219,218,217,216,216,215,215,214,214,213,213,212,212,211,211,210,210,209,209,208,208,207,207,207,206,206,205,205,205,204,204,203,203,202,202,201,201,201,200,200,199,199,199,198,198,198,197,197,197,196,196,196,195,195,195,194,194,193,193,193,192,192,192,191,191,191,190,190,189,189,188,188,187,187,187,186,186,185,185,184,184,184,183,183,182,181,181,180,180,179,179,178,178,177,177,176,176,175,175,175,174,174,173,172,172,171,171,170,170,170,169,169,168,168,167,167,166,166,165,165,164,164,163,162,162,161,161,160,159,159,158,157,156,155,154,153,152,148,145,140,133,127,119,114,110,108,108,107,106,105,105,104,103,103,102,102,101,101,100,99,99,98,98,97,97,96,96,95,95,94,94,93,93,92,92,91,91,90,90,89,89,88,88,87,87,86,86,85,85,84,84,83,83,82,82,81,81,80,80,80,79,79,78,78,78,77,77,76,76,76,75,75,75,74,74,74,73,73,73,72,72,72,71,71,71,70,70,70,69,69,69,68,68,68,67,67,67,67,66,66,66,65,65,65,64,64,64,63,63,62,62,62,61,61,61,60,60,59,59,59,58,58,57,57,57,56,56,56,55,55,54,54,53,53,53,52,52,51,51,51,50,50,50,49,49,49,48,48,48,47,47,46,46,45,45,45,44,44,44,43,43,42,42,41,41,41,40,40,40,39,39,38,38,37,37,37,36,36,35,35,35,34,34,33,33,33,32,32,31,31,30,30,29,28,28,26,22},
};

static uint16_t i = 0;

uint16_t stepforwardM1(){
   if (i == 1023) {
      i = 0;
	  //PORTC ^= (1 << PC1);
   }
   else{
      i++;
   }
   OCR1A = pgm_read_byte(&(M1[0][i]));; 
   OCR1B = pgm_read_byte(&(M1[1][i]));; 
   OCR2A = pgm_read_byte(&(M1[2][i]));; 
   return i;
}

uint16_t stepbackwardM1(){
   if (i == 0) {
      i = 1023;
	  //PORTC ^= (1 << PC1);
   }
   else{
      i--;
   }
   OCR1A = pgm_read_byte(&(M1[0][i]));; 
   OCR1B = pgm_read_byte(&(M1[1][i]));; 
   OCR2A = pgm_read_byte(&(M1[2][i]));; 
   return i;
}

void stepM1(int16_t *steps){
	if (*steps > 0){
		stepforwardM1();
		(*steps)--;
	}
	if (*steps < 0){
		stepbackwardM1();
		(*steps)++;
	}
}

int main(void) {
   // set clock divider to /1
   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
   //
   DDRB = (1 << PB1) | (1 << PB2) | (1 << PB3);
   DDRC = (1 << PC1);
   
   GTCCR = (1<<TSM) | (1<<PSRSYNC) | ((0<<PSRASY)); // halt timers; (1<<PSRASY) halt timer2, (1<<PSRSYNC) halt timer 0&1
   
   // TCCR0A |= (1 << WGM00); // phase correct 8bits mode
   // //TCCR0B |= (1 << CS00); //1 prescale
   // TCCR0B |= (1 << CS01);
   // TIMSK0 |= (1 << OCIE0A)|(1 << OCIE0B); // compare A match interrupt enable
   // OCR0A = Cmm[0]; // compare value, 8bit
   // OCR0B = Cpm[0];
   
   TCCR1A |= (1 << COM1A1) | (1<< COM1A0) | (1 << COM1B1) | (1<< COM1B0);
   TCCR1A |= (1 << WGM10); // phase correct 8bits mode
   TCCR1B |= (1 << CS10); //1 prescale
   //TIMSK1 |= (1 << OCIE1A) | (1 << OCIE1B); // compare A match interrupt enable
   OCR1A = M1[0][0];//Cmm[0]; // compare value, 16bit
   OCR1B = M1[1][0];

   TCCR2A |= (1 << COM2A1) | (1<< COM2A0) | (1 << COM2B1) | (1<< COM2B0);//set on upcounting, clear on down couting
   TCCR2A |= (1 << WGM20); // phase correct 8bits mode
   //TCCR0B |= (1 << CS00); //1 prescale
   TCCR2B |= (1 << CS20);
   //TIMSK2 |= (1 << OCIE2A) | (1 << OCIE2B); // compare A match interrupt enable
   OCR2A = M1[2][0]; // compare value, 8bit
   //OCR2B = Cpp[0];
   
   //TCNT0= 0x00;
   TCNT1L= 0x00;
   TCNT1H= 0x00;
   TCNT2= 0x00;
   //PORTC &= ~(1 << PC0) & ~(0 << PC1) & ~(0 << PC2);
   
   GTCCR = 0;//release all timer
   
   stdout = &mystdout;// setup stdio stream
   uart_init();
   //sei(); // interrupt enable
   
   uint8_t count = 0;
   int16_t steps = 0;
   double feedAcZ[5];
   double feedGyroX[5];
   uint16_t j = 0;
   double setpoint = 0.0;
   uint16_t speed = 1;
   
   int16_t AcX;
   int16_t AcY;
   int16_t AcZ;
   int16_t GyroX;
   int16_t GyroY;
   int16_t GyroZ;
   MPU_init();
   MPU_write(0x1A, 0);
   //MPU_write(0x1B, (2 << 3));
   
   while (1) {
        //uint16_t count = stepforwardM1();
/* 	    MPU_read(0x3B,&AcX);
		MPU_read(0x3D,&AcY); */
		//MPU_read(0x3F,&AcZ);
		
		//MPU_read(0x43,&GyroX);
		//MPU_read(0x45,&GyroY);
	    // */
 		//printf("%" PRIu16 ",",count);
		//printf("%" PRId16 ",",AcX);
		//printf("%" PRId16 "\n",AcY); 
		if (j >= sensormotor) {
			MPU_read(0x3F,&AcZ);
			MPU_read(0x43,&GyroX);
			//printf("%" PRId16 "\n",GyroZ); 
			feedAcZ[5] = feedAcZ[4];
			feedAcZ[4] = feedAcZ[3];
			feedAcZ[3] = feedAcZ[2];
			feedAcZ[2] = feedAcZ[1];
			
			feedGyroX[5] = feedGyroX[4];
			feedGyroX[4] = feedGyroX[3];
			feedGyroX[3] = feedGyroX[2];
			feedGyroX[2] = feedGyroX[1];
			
			feedAcZ[1] = AcZ*4.0/65536.0*1048.0*7.0/3.14/2;
			feedGyroX[1] = (GyroX+500)*500.0/65536.0*interval*sensormotor/1000000.0*1048.0*7.0/360.0;
/* 			if (feedGyroX[1] > 30.0) feedGyroX[1] = 30.0;//250*interval*sensormotor(s)*20.4~5000*sensor time
			else if (feedGyroX[1] < -30.0) feedGyroX[1] = -30.0;  */
			//seems sensitivity is 500deg*1.3 instead of 500deg
			if ((GyroX > 32760) | (GyroX <  -32760)) setpoint = feedAcZ[1]*0.02+GyroX*500.0/65536.0*interval*sensormotor/1000000.0*1048.0*7.0/360.0*30;
			else setpoint = -feedAcZ[1]*0.1+(feedAcZ[1]-feedAcZ[2])*0.0+feedGyroX[1]*30+(feedGyroX[1]-feedGyroX[2])*70;
			steps = (int) setpoint;
			if (steps >= sensormotor) speed=1;
			else speed = sensormotor/abs(steps);
			printf("%" PRId16 "\n",GyroX); 
			j=0;
		}
		else{
			j++;
		}
		if (j % speed == 0) stepM1(&steps);
	   _delay_us(interval);
   }
}
// ISR(TIMER0_COMPA_vect){
//  	if ((TIFR1 | ~(1 << TOV1)) == 0){
// 		PORTA |= (1 << PA2);
// 		TIFR1 |= (1 << TOV1);
// 	}else{
// 		PORTA &= ~(1 << PA2);
// 	} 
// 	//PORTC ^= (1 << PC0);
// }
// ISR(TIMER0_COMPB_vect){
// 	//PORTC ^= (1 << PC1);
// }
// ISR(TIMER1_COMPA_vect){
// 	//PORTC ^= (1 << PC2); 
// }