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

#define ECHO_TIMEOUT		(CLOCK_HZ/10)	// 100 ms
#define MEASUREMENT_WAIT	(2*ECHO_TIMEOUT)
#define MEASUREMENT_SHIFT	0		// running avg (1 << M_SHIFT)

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

// static int16_t distances[N_SENSORS];

// hold_regs[0] is unit ID
#define thresholds	(hold_regs+1)
#define led1_sensors	(hold_regs[13])
#define led2_sensors	(hold_regs[14])
#define long_as_free	(hold_regs[15])
#define long_thr	(hold_regs[16])

#define led_bitmap	(hold_regs[MB_N_HOLD_REGS_EEPROM])
#define distances	(hold_regs+MB_N_HOLD_REGS_EEPROM+1)
#define free_bitmap	(hold_regs[MB_N_HOLD_REGS_EEPROM+13])
#define err_bitmap	(hold_regs[MB_N_HOLD_REGS_EEPROM+14])
#define long_bitmap	(hold_regs[MB_N_HOLD_REGS_EEPROM+15])
#define max_distances	(hold_regs+MB_N_HOLD_REGS_EEPROM+21)
#define err_counts	(hold_regs+MB_N_HOLD_REGS_EEPROM+41)

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;
	uint16_t now;

	pull_trigger(trig);

	starttime = get_clock();

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

		if (now-starttime >= ECHO_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)) {
#if MEASUREMENT_SHIFT > 0
				uint16_t old_d;
#endif
				uint16_t new_d;
				uint8_t idx = trig*N_TRIG_SENSORS+i;
				// echo end
				to_measure &= ~mask;
				new_d = now - starttimes[i];
				if (new_d > max_distances[idx])
					max_distances[idx] = new_d;

#if MEASUREMENT_SHIFT > 0
				old_d = distances[idx];

				if (old_d == 0
					|| old_d == -1) {
					distances[idx] = new_d;
				} else {
					distances[idx] = (
						(old_d << MEASUREMENT_SHIFT)
						+ new_d
						- old_d
						) >> MEASUREMENT_SHIFT;
				}
#else
				distances[idx] = new_d;
#endif
			}
		}
	}

	for (i = 0; i < N_TRIG_SENSORS; i++) {
		uint8_t off = trig*N_TRIG_SENSORS + i;

		if (to_start & (1 << i)) { // echo not received
			uint16_t err_count = err_counts[off] & 0xFF;
			if (distances[off] != -1 && err_count < 255) {
				err_count++;
				err_counts[off] = (err_counts[off] & 0xFF00)
					| err_count;
			}
			distances[off] = -1;
		} else if (to_measure & (1 << i)) { // echo pulse too long
			uint16_t err_count = err_counts[off] >> 8;

			if (err_count < 255) {
				err_count++;
				err_counts[off] = (err_counts[off] & 0x00FF)
					| (err_count << 8);
			}
			/*
			 * If the echo pulse is too long, do not treat it
			 * as error, just count it as maximum length
			 * and notify the state in the bitmap.
			 */
			distances[off] = now - starttimes[i];
		}
	}
}

static void led_set(uint8_t led, uint8_t state)
{
	if (led == 0) {
		switch (state) {
		case 0:
			led_bitmap &= ~1;
			led_bitmap &= ~2;
			break;
		case 1:
			led_bitmap |= 1;
			led_bitmap &= ~2;
			break;
		default: // error
			led_bitmap |= 2;
			break;
		}
	} else {
		switch (state) {
		case 0:
			led_bitmap &= ~4;
			led_bitmap &= ~8;
			break;
		case 1:
			led_bitmap |= 4;
			led_bitmap &= ~8;
			break;
		default:
			led_bitmap |= 8;
			break;
		}
	}
}

static void leds_update()
{
	if (led_bitmap & 1) {
		PORTC |= _BV(PC5);
	} else {
		PORTC &= ~_BV(PC5);
	}

	if (led_bitmap & 2) {
		DDRC &= ~_BV(PC5);
	} else {
		DDRC |= _BV(PC5);
	}

	if (led_bitmap & 4) {
		PORTB |= _BV(PB5);
	} else {
		PORTB &= ~_BV(PB5);
	}

	if (led_bitmap & 8) {
		DDRB &= ~_BV(PB5);
	} else {
		DDRB |= _BV(PB5);
	}
}

static void eval_bitmaps()
{
	uint16_t free_b = 0, err_b = 0, long_b = 0, mask;
	uint8_t i;

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

		if (thresholds[i]) {
			if (distances[i] == -1) {
				err_b |= mask;
			} else if (distances[i] > thresholds[i]) {
				if (long_thr && distances[i] > long_thr) {
					long_b |= mask;
					if (long_as_free & mask) {
						free_b |= mask;
					}
				} else {
					free_b |= mask;
				}
			}
		}
	}

	free_bitmap = free_b;
	err_bitmap  = err_b;
	long_bitmap = long_b;

	if (led1_sensors) {
		if (led1_sensors & err_bitmap) {
			led_set(0, 2);
		} else if (led1_sensors & free_bitmap) {
			led_set(0, 1);
		} else {
			led_set(0, 0);
		}
	}

	if (led2_sensors) {
		if (led2_sensors & err_bitmap) {
			led_set(1, 2);
		} else if (led2_sensors & free_bitmap) {
			led_set(1, 1);
		} else {
			led_set(1, 0);
		}
	}
}

uint8_t hold_reg_is_valid(uint16_t reg, uint16_t val)
{
        if (reg == MB_HOLD_REGS_BASE) 
                return val > 0 && val <= 247;

        return 1;
}

int main()
{
	modbus_init(0);

	// 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

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

	// enable interrupts
	sei();

	while(1) {
		uint8_t trig;
		for (trig = 0; trig < N_TRIGGERS; trig++) {
			uint16_t now;
			do_measurement(trig);
			now = get_clock();
			while (get_clock()-now < MEASUREMENT_WAIT)
				modbus_poll();
		}

		eval_bitmaps();
		leds_update(); // might be written from modbus
//		led_set(0,
//			distances[4] > 100 || distances[11] > 100);
	}
}