/* third party dummy test process 6 */ 
void test6()
{
	rtx_dbug_outs((CHAR *)"\r\nTest6 Round #1");
	int success = 0;
	int failure = 0;
	int i;
	
	for(i=0; i<32; i++){
		received = g_test_fixture.request_memory_block();
		if(received != 0x00){
			*((CHAR*)received+64) = 'W';
			success++;
			rtx_dbug_outs((CHAR *)"\r\nPASS");
		} else {
			failure++;
			rtx_dbug_outs((CHAR *)"\r\nFAIL: fails to request memory block.");
		}
	}
	
	for(i=0; i<32; i++){
		return_value = g_test_fixture.send_message(1,mblocks[i]);
		if(return_value >= 0){
			success++;
			rtx_dbug_outs((CHAR *)"\r\nPASS");
		} else {
			failure++;
			rtx_dbug_outs((CHAR *)"\r\nFAIL: fails to send message.");
		}
	}
	
	success_rate(success, failure);
  	g_test_fixture.release_processor();
	return;
}

void uart() {
	struct io_message * msg = NULL;
	
	while(1) {
		#ifdef _IO_DEBUG
			rtx_dbug_outs((CHAR *) "Enter: uart - waiting for message\r\n");
		#endif
		
		msg = (io_message *)receive_message(NULL);

		#ifdef _IO_DEBUG
			rtx_dbug_outs((CHAR *) "Enter: uart - received message\r\n");
		#endif
		
		char_out = msg->msg[0];
		release_memory_block((void *)msg);
		
		// irene said this is OK
		while( !(SERIAL1_UCSR & 4) ) { }

		// write data to port
		SERIAL1_WD = char_out;
		
		#ifdef _IO_DEBUG
			rtx_dbug_outs((CHAR *) "Writing data: ");
			rtx_dbug_out_char(char_out);
			rtx_dbug_outs((CHAR *) "\r\n");
		#endif
	}
}

void test4()
{
    
    VOID * tmp41;
    UINT8 tmp4;
    while (1) 
    {
		rtx_dbug_out_char('4');
		rtx_dbug_outs("\r\n");
		
		g_test_fixture.receive_message(&tmp4);
				
                if(tmp4 == 2){
                   rtx_dbug_outs("4 Received Message From 2 \r\n");
                }else{
                   rtx_dbug_outs("ERROR 42 \r\n");
                }
		
	       tmp41 = g_test_fixture.request_memory_block();
               g_test_fixture.send_message(3, tmp41);

        	
//		g_test_fixture.release_processor();
    }
}

//Second part to the test (Test 4)
void mem_test5() {
	rtx_dbug_outs("Starting Memory Test 5\r\n");
	
	//request 31 blocks of mem (Mem should now fill up)
	void * testArray[32];
	
	//request 31 memory blocks
	int i;
	for (i = 0; i < 31; i++) {
		testArray[i] = request_memory_block();
	}
	
	//print_availible_mem_queue();
	//print_used_mem_queue();	
	
	rtx_dbug_outs("Requested 31 blocks. Requesting one more\r\n");
	
	//this should get blocked
	testArray[31] = request_memory_block();
	
	//PROC 4 SHOULD HAPPEN BEFORE THIS AND SHOULD RELEASE IT'S MEMORY
	rtx_dbug_outs_int("Got Block! Address is: \r\n", (int)testArray[31]);
	
	//release all memory blocks
	for (i = 0; i < 32; i++) {
		release_memory_block(testArray[i]);
	}
	
	//print_availible_mem_queue();
	//print_used_mem_queue();	
	
	rtx_dbug_outs("Finished Memory Test 5\r\n");
	
}

/**
 * @brief: Handles all the initilization of the OS
 * @param: stack_start the start of free memory
 */
void init(void* memory_start) {

#ifdef INIT_DEBUG
    rtx_dbug_outs("Initilizating memory...");
#endif

    init_memory(memory_start);

#ifdef INIT_DEBUG
    rtx_dbug_outs("done\r\nInitilizating processes...");
#endif

    init_processes(memory_head);

#ifdef INIT_DEBUG
    rtx_dbug_outs(" done\r\nInitializing priority queues...");
#endif

    init_priority_queues();

#ifdef INIT_DEBUG
    rtx_dbug_outs(" done\r\nInitilizating interrupts...");
#endif

    init_interrupts();

    g_profiler.timer = &timer;

#ifdef INIT_DEBUG
    rtx_dbug_outs(" done\r\n");
#endif

}

void process2() {

	MessageEnvelope * newmessageproc2 = (MessageEnvelope *)g_test_fixture.receive_message(NULL);

	if (newmessageproc2->tx == g_test_proc[0].pid) { 
		
		#ifdef _TEST_DEBUG
		rtx_dbug_outs((CHAR *)"G04_test: test 4 OK\n\r");
		success_count++;
	#endif
		

	} else {
		
		#ifdef _TEST_DEBUG
		rtx_dbug_outs((CHAR *)"G04_test: test 4 FAIL\r\n");
		fail_count++;
	#endif
		
	}

	g_test_fixture.release_memory_block(newmessageproc2);

	//sends message to proc 1
	proc2toproc1 = g_test_fixture.request_memory_block();
	g_test_fixture.send_message(g_test_proc[1].pid, proc2toproc1);
	
	//sends a message to master
	VOID * proc2tomaster = g_test_fixture.request_memory_block();
	g_test_fixture.send_message(g_test_proc[0].pid, proc2tomaster);
	
}

//this tests removing arbitrary blocks from used mem queue
void mem_test6() {
	rtx_dbug_outs("Starting Memory Test 6\r\n");
	
	//request half of the blocks
	void * testArray[16];
	
	//print_availible_mem_queue();
	//print_used_mem_queue();	
	
	int i;
	for (i = 0; i < 16; i++) {
		testArray[i] = request_memory_block();
	}

	rtx_dbug_outs("Blocks requested\r\n");
	
	//remove some from the middle
	release_memory_block(testArray[4]);
	release_memory_block(testArray[10]);
	
	//print_availible_mem_queue();
	//print_used_mem_queue();	
	
	//release all memory blocks
	for (i = 0; i < 16; i++) {
		if (i != 4 && i != 10) {
			release_memory_block(testArray[i]);
		}
	}
	
	rtx_dbug_outs("Finished Memory Test 6\r\n");
	
}

int main( VOID )
{
    int i;
    void* p_mem_array[NUM_MEM_BLKS];

    init_memory();

    for ( i=0; i< NUM_MEM_BLKS; i++ ) 
    {
        p_mem_array[i] = s_request_memory_block();
        if (p_mem_array[i] == NULL) {
            rtx_dbug_outs((CHAR *) "Null pointer.\r\n");
        } else if (p_mem_array[i] > 0x10200000) {
            rtx_dbug_outs((CHAR *) "Memory out of bound. \r\n");
        } else {
            rtx_dbug_outs((CHAR *) "Request memory block: almost OK\r\n");
        }
    }

    for ( i=0; i< NUM_MEM_BLKS; i++ ) 
    {
        int temp;
        temp = s_release_memory_block( p_mem_array[i] );
        if (temp == 0 ) {
            rtx_dbug_outs((CHAR *) "Release memory block: OK\r\n");
        } else {
            rtx_dbug_outs((CHAR *) "Release memory block: Failed \r\n");
        }
    }

    return 0;
}

void process4() {
	MessageEnvelope * newmessage = (MessageEnvelope *)g_test_fixture.receive_message(NULL);
	
	if(newmessage->tx != g_test_proc[0].pid) {
		#ifdef _TEST_DEBUG
		rtx_dbug_outs((CHAR *)"G04_test: test 12 FAIL\n\r");
		fail_count++;
		#endif
	} else if(newmessage->rx != g_test_proc[4].pid) {
		#ifdef _TEST_DEBUG
		rtx_dbug_outs((CHAR *)"G04_test: test 12 FAIL\n\r");
		fail_count++;
		#endif
	} else {
		#ifdef _TEST_DEBUG
		rtx_dbug_outs((CHAR *)"G04_test: test 12 OK\n\r");
		success_count++;
		#endif
	}
	
	g_test_fixture.release_memory_block(newmessage);
	
	//Immediately send a msg back to the master
	VOID * proc4_to_master = g_test_fixture.request_memory_block();
	g_test_fixture.send_message(g_test_proc[0].pid, proc4_to_master);

}

void process5() {
	int failed = 0;
	
	int i;
	for (i = 42; i < 72; i ++) {
		MessageEnvelope * newmessage = (MessageEnvelope *)g_test_fixture.receive_message(NULL);
		
		if (newmessage->type != i) {
			failed = 1;
			//rtx_dbug_outs_int("test 16 - expected: ", i - 42);
			//rtx_dbug_outs_int("test 16 - got: ", newmessage->type - 42);
		}

		g_test_fixture.release_memory_block(newmessage);
	}

	if (failed) {
		#ifdef _TEST_DEBUG
		rtx_dbug_outs("G04_test: test 16 FAIL\r\n");
		#endif

		fail_count ++;
	} else {
		#ifdef _TEST_DEBUG
		rtx_dbug_outs("G04_test: test 16 OK\n\r");
		#endif

		success_count++;
	}

	g_test_fixture.delayed_send(g_test_proc[0].pid, g_test_fixture.request_memory_block(), 50);
}

void test3()
{
  
    UINT8 tmp3;
    while (1) 
    {
		rtx_dbug_out_char('3');
		rtx_dbug_outs("\r\n");
		g_test_fixture.receive_message(&tmp3);
		
		if(tmp3 == 4){
		   rtx_dbug_outs("3 Received Message From 4 \r\n");
		}else{
		   rtx_dbug_outs("ERROR 34 \r\n");
		}
	
		
                g_test_fixture.receive_message(&tmp3);

                if(tmp3 == 5){
                   rtx_dbug_outs("3 Received Message From 5 \r\n");
                }else{
                   rtx_dbug_outs("ERROR 35 \r\n");
                }
	
//		g_test_fixture.release_processor();
    }
}

/* third party dummy test process 2 */ 
void test2()
{
	void* newMsg = g_test_fixture.request_memory_block();
	*((char *)newMsg + 64) = 'c';
	rtx_dbug_outs("Gid_test: TEST2 delay send message from PID1 with 8 second\r\n");
	g_test_fixture.delayed_send(1, newMsg, 8000);
	
	void* newMsg2 = g_test_fixture.request_memory_block();
	*((char *)newMsg2 + 64) = 'a';
	rtx_dbug_outs("Gid_test: TEST2 delay send message from PID1 with 4 second\r\n");
	g_test_fixture.delayed_send(1, newMsg2, 4000);
	
	while (1) {
		g_test_fixture.release_processor();
	}
	
	// nobody sends message to test2. should be blocked forever
	/*int sender;
	void* msg = g_test_fixture.receive_message(&sender);
	int fail = *((int*)msg+26);
	rtx_dbug_outs("Gid_test: TEST1 received message from PID");
	rtx_dbug_out_char(*sender+48);
	rtx_dbug_outs("\r\n");
	fail++;
	rtx_dbug_outs("Gid_test: test 1 FAIL\r\n");
	g_test_fixture.release_memory_block(msg);*/
}

void success_rate(int success, int failure){
	if(failure == 0){
		rtx_dbug_outs((CHAR *)"\r\nOVERALL: PASS\r\n");
	} else {
		rtx_dbug_outs((CHAR *)"\r\nOVERALL: FAIL\r\n");
	}
	RTX_SUCCESS_COUNT += success;
	RTX_FAIL_COUNT += failure;
	return;
}

void  __attribute__ ((section ("__REGISTER_RTX__"))) register_rtx() {
    rtx_dbug_outs((CHAR *)"rtx: Entering register_rtx()\r\n");
    g_test_fixture.send_message = send_message;
    g_test_fixture.receive_message = receive_message;
    g_test_fixture.request_memory_block = request_memory_block;
    g_test_fixture.release_memory_block = release_memory_block;
    g_test_fixture.release_processor = release_processor;
    g_test_fixture.delayed_send = delayed_send;
    g_test_fixture.set_process_priority = set_process_priority;
    g_test_fixture.get_process_priority = get_process_priority;
    rtx_dbug_outs((CHAR *)"rtx: leaving register_rtx()\r\n");
}

/* third party dummy test process 5 */ 
void test5()
{	
	rtx_dbug_outs("Gid_test: TEST5 sets priority of PID6 to 2\r\n");
	g_test_fixture.set_process_priority(6, 2);
	
	void* newMsg = g_test_fixture.request_memory_block();
	rtx_dbug_outs("Gid_test: TEST5 sends message to PID4\r\n");
	g_test_fixture.send_message(4, newMsg);
	
	while (1) {
		g_test_fixture.release_processor();
	}
}

int pcb_print(pcb* in_pcb) {
    if(in_pcb == NULL) {
        return RTX_ERROR;
    }

    /*
    rtx_dbug_outs("in_pcb: ");
    rtx_dbug_out_int16((UINT32)in_pcb);
    rtx_dbug_outs("\n\r");
    rtx_dbug_outs("next: ");
    rtx_dbug_out_int16((UINT32)in_pcb->next);
    rtx_dbug_outs("\n\r");
    rtx_dbug_outs("prev: ");
    rtx_dbug_out_int16((UINT32)in_pcb->prev);
    rtx_dbug_outs("\n\r");
    rtx_dbug_outs("pid: ");
    rtx_dbug_out_int16(in_pcb->pid);
    rtx_dbug_outs("\n\r");
    */
    rtx_dbug_outs("name: ");
    rtx_dbug_outs(in_pcb->name);
    //rtx_dbug_outs("\n\r");
    rtx_dbug_outs("; ");
    rtx_dbug_outs("priority: ");
    rtx_dbug_out_int16(in_pcb->priority);
    //rtx_dbug_outs("\n\r");
    rtx_dbug_outs("; ");
    rtx_dbug_outs("state: ");
    rtx_dbug_outs(get_process_state_name(in_pcb->state));
    rtx_dbug_outs("\n\r");



    return RTX_SUCCESS;
}

int insert_process(pcb * control_block, UINT32 process_priority){
	pcb * current_process;
	if(process_priority < 0 || process_priority > 3){
		rtx_dbug_outs((CHAR *)"\r\n Process priority invalid value.");
		return -1;
	} else {
		control_block->process_priority = process_priority;
	}
	
	if(ready_queue[process_priority] != NULL)
	{
		current_process = ready_queue[process_priority];
		while(current_process->next_process != NULL){
			current_process = current_process->next_process;
		}
		control_block->process_state = READY_STATE;
		current_process->next_process = control_block;
		return 0;
	}
	else
	{
		ready_queue[process_priority] = control_block;
		return 0;
	} 
	
	return -1;
}

/* register the third party test processes with RTX */
void __attribute__ ((section ("__REGISTER_TEST_PROCS__")))register_test_proc()
{
    int i;

    rtx_dbug_outs((CHAR *)"rtx_test: register_test_proc()\r\n");

    for (i =0; i< NUM_TEST_PROCS; i++ ) {
        g_test_proc[i].pid = i + 1;
        g_test_proc[i].priority = 3;
        g_test_proc[i].sz_stack = 2048;
    }

    g_test_proc[0].entry = test1;

    g_test_proc[1].entry = test2;
    g_test_proc[1].priority = 2;

    g_test_proc[2].entry = test3;

    g_test_proc[3].entry = test4;
    g_test_proc[3].priority = 1;

    g_test_proc[4].entry = test5;

    g_test_proc[5].entry = test6;
    g_test_proc[5].priority = 0;
}

int32_t rtx_dbug_uint(uint32_t num)
{
    if (num == 0)
    {
        return rtx_dbug_outs("0");
    }

    char buf[128];
    buf[127] = '\0';
    int i = 126;
    while (num)
    {
        buf[i--] = (num % 10) + '0';
        num /= 10;
    }
    return rtx_dbug_outs(&buf[i+1]);
}

/**
 * Main entry point for this program.
 * never get invoked
 */
int main(void)
{
 #ifdef _DEBUG
    rtx_dbug_outs((CHAR *)"rtx_test: started\r\n");
#endif

    return 0;
}