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

#define TIMEOUT 0x2FF

#define N_TRIGGERS 3
#define N_SENSORS 12
#define N_TRIG_SENSORS 4

static int16_t distances[N_SENSORS];

static void pull_trigger(uint8_t trig)
{
	switch (trig) {
	case 0: PORTD |= _BV(PD7); _delay_us(10); PORTD &= ~_BV(PD7); break;
	case 1: PORTB |= _BV(PB4); _delay_us(10); PORTB &= ~_BV(PB4); break;
	case 2: PORTC |= _BV(PC4); _delay_us(10); PORTC &= ~_BV(PC4); break;
	}
}

static uint16_t get_pin(uint8_t trig)
{
	switch (trig) {
	case 0: return (PIND & 0x78) >> 3;
	case 1: return PINB & 0x0F;
	default: return PINC & 0x0F;
	}
}

static void do_measurement(unsigned char trig)
{
	uint16_t starttimes[N_TRIG_SENSORS], starttime;
	uint8_t to_start = (1 << N_TRIG_SENSORS) - 1;
	uint8_t to_measure = 0, i;

	pull_trigger(trig);

	starttime = TCNT1;

	while (to_start || to_measure) {
		uint8_t bits = 0;
		uint16_t now = TCNT1;

		if (now-starttime >= TIMEOUT)
			break;

		bits = get_pin(trig);

		for (i = 0; i < N_TRIG_SENSORS; i++) {
			uint8_t mask = 1 << i;

			if ((to_start & mask) && (bits & mask)) {
				// echo start
				starttimes[i] = now;
				to_start &= ~mask;
				to_measure |= mask;
			} else if ((to_measure & mask) && !(bits & mask)) {
				// echo end
				to_measure &= ~mask;
				distances[trig*N_TRIG_SENSORS + i]
					= now - starttimes[i];
			}
		}
	}

	for (i = 0; i < N_TRIG_SENSORS; i++)
		if (to_start & (1 << i))
			distances[trig*N_TRIG_SENSORS + i] = -1;
		else if (to_measure & (1 << i))
			distances[trig*N_TRIG_SENSORS + i] = 0;
}

static void do_measurements()
{
	uint8_t trig;

	for (trig = 0; trig < N_TRIGGERS; trig++) {
		do_measurement(trig);
		_delay_ms(200);
	}
}

static void led_set(uint8_t led, uint8_t state)
{
	if (led == 0) {
		if (state) {
			PORTD |= _BV(PD4);
			// PORTC |= _BV(PC5);
		} else {
			PORTD &= ~_BV(PD4);
			// PORTC &= ~_BV(PC5);
		}
	} else {
		if (state) {
			PORTB |= _BV(PB5);
		} else {
			PORTB &= ~_BV(PB5);
		}
	}
}

int main()
{
	modbus_init();

	// output pins
	DDRD |= _BV(PD7); // Trig D
	DDRB |= _BV(PB4) | _BV(PB5); // Trig B, LED 2
	DDRC |= _BV(PC4) | _BV(PC5); // Trig C, LED 1
	// temporary LED
	DDRD |= _BV(PD4);

	// set up the timer
	TCCR1A = 0;
	TCCR1B = _BV(CS12)|_BV(CS10); // CLK/1024

	// enable interrupts
	sei();

	while(1) {
		// do_measurements();
		modbus_poll();
		led_set(0,
			distances[4] > 100 || distances[11] > 100);
	}
}