/*
 * labsw.c - Lab Switch initialization and main loop
 *
 * Written 2011 by Werner Almesberger
 * Copyright 2011 Werner Almesberger
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 */


#include <stdint.h>

#include "regs.h"
#include "usb.h"

#include "config.h"
#include "io.h"
#include "labsw/ep0.h"


int local = 1;


#define	DEBOUNCE_CYCLES	10


#define	IS_ON(ch)	(CH##ch##_RELAY || !CH##ch##_OPT)

#define	LED_R_COLOR_R	1
#define	LED_R_COLOR_G	0
#define	LED_R_COLOR_OFF	0
#define	LED_G_COLOR_R	0
#define	LED_G_COLOR_G	1
#define	LED_G_COLOR_OFF	0

#define	LED(ch, color)					\
	do {						\
		LED_##ch##_R = LED_R_COLOR_##color;	\
		LED_##ch##_G = LED_G_COLOR_##color;	\
	} while (0)

#define	TURN(ch, on)			\
	do {				\
		CH##ch##_RELAY = on;	\
		CH##ch##_OPT = !on;	\
		if (on)			\
			LED(CH##ch, R);	\
		else			\
			LED(CH##ch, G);	\
	} while (0)


static void init_io(void)
{
	P0SKIP = 0xff;
	P1SKIP = 0xff;
	P2SKIP = 0xff;

	/* @@@ we need this while using the boot loader of cntr */
	P0MDOUT = 0;
	P1MDOUT = 0;
	P2MDOUT = 0;

	LED_MAIN_R_MODE |= 1 << LED_MAIN_R_BIT;
	LED_MAIN_G_MODE |= 1 << LED_MAIN_G_BIT;
	LED_CH1_R_MODE |= 1 << LED_CH1_R_BIT;
	LED_CH1_G_MODE |= 1 << LED_CH1_G_BIT;
	LED_CH2_R_MODE |= 1 << LED_CH2_R_BIT;
	LED_CH2_G_MODE |= 1 << LED_CH2_G_BIT;

	CH1_RELAY = 0;
	CH2_RELAY = 0;
	CH1_RELAY_MODE |= 1 << CH1_RELAY_BIT;
	CH2_RELAY_MODE |= 1 << CH2_RELAY_BIT;
}


void main(void)
{
	int debounce_main = 0, last_main = 0, but_main = 0;
	int debounce_ch1 = 0, last_ch1 = 0, but_ch1 = 0;
	int debounce_ch2 = 0, last_ch2 = 0, but_ch2 = 0;
	init_io();

	usb_init();
	ep0_init();

	LED(MAIN, G);
	TURN(1, 0);
	TURN(2, 0);

	while (1) {
		if (debounce_main)
			debounce_main--;
		else {
			last_main = but_main;
			but_main = !BUT_MAIN;
			if (last_main != but_main)
				debounce_main = DEBOUNCE_CYCLES;
		}

		if (debounce_ch1)
			debounce_ch1--;
		else {
			last_ch1 = but_ch1;
			but_ch1 = !BUT_CH1;
			if (last_ch1 != but_ch1)
				debounce_ch1 = DEBOUNCE_CYCLES;
		}

		if (debounce_ch2)
			debounce_ch2--;
		else {
			last_ch2 = but_ch2;
			but_ch2 = !BUT_CH2;
			if (last_ch2 != but_ch2)
				debounce_ch2 = DEBOUNCE_CYCLES;
		}

		if (but_main && !last_main) {
			if (local) {
				TURN(1, 0);
				TURN(2, 0);
			}
			local = 1;
		}

		if (local) {
			LED(MAIN, G);
			if (IS_ON(1))
				TURN(1, 1);
			else
				TURN(1, 0);
			if (IS_ON(2))
				TURN(2, 1);
			else
				TURN(2, 0);
		}

		if (local) {
			if (but_ch1 && !last_ch1)
				TURN(1, !IS_ON(1));
			if (but_ch2 && !last_ch2)
				TURN(2, !IS_ON(2));
		}
		
		usb_poll();
	}
}