/*! \file   Layout.H c++/Layout.H
    \brief  C++ Train Layout Monitor Class Interface
    \author Zhengrong.Zang@gmail.com
	\date   2013.10.01
	
    Defines interface to Train Layout Monitor
*/
// The contents of this file are subject to the Mozilla Public License
// Version 1.0 (the "License"); you may not use this file except in
// compliance with the License. You may obtain a copy of the License
// at http://www.mozilla.org/MPL/
// 
// Software distributed under the License is distributed on an "AS IS"
// basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See
// the License for the specific language governing rights and
// limitations under the License.

#ifndef _Layout_H_
#define _Layout_H_

#include <c++/RotationSensor.H>
#include <c++/ConstantMotor.H>

#define COUNTS_PER_CYCLE	16.0

enum TrainWheel {
	FlangedTrainWheel = 0,		// Standard Lego Train Wheel
	MediumFlangedDriver,
	MediumBlindDriver,
	LargeFlangedDriver,
	LargeBlindDriver,
	XLFlangedDriver,
	XLBlindDriver
};

enum Track {
	NON = 0,// None
	STH,		// Straight Horizontal
	STV,		// Straight Vertical
	CLU,		// Curve Left Up
	CLD,		// Curve Left Down
	CRU,		// Curve Right Up
	CRD,		// Curve Right Down
	SPH,		// Switch Point Horizontal
	SSH,		// Switch Straight Horizontal
	SCH,		// Switch Curve Horizontal
	SPV,		// Switch Point Vertical
	SSV,		// Switch Straight Vertical
	SCV,		// Switch Curve Vertical
	CRO			// Cross
};

float trainWheelDiameter[] = {
	1.76,		// FlangedTrainWheel
	2.40,		// MediumFlangedDriver
	2.40,		// MediumBlindDriver
	3.04,		// LargeFlangedDriver
	3.04,		// LargeBlindDriver
	3.68,		// XLFlangedDriver
	3.68		// XLBlindDriver
};

float trackLength[] = {
	0.0, 		// NON:None
	12.8,   // STH:Straight Horizontal
	12.8,   // STV:Straight Vertical
	12.56,  // CLU:Curve Left Up
	12.56,  // CLD:Curve Left Down
	12.56,  // CRU:Curve Right Up
	12.56,  // CRD:Curve Right Down
	12.8,   // SPH:Switch Point Horizontal
	12.8,   // SSH:Switch Straight Horizontal
	16.0,   // SCH:Switch Curve Horizontal
	12.8,   // SPV:Switch Point Vertical
	12.8,   // SSV:Switch Straight Vertical
	16.0,   // SCV:Switch Curve Vertical
	12.8    // CRO:Cross
};

typedef struct {
	int x;
	int y;
} Location;

static RotationSensor *rotationSensor;
static ConstantMotor *trainMotor;

Track trackLayout[7][14] = {
/*      00   01   02   03   04   05   06   07   08   09   10   11   12   13         */
/*00*/ {NON, CRD, CRD, SPH, SSH, STH, STH, STH, STH, SSH, SPH, CLD, CLD, NON},	/*00*/
/*01*/ {CRD, NON, NON, NON, SCH, CRU, STH, STH, CLU, SCH, NON, NON, NON, CLD},  /*01*/
/*02*/ {CRD, NON, NON, NON, NON, NON, NON, NON, NON, CRD, STH, STH, STH, CLD},  /*02*/
/*03*/ {STV, NON, NON, NON, NON, NON, NON, NON, NON, SCH, STH, STH, NON, STV},  /*03*/
/*04*/ {CRU, NON, NON, NON, NON, NON, NON, NON, SSH, SPH, SSH, NON, NON, CLU},  /*04*/
/*05*/ {CRU, NON, NON, NON, SCH, CRD, SPH, SSH, CLD, SCH, NON, NON, NON, CLU},  /*05*/
/*06*/ {NON, CRU, CRU, SPH, SSH, STH, STH, STH, STH, SSH, SPH, CLU, CLU, NON}   /*06*/
/*      00   01   02   03   04   05   06   07   08   09   10   11   12   13         */
};

Location runRoute[] = {
	{06, 06}, {05, 06}, {04, 06}, {03, 06}, {02, 06}, {01, 06}, 
	{00, 05}, {00, 04}, {00, 03}, {00, 02}, {00, 01}, {01, 00}, 
	{02, 00}, {03, 00}, {04, 00}, {05, 00}, {06, 00}, {07, 00}, 
	{13, 02}, {13, 03}, {13, 04}, {13, 05}, {12, 06}, {11, 06}, 
	{02, 01}, {02, 00}, {02, 00}, {01, 00}, {02, 00}, {02, 01}, 
	{01, 06}, {02, 06}, {01, 06}, {07, 06}, 
	{-1, -1}
};

wakeup_t countWheel(wakeup_t data) {
//	return abs(rotationSensor->pos()) == abs(data);
	return abs(ROTATION_3) >= 33;
}

class Layout {
private:
	TrainWheel wheel;
	Location *runLayout;
	Track **trackLayout;
	
	
public:
//	Layout(ConstantMotor *motor, RotationSensor *sensor, Track **trackLayout, Location *runLayout, TrainWheel wheel = FlangedTrainWheel) {
	Layout(ConstantMotor *motor, RotationSensor *sensor, TrainWheel wheel = FlangedTrainWheel) {
		this->wheel = wheel;
		this->runLayout = runRoute;
		this->trackLayout = trackLayout;
		trainMotor = motor;
		rotationSensor = sensor;
	}
	
	~Layout() {
	}
	
	void sendMsgToRCX(char *msg) {
		cputs("msg");
	}
	
	void checkTrack(Track track, int index) {
		if (track == SPH || track == SPV) {
			Location next2Loc = runLayout[index + 1];
			
			Track next2Track = trackLayout[next2Loc.y][next2Loc.x];
			
			if (next2Track == SSH || next2Track == SSV) {
				sendMsgToRCX("SwitchToStaright");
			}
			else if (next2Track == SCH || next2Track == SCV) {
				sendMsgToRCX("SwitchToCurve");
			}
		}
		else if (track == SSH || track == SSV) {
			sendMsgToRCX("SwitchToStaright");
		}
		else if (track == SCH || track == SCV) {
			sendMsgToRCX("SwitchToCurve");
		}
	}
	
	int calcCount(Track track) {
		return (int)(COUNTS_PER_CYCLE * trackLength[track] / trainWheelDiameter[wheel] / M_PI);
//		return (int)(COUNTS_PER_CYCLE * 12.8 / 1.76 / 3.14);
	}
	
	void run() {
		int index = 1;
		Location prevLoc;
		Location startLoc = runLayout[index];
		Location currentLoc = startLoc;
		
		if (currentLoc.x == -1 || currentLoc.y == -1) {
			return;
		}
		
		Track currentTrack = trackLayout[currentLoc.y][currentLoc.x];
		
//		if (currentTrack == NON) {
//			return;
//		}
		
		index++;
		Location nextLoc = runLayout[index];
		Track nextTrack = trackLayout[nextLoc.y][nextLoc.x];
		
		checkTrack(nextTrack, index);
		int count = calcCount(currentTrack);
		lcd_int(count);
		
		while (!shutdown_requested()) {
			rotationSensor->pos(0);
			trainMotor->forward(150, 150);
			
			wait_event(&countWheel, count);
			
			trainMotor->brake();
			trainMotor->forward(0, 0);
			
			prevLoc = currentLoc;
			currentLoc = nextLoc;
			
			if (currentLoc.x == -1 || currentLoc.y == -1) {
				trainMotor->velocity(0);
				trainMotor->stop();
				sleep(5);
				index = 1;
			}
			
			currentTrack = trackLayout[currentLoc.y][currentLoc.x];
			
//			if (currentTrack == NON) {
//				trainMotor->velocity(0);
//				trainMotor->stop();
//				break;
//			}
			
			index++;
			
			nextLoc = runLayout[index];
			nextTrack = trackLayout[nextLoc.y][nextLoc.x];
			
			checkTrack(nextTrack, index);
			count = calcCount(currentTrack);
		}
	}
};


#endif // _Layout_H_