#define F_CPU 20000000UL    // Baby Orangutan frequency (20MHz)
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

//Leds. Salidas.
#define LEDP PORTD1
#define    LEDV PORTB1
#define    LEDR PORTD0
//Interruptores. Entradas.
#define SW1    PORTB0
#define SW2 PORTB2
//Sensores. Entradas y salidas.
#define    D3 PORTD7
#define D2 PORTD4
#define D1 PORTD2
#define D0 PORTB5
#define I0 PORTB4
#define I1 PORTC3
#define I2 PORTC2
#define I3 PORTC1
#define LED_ON PORTC0

void inicializar_puertos(void);
void reset(void);
void M1_forward(unsigned char pwm);
void M1_reverse(unsigned char pwm);
void M2_forward(unsigned char pwm);
void M2_reverse(unsigned char pwm);
void motors_init(void);
int obtener_errorp(void);
void inicializar_timer1(void);
int obtener_errord(void);

/*********** Ajuste comportamiento robot *********/
//Constantes Regulador PD.
int Kp=10;
int Kd=65;
int velocidad = 100; //Máxima 255.
/*************************************************/

int main( void )
{
//    int errorp = 0;

    inicializar_puertos();
    motors_init();
    reset();
    inicializar_timer1();

    PORTC |= (1<<LED_ON);//Se encienden los sensores.
    
    M1_forward(0);        //Motor derecho.
    M2_forward(0);        //Motor izquierdo.     
    while ( 1 )
    {    
    }
    return 0;
}

void inicializar_puertos(void)
{
   DDRD=0x6B;     //0110 1011  0,1,3,5,6 Salidas
   PORTD=0x00;   
   DDRB=0x0A;     //0000 1010  1,3 Salidas
   PORTB=0x00;
   DDRC=0x01;     //0000 0001  0 Salida
   PORTC=0x00;  
}

void reset(void)
{
    PORTD |= (1<<LEDP); //Encendemos Led en Baby Orangutan.
    _delay_ms(300);
    PORTD &= ~(1<<LEDP);//Apagamos Led en Baby Orangutan.
    _delay_ms(300);
    PORTD |= (1<<LEDP);
    _delay_ms(300);
    PORTD &= ~(1<<LEDP);
    _delay_ms(300);
    PORTD |= (1<<LEDP);
    _delay_ms(300);
    PORTD &= ~(1<<LEDP);
}

//Funciones para controlar la velocidad y dirección de los
//motores. PWM controla la velocidad, valor entre 0-255.
void M1_reverse(unsigned char pwm)
{
    OCR0A = 0;
    OCR0B = pwm;
}
void M1_forward(unsigned char pwm)
{
    OCR0B = 0;
    OCR0A = pwm;
}
void M2_reverse(unsigned char pwm)
{
    OCR2A = 0;
    OCR2B = pwm;
}
void M2_forward(unsigned char pwm)
{
    OCR2B = 0;
    OCR2A = pwm;
}

//Configuración del hardware del micro que controla los motores.
void motors_init(void)
{
    // configure for inverted PWM output on motor control pins:
    // set OCxx on compare match, clear on timer overflow
    // Timer0 and Timer2 count up from 0 to 255
    TCCR0A = TCCR2A = 0xF3;
    // use the system clock/8 (=2.5 MHz) as the timer clock
    TCCR0B = TCCR2B = 0x02;
    // initialize all PWMs to 0% duty cycle (braking)
    OCR0A = OCR0B = OCR2A = OCR2B = 0;
    // set PWM pins as digital outputs (the PWM signals will not
    // appear on the lines if they are digital inputs)
    DDRD |= (1 << PORTD3) | (1 << PORTD5) | (1 << PORTD6);
    DDRB |= (1 << PORTB3);
}

void inicializar_timer1(void) //Configura el timer y la interrupción.
{
    OCR1A= 0x0C35;
    TCCR1B |=((1<<WGM12)|(1<<CS11)|(1<<CS10));    //Los bits que no se tocan a 0 por defecto
    TIMSK1 |= (1<<OCIE1A);
    sei();
}

int obtener_errorp(void)
{
    char errorp=0;
    static char ultimo_errorp=0;
    char contador_sensor=0;

    if(((PINB & 0x10) != 0) && ((PINB & 0x20) != 0))
    {
        errorp=0;
        return(0);
     }

    if((PINC & 0x02) != 0) //I3 PC1 -7
    {
        errorp = errorp - 0x07;
        contador_sensor++;
    }

    if((PINC & 0x04) != 0) //I2 PC2 -5
    {
        errorp = errorp - 0x05;
        contador_sensor++;
    }

    if((PINC & 0x08) != 0) //I1 PC3 -3
    {
        errorp = errorp - 0x03;
        contador_sensor++;
    }

    if((PINB & 0x10) != 0) //I0 PB4 -1
    {
        errorp = errorp - 0x01;
        contador_sensor++;
    }

    if((PINB & 0x20) != 0) //D0 PB5 +1
    {
        errorp = errorp + 0x01;
        contador_sensor++;
    }

    if((PIND & 0x04) != 0) //D1 PD2 +3
    {
        errorp = errorp + 0x03;
        contador_sensor++;
    }

    if((PIND & 0x10) != 0) //D2 PD4 +5
    {
        errorp = errorp + 0x05;
        contador_sensor++;
    }

    if((PIND & 0x80) != 0) //D3 PD7 +7
    {
        errorp = errorp + 0x07;
        contador_sensor++;
    }

    if(contador_sensor != 0)
    {
        errorp = errorp / contador_sensor;
        ultimo_errorp = errorp;
        return(Kp * (int)errorp);
    }
    else
    {
        if(ultimo_errorp < 0)
            errorp = -0x09;
        else
            errorp = 0x09;

        ultimo_errorp = errorp;
        return((int)errorp * Kp);
    }        
}

int obtener_errord(void)
{
    int error = 0;
    static int error_old = 0;
    static int errord=0;
    static int errord_old = 0;
    static int tic = 1;
    static int tic_old = 1;

    int diferencia = 0;

    if(((PINB & 0x10) != 0) && ((PINB & 0x20) != 0))
        error=0;

    else if((PINB & 0x20) != 0) //D0 PB5 +1
        error = 1;

    else if((PINB & 0x10) != 0) //I0 PB4 -1
        error = -1;

    else if((PIND & 0x04) != 0) //D1 PD2 +3
        error = 3;

    else if((PINC & 0x08) != 0) //I1 PC3 -3
        error = -3;

    else if((PIND & 0x10) != 0) //D2 PD4 +5
        error = 5;

    else if((PINC & 0x04) != 0) //I2 PC2 -5
        error = -5;

    else if((PIND & 0x80) != 0) //D3 PD7 +7
        error = 7;

    else if((PINC & 0x02) != 0) //I3 PC1 -7
        error = -7;

    else
        {
            if (error_old < 0)
                error = -9;
            else if(error_old > 0)
                error = 9;
        }

    //Cálculo de la velocidad media del error.
    if (error == error_old)
    {
        tic = tic + 1;
        if(tic > 30000)
            tic = 30000;
        if(tic > tic_old)
            errord = errord_old/tic;
    }
    else
    {
        diferencia = error - error_old;
        errord = Kd*(diferencia)/tic; //error medio
        errord_old = errord;
        tic_old=tic;
        tic=1;
    }

    error_old = error;
    return(errord);
}

ISR(TIMER1_COMPA_vect)
{
    PORTD |=(1<<LEDP);

    int errort=0;
    int proporcional = obtener_errorp();
    int derivativo = obtener_errord();

    errort = proporcional + derivativo;

    if(errort > velocidad)
        errort = velocidad;
    else if(errort < - velocidad)
        errort = - velocidad;
    
    if(errort>0)
    {
        M1_forward(velocidad - errort);     //Motor derecho.
        M2_forward(velocidad);              //Motor izquierdo.
        PORTB |= (1<<LEDV);
        PORTD &= ~(1<<LEDR);
    }
    else if(errort<0)
    {
        M1_forward(velocidad);              //Motor derecho.  
        M2_forward(velocidad + errort);     //Motor izquierdo.
        PORTD |= (1<<LEDR);
        PORTB &= ~(1<<LEDV);
    }

    else
    {
        M1_forward(velocidad);       
        M2_forward(velocidad);
        PORTB &= ~(1<<LEDV);
        PORTD &= ~(1<<LEDR);
    }

    PORTD &= ~(1<<LEDP);
    TIFR1 |= (1<<OCF1A);
}