/*
 * FEKTBOT___jizda_po_care.c
 *
 * Created: 2.10.2013 16:36:15
 *  Author: Tomáš Feik
 */ 

#ifndef F_CPU
	#define F_CPU 16000000UL  // 16 MHz
#endif

// Zakomentovanim "#define PSD" se modul chova jako stavovy automat
//#define PSD

#ifndef TF_MAIN
	#define HMC5883L_ADRESS (0x3C)
	
	#define HMC5883L_X_AXIS_MSB_ADRESS (0x03)
	#define HMC5883L_Y_AXIS_MSB_ADRESS (0x07)
	#define HMC5883L_Z_AXIS_MSB_ADRESS (0x05)
	
	#define SRF08_L_ADRESS (0xE0)
	#define SRF08_M_ADRESS (0xE2)
	#define SRF08_R_ADRESS (0xE4)
	
	#define TF_MAIN
#endif

#include <avr/io.h>
#include <avr/interrupt.h> 
#include <util/delay.h>
#include <stdio.h>
#include <math.h>
#include "twi_lib.h"
#include "uart.h"
#include "function.h"

volatile unsigned int measureSRF, isRunning;
volatile unsigned int barrierDistance, barrierDistanceL, barrierDistanceM, barrierDistanceR; // promenne pro ulozeni vzdalenosti nejblizsiho echa
volatile int hmc5883l_x_value, hmc5883l_z_value, hmc5883l_prev_x_value, hmc5883l_prev_z_value;
volatile float sumI;
/* 
 * Rutina preruseni casovace 0
 * 
 * Cteni dat ze sonaru SRF 08
 *
 * T = 1 / (16Mhz / 1024) = 64 us
 * 4.5ms/64us = 70.3125 => 71
 */
ISR (TIMER0_OVF_vect)
{
	switch (measureSRF) 
	{
		case ' ':
			SRF_start(SRF08_L_ADRESS);
			measureSRF = 'L';
			break;
		case 'L':
			barrierDistanceL = SRF_read(SRF08_L_ADRESS);
//			SRF_start(SRF08_M_ADRESS);
			measureSRF = 'L';
			break;
		case 'M':
//			barrierDistanceM = SRF_read(SRF08_M_ADRESS);
//			SRF_start(SRF08_R_ADRESS);
			measureSRF = 'R';
			break;
		case 'R':
//			barrierDistanceR = SRF_read(SRF08_R_ADRESS);
			SRF_start(SRF08_L_ADRESS);
			measureSRF = 'L';
			
//			barrierDistance = findMin(barrierDistanceL, barrierDistanceM, barrierDistanceR);
			barrierDistance = barrierDistanceL;
			break;
	}
	TCNT0 = 156;
}
 
/* 
 * Rutina přerušení časovače 1 pri shode OCR1A a TCNT1
 * 
 * Čtení dat z kompasu HMC5883L
 *
 * 10ms
 */
ISR (TIMER1_COMPA_vect)
{
	hmc5883l_prev_x_value = hmc5883l_x_value;
	hmc5883l_prev_z_value = hmc5883l_z_value;
	
	#ifdef PSD
		HMC5583L_UpdateValue(hmc5883l_x_value, HMC5883L_Read(HMC5883L_ADRESS, HMC5883L_X_AXIS_MSB_ADRESS));
		HMC5583L_UpdateValue(hmc5883l_z_value, HMC5883L_Read(HMC5883L_ADRESS, HMC5883L_Z_AXIS_MSB_ADRESS));
	#endif
	
	#ifndef PSD
		hmc5883l_x_value = HMC5883L_Read(HMC5883L_ADRESS, HMC5883L_X_AXIS_MSB_ADRESS);
		hmc5883l_z_value = HMC5883L_Read(HMC5883L_ADRESS, HMC5883L_Z_AXIS_MSB_ADRESS);
	#endif
}

/* 
 * Rutina přerušení časovače 1 pri shode OCR1B a TCNT1
 * 
 * Čtení dat z kompasu HMC5883L
 *
 * 20ms
 */
ISR (TIMER1_COMPB_vect)
{
	#ifdef PSD
		// vyhodnoceni mereni pomoci PSD regulatoru
		evaluateMeasurementPSD(hmc5883l_x_value, hmc5883l_z_value, hmc5883l_prev_x_value, hmc5883l_prev_z_value, modifySpeedValue(barrierDistance), sumI);
	#endif
	
	#ifndef PSD
		// vyhodnoceni mereni jako stavovy automat
		evaluateMeasurementSM(hmc5883l_x_value, hmc5883l_z_value, modifySpeedValue(barrierDistance));
	#endif
}

/* 
 * Rutina externího přerušení 0 - ZASTAVENÍ
 */
ISR(INT0_vect)
{
	if(isRunning)
	{
		Move_Stop();
		sendEndToMotors();
		isRunning = 0;
	}		
		
	TCNT0 = 156;
	TCNT1 = 0;
}

int main(void)
{
	// Nastaveni portu
	DDRD &= ~(1 << PD2); // PD2 (INT0) vstupní
	PORTD |= (1 << PD2); // vnitřní pull-up resistor PD2

	// T0
	TCCR0 |= (1 << CS02) | (1 << CS00); // preddelicka /1024 (128us)
	TIMSK |= (1 << TOIE0); // přerušení po přetečení časovače 0
	
	// T1
	TCCR1B |= (1 << CS11); // preddelicka /8 (0.5us)
	TIMSK |= (1 << OCIE1A) | (1 << OCIE1B); // přerušení po shode OCR1A a TCNT1 nebo OCR1B a TCNT1
	OCR1A = 19999; // preruseni pri hodnote 20 000
	OCR1B = 39999; // preruseni pri hodnote 40 000
	
	// INT0
	// Nastavení přerušení na hodnotu log. 0 (MCUCR: ISC00 a ISC01 = 0)
	GICR |= (1 << INT0);
			
	measureSRF = ' ';
	barrierDistance = 55;
	hmc5883l_x_value = hmc5883l_z_value = hmc5883l_prev_x_value = hmc5883l_prev_z_value = 0;
	isRunning = 0;
	sumI = 0.0;
	
	i2c_init();
	uart_init();
	HMC5883L_init(HMC5883L_ADRESS);
	SRF_init(SRF08_L_ADRESS);
//	SRF_init(SRF08_M_ADRESS);
//	SRF_init(SRF08_R_ADRESS);

	sei(); // povolení přerušení

    while(1)
    {
	    if(!isRunning)
		{
			sendStartToMotors();
			isRunning = 1;
		}	
    }
}