#include <inttypes.h>
#include <avr/io.h>
#include <avr/twi.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <usart.h>
#include <i2c.h>
#include <srf08.h>
#include <delay.h>

#define FOSC 14745600L // Crystal frequency in Hz.

#if defined(__AVR_ATmega32__) || defined(__AVR_ATmega16)
#	define PORTSPI		PORTB
#	define DDRSPI		DDRB
#	define PMISO		PB6
#	define PMOSI		PB5
#	define PSCK			PB7
#	define PSS			PB4
#	define SIG_SS_INT 	SIG_INTERRUPT2
#	define SS_INT_PIN	INT0
#elif defined(__AVR_ATmega8__)
#	define PORTSPI		PORTB
#	define DDRSPI		DDRB
#	define PMISO		PB4
#	define PMOSI		PB3
#	define PSCK			PB5
#	define PSS			PB2
#	define SIG_SS_INT 	SIG_INTERRUPT0
#	define SS_INT_PIN	INT0
#endif

#define MAX_SENSORS 8
#define MAX_BYTES	MAX_SENSORS * 2
static uint8_t pingVals[MAX_BYTES];  // 16 bytes to store up to 8 sensor values.

static uint8_t SPIByteCounter;

// Handle the SS line seperatly.  
// This ISR is used to initialize the SPI state machine when we are selected,
// and cleanup when we are deselected (if there is any cleanup)
SIGNAL(SIG_SS_INT)
{
	SPIByteCounter = 0;
}

// Handle the SPI communications here.  
SIGNAL(SIG_SPI)
{
	SPDR = pingVals[SPIByteCounter++];
	if (SPIByteCounter >= MAX_BYTES)
		SPIByteCounter = 0;
}

void
SPIInit(void)
{
	// Configure the interrupt line connected to the SS pin.
	MCUCR &= ~(_BV (ISC00) | _BV (ISC01)); // Clear config
	MCUCR |= _BV (ISC01); // Make edge sensitive, falling edge
	GIFR |= _BV(INTF0);   // Clear flag (just in case)
	GICR |= _BV(INT0);	  // Enable interrupt 0
	
	// Configure the SPI as slave
	DDRSPI |= _BV(PMISO); // Make MISO an ouput.
	SPCR = _BV (SPIE) | _BV (SPE); //Enable SPI
}
	
int
main (void)
{
	int32_t retval;
	int32_t delay;
	uint8_t pingdata[2];
	int8_t sensor;

	DDRA = _BV(DDA0);

	//USART_Init(191); // 9600
	USART_Init(15); // 115200
	i2c_init();
	SPIInit();

	sei();

	USART_PrintS("Testing\n");
	fdevopen(USART_Transmit, USART_Receive, 0);
	printf("Test2\n");

	printf(""); // Clear screen
	retval = 1000000;
	while (retval--);
	//retval = srf08_change_address(1, 3);
	printf("retval: %ld\n", retval);

#ifdef DELAY_TEST
	while (1)
	{
		PORTA |= _BV (PA0);
		delay = 5;
		while (delay--);
		PORTA &= ~(_BV(PA0));
	}
#endif

	printf("Entering forever.\n");	
	while (1)
	{
		printf("Sending global ping.\n");
		srf08_ping(0);

		printf("Polling for ready.\n");
		do
		{
			//delay = 10;
			//while (delay--);
			retval = srf08_read(1, 0, pingdata, 2);
			//printf("%d ", retval);
		} while (retval != 2);
		printf("Gathering ping data from sensors.\n");	

		printf(""); // Home cursor
#if 1	
		// Scan the sensors, from 0 (1) up to MAX_SENSORS
		for(sensor = 0; sensor < MAX_SENSORS; sensor++)
		{
			printf("Retriving ping data for sensor %d:\n", sensor);
			pingdata[0] = pingVals[(sensor * 2)];
			pingdata[1] = pingVals[(sensor * 2) + 1];

			srf08_read((sensor + 1), 2, pingdata, 2);
			
			cli();
			pingVals[(sensor * 2)]     = pingdata[0];
			pingVals[(sensor * 2) + 1] = pingdata[1];
			sei();

			delay = 10;
			while(delay--);	
			printf("Ping %d: %d\n", sensor, (uint16_t)(pingdata[0] << 8 | pingdata[1]));
		}

#else	
		pingdata[0] = pingVals[0];
		pingdata[1] = pingVals[1];

		srf08_read(1, 2, pingdata, 2);
		
		cli();
		pingVals[0] = pingdata[0];
		pingVals[1] = pingdata[1];
		sei();

		delay = 10;
		while(delay--);	

		pingdata[0] = pingVals[2];
		pingdata[1] = pingVals[3];

		srf08_read(2, 2, pingdata, 2);
		
		cli();
		pingVals[2] = pingdata[0];
		pingVals[3] = pingdata[1];
		sei();

		delay = 10;
		while(delay--);	

		pingdata[0] = pingVals[4];
		pingdata[1] = pingVals[5];

		srf08_read(3, 2, pingdata, 2);
		
		cli();
		pingVals[4] = pingdata[0];
		pingVals[5] = pingdata[1];
		sei();

		delay = 10;
		while(delay--);	

		pingdata[0] = pingVals[6];
		pingdata[1] = pingVals[7];

		srf08_read(4, 2, pingdata, 2);
		
		cli();
		pingVals[6] = pingdata[0];
		pingVals[7] = pingdata[1];
		sei();

		delay = 10;
		while(delay--);	

		pingdata[0] = pingVals[8];
		pingdata[1] = pingVals[9];

		srf08_read(5, 2, pingdata, 2);
		
		cli();
		pingVals[8] = pingdata[0];
		pingVals[9] = pingdata[1];
		sei();

		delay = 10;
		while(delay--);	

		pingdata[0] = pingVals[10];
		pingdata[1] = pingVals[11];

		srf08_read(6, 2, pingdata, 2);
		
		cli();
		pingVals[10] = pingdata[0];
		pingVals[11] = pingdata[1];
		sei();

		delay = 10;
		while(delay--);	

		pingdata[0] = pingVals[12];
		pingdata[1] = pingVals[13];

		srf08_read(7, 2, pingdata, 2);
		
		cli();
		pingVals[12] = pingdata[0];
		pingVals[13] = pingdata[1];
		sei();

		delay = 10;
		while(delay--);	

		pingdata[0] = pingVals[14];
		pingdata[1] = pingVals[15];

		srf08_read(8, 2, pingdata, 2);
		
		cli();
		pingVals[14] = pingdata[0];
		pingVals[15] = pingdata[1];
		sei();
#endif

		printf("Ping1: %d\n", (uint16_t)(pingVals[0] << 8 | pingVals[1]));
		printf("Ping2: %d\n", (uint16_t)(pingVals[2] << 8 | pingVals[3]));
		printf("Ping3: %d\n", (uint16_t)(pingVals[4] << 8 | pingVals[5]));

	}
		
    return (0);
}

// vim: ts=4
// vim: sw=4