/*************************************************************************
                            test.c
                            ------
   Detta ar huvudprogrammet som ar skrivet i ANSI C. Exekveringen av hela
   programpaketet borjar i pmain.68k (lage __main). 

   exp4() anropas fran assemblyprogrammet exp4.68k vid avbrott.
   
   _avben() anropar avben.68k vilket tillater avbrott fran PI/T.
****************************************************************************/

/* Nedan är alla pekare till de olika registrerna i multichippet */
unsigned short int *multidir,*multiio,*timerCDControl,*timerCData,*interuptEnable2,*interuptMask2,*interuptEnable1,*interuptMask1;
unsigned short int *multiVector,*multiInteruptPending2,*multiInteruptPending1,*timerDData,*uartControlRegister,*uartReciveStatus;
unsigned short int *timerAData,*timerAControl,*timerBControl,*timerBData,*interuptInServiceA,*interuptInServiceB;
unsigned short int *uartData;
unsigned short int *ram;

/* Vilken växel det är */
unsigned short int gear;

/* En variabel som räknar upp, används för att blinka med den gröna dioden */
unsigned short int ledBlink;

/* Makron för att enkelt sätta bitar på processorn */
#define  SETBIT(i, n)   ((i) |= (1 << (n))) 
#define  GETBIT(i, n)   ((i) & (1 << (n))) 
#define  CLRBIT(i, n)   ((i) &= ~(1 << (n))) 

/* pwmCounter=räknare för hastigheten. turnCounter=räknare ifall ena hjulet skall köras långsammare 
   direction=(3=höger 2=vänster 1=framåt 0=stop -1=bakåt) 
   linkError= 1 ifall det blir fel vid UART överföring */
int pwmCounter,turnCounter,direction,linkError;

/* Buffer som används för packet inläsning. */
short int buffer[100];

/* Position i paketet */
int  packetpos;

/* Ett packet från bluetooth */
struct Packet
{
	int packet_type;
	int operation_code;
	int datalength;
	int checksum;
	short int *data;
};

/* Konstanter som används av funktionen SetWheelStatus */
#define LEFT 1
#define RIGHT 2
#define FORWARD 1
#define REVERSE 2
#define STOP	3

/* Sätter status på ett hjul */
void SetWheelStatus(int wheel,int direction);

/* Nuvarande paket */
struct Packet currentPacket;

/* Väntar en liten stund */
void wait1();

/* Anropas när en ny byte kommer från interuptet, denna läser in paketet */
void newByte(unsigned short c);

/* Anropas av newByte när ett helt paket är inläst, denna funktion sätter direction osv */
void newPacket(struct Packet *currentPacket);

/* Nedan stående sätter up timrar och UART */
void SetupEngineTimer();
void SetupUart();

main()
{
  int i;
  unsigned short int *addr=(unsigned short int *) 0x40000;
  
  /* Nollställa alla globala variabler */
  pwmCounter=0;
  turnCounter=0;
  direction=0;
  linkError=0;
  packetpos=0;
  ledBlink=0;
  gear=1;

  /* Nollställa multichippet */
  for(i=0;i<=17;i++)
	  addr[i]=0;

  /* Sätta upp pekare till de olika registerna i multichippet */
  multidir = (unsigned short int *) 0x40002; 
  multiio =  (unsigned short int *) 0x40000; 

  interuptEnable1=(unsigned short int *) 0x40003; 
  interuptEnable2=(unsigned short int *) 0x40004;
  interuptMask1  =(unsigned short int *) 0x40009;
  interuptMask2  =(unsigned short int *) 0x4000A;

  timerAControl = (unsigned short int *) 0x4000C;  
  timerAData    = (unsigned short int *) 0x4000F; 

  timerBControl = (unsigned short int *) 0x4000D; 
  timerBData    = (unsigned short int *) 0x40010; 

  timerCDControl = (unsigned short int *) 0x4000E;
  timerCData    = (unsigned short int *) 0x40011; 
  timerDData    = (unsigned short int *) 0x40012; 
 
  multiVector			= (unsigned short int *) 0x4000B;
  multiInteruptPending1  = (unsigned short int *) 0x40005;
  multiInteruptPending2  = (unsigned short int *) 0x40006;

  interuptInServiceA = (unsigned short int *) 0x40007;
  interuptInServiceB = (unsigned short int *) 0x40008;

  uartControlRegister    = (unsigned short int *) 0x40014;
  uartReciveStatus       = (unsigned short int *) 0x40015;
  uartData		         = (unsigned short int *) 0x40017;
	

  /* Initiera interupt och timrar */
  SetupEngineTimer(); 
  SetupUart(); 

  /* Blinka lite med lysdioden */
  *multiio=0;  
  wait1();
  *multidir=0xFF;  
  SETBIT(*multiio,0); 
  SETBIT(*multiio,5); 
  wait1();  
  CLRBIT(*multiio,0); 
  CLRBIT(*multiio,5); 
  
    
  /* Tillåt interupter */
  _avben(); 

  /* Stanna i en oändlig loop, timeri och uarti är de interupt funktioner som kommer anropas */
   g:  goto g;
}

/* Anropas när ett helt paket är färdig läst */
void newPacket(struct Packet *currentPacket)
{	
	/* Kolla vad det är för typ av paket */
	switch(currentPacket->operation_code)
	{
		/* Bluetooth Established */
		case 12:
			linkError=0;
			*multiio=0;			
			direction=0;
			gear=1;
		break;
	
		/* Bluetooth "link released */
		case 14:
			linkError=1;
			*multiio=0;			
			break;
		break;
		/* Data in */
		case 16:
			{
				/* Kolla vilket kommando
				currentPacket->data[0]= port
				currentPacket->data[1]= data len
				currentPacket->data[2]= data len (HI) */
				switch(currentPacket->data[3])
				{
				case -1:
					/* Fram åt */					
					direction=1;						
					break;
				case -2:
					/* Bakåt */
					direction=-1;
					break;
				case -3:
					direction=2;
					break;
				case -4:
					direction=3;
					break;
				case 49:					
					gear=1;
					break;
				case 50:
					gear=2;
					break;
				case 51:					
					gear=3;
					break;
				case 52:
					gear=4;
					break;
				case 53:					
					gear=5;
					break;
				case 0:					
					direction=0;
					break;
				}
				break;
			}
	}
}
/* Anropas av uarti när en ny byte är motagen */
void newByte(unsigned short c)
{		
	
	/* Ta reda på var i headern */
	switch(packetpos)
	{
		case 0:
			/* Nollställ paketet */
			currentPacket.datalength=0;			
			/* Början på ett packet börjar alltid med en 2:a */			
			if(c!=2)
			{
				/* första byten skall vara en 2:a, det har blivit fel,
				   tänd den röda lysdioden */
				SETBIT(*multiio,5);
				/* Börja om */
				packetpos=0;
				return;
			}
			break;
		case 1:
			/* Spara paket typen */
			currentPacket.packet_type=c;
			break;
		case 2:
			/* Spara operation code */
			currentPacket.operation_code=c;
			break;
		case 3:
			 /* Spara längden på datan (LO) */
			 currentPacket.datalength=c;
			 break;
		case 4:
			 /* Spara ändra byten för längden (HI) */
			 currentPacket.datalength+=c*(2<<8);
			 break;		
		case 5:
			/* Kontrolera så att det är rimlig datalängd */
			if(currentPacket.datalength >= 100)
			{
				/* Inga paket är så stora, nollställ pekaren och vänta på nytt paket. */
				packetpos=0;
				return;
			}
			currentPacket.checksum=c;
			break;
	};
	/* Ifall packetpos > 4 så är det data som läses in. Spara det i buffern */
	if(packetpos>5 && (currentPacket.datalength-(packetpos-6)) >0)
	{
		buffer[packetpos-6]=c;
	}
	/* Packetpos + 4 är slutet på datan vilket är slutet på paketet. */
	if(packetpos>=currentPacket.datalength+6)
	{
		packetpos=0;
		currentPacket.data=buffer;
		newPacket(&currentPacket);	
	}
	else
		packetpos++;
}
/* Väntar en stund */
void wait1()
{
	int k,j;
	for(k=0;k<1000;k++)
		for(j=0;j<50;j++);
}
void SetupEngineTimer()
{  
   /* Sätta upp timern hastghet */    
  *timerCDControl=3;  
  *timerDData=0xC;

  *multiVector=64;  

  *uartReciveStatus=0;
  
  /* Enable Timer D */
  *interuptMask2=16; 
  *interuptEnable2=16;  
}
void SetupUart()
{
	/* Sätta uart till N,8,1 */
	*uartControlRegister=8;  

	
	/* Enable reciver */
	*uartReciveStatus=1; 

	/* Enable Recive interupt */
	*interuptEnable1=16; 
	*interuptMask1=16; 
}
/* Timer avbrott */
void timeri(void)               
{ 
	int limit=gear*7;

	/* *multiInteruptPending2=0; */
	
	/* Blinkar lite med lysdioden */
	if(ledBlink < 100)	
	{
		SETBIT(*multiio,0);
		if(linkError==1)
			SETBIT(*multiio,5);
	}	
	else
	{
		CLRBIT(*multiio,0);
		if(linkError==1)
			CLRBIT(*multiio,5);

	}
	/* Nollställ räknaren för blinkningen */
	if(ledBlink >  200)
		ledBlink=0;
	/* Öka blinkningen */
	ledBlink++;

	/* Ifall anslutningen med mobiltelefonen inte finns så skall inte motorerna köras */
	if(linkError==1)
		return;

	/* Nollställ PWM räknaren */
	if(pwmCounter >= 21)
		pwmCounter=0;

	if(turnCounter >= 20)
		turnCounter=0;

	
	/* Ifall pwmCounter är under limit så skall motorerna köras */
    if( (pwmCounter < limit && direction!=0) )
	{
			/* höger */
			if(direction==3)
			{
				SetWheelStatus(RIGHT,FORWARD);

				/* Driv lite gran på vänster hjulet, annars orkar inte bilen */
				if(turnCounter < 8)
					SetWheelStatus(LEFT,STOP);			
				else
					SetWheelStatus(LEFT,FORWARD);			
			}

			/* vänster */
			if(direction==2)
			{
				SetWheelStatus(LEFT,FORWARD);

				/* Driv lite gran på höger hjulet, annars orkar inte bilen */
				if(turnCounter < 8)
					SetWheelStatus(RIGHT,STOP);
				else
					SetWheelStatus(RIGHT,FORWARD);
			}
			/* framåt */
			if(direction==1)
			{				
				SetWheelStatus(LEFT,FORWARD);  
				SetWheelStatus(RIGHT,FORWARD);
			}
			/* backa */
			if(direction==-1)
			{
				SetWheelStatus(LEFT,REVERSE);
				SetWheelStatus(RIGHT,REVERSE);
			}
	}
	else
	{		
		SetWheelStatus(LEFT,STOP);
		SetWheelStatus(RIGHT,STOP);

	}
	pwmCounter++;
    
	/* Ändra turnCounter ifall bilen svänger */
	if(direction == 2 || direction==3)	
		turnCounter++;
	
	return; 
}
/* Denna funktion är en interupt rutin som anropas när en byte har kommit in*/
void uarti(void)               
{     
  /* Läs in den nya datan till newbyte */
  newByte(*uartData);

  /* Ifall det är buffer overflow på uarten så läs tills det är ok, detta bör inte hända */
  while(*uartReciveStatus & 0x40);

  /* Tillåt interupts. */
  *multiInteruptPending1=0; 
  return;
}
/* Ändrar status på hjulen */
void SetWheelStatus(int wheel,int direction)
{
	int bit1,bit2,temp;

	if(wheel==RIGHT)
	{
		bit1=3;
		bit2=4;		
	}
	else
	{
		bit1=2;
		bit2=1;
	}
	if(direction==REVERSE)
	{
		temp=bit1;
		bit1=bit2;
		bit2=temp;
	}

	if(direction!=STOP)
	{
		SETBIT(*multiio,bit1);
		CLRBIT(*multiio,bit2);
	}
	else
	{
		CLRBIT(*multiio,bit1);
		CLRBIT(*multiio,bit2);					
	}
}