//#
//#   main.c
//#
//#  =======================================================
//#   Lab i realtidssystem
//#   Styrning av tåg
//#
//#
//#   Christian Ohlsson, Daniel Alfredsson
//#   Karlstads universitet
//#   991008
//#  =======================================================

#include <stdio.h>
#include <personal/mon.h>
#include <red/r_thread.h>
#include <blue/b_thread.h>
#include "taglab_r.h"
#include "train.h"
#include "hal.h"

//#   
//#   SPEED
//#   Här anger vi standard hastighet på tågen.
#define SPEED 12

//#   
//#   bsPosixTime_t
//#   Anger Rubustid.
static bsPosixTime_t const timeSec = {90L, 0L};

//#   
//#   blueSleep()
//#   Berättar för kompilatorn att denna
//#   externa funktion används.
extern void blueSleep();

//#   
//#   Train_t
//#   Skapar våra två tåg. Dessa är globala.
Train_t Train0;	
Train_t Train8;	

//#   
//#   sleep()
//#   Låter Rubus sova i ett antal millisekunder.
void sleep(unsigned int millisecs) {
	TYPE_TIME tp;                       
	bsPosixTime_t t;	
	t.tvSec = 0;
	t.tvNsec = millisecs * 1000000L;   
	tp = bsTvToJiffies((bsPosixTime_t*)&t);	
	blueSleep(&tp);                    
}

//#   
//#   blueError()
//#   Anropas om fel i blå kärnan inträffar.
INT blueError(INT code, blueThreadId_t thread) { 
	INT error = code;
	blueThreadId_t id = thread;
	halWrite("Det blev blått fel...");
	halHalt();
	return(RCODE_OK);
}

//#   
//#   redError()
//#   Anropas om fel i röda kärnan inträffar.
INT redError(INT code, VOIDP thread) {
	halWrite("Det blev rött fel...");
	sleep(300);
	halExit();
	return(RCODE_OK);
}

//#   
//#   train_init()
//#   Initierar tågbibliotek, skapar tåg,
//#   ställer växlar och kör igång tågen.
bool_t train_init() {
	if(trainlib_init() != True)	{
		halWrite("Failed to initialized library.\n");
		return False;
	}
	else {
		halWrite("Library initialized.\n"); sleep(200); 
		trainlib_start();			sleep(100);
		Train0 = train_create(60); 	sleep(100);
		Train8 = train_create(78); 	sleep(100);
		switch_open(1);				sleep(100);
		switch_open(4);				sleep(100);
		switch_close(2);			sleep(100);
		switch_close(3);			sleep(100);
		train_set_speed(Train8, SPEED);	sleep(100);
		train_set_speed(Train0, SPEED);	sleep(100);
		return True;
	}
}

//#   
//#   blueIdleMain()
//#   Körs om ingen annan process körs.
void blueIdleMain(void) { 
	halWrite("\nblueIdleMain()");
	while(1);
}

//#   
//#   blueFunk()
//#   Styr hela förloppet, dvs ställer om
//#   växlar och startar och stoppar tåg.
void blueFunk() {
	int go=0;
	uint mod1, mod2;
	if(!train_init()) halHalt();	
	while(1) {
//		sleep(5);
		mod1 = sensor_read(1);
//		sleep(10);
		mod2 = sensor_read(2);
//		if(mod1 != 0) printf("\nmod1: %d",mod1);
//		if(mod2 != 0) printf("\nmod2: %d", mod2);
		
		// Begin switch                (Behövs detta?)
		if(mod2&0x0080 || mod2&0x8000) {
			switch_open(2);					// 0:an kan köra
			sleep(100);
		}
		
		if(mod2&0x0040 || mod2&0x4000) {
			switch_close(2);				// 8:an kan köra
			sleep(100);
		}
		
		//  Hastighetsändringar för tåget
		if (mod1 & 0x0020 || mod2 & 0x0001) {	//8 kommer till krysset
			train_set_speed(Train8, 0);
			sleep(100);
			switch_open(2);						//från vänster
			go=1;
			sleep(100);
		}
		if (mod2 & 0x8000 || mod1 & 0x0010) {  //8 kommer till krysset
			train_set_speed(Train8, 0);
			sleep(100);
			switch_open(2);							//från höger
			go=2;
			sleep(100);
		}
		if (go == 1) {     //Tåget väntar i krysset
			if (mod2 & 0x0040 || mod2 & 0x4000 || mod1 & 0x0080 || 
				mod1 & 0x4000 || mod1 & 0x8000 || mod1 & 0x0001 ) {
				switch_close(2);
				sleep(100);
				train_set_speed(Train8, SPEED);
				go=0;
				sleep(100);
			}
		}
		if (go == 2) {     //Tåget väntar i krysset
			if (mod1 & 0x0001 ||mod1 & 0x0002 || mod1 & 0x0008 || 
				mod2 & 0x0004 || mod2 & 0x0008 || mod2 & 0x0010 ) {
				switch_open(2);
				sleep(100);
				train_set_speed(Train8, SPEED);
				go=0;
				sleep(100);
			}
		}
	}
}

//#   
//#   dummy()
//#   Den röda dummytrådens funktion.
void dummy() {
	halWrite("\nDummy thread running...");
}

//#   
//#   main()
//#   Initierar Rubus och avbryter om
//#   om något går fel.
void main(void) {
	halWrite("\nProgrammet startades!\n");
	
	if (halInit() != RCODE_OK)
		halHalt();
	if (bsInit(OBJECT_BASIC_QUEUE | _RO_RESET) != RCODE_OK) 
		halHalt();
	if (redInit(&redSchedule) != RCODE_OK) 
		halHalt();
	if (blueInit(OBJECT_BLUE_MUTEX | OBJECT_BLUE_MSG | OBJECT_BLUE_THREAD | _RO_RESET) != RCODE_OK)
		halHalt();
	
	halBsTimerStart();
	blueStart();
}

/*
GAMMAL KOD

	TYPE_TIME startTime, stopTime, timePeriod;
	timePeriod = bsTvToJiffies((bsPosixTime_t *)&timeSec);
  
	printf("start");
	if(!train_init()) halHalt();	
	printf("stopp");
	bsClockGetTime(&startTime);
	
	  
	while(1) {printf("hey");};
	stopTrain();
	halWrite("\nProgrammet avslutades...");
	halExit();
		
	void readSensor(uint mod1, uint mod2) {
	halWrite("\nREAD_SENSOR");
	mod1 = sensor_read(1);
	sleep(100);
	mod2 = sensor_read(2);
		  
	if(mod1 & 1) { halWrite("\n mod:1 & __1__\n"); }
	else if(mod1 &  2) { halWrite("\n mod:1 & __2__\n"); }
	else if(mod1 &  3) { halWrite("\n mod:1 & __3__\n"); }
	else if(mod1 &  4) { halWrite("\n mod:1 & __4__\n"); }
	else if(mod1 &  5) { halWrite("\n mod:1 & __5__\n"); }
	else if(mod1 &  6) { halWrite("\n mod:1 & __6__\n"); }
	else if(mod1 &  7) { halWrite("\n mod:1 & __7__\n"); }
	else if(mod1 &  8) { halWrite("\n mod:1 & __8__\n"); }
	else if(mod1 &  9) { halWrite("\n mod:1 & __9__\n"); }
	else if(mod1 & 10) { halWrite("\n mod:1 & __10_\n"); }
	else if(mod2 &  1) { halWrite("\n mod:2 & __1__\n"); }
	else if(mod2 &  2) { halWrite("\n mod:2 & __2__\n"); }
	else if(mod2 &  3) { halWrite("\n mod:2 & __3__\n"); }
	else if(mod2 &  4) { halWrite("\n mod:2 & __4__\n"); }
	else if(mod2 &  5) { halWrite("\n mod:2 & __5__\n"); }
	else if(mod2 &  6) { halWrite("\n mod:2 & __6__\n"); }
	else if(mod2 &  7) { halWrite("\n mod:2 & __7__\n"); }
	else if(mod2 &  8) { halWrite("\n mod:2 & __8__\n"); }
	else if(mod2 &  9) { halWrite("\n mod:2 & __9__\n"); }
	else if(mod2 & 10) { halWrite("\n mod:2 & __10_\n"); }
			


	//Begin stop   (old try...)
	if(mod1 & 0x0020 || mod2 & 0x0001) {
		train_set_speed(Train8, 0);
		halWrite("\nStoppar tåg 78");
		sleep(100);
		switch_close(2);
		set = 1;
	}
	if(set == 1)
		if(mod1 != 0 || mod2 != 0)
			if(!(mod1 & 0x0008) && !(mod2 & 0x0004) && !(mod2 & 0x0002)){
				train_set_speed(Train8, 8);
				halWrite("\nstartar tåg 78");
				sleep(100);
				switch_open(2);
				set = 0;
			}
	if(mod1 & 0x0020 || mod2 & 0x0001) {
		train_set_speed(Train8, 0);
				  
						
*/