#include <stddef.h>

static void task1();
static void task2();
static void task3();

void portSWI();

unsigned int PCB1_[17];
unsigned int PCB2_[17];
unsigned int PCB3_[17];
unsigned int PCBmain_[17];
unsigned int STACK1_[1001];
unsigned int STACK2_[1001];
unsigned int STACK3_[1001];
unsigned int (*pTask1)() = &task1;
unsigned int (*pTask2)() = &task2;
unsigned int (*pTask3)() = &task3;

unsigned int * current_PCB=PCBmain_;

int count1=0;
int count2=0;
char count3=126;
short int flag=0;

int result=0, number=0, ready=0, ack=0;


main(int argc, char *argv)
{
   PCB1_[1]=(int)pTask1; //update restart address
   PCB2_[1]=(int)pTask2; //update restart address
   PCB3_[1]=(int)pTask3; //update restart address
   PCB1_[15] = (int) STACK1_+4000; //update stack pointer
   PCB2_[15] = (int) STACK2_+4000; //update stack pointer
   PCB3_[15] = (int) STACK3_+4000; //update stack pointer

#define TEST_REG
#ifdef TEST_REG   
   asm volatile ("MOV		R1, #1"	);	
   asm volatile ("MOV		R4, #4"	);	
   asm volatile ("MOV		R5, #5"	);	
   asm volatile ("MOV		R6, #6"	);
   asm volatile ("MOV		R7, #7"	);	
   asm volatile ("MOV		R8, #8"	);	
   asm volatile ("MOV		R9, #9"	);	
   asm volatile ("MOV		R10, #10"	);	

#endif


   //asm volatile ("NOP");
   asm volatile ("SWI");// Software Interrupt
   //asm volatile (" msr   CPSR_c, # 0x10"); /* USR Mode */
  while(1);//should never return here  
   
}

void portSWI() //Supervisor mode software interrupt function
{
	/* ARM CPU Context, CPSR, Rest Add, Reg File
   ___________________________________________________
   CPSR   | Restart Add| R0| R1| R2| R3| ..... | R14 |
   PCB_[0]|     PCB_[1]|[2]|[3]|[4]|[5]|.......| [16]|
   ___________________________________________________
   */
	/*Store Context in PCB*/  
	asm volatile ("STMFD SP!, {R0-R3}"); //PUSH r0-r1
	asm volatile ("LDR		R0, %0" : : "m" (current_PCB) ); //Load PCB_[0] into R0	
    asm volatile ("MRS R1, SPSR"); //Copy SPSR into R1
	asm volatile ("STMIA R0!, {R1}"); //Store SPSR into PCB[0]
	asm volatile ("STMIA R0!, {LR}"); //Store LR (return address) into PCB[1]
	asm volatile ("ADD R0, R0, #16"); //location of R[4]
	asm volatile ("STMIA R0!, {R4-R10}"); //store R2-R10 into PCB
	asm volatile ("ADD R0, R0, #8"); //location of R[13]
	asm volatile ("STMIA R0, {R13-R14}^");// store R13-R14 into PCB
	
	asm volatile ("SUB R4, R0, #52"); //R3 = PCB_[2]
	asm volatile ("LDMFD SP!, {R0-R3}");//R0, R1 restored from Stack//
	asm volatile ("STMIA R4, {R0-R3}");//R0, R1 stored in PCB
	
	asm volatile ("ADD R4, R4, #44"); //update R4 to point to PCB[13]
	asm volatile ("LDMFD SP!, {R0}");//pop from stack R11
	asm volatile ("STMIA R4, {R0}");//Store R11 in PCB

	asm volatile ("LDMFD SP!, {R0-R3}");//pop from stack 
	
	asm volatile ("ADD R4, R4, #4");//PCB[14]
	asm volatile ("STMIA R4, {R3}");//Store R12 (ip) in PCB
	
	 
   //Add functionality to decide PCB;
 	if (current_PCB==PCBmain_)
			current_PCB=PCB1_;
	else if (current_PCB==PCB1_)
   		current_PCB=PCB2_;
	else if (current_PCB==PCB2_)
		current_PCB=PCB1_;
	else if (current_PCB==PCB3_)
			current_PCB=PCB1_;
	if (flag==1)
		current_PCB=PCB3_;

   // load context from PCB
   asm volatile ("LDR		R0, %0" : : "m" (current_PCB) ); //Load PCB_[0] into R0	
   asm volatile ("LDMIA R0!, {R1, R14}"); //Copy top 2 contents of PCB i.e. SPSR, ret Address into R1, R14
   asm volatile ("MSR SPSR_fsxc, R1"); // Copy Saved StateRegister into usr SPSR
   asm volatile ("LDMIA R0, {R0-R14}^"); //Load from PCB R0-R14
   asm volatile ("NOP"); //Debug 
   asm volatile ("MOVS PC, R14"); //Update Program Counter
      
}

void task1()
{
	current_PCB=PCB1_;
	while(1){
		number=number+1;
		ready=1;
		asm volatile ("SWI");// Software Interrupt
		if (ack==1)
			ack=0;
		else 
			while(1);
        
	    }
	count1++;
}
void task2()
{
	current_PCB=PCB2_;
	while(1)
	{	
		if (ready==1)
		{
				result=number+result;
				ack=1;
				ready=0;
		}
		asm volatile ("SWI");// Software Interrupt
		count2++;
		if (count2==100)
		{	
			count2=0;
			flag=1;	//raise flag
		}
	}
}

void task3 ()
{
	while(1)
	{
		current_PCB=PCB3_;
		count3++;
		flag=0; //lower flag
		asm volatile ("SWI");// Software Interrupt
	}
}