#include "libmaple.h"
#include "arduino/WProgram.h"
#include "arduino/LiquidCrystal.h"

void setup(void);
void loop(void);
bool maple_transact(void);

struct maplepacket {
	unsigned char data_len; /* Bytes: header, data, and checksum */
	unsigned short data_len_rx; /* For Rx -- didn't realise we could get up to 512 bytes */

	unsigned char header[4];
	unsigned char data[600]; /* Our maximum packet size */
} packet;
	
LiquidCrystal lcd(34, 32, 39, 37, 35, 33);

void setup()
{
	Serial.begin(57600);

	pinMode(7, OUTPUT); // debug pin
	pinMode(13, OUTPUT); // More different debug pin
	
	pinMode(44, OUTPUT);
	pinMode(46, OUTPUT);
	digitalWrite(46, HIGH);
	digitalWrite(44, LOW);

	// Maple bus data pins
	pinMode(4, INPUT);
	digitalWrite(4, HIGH);
	pinMode(5, INPUT);
	digitalWrite(5, HIGH);
	
	pinMode(7, OUTPUT);
	analogWrite(7, 0);
	
	lcd.begin(16, 2);
	lcd.clear();
	
	packet.header[0] = 0; // Number of additional words in frame
	packet.header[1] = 0; // Sender address = Dreamcast
	packet.header[2] = (1 << 5); //(1 << 5); // Recipient address = main peripheral on port 0
	packet.header[3] = 1; // Command = request device information
	packet.data[0]   = 0x21;
	packet.data_len  = 5;
	
	maple_transact();
}

bool maple_transact() {
	unsigned char *rx_buf_end;
	maple_tx_raw(&(packet.header[0]), packet.data_len);
	rx_buf_end = maple_rx_raw(&(packet.header[0]));
	packet.data_len_rx = (rx_buf_end - (&(packet.header[0])));
	return true;
}

bool read_packet(void) {
	/* First byte: #bytes in packet (including header and checksum)*/
	while(!(Serial.available()));
	packet.data_len = Serial.read();
	if(packet.data_len > 0) {
		unsigned char *data = &(packet.header[0]);
		for(int i = 0; i < packet.data_len; i++) {
			while(!(Serial.available()))
				;
			*data = Serial.read();
			data ++;
		}
		return true;
	} else {
		return false;
	}
}

void send_packet(void) {
	Serial.write((packet.data_len_rx & 0xff00) >> 8);
	Serial.write(packet.data_len_rx & 0xff);
	if(packet.data_len_rx) Serial.write(&(packet.header[0]), packet.data_len_rx);
}

void create_read_controller_packet() {
	//01 00 20 09 01 00 00 00 29
	packet.header[0] = 0x01; // Number of additional words in frame
	packet.header[1] = 0x00; // Sender address = Dreamcast
	packet.header[2] = (1 << 5); //(1 << 5); // Recipient address = main peripheral on port 0
	packet.header[3] = 0x09; // Command = request device information
	packet.data[0]   = 0x01;
	packet.data[1]   = 0x00;
	packet.data[2]   = 0x00;
	packet.data[3]   = 0x00;
	packet.data[4]   = 0x29;
	packet.data_len  = 9;
}

const int throttle_positions = 21;
const int throttle_absolute_maximum_speed = 255;
const int throttle_minimum_speed = 20;
int current_throttle_position = -1; // 0 is EM FULL. Lever must be moved to EM FULL to begin.
float current_speed = 0;
float target_speed = 0;
int throttle_max_speed[throttle_positions] =  {
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, //brake positions and Throttle 0
	0, 55, 75, 90, 100, 120
};
float throttle_delta[throttle_positions] = 	{
	0.025, 0.05, 0.1, 0.2, 0.4, 0.8, 1.2, 1.8, 2.4, 10, 20, 25, 30, 50, 9999, //brake
	0.00 /*coast*/, 0.25, 0.5, 1, 1.75, 2.5
};



#define BRAKE_MASK 0xf0
#define BRAKE_SHIFT 4
#define ACCEL_MASK 0x07
void read_throttle_position() {
	int accel = packet.data[6] & ACCEL_MASK;
	int brake = (int)((packet.data[7] & BRAKE_MASK) >> BRAKE_SHIFT);
	
	if (current_throttle_position == -1) {
		//check that we have EM FULL and Neutral
		if (brake  == 15 && accel  == 1) {
			//set the initial '0' (EM FULL) position.
			current_throttle_position = 0; 
			lcd.clear();
		}
	} else {
		if (brake != 0x01) {
			//then we're braking.
			if (brake > 0) current_throttle_position = brake;
		} else {
			//we're accelerating.
			if ((accel + 15) < 22) current_throttle_position = accel + 15;
		}
	}
}

void update_speed() {
	digitalWrite(13, LOW);
	//make sure we are allowed to go.
	if (current_throttle_position >= 0) {
		if (current_speed > throttle_max_speed[current_throttle_position - 1]) {
			current_speed -= throttle_delta[current_throttle_position - 1];
			if (current_speed < throttle_minimum_speed) current_speed = 0; //braking... don't go negative.	
		} else if (current_speed < throttle_max_speed[current_throttle_position - 1]) {
			if (current_speed < throttle_minimum_speed) current_speed = throttle_minimum_speed;
			current_speed += throttle_delta[current_throttle_position - 1];
			if (current_speed > throttle_max_speed[current_throttle_position - 1])
				current_speed = throttle_max_speed[current_throttle_position - 1];
		}

		//set light if we have met max speed for throttle.
		if (current_speed == throttle_max_speed[current_throttle_position - 1]) digitalWrite(13, HIGH);
		//output speed to railway.
		analogWrite(7, current_speed);		
	} else {
		//flash the LED to alert user to reset controls.
		delay(200); //delay a little to flash the LED
		digitalWrite(13, HIGH);
		delay(200);
	}
}

String throttle_names[] = {
	"", "Brake 1", "Brake 2", "Brake 3", "Brake 4", "Brake 5", "Brake 6", "Brake 7", "Brake 8", "EMERGENCY 1",
	"EMERGENCY 2", "EMERGENCY 3", "EMERGENCY 4", "EMERGENCY 5", "EMERGENCY FULL", "Neutral", "Throttle 1", "Throttle 2", 
	"Throttle 3", "Throttle 4", "Throttle FULL"
};


int main(void) {
    init();
    setup();

    for (;;) {
		create_read_controller_packet();
		maple_transact();
		read_throttle_position();
		
		lcd.setCursor(0, 0);
		if (current_throttle_position == -1) {
			lcd.print("Set BR:EM-FULL");
			lcd.setCursor(0, 1);
			lcd.print("& TH:Neutral");
		} else {
			lcd.setCursor(0, 0);
			lcd.print("                          ");
			lcd.setCursor(0, 0);
			lcd.print(throttle_names[current_throttle_position-1]);
			lcd.setCursor(0, 1);
			lcd.print(current_speed);
			lcd.print("    ");
			lcd.setCursor(8, 1);
			
			if (current_speed >= throttle_max_speed[current_throttle_position - 1]) {
				lcd.print("-");
				lcd.print(throttle_delta[current_throttle_position - 1]);	
			} else if (current_speed < throttle_max_speed[current_throttle_position - 1]) {
				lcd.print("+");
				lcd.print(throttle_delta[current_throttle_position - 1]);
			}
			lcd.print("    ");
		}
		update_speed();
	}
	
	return 0;
}