/*
 * This is a simple program to test multi-core microblaze SoC
 */
#include "u_kernel.h"

#include "AVC/DecTest.h"

#define APP_MUTEX_NUM 10
#define STACK_SIZE 4096

volatile kernelTable *KTBL = (kernelTable *) (KERNEL_TABLE_ADDR);
volatile int *main_stack[NUM_CORES];
volatile void *AppThreadPtr[NUM_CORES];


void startApplicationThreads(void);
void stack_init(void);
void core_1_thread(void *parameter);
void core_2_thread(void *parameter);
void core_3_thread(void *parameter);


volatile int* control_reg = (int*) (0xFFF8);
volatile int* read_reg = (int*) (0xFFF8);


int main()
{

    unsigned int core_id = getCurrentCoreID();

    enableCaches();
    microblaze_flush_dcache();

    mutex_lock(10);
    printf("Core %d: Hello World!\r\n", core_id);
    mutex_unlock(10);

    AppThreadPtr[1] = core_1_thread;
    AppThreadPtr[2] = core_2_thread;
    AppThreadPtr[3] = core_3_thread;

    /* lock other core */
//    mutex_lock(INITIAL_MUTEX);
    int i;

    mutex_lock_id(1,0);
    mutex_lock_id(2,0);
    mutex_lock_id(3,0);


    startApplicationThreads();

    /* application part */

    while(1);

//    int wfState = waveFront();
//    if(wfState){
//    	mutex_lock(APP_MUTEX_NUM);
//    	printf("Decode error");
//    	mutex_unlock(APP_MUTEX_NUM);
//    }
//
//	mutex_lock(APP_MUTEX_NUM);
//		printf("app over\n");
//	mutex_unlock(APP_MUTEX_NUM);


    /* end */

    disableCaches();

    return 0;
}


void core_1_thread(void *parameter)
{

    unsigned int core_id = getCurrentCoreID();

    while(1){


    	/* start application */
    	while(1){

//        	mutex_lock( INITIAL_MUTEX );
//        	mutex_unlock(INITIAL_MUTEX);

    		mutex_lock_id(core_id,0);


    		mutex_lock(10);
    		printf("core %d pass \n",arg+1);
    		mutex_unlock(10);

//    		int currState = 0;
//    		currState = thr_decode_one_row(0);
//    		if(currState){
//    			mutex_lock(APP_MUTEX_NUM);
//    			printf("thr_decode_one_row error\n");
//    			mutex_unlock(APP_MUTEX_NUM);
//    		}
//    		else
//    			break;
    	}
    }

    while (1) { /* thread never returns */ };
}


void core_2_thread(void *parameter)
{

    while(1){

    	/* start application */
    	while(1){

//        	mutex_lock( INITIAL_MUTEX );
//        	mutex_unlock(INITIAL_MUTEX);

    		mutex_lock_id(core_id,0);


    		mutex_lock(10);
    		printf("core %d pass \n",arg+1);
    		mutex_unlock(10);

//    		int currState = 0;
//    		currState = thr_decode_one_row(1);
//    		if(currState){
//    			mutex_lock(APP_MUTEX_NUM);
//    			printf("thr_decode_one_row error\n");
//    			mutex_unlock(APP_MUTEX_NUM);
//    		}
//    		else
//    			break;
    	}
    }

    while (1) { /* thread never returns */ };
}


void core_3_thread(void *parameter)
{

	while(1){

		/* start application */
		while(1){

//			mutex_lock( INITIAL_MUTEX );
//			mutex_unlock(INITIAL_MUTEX);

    		mutex_lock_id(core_id,0);


    		mutex_lock(10);
    		printf("core %d pass \n",arg+1);
    		mutex_unlock(10);

//			int currState = 0;
//			currState = thr_decode_one_row(2);
//			if(currState){
//				mutex_lock(APP_MUTEX_NUM);
//				printf("thr_decode_one_row error\n");
//				mutex_unlock(APP_MUTEX_NUM);
//			}
//			else
//				break;
		}
	}

    while (1) { /* thread never returns */ };
}

void startApplicationThreads(void)
{
    /* Tell the slave cores to start running its thread */
    KTBL->applicationThreadPtr[1] = core_1_thread;
    KTBL->applicationThreadPtr[2] = core_2_thread;
    KTBL->applicationThreadPtr[3] = core_3_thread;
    KTBL->bootSync = MASTER_CPU_SYNC_MAGIC_CODE;
}
