//	Shuttle Controller Software
//	Copyright 2007 by
//	Villy G Madsen
//	
//	Version 1.0b
//	2007-04-25
//	All rights reserved..
//



#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <ctype.h>
#include <string.h> 
#include <avr/io.h>
#include <avr/interrupt.h> 
#include <avr/pgmspace.h>
#include <avr/eeprom.h> 


#define USART_RX_BUFFER_SIZE 8
#define USART_TX_BUFFER_SIZE 64

#define USART_TX_BUFFER_MASK (USART_TX_BUFFER_SIZE - 1)
#define USART_RX_BUFFER_MASK (USART_RX_BUFFER_SIZE - 1)

#define EastStop  ((PINC & (unsigned char) (1 << 2)) != 0)
#define EastBrake ((PINC & (unsigned char) (1 << 3)) != 0)
#define WestStop  ((PINC & (unsigned char) (1 << 4)) != 0)
#define WestBrake ((PINC & (unsigned char) (1 << 5)) != 0)

#define EastRedOn (PORTB |= (unsigned char) (1<<0))
#define EastRedOff (PORTB &= (unsigned char) ~(1<<0))
#define EastYellowOn (PORTB |= (unsigned char) (1<<6))
#define EastYellowOff (PORTB &= (unsigned char) ~(1<<6))
#define EastGreenOn (PORTB |= (unsigned char) (1<<7))
#define EastGreenOff (PORTB &= (unsigned char) ~(1<<7))
#define WestRedOn (PORTD |= (unsigned char) (1<<2))
#define WestRedOff (PORTD &= (unsigned char) ~(1<<2))
#define WestYellowOn (PORTD |= (unsigned char) (1<<4))
#define WestYellowOff (PORTD &= (unsigned char) ~(1<<4))
#define WestGreenOn (PORTD |= (unsigned char) (1<<7))
#define WestGreenOff (PORTD &= (unsigned char) ~(1<<7))
#define SetEast (PORTB |= (unsigned char) (1<<2))
#define SetWest (PORTB &= (unsigned char) ~(1<<2))
#define West (unsigned char) (0xff)
#define East (unsigned char) (0)
#define TrainRecSize (22)
struct TrainRecord
  {	unsigned char Valid;
    	unsigned char Name[9];
	unsigned int  MinWest;
	unsigned int  MinEast;
	unsigned int  MaxWest;
	unsigned int  MaxEast;
	unsigned int  PulseMax;
	unsigned int  UnUsed;
	unsigned int  EastDec;
	unsigned int  WestDec;
	unsigned int  WaitWest;
	unsigned int  WaitEast;
  }Train,TempTrain;

static unsigned char USART_RxBuf[USART_RX_BUFFER_SIZE];
static volatile unsigned char USART_RxHead;
static volatile unsigned char USART_RxTail;

static unsigned char USART_TxBuf[USART_TX_BUFFER_SIZE];
static volatile unsigned char USART_TxHead;
static volatile unsigned char USART_TxTail;
static volatile unsigned char WestBrakeStatus = 0,
					WestStopStatus = 0,
					EastBrakeStatus = 0,
					EastStopStatus = 0,
					EastStopSet = 0,
					EastBrakeSet = 0,
					WestStopSet = 0,
					WestBrakeSet = 0,
					Auto = 0,
					EastBrakeOK = 0,
					EastStopOK = 0,
					WestStopOK = 0,
					WestBrakeOK = 0,
					Direction = 0,
					TimerActive = 0,
					TimerTick = 0;


static volatile unsigned int ADCStatus = 0,
                         ADCVal = 0,
				 Throttle, 
				 OldThrottle,
				 Want=0,
				 DeltaW = 8,
				 DeltaE = 8,
				 ThrottleInc,
				 WaitTime = 0,
				 MaxWait = 500,
				 Tick = 0,
				 LastThrottle = 0,
				 Temp,
				 TempT,
				 NoChg = 2,
				 ADCVal;


static int DataInReceiveBuffer(void);
static int TXBufferEmpty(void);

/* prototypes */
void USART0_Init( unsigned int baudrate );
void EchoSensors(void);
static unsigned char InChar;
static int USART0_Receive(void);
static int USART0_Transmit (char data, FILE *stream );
void ResetCPU(void);
void SetDirection(unsigned int WhichWay);
static FILE mystdout = FDEV_SETUP_STREAM(USART0_Transmit,NULL, _FDEV_SETUP_WRITE);
void CheckAll(void);
void Init(void);
void CheckSensors(void);
unsigned int CalcDelta(unsigned int TickVal);
void SaveConfiguration(void);
void DisplayTrains(void);
void GetTrain(unsigned char TrainNo);
void PutTrain(unsigned char TrainNo);
unsigned int GetTrainNo(void);
void DisplayTrain(unsigned char TrainNo);
/*MAIN */
int main( void )
{	Init();
//	for (;;)  EchoSensors();
	GetTrain(0);
	if (TempTrain.Valid != 0x5A) // has the eeprom been initialized ??
	{  Train.Valid = 0x5A;
	   strcpy(Train.Name,"default\0");
	   Train.MinWest = 8192;
	   Train.MinEast = 8192;
	   Train.MaxWest = 20000;
	   Train.MaxEast = 20000;
	   Train.PulseMax = 10000;
	   Train.EastDec = 2500;
	   Train.WestDec = 2500;
	   Train.WaitWest = 200;
	   Train.WaitEast = 200;
	   PutTrain(0);
	   printf("Train table initialized\n");
	}

	DisplayTrains();
	printf_P(PSTR("\n Select a locomotive ==>"));
	InChar = GetTrainNo();
	if (InChar <= 9) 
	  GetTrain(InChar);
	else GetTrain(0);
	Train = TempTrain;
	Train.PulseMax = 12000;
      Train.WaitWest = 200;
      Train.WaitEast = 200;

	DisplayTrain(99);
	WaitTime = 200;
	while (WaitTime > 0);


//	CheckSensors();
	SetDirection(East);
	SetDirection(West);
	WaitTime = 100;
	ThrottleInc = Train.MaxWest >> 8;
	while (WaitTime > 0);
	if (WestBrake || WestStop)
	{ Want=Train.MaxWest;
	  SetDirection(East);
      }
	else 
	{ SetDirection(West);
	  Want = Train.MaxWest;
	}
	Auto = 1;
	for (;;)
    {
	if (Auto)
	{if (Direction == West)
	  { if (WestStopSet)
         { TimerActive = 0;
	     if (NoChg == 0)
	     { if (LastThrottle > Train.MinWest)
	       { DeltaW = (LastThrottle - Train.MinWest) >> 8; // In other words it hit the stop with speed on
	         Train.WestDec = Train.WestDec + DeltaW; // increase the deceleration
	       } else 
	       { if (Tick < 50) ++Train.WestDec;
	         else 
		   { DeltaW = CalcDelta(Tick);
	           if (DeltaW <= Train.WestDec) Train.WestDec = Train.WestDec - DeltaW;
	           else if (Train.WestDec > 0) Train.WestDec = Train.WestDec / 2;	         
               }
	       } 
	     } else --NoChg;
	    printf_P(PSTR("\nWest Throttle = %6d; Last = %6d; Tick = %6d;  Decrement = %4d;  DeltaW = %4d;\n"),
	              Throttle,LastThrottle,Tick,Train.WestDec,DeltaW);
	    WaitTime = 10;
	    while (WaitTime > 0);
	    WaitTime = Train.WaitWest;
	    while (WaitTime >0);
	    SetDirection(East);
	    WaitTime = 50;
	    while (WaitTime >0);
	    Want = Train.MaxEast;
	    ThrottleInc = Train.MaxEast >> 8;
	   }
	  }else
	  {if (EastStopSet)
	    { TimerActive = 0;
	      if (NoChg == 0)
		{ if (LastThrottle > Train.MinEast)
		  { DeltaE = (LastThrottle - Train.MinEast) >> 8; // In other words it hit the stop with speed on
		    Train.EastDec = Train.EastDec + DeltaE; // increase the deceleration
 	        } else 
		  {if (Tick < 50) ++Train.EastDec;
	         else 
	         { DeltaE = CalcDelta(Tick);
	           if (DeltaE <= Train.EastDec) Train.EastDec = Train.EastDec - DeltaE;
	           else if (Train.EastDec > 0) Train.EastDec = Train.EastDec/2;
       	   }
		  }
            } else --NoChg;
	     printf_P(PSTR("\nEast Throttle = %6d; Last = %6d; Tick = %4d;  Decrement = %4d;  DeltaE = %4d;\n"),
	              Throttle,LastThrottle,Tick,Train.EastDec,DeltaE);
	     WaitTime = 10;
	     while (WaitTime > 0);
	     WaitTime = Train.WaitEast;
	     while (WaitTime > 0);
	     SetDirection(West); 
	     WaitTime = 50;
	     while (WaitTime >0);
	     Want = Train.MaxWest;
	     ThrottleInc = Train.MaxWest >> 8;
         }
	 }
 	}
	if (DataInReceiveBuffer())
	{ InChar = toupper(USART0_Receive());
	  if (InChar == 'W') 
	  { printf_P(PSTR("set west executed\n"));
	    SetDirection(West);
	  }
	  else if (InChar == 'E') 
	  { printf_P(PSTR("set east executed\n"));
	    SetDirection(East);
	  }
	  else if (InChar == 'L') DisplayTrains();
	  else if (InChar == 'Y') 
	          { if (Auto) printf_P(PSTR("Save only allowed in manual mode\n"));
		      else SaveConfiguration();
		    }
	  else if (InChar == '1') 
	       {Train.MinWest = Throttle;
		    printf_P(PSTR("\n Minimum Throttle West Set= %6d \n"),Train.MinWest);
             }
	  else if (InChar == '!') 
	       {Train.MaxWest = Throttle;
		    printf_P(PSTR("\n Maximum Throttle West Set= %6d \n"),Train.MaxWest);
             }
	  else if (InChar == '2') 
	       {Train.MinEast = Throttle;
		    printf_P(PSTR("\n Minimum Throttle East Set= %6d \n"),Train.MinEast);
             }
	  else if (InChar == '@') 
	       {Train.MaxEast = Throttle;
		    printf_P(PSTR("\n Maximum Throttle East Set= %6d \n"),Train.MaxEast);
             }
	  else if (InChar == 'X') {Auto = 1; 
	                           printf_P(PSTR("Auto Mode engaged\n"));
					   NoChg = 2;
					  }
	  else if (InChar == 'Z') {Auto = 0; printf_P(PSTR("Manual Mode engaged\n"));}
	  else if (InChar == 'R') {cli(); ResetCPU();}
	  else if (InChar == 'S') {Throttle = 0; printf_P(PSTR("Emergency stop - Manual Mode engaged\n")); Auto = 0;}
	  else if (InChar == 'D') {Want = 0; 
	  				   printf_P(PSTR("Stopping - Manual Mode engaged\n"));
					  }

	  else if (InChar == 'T')
	          {	printf_P(PSTR("testing sensors \n hit q to quit"));
   		      Want = 0;
  	            EchoSensors();
	          }
//        else if (InChar = 'Q')
//                { cli();
//		      ResetCPU();
//	          }
	  else if (InChar == '?') DisplayTrain((unsigned char) 99);
	  else if ((InChar == '-') && (Throttle >=0x80))
	           { Want = Want - 0x80;
		       Throttle = Want;
		     }
	  else if ((InChar == '+') || (InChar == '='))
	           { Want = Want + 0x80;
		       Throttle = Want;
                 }
	  else if (InChar == '[')
	          { if (Direction)
		      { Train.MaxWest = Train.MaxWest - 250;
			  Temp = Train.MaxWest;
                  } else
		      { Train.MaxEast = Train.MaxEast - 250;
			  Temp = Train.MaxEast;
			}
                  printf_P(PSTR(" \nMax Throttle ==> %6d \n"),Temp);
		    }
	  else if (InChar == ']')
		 { if (Direction) 
		   { Train.MaxWest = Train.MaxWest + 250;
		     Temp = Train.MaxWest;
               } else 
		   {  Train.MaxEast = Train.MaxEast + 250;
		      Temp = Train.MaxEast;
               }
  	         printf_P(PSTR(" \nMax Trottle  ==> %6D\n"),Temp);
             }
	  else if (InChar == 'H')
	       {printf_P(PSTR("\n\n\n help for Villy's Auto Reverser\n"));
		  printf_P(PSTR("x --->Set Auto mode;   z --->Set manual mode/n"));
		  printf_P(PSTR("\nIn manual mode only"));
		  printf_P(PSTR("\n===================\n"));
		  printf_P(PSTR("+ --->accelerate       - (minus) -->brake;\n"));
		  printf_P(PSTR("s --->emergency stop   d decelerate and stop\n"));
		  printf_P(PSTR("w --->Go West          e --->Go East\n"));
		  printf_P(PSTR("1 --->set minimum Westbound Speed\n"));
		  printf_P(PSTR("! --->set maximum Westbound Speed\n"));
		  printf_P(PSTR("2 --->set minimum Eastbound Speed\n"));
		  printf_P(PSTR("@ --->set maximum Eastbound Speed\n"));
		  printf_P(PSTR("3 --->set Westbound pulse midpoint setting\n"));
		  printf_P(PSTR("# --->set Eastbound pulse midpoint setting\n"));
		  printf_P(PSTR("[ --->decrease max throttle setting\n"));
		  printf_P(PSTR("] --->increase max throttle setting\n"));
		  printf_P(PSTR("{ --->decrease pulse midpoint setting\n"));
		  printf_P(PSTR("} --->increase pulse midpoint setting\n"));
		  printf_P(PSTR("l --->display Locomotive settings\n"));
		  printf_P(PSTR("y --->save Locomotive settings\n"));
		  printf_P(PSTR("\nIn all modes"));
		  printf_P(PSTR("\n============\n"));
		  printf_P(PSTR("r --->Reset program\n"));
		  printf_P(PSTR("t --->Test sensors and signals\n"));
		 }
	  }
// 	  if (Throttle > MaxThrottle) Throttle=MaxThrottle;
	  if ((OldThrottle /500) != (Throttle/500))
	  { printf_P(PSTR("%6d "),Throttle);
//	    if (Throttle == MaxThrottle) printf_P(PSTR("\n"));
	    OldThrottle = Throttle;
	  }
     }return 0;
}  //end main

void DisplayTrains(void)
{ unsigned char I;
     	I = 0;
	while (I < 10)
	{
	  DisplayTrain(I);
	  I++;
	}
	printf_P(PSTR("end-of-list\n"));
}

void DisplayTrain(unsigned char TrainNo)
{	  if (TrainNo == 99) TempTrain = Train;
	  else GetTrain(TrainNo);
	  if (TempTrain.Valid == 0x5a)
	  {  printf_P(PSTR("\nTrain No. %2d   %10s\n"),TrainNo,TempTrain.Name);
	    printf_P(PSTR("  Min Speed East =>  %6d  West => %6d"),TempTrain.MinEast,TempTrain.MinWest);
	    printf_P(PSTR("      Max Speed East =>  %6d  West => %6d\n"),TempTrain.MaxEast,TempTrain.MaxWest);
	    printf_P(PSTR("  Decrement East =>  %6d  West => %6d"),TempTrain.EastDec,TempTrain.WestDec);
	    printf_P(PSTR("      Wait time East =>  %6d  West => %6d\n"),TempTrain.WaitEast,TempTrain.WaitWest);
	    printf_P(PSTR("  Pulse Max      =>  %6d\n"),TempTrain.PulseMax);
	  } 
}

void GetTrain(unsigned char TrainNo)
{	unsigned int *Addr;
	Addr = (int *) (TrainNo * sizeof(Train));
	eeprom_read_block(&TempTrain,Addr,sizeof(Train));
}

void PutTrain (unsigned char TrainNo)
{	unsigned int *Addr;
	Addr = (int *) (TrainNo * sizeof(Train));
	eeprom_write_block(&Train,Addr,sizeof(Train));
}

unsigned int GetTrainNo(void)
{ unsigned int TrainNo;
  WaitTime = 100;
  while ((!DataInReceiveBuffer()) && (WaitTime > 0)){};
  if (DataInReceiveBuffer())
  { InChar = USART0_Receive();
//    USART0_Transmit(InChar,stdout);
//    USART0_Transmit("\n",stdout);
    if ((InChar >= '0') && (InChar <= '9'))
    	return (InChar - '0');
    else printf_P(PSTR("Invalid train no\n"));
    return (unsigned int) 99;
  }
}
	

void SaveConfiguration(void)
{ unsigned char I, TrainNo;
  DisplayTrains();
  printf(" Enter the Locomotive number to save (0 - 9) ==>");
    TrainNo = GetTrainNo();
    if (TrainNo <= 9)
    {
      printf("saving loco %4x\n",TrainNo);
	while (DataInReceiveBuffer()) InChar = USART0_Receive();
	printf("\nEnter name of train (followed by enter)");
	I = 0;
	InChar = USART0_Receive();
	if (InChar != 13)
	{
	while ((I < 9) && (InChar != 13))
	{  Train.Name[I] = InChar;
	   USART0_Transmit(InChar,stdout);
	   if (InChar == 8)
	   { if (I > 0)
	     {  --I;
	       Train.Name[I] = ' ';
	     }
	   } 
  	   if (Train.Name[I] != ' ')I++;
	   InChar = USART0_Receive();
	}
	Train.Name[I] = 0;
	}
	Train.Valid = 0x5A;
	PutTrain(TrainNo);
	printf("\nSave complete for %10s\n",Train.Name);
    } else printf_P(PSTR(" no input - save request ignored"));
}



unsigned int CalcDelta(unsigned int TickVal)
{	unsigned int Delta;       
	if (Tick < 100) Delta = 0;
	if (Tick > 100) Delta = (Tick - 100) / 10;
//      if (Tick > 200) Delta = (Tick - 100) / 5;
	return Delta;
}
	       

void SetDirection(unsigned int WhichWay)
{
	WestYellowOff;
	EastYellowOff;
	if (WhichWay == West) 
	{ SetWest;
	  WestGreenOn;
	  WestRedOff;
	  EastGreenOff;
	  EastRedOn;
      } else 
	{ SetEast;
	  EastGreenOn;
	  EastRedOff;
	  WestGreenOff;
	  WestRedOn;
      }
  	Direction = WhichWay;
	EastStopSet = 0;
	EastBrakeSet  = 0;
   	EastStopStatus = 0;
  	EastBrakeStatus = 0;
	WestStopSet = 0;
	WestBrakeSet  = 0;
   	WestStopStatus = 0;
  	WestBrakeStatus = 0;
	LastThrottle = 0;       
}

void Init(void)
{	unsigned int temp;
	MCUSR &= (unsigned char) ~(1 << WDRF);
	WDTCSR |= (unsigned char) (1 << WDCE) | (1<<WDE);
	WDTCSR = 0;

	PORTB = 0b00000000;
	DDRB  = 0b11111111;
	temp  = PINB;
	PORTB = 0;


	PORTC = 0b00000000;
	DDRC  = 0b11111111;
	temp	= PINC;

	PORTD = 0x00;
	DDRD  = 0xff;
	temp  = PIND;
	PORTD = 0;

	OCR2A  = 0x00;	  

	TCCR2A = (unsigned char)(2 << COM2B0)  |  (2 << COM2A0) | (3 << WGM20);
	TIMSK2 = 0;
	TCCR2B = (4 << CS20);

	OCR1AH = 0x0;
	OCR1AL = 0x0;
	ICR1H  = 0x61;		// Set divisor to give us 40hz 
	ICR1L  = 0xa8;
//	ICR1H  = 0x27;		// Set divisor to give us 100hz 
//	ICR1L  = 0x10;

	TCCR1A = (unsigned char) (2 << COM1A0) | (2 << WGM10);
					  // FAST PWM, ICR1 clr OC1A on match, set at bottom
	TCCR1B = (unsigned char) (2 << CS10) | (3 << WGM12);
					  // clk/8 from prescaler (to give 1 mhz)
	TIMSK1 = (unsigned char) (1 << TOIE1);

	TCCR0A = (unsigned char) (2 << WGM00);	// set to CTC mode
	TCCR0B = (unsigned char) (5 << CS00); 	//  set divisor to 1024
	OCR0A  = 39;			// give a timeout of ~ 100/second
	TIMSK0 = (1<< OCIE0A);	// enable interrupt on compare A


	PORTB = 0b11111111;
	DDRB  = 0b11001011;
	temp  = PINB;
	PORTB = 0;


	PORTC = 0b01111100;
	DDRC  = 0b01000000;
	temp	= PINC;

	PORTD = 0xff;
	DDRD  = 0xff;
	temp  = PIND;
	PORTD = 0;

	ADMUX = (1 << ADLAR) | (1 << REFS0);
	ADCSRB = 0;
	ADCSRA = (1 << ADEN) | (6 << ADPS0) | (1 << ADIE) | (1 << ADSC) | (1 << ADATE);
	
	USART0_Init( 51);		/* set baud rate to 9600 baud at 8mhz */
	sei();
	stdout = &mystdout;
	printf_P(PSTR("\n\nVilly's Auto Reverse controller working !\n"));
	}

void	CheckSensors(void)
{	unsigned char Stop = 0;
	printf_P(PSTR("\n\nChecking sensors\n\n"));
	WaitTime = 40;
	Throttle = 0;
	CheckAll();
	while (WaitTime > 0) CheckAll();
	while (!Stop) 
	{ if ((EastBrakeOK) 
	       || (!EastBrake && !EastStop && !WestStop && !WestBrake))
	  { if (!EastStopOK)
	    { SetDirection(East);
	      Throttle = Train.MinEast;
	      while (!EastStopOK) CheckAll();
          } else 
	      { if ((WestBrakeOK) || (EastStopOK && EastBrakeOK))
	        { if (!WestStopOK)
	          { SetDirection(West);
  	            Throttle = Train.MinWest;
	            while (!WestStopOK) CheckAll();
                } else 
		      { SetDirection(East);
			  Throttle = Train.MinEast;
			  while (!WestBrakeOK) CheckAll();
                  }
		   }
		} 
        } else if (EastStopOK || (WestBrakeOK && !WestStopOK))
	         { SetDirection(West);
		     Throttle = Train.MinWest;
		     while (!WestStopOK) CheckAll();
		   }
	    else if (WestStopOK)
	         {SetDirection(East);
		    Throttle = Train.MinEast;
		    while (!EastStop) CheckAll();
               } 
	  Throttle = 0;
	  WaitTime = 40;
	  while (WaitTime > 0);
	  Stop = WestStopOK && WestBrakeOK 
	      && EastStopOK && EastBrakeOK;
	} // end while	
	Throttle = 0;
	WaitTime = 40;
	while (WaitTime > 0);
      printf_P(PSTR("All sensors ok\n"));
}


 void CheckAll(void)
{
	if (EastBrakeSet && !EastBrakeOK)
	  { EastBrakeOK = 1;
	    printf_P(PSTR("EastBrake ok \n"));
	  }
	if (WestBrakeSet && !WestBrakeOK)
	  { WestBrakeOK = 1;
	    printf_P(PSTR("WestBrake ok \n"));
        }
	if (EastStopSet && !EastStopOK)
	  { EastStopOK = 1;
	    printf_P(PSTR("EastStop ok \n"));
	  }
	if (WestStopSet && !WestStopOK)
	  { WestStopOK = 1;
	    printf_P(PSTR("WestStop ok \n"));
	  }
	EchoSensors();
}

void	EchoSensors(void)
{
	if (WestStop) WestRedOn; 
	   else WestRedOff;
	if (WestBrake) WestYellowOn; 
	   else WestYellowOff;
	if (EastStop) EastRedOn; 
	   else EastRedOff;
	if (EastBrake) EastYellowOn; else EastYellowOff;
	EastGreenOff;
	WestGreenOff;
}


void	ResetCPU(void)
{
		sei();
		asm("wdr");
		WDTCSR |= (unsigned char) (1<<WDCE) | (1<<WDE);
		WDTCSR = (unsigned char) (1<<WDE) | (0x07<<WDP0);
		for (;;);
}
static int USART0_Receive(void)
{
	unsigned char tmptail;

	while (USART_RxHead == USART_RxTail) ;

	tmptail = (USART_RxTail + 1) & USART_RX_BUFFER_MASK;

	USART_RxTail = tmptail;

	return USART_RxBuf[tmptail];
}

static int 	USART0_Transmit ( char data, FILE *stream)
{
	unsigned char tmphead;
	if (data == '\n') USART0_Transmit('\r',stream);


	tmphead = (USART_TxHead + 1) & USART_TX_BUFFER_MASK;

	while (tmphead == USART_TxTail);

	USART_TxBuf[tmphead] = data;
	USART_TxHead = tmphead;

	UCSR0B |= (1<<UDRIE0);
	return 0;
}

static int DataInReceiveBuffer(void)
{
	return (USART_RxHead != USART_RxTail);
}

static int TXBufferEmpty(void)
{
	return (USART_TxHead == USART_TxTail);
}


unsigned int max(unsigned int first, unsigned int second)
{
	if (first > second) return first;
	else return second;
}



//Initialize USART

void USART0_Init( unsigned int baudrate)
{
	unsigned char x;

	UBRR0H = (unsigned char) (baudrate >> 8);
	UBRR0L = (unsigned char) baudrate;

	UCSR0B = (unsigned char) ((1<<RXCIE0) | (1 << RXEN0) | (1<<TXEN0));

	UCSR0C = (unsigned char) (1<<USBS0) | (3 << UCSZ00);

	x = 0;

	USART_RxTail = 0;
	USART_RxHead = 0;
	USART_TxTail = 0;
	USART_TxHead = 0;
}
ISR(TIMER0_COMPA_vect)
{  unsigned int MinThrottle;


  if (TimerTick < 10) TimerTick++;
  else	// we are going to update throttle setting 10x per second
  {
    TimerTick = 0;

	if (WaitTime > 0) --WaitTime;
	if (TimerActive && (Tick < 0xFFFF)) Tick++;


//	if (Direction == West)
	{ if (WestStop) 
	  {  if (WestStopStatus < 10) WestStopStatus++;}
	  else if (WestStopStatus > 0) --WestStopStatus;

        if (WestBrake)
	  { if (WestBrakeStatus < 10) WestBrakeStatus++;}
	  else if (WestBrakeStatus > 0) --WestBrakeStatus;

	  if (WestStopStatus > 5)WestStopSet = 1;
	  if (WestBrakeStatus > 5) WestBrakeSet = 1;
      }

//	else  // direction is east
	{ if (EastStop) 
	  { if (EastStopStatus < 10) EastStopStatus++;}
        else if (EastStopStatus > 0) --EastStopStatus;
        
        if (EastBrake) 
        { if (EastBrakeStatus < 10 ) EastBrakeStatus++;}
        else if (EastBrakeStatus > 0) --EastBrakeStatus;
	  
	  if (EastStopStatus > 3)  EastStopSet = 1;	
	  if (EastBrakeStatus > 5) EastBrakeSet = 1;
	}


	
	  if (Direction == East) 
	  { MinThrottle = Train.MinEast;
          if (EastStopSet)
	    { EastRedOn;
	      EastYellowOff;
		EastGreenOff;
	      LastThrottle = Throttle;
	      Want = 0;
	      Throttle = 0;
	    }
	    else if (EastBrakeSet && Auto)
	         { EastYellowOn;
		       EastGreenOff;
		       if (!TimerActive)
		       {if (Throttle >= Train.EastDec) Throttle = Throttle - Train.EastDec;
	            if (Throttle <= Train.MinEast)
		        { Want = Train.MinEast;
		          Throttle = Train.MinEast;
			      Tick = 0;
			      TimerActive = 0xFF;
			    }
               }
            }
   	  }
	  else 
	  { MinThrottle = Train.MinWest;
	    if (WestStopSet)
	    { WestRedOn;
	      WestYellowOff;
		WestGreenOff;
	      LastThrottle = Throttle;
	      Want = 0;
	      Throttle = 0;
 	     } else if (WestBrakeSet && Auto)
	            { WestYellowOn;
	        	  WestGreenOff;
	        	  if (!TimerActive)
	   	        { if (Throttle >= Train.WestDec) Throttle = Throttle - Train.WestDec;
		          if (Throttle <= Train.MinWest)
			    { Want = Train.MinWest;
			      Throttle = Train.MinWest;
			      Tick = 0;
			      TimerActive = 0xFF;
      		    }
			  }
		      }
	}
      

        if (Auto == 1) 
	  { if (Throttle < Want)
	    { if ((Throttle + ThrottleInc) < Want)
	       Throttle = max(Throttle + ThrottleInc,MinThrottle/2);
		else  Throttle = Want;
          } else if (Throttle > Want)
	           { if (Throttle > (Throttle - ThrottleInc))
	                Throttle = Throttle - ThrottleInc;
	             else Throttle = Want;
	           }
        }
		
      if (Throttle > (MinThrottle/3))
	  TempT = Throttle;
	else TempT = (MinThrottle/3);
	OCR2A = (unsigned char) (TempT >> 8);  // was 8
//	Temp = Temp >> 1;
	OCR2B = (unsigned char) (TempT >> 8);  // was 8
	if (TempT >= (Train.PulseMax << 1)) TempT = 0; // was 0x7000
	else
	if (TempT >= Train.PulseMax) 		// was 0x3800
	TempT = max(TempT - ((TempT - Train.PulseMax) << 1),500);
	TempT = TempT >> 2;  // was 2
	OCR1AH = (unsigned char) (TempT >> 8);
	OCR1AL = (unsigned char) (TempT & 0xFF);
  }
}
ISR(TIMER1_OVF_vect)
{
}

ISR(USART_RX_vect)
{
	unsigned char data;
	unsigned char tmphead;

	data = UDR0;
	tmphead = (USART_RxHead + 1) & USART_RX_BUFFER_MASK;
	USART_RxHead = tmphead;

	if ( tmphead == USART_RxTail)
	{
		// error receiver buffer overflow

	}

	USART_RxBuf[tmphead] = data;
}

ISR(USART_UDRE_vect)
{
	unsigned char tmptail;

	if (USART_TxHead != USART_TxTail)
	{
		tmptail = (USART_TxTail + 1) & USART_TX_BUFFER_MASK;
		USART_TxTail = tmptail;

		UDR0 = USART_TxBuf[tmptail];
	}
	else
	{

		UCSR0B &= (unsigned char) ~(1<<UDRIE0);  // disable UDRE interrupt
	}
}
ISR(ADC_vect)
{
	unsigned int Temp;
	if (ADCStatus == 0)
	{	ADMUX = (1 << ADLAR) | (1<<REFS0) | (1 << MUX0);
		ADCStatus = 1;
	} else
	{ 	ADCVal =  ADCL | (ADCH << 8);
//		Temp = ADCVal;
//		OCR2A = (unsigned char) (Temp >> 8);
//		Temp = Temp >> 1;
//		OCR2B = (unsigned char) (Temp >> 8);
//		if (Temp > 0x3100)
//		{ 
//		Temp = 0;
//		}
//		else
//		if (Temp >= 0x1900) Temp = Temp - ((Temp - 0x1900) << 1);
//		OCR1AH = (unsigned char)(Temp >> 8);
//		OCR1AL = (unsigned char)(Temp & 0xFF);
//		ADMUX = (1 << ADLAR) | (1 <<REFS0) | (0 << MUX0);	
//		ADCStatus = 0;
	} 
}

EMPTY_INTERRUPT(TIMER0_COMPB_vect);
EMPTY_INTERRUPT(SPI_STC_vect);
EMPTY_INTERRUPT(USART_TX_vect);
EMPTY_INTERRUPT(TIMER2_COMPB_vect);
EMPTY_INTERRUPT(TIMER2_OVF_vect);
EMPTY_INTERRUPT(TIMER1_CAPT_vect);
EMPTY_INTERRUPT(TIMER1_COMPA_vect);
EMPTY_INTERRUPT(TIMER1_COMPB_vect);
EMPTY_INTERRUPT(EE_READY_vect);
EMPTY_INTERRUPT(ANALOG_COMP_vect);
EMPTY_INTERRUPT(TWI_vect);
EMPTY_INTERRUPT(SPM_READY_vect);
EMPTY_INTERRUPT(INT0_vect);
EMPTY_INTERRUPT(INT1_vect);
EMPTY_INTERRUPT(PCINT0_vect);
EMPTY_INTERRUPT(PCINT1_vect);
EMPTY_INTERRUPT(PCINT2_vect);
EMPTY_INTERRUPT(WDT_vect);
EMPTY_INTERRUPT(TIMER2_COMPA_vect);