#include <stdio.h>
#include <stdlib.h>
#include <malloc.h>
#include <memory.h>
#include "Hardware.h"
#include "Hardware_Tools.h"
#include "ordo.h"

#define MAGICWORD 0xDECAFED

/**
 * Fatal error tools...
 */
void fatal(int errcode,const char *msg)
{
	printf("%s\n",msg);
	exit(errcode);
} 

/**
 * address of the Current Context.
 * N.B. currentCtx is initialized by initCtx()
 *      and it is updated by switchCtx()
 */
static struct CPUCtx *currentCtx = NULL;

/**
 * build an Ctx associated with the very first Ctx...
 */
void initMainCtx( struct CPUCtx *theCtx /* the "main" context */)
{
	currentCtx = theCtx;
}

/**
 * build a new Context with a new call stack and define the linked entry point.
 */
void openCtx( struct CPUCtx *theCtx, /* the context */
              int stackSize,         /* size (in byte) of the associated stack */
              CtxFunc *fct,          /* entry point for this context */
              void *arg)             /* starting arguments */
{
	
	theCtx->stack = malloc(stackSize);
	printf("stack from %X to %X \n",(int)theCtx->stack,(int)&theCtx->stack[stackSize-4]);
	theCtx->savedESP = &theCtx->stack[stackSize-4];
	theCtx->savedEBP = &theCtx->stack[stackSize-4];
	
	theCtx->entryFunc = fct;
	theCtx->entryArgs = arg;
	theCtx->status    = CtxOpen;
	theCtx->magic     = MAGICWORD;

	if(currentCtx == NULL)
	{
		theCtx->next = theCtx->next;
		currentCtx = theCtx ;
	}
	else
	{
		theCtx->next = currentCtx->next ;
		currentCtx->next = theCtx ;
	}

}

/**
 * bounding proc for a the entry point of a new context...
 */
static void startCurrentCtx()
{
	struct CPUCtx *theCtx = currentCtx;
	theCtx->status = CtxRunning;
	printf("...starting Ctx %X with %s arg...\n",(int)theCtx->entryFunc,(char *)theCtx->entryArgs);
	theCtx->entryFunc(theCtx->entryArgs);
	printf("...ending   Ctx %X with %s arg...\n",(int)theCtx->entryFunc,(char *)theCtx->entryArgs);
	if(currentCtx->next == currentCtx)
	{
		currentCtx = NULL;	
		while(1) ;
	}
	else
	{
		struct CPUCtx *aCtx = currentCtx ;
		struct CPUCtx *anotherCtx = currentCtx ;
		
		while( anotherCtx->next!=aCtx ) {
			anotherCtx = anotherCtx->next ;
		}
		anotherCtx->next = aCtx->next ;			
		yield() ;
	}
}

/**
 * switch the current context with the newCtx context.
 */
volatile int yield()
{
	switchToCtx(currentCtx->next);
	return 1;
}

/**
 * switch the current context with the newCtx context.
 */
volatile int switchToCtx(struct CPUCtx *newCtx)
{
	/* is it the first Switch of the main context ? */
	if( currentCtx->status == CtxInit)
		currentCtx->status = CtxRunning ;

	/* is the targetCtx "open" ? */
	if(newCtx->magic != MAGICWORD) 
		fatal(0,"MAGICWORD not initialized...");
		
	/* is the targetCtx activable ? */
	if( (newCtx->status == CtxClose) || (newCtx->status == CtxInit) ) 
		fatal(0,"Cannot switch to a Closed nor an Init Context...");
			
	asm(	"  mov %%esp,%0 " : "=r" (currentCtx->savedESP) );
	asm(	"  mov %%ebp,%0 " : "=r" (currentCtx->savedEBP) );
	currentCtx = newCtx;
	asm(	"  mov %0,%%esp " : : "r" (currentCtx->savedESP) );	
	asm(	"  mov %0,%%ebp " : : "r" (currentCtx->savedEBP) );	
	
	if(currentCtx->status == CtxOpen) /* is it the first time switch ? */
		startCurrentCtx();
	
	return 1;
}


void f2(void *arg)
{
	while(1) {
		printf("%s>running f2().\n",(char *)arg);
	}
}

void f3(void *arg)
{
	while(1) {
		printf("%s>running f3().\n",(char *)arg);
	}
}

void f4(void *arg)
{
	while(1) {
		printf("%s>running f4().\n",(char *)arg);
	}
}

/**
 * default implementation for the interruptions handler... (return !)
 */
void emptyIT() { return ; }

void switchIT() {
	_out(0xF4,128+64+32);
	_out(0xF8,0xFFFFFFFE);
	yield();
}

struct CPUCtx Ctx1;
struct CPUCtx Ctx2;
struct CPUCtx Ctx3;
struct CPUCtx Ctx4;

int main(int argc, char * argv[])
{
	int i;
	/* initialisation du matériel */
	if( initHardware("Hardware.ini") == 0 )
		return 0;

	IRQVECTOR[14] = emptyIT;
	IRQVECTOR[2] = switchIT;
	_out(0xF4,128+64+32);
	_out(0xF8,0xFFFFFFFE);
	_mask(1); /* autoriser toutes les interruptions > 1 */

	/**/
	initMainCtx(&Ctx1);
	openCtx(&Ctx1, 10240, f2, "Ctx1");
	openCtx(&Ctx2, 10240, f3, "Ctx2");
	openCtx(&Ctx3, 10240, f4, "Ctx3");
	/**/
	printf("main> callyield() :o)\n");
	yield();
	printf("main> exit ???\n");
	return 1;		

}