#include "simdata.h"
#include "cpu.h"
#include <stdlib.h>
#include <math.h>



struct fetch_data_t *malloc_fetch_data(){
	return NULL;
}
void free_fetch_data(__attribute__((unused)) struct fetch_data_t *tofree){
}
int fetch(struct simdata_t *simdata){
	free_instr_list(&simdata->cpu_gui_hints->fetching_list);
	simdata->decode_data->in_bytecode=(uint32_t)(simdata->RAM[simdata->registers->PC])<<24|(uint32_t)(simdata->RAM[simdata->registers->PC+1])<<16|(uint32_t)(simdata->RAM[simdata->registers->PC+2])<<8|(uint32_t)(simdata->RAM[simdata->registers->PC+3]);
	simdata->decode_data->address=simdata->registers->PC;
	add_to_instr_list(&simdata->cpu_gui_hints->fetching_list,simdata->registers->PC);
	simdata->registers->PC+=4;
	return 0;
}




struct decode_data_t *malloc_decode_data(){
	return malloc(sizeof(struct decode_data_t));
}
void free_decode_data(struct decode_data_t *tofree){
	free(tofree);
}
int decode(struct simdata_t *simdata){
	uint16_t opcode,op1,op2,imm;
	free_instr_list(&simdata->cpu_gui_hints->decoding_list);
	switch((simdata->decode_data->in_bytecode&0xE0000000)>>29){
		case 0:
			opcode=(simdata->decode_data->in_bytecode&0x1F000000)>>24;
			switch(opcode){
				case 0x00:
					simdata->exec_data->EXEC_ACTION=NOP;
					break;
				case 0x01:
				case 0x02:
				case 0x03:
				case 0x04:
				case 0x05:
				case 0x11:
				case 0x12:
				case 0x13:
				case 0x14:
				case 0x15:
					switch(opcode){
						case 0x01: case 0x11: simdata->exec_data->COND=NONE; break;
						case 0x02: case 0x12: simdata->exec_data->COND=ZERO; break;
						case 0x03: case 0x13: simdata->exec_data->COND=NZERO; break;
						case 0x04: case 0x14: simdata->exec_data->COND=CARRY; break;
						case 0x05: case 0x15: simdata->exec_data->COND=NCARRY; break;
						default: return 1;
					}
					simdata->exec_data->out_op->OP_ADDR=IMMEDIATE;
					simdata->exec_data->out_op->data=simdata->decode_data->in_bytecode&0x00FFFFFF;
					simdata->exec_data->EXEC_ACTION=(opcode&0x10)?CALL:JUMP;
					break;
				case 0x0F:/* load SP */
					simdata->exec_data->EXEC_ACTION=MOVE;
					simdata->exec_data->in_op1->OP_ADDR=IMMEDIATE;
					simdata->exec_data->in_op1->data=simdata->decode_data->in_bytecode&0x00FFFFFF;
					simdata->exec_data->out_op->OP_ADDR=REGISTER;
					simdata->exec_data->out_op->data=0xFF;
					break;
				case 0x10:
					simdata->exec_data->EXEC_ACTION=RET;
					break;
				default:
					return 1;
			}
			break;
		case 1:
			opcode=(simdata->decode_data->in_bytecode&0x1FFF0000)>>16;
			op1=(simdata->decode_data->in_bytecode&0x0000FF00)>>8;
			op2=(simdata->decode_data->in_bytecode&0x000000FF);


			simdata->exec_data->in_op1->data=op1;
			simdata->exec_data->in_op2->data=op2;
			switch(opcode){
				case 0x00:
				case 0x01:
				case 0x02:
				case 0x03:
				case 0x04:
				case 0x05:
				case 0x06:
				case 0x07:
				case 0x0A:
				case 0x0B:
				case 0x0C:
				case 0x0D:
				case 0x0E:
				case 0x0F:
					simdata->exec_data->in_op1->OP_ADDR=REGISTER;
					simdata->exec_data->in_op2->OP_ADDR=REGISTER;
					break;
				case 0x08:
					simdata->exec_data->in_op1->OP_ADDR=REGISTER;
					simdata->exec_data->in_op2->OP_ADDR=REGISTER_IND;
					break;
				case 0x09:
					simdata->exec_data->in_op1->OP_ADDR=REGISTER_IND;
					simdata->exec_data->in_op2->OP_ADDR=REGISTER;
					break;
			}

			switch(opcode){
				case 0x05:
					simdata->exec_data->EXEC_ACTION=HALT;
					break;
				case 0x06:
					simdata->exec_data->EXEC_ACTION=PUSH;
					break;
				case 0x07:
					simdata->exec_data->EXEC_ACTION=POP;
					break;
				case 0x08:
				case 0x09:
					simdata->exec_data->EXEC_ACTION=MOVE;
					break;
				default:
					simdata->exec_data->EXEC_ACTION=EXEC_ALU;
					break;
			}

			switch(opcode){
				case 0x00:
				case 0x01:
				case 0x04:
				case 0x0A:
				case 0x0B:
				case 0x0C:
				case 0x0D:
					simdata->exec_data->out_op->OP_ADDR=REGISTER;
					simdata->exec_data->out_op->data=op2;
					break;
				case 0x02:
				case 0x03:
				case 0x0E:
				case 0x0F:
					simdata->exec_data->out_op->OP_ADDR=REGISTER;
					simdata->exec_data->out_op->data=op1;
					break;
				case 0x08:
					simdata->exec_data->out_op->data=op2;
					simdata->exec_data->out_op->OP_ADDR=REGISTER_IND;
					break;
				case 0x09:
					simdata->exec_data->out_op->data=op2;
					simdata->exec_data->out_op->OP_ADDR=REGISTER;
					break;
			}


			switch(opcode){
				case 0x00: simdata->exec_data->ALU_OP=ALU_ADD;  break;
				case 0x01: simdata->exec_data->ALU_OP=ALU_SUB;  break;
				case 0x02: simdata->exec_data->ALU_OP=ALU_SL;  break;
				case 0x03: simdata->exec_data->ALU_OP=ALU_SR;  break;
				case 0x04: simdata->exec_data->ALU_OP=ALU_CMP;  break;
				case 0x05:
				case 0x06:
				case 0x07:
				case 0x08:
				case 0x09:
					break;
				case 0x0A: simdata->exec_data->ALU_OP=ALU_FDIV;  break;
				case 0x0B: simdata->exec_data->ALU_OP=ALU_FMUL;  break;
				case 0x0C: simdata->exec_data->ALU_OP=ALU_FADD;  break;
				case 0x0D: simdata->exec_data->ALU_OP=ALU_FSUB;  break;
				case 0x0E: simdata->exec_data->ALU_OP=ALU_FSIN;  break;
				case 0x0F: simdata->exec_data->ALU_OP=ALU_FCOS;  break;
				default:
					return 1;
			}
			break;
		case 2:
			opcode=(simdata->decode_data->in_bytecode&0x1F000000)>>24;
			op1=(simdata->decode_data->in_bytecode&0x00FF0000)>>16;
			imm=(simdata->decode_data->in_bytecode&0x0000FFFF);
			switch(opcode){
				case 0:
					simdata->exec_data->EXEC_ACTION=MOVE;
					simdata->exec_data->in_op1->OP_ADDR=IMMEDIATE;
					simdata->exec_data->in_op1->data=imm;

					simdata->exec_data->out_op->OP_ADDR=REGISTERL;
					simdata->exec_data->out_op->data=op1;
					break;
				case 1:
					simdata->exec_data->EXEC_ACTION=MOVE;
					simdata->exec_data->in_op1->OP_ADDR=IMMEDIATE;
					simdata->exec_data->in_op1->data=imm;

					simdata->exec_data->out_op->OP_ADDR=REGISTERH;
					simdata->exec_data->out_op->data=op1;
					break;
			}
			break;
		default:
			return 1;
	}
	simdata->exec_data->address=simdata->decode_data->address;
	add_to_instr_list(&simdata->cpu_gui_hints->decoding_list,simdata->decode_data->address);
	return 0;
}




struct exec_data_t *malloc_exec_data(){
	struct exec_data_t *ret;
	ret=malloc(sizeof(struct exec_data_t));
	if(!ret)
		return 0;
	ret->in_op1=malloc(sizeof(struct exec_op_t));
	if(!ret->in_op1){
		free(ret);
		return 0;
	}
	ret->in_op2=malloc(sizeof(struct exec_op_t));
	if(!ret->in_op2){
		free(ret->in_op1);
		free(ret);
		return 0;
	}
	ret->out_op=malloc(sizeof(struct exec_op_t));
	if(!ret->out_op){
		free(ret->in_op1);
		free(ret->in_op2);
		free(ret);
		return 0;
	}
	return ret;
}
void free_exec_data(struct exec_data_t *tofree){
	free(tofree->in_op1);
	free(tofree->in_op2);
	free(tofree->out_op);
	free(tofree);
}
int exec(struct simdata_t *simdata){
	free_instr_list(&simdata->cpu_gui_hints->executing_list);
	int condition=0;
	switch(simdata->exec_data->EXEC_ACTION){
		case CALL:
		case JUMP:
			switch(simdata->exec_data->COND){
				case NONE:   condition=1;                              break;
				case ZERO:   condition=simdata->registers->FLAGS&1;    break;
				case NZERO:  condition=!(simdata->registers->FLAGS&1); break;
				case CARRY:  condition=simdata->registers->FLAGS&2;    break;
				case NCARRY: condition=!(simdata->registers->FLAGS&2); break;
			}
			if(condition){
				if(simdata->exec_data->EXEC_ACTION==CALL){
					*(uint32_t*)(simdata->RAM+simdata->registers->SP)=simdata->registers->PC;
					simdata->registers->SP+=4;
				}
				if(simdata->exec_data->out_op->OP_ADDR==IMMEDIATE)
					simdata->registers->PC=(uint32_t)(simdata->exec_data->out_op->data);
				else
					return 1;
			}
			break;
		case RET:
			simdata->registers->SP-=4;
			simdata->registers->PC=simdata->RAM[simdata->registers->SP];
			break;
		case EXEC_ALU:
			if(	simdata->exec_data->in_op1->OP_ADDR==REGISTER &&
					simdata->exec_data->in_op2->OP_ADDR==REGISTER &&
					simdata->exec_data->out_op->OP_ADDR==REGISTER	){

				uint32_t result;
				switch(simdata->exec_data->ALU_OP){
					case ALU_ADD:
						result = simdata->registers->GPR[simdata->exec_data->in_op1->data] +
							simdata->registers->GPR[simdata->exec_data->in_op2->data];
						simdata->registers->FLAGS=(simdata->registers->FLAGS&0xFFFFFFFD)|
							(simdata->registers->GPR[simdata->exec_data->out_op->data] < simdata->registers->GPR[simdata->exec_data->in_op1->data])<<1;
						break;
					case ALU_SUB:
					case ALU_CMP:
						result = simdata->registers->GPR[simdata->exec_data->in_op2->data] -
							simdata->registers->GPR[simdata->exec_data->in_op1->data];
						simdata->registers->FLAGS=(simdata->registers->FLAGS&0xFFFFFFFD)|
							(simdata->registers->GPR[simdata->exec_data->out_op->data] > simdata->registers->GPR[simdata->exec_data->in_op1->data])<<1;
						break;
					case ALU_SL:
						simdata->registers->FLAGS=(simdata->registers->FLAGS&0xFFFFFFFD)|
							((simdata->registers->GPR[simdata->exec_data->in_op1->data]&0x80000000)?2:0);
						result = simdata->registers->GPR[simdata->exec_data->in_op1->data]<<1;
						break;
					case ALU_SR:
						simdata->registers->FLAGS=(simdata->registers->FLAGS&0xFFFFFFFD)|
							((simdata->registers->GPR[simdata->exec_data->in_op1->data]&0x00000001)?2:0);
						result = simdata->registers->GPR[simdata->exec_data->in_op1->data]>>1;
						break;
					case ALU_FDIV:
						*(float*)&result = *(float*)(&simdata->registers->GPR[simdata->exec_data->in_op2->data])/
							*(float*)(&simdata->registers->GPR[simdata->exec_data->in_op1->data]);
						break;
					case ALU_FMUL:
						*(float*)&result = *(float*)(&simdata->registers->GPR[simdata->exec_data->in_op1->data])*
							*(float*)(&simdata->registers->GPR[simdata->exec_data->in_op2->data]);
						break;
					case ALU_FADD:
						*(float*)&result = *(float*)(&simdata->registers->GPR[simdata->exec_data->in_op2->data])+
							*(float*)(&simdata->registers->GPR[simdata->exec_data->in_op1->data]);
						break;
					case ALU_FSUB:
						*(float*)&result = *(float*)(&simdata->registers->GPR[simdata->exec_data->in_op2->data])-
							*(float*)(&simdata->registers->GPR[simdata->exec_data->in_op1->data]);
						break;
					case ALU_FSIN:
						*(float*)&result = sinf(*(float*)(&simdata->registers->GPR[simdata->exec_data->in_op1->data]));
						break;
					case ALU_FCOS:
						*(float*)&result = cosf(*(float*)(&simdata->registers->GPR[simdata->exec_data->in_op1->data]));
						break;
					default:
						return 1;
				}
				simdata->registers->FLAGS=(simdata->registers->FLAGS&0xFFFFFFFE)|(result==0);
				if(simdata->exec_data->ALU_OP!=ALU_CMP)
					simdata->registers->GPR[simdata->exec_data->out_op->data]=result;
			}else
				return 1;
			break;
		case MOVE:
			switch(simdata->exec_data->out_op->OP_ADDR){
				case REGISTERL:
					if(simdata->exec_data->in_op1->OP_ADDR==IMMEDIATE)
						simdata->registers->GPR[simdata->exec_data->out_op->data]=(simdata->registers->GPR[simdata->exec_data->out_op->data]&0xFFFF0000)|(0x0000FFFF&
							simdata->exec_data->in_op1->data);
					else
						simdata->registers->GPR[simdata->exec_data->out_op->data]=(simdata->registers->GPR[simdata->exec_data->out_op->data]&0xFFFF0000)|(0x0000FFFF&
							simdata->registers->GPR[simdata->exec_data->in_op1->data]);
					break;
				case REGISTERH:
					if(simdata->exec_data->in_op1->OP_ADDR==IMMEDIATE)
						simdata->registers->GPR[simdata->exec_data->out_op->data]=(simdata->registers->GPR[simdata->exec_data->out_op->data]&0x0000FFFF)|((0x0000FFFF&
							simdata->exec_data->in_op1->data)<<16);
					else
						simdata->registers->GPR[simdata->exec_data->out_op->data]=(simdata->registers->GPR[simdata->exec_data->out_op->data]&0x0000FFFF)|((0x0000FFFF&
							simdata->registers->GPR[simdata->exec_data->in_op1->data])<<16);
					break;
				case REGISTER: /* This is for special registers like the SP which is 24bits long */
					if(simdata->exec_data->out_op->data==0xFF){
						if(simdata->exec_data->in_op1->OP_ADDR==IMMEDIATE){
							if( (simdata->exec_data->in_op1->data&0xFF000000) == 0 )
								simdata->registers->SP=simdata->exec_data->in_op1->data;
							else
								return 2;
						}else if( simdata->exec_data->in_op1->OP_ADDR==REGISTER ){ /*for completeion, not valid ( yet )*/
							if( (simdata->registers->GPR[simdata->exec_data->in_op1->data]&0xFF000000) == 0 )
								simdata->registers->SP=simdata->registers->GPR[simdata->exec_data->in_op1->data];
							else
								return 2;
						}
					}else if(simdata->exec_data->in_op1->OP_ADDR==REGISTER_IND){
						if( (simdata->registers->GPR[simdata->exec_data->in_op1->data]&0xFF000000) == 0 )
							simdata->registers->GPR[simdata->exec_data->out_op->data]=*(uint32_t*)(simdata->RAM+simdata->registers->GPR[simdata->exec_data->in_op1->data]);
						else
							return 2;
					}else
						return 1;
					break;
				case REGISTER_IND:
					if(simdata->exec_data->in_op1->OP_ADDR==REGISTER){
						*(uint32_t*)(simdata->RAM+(simdata->registers->GPR[simdata->exec_data->out_op->data]&0x00FFFFFF))=simdata->registers->GPR[simdata->exec_data->in_op1->data];
						if(simdata->registers->GPR[simdata->exec_data->out_op->data]==0x00FFFFFC)
							if(terminal_output(simdata->registers->GPR[simdata->exec_data->in_op1->data],simdata))
								return 1;
					}else
						return 1;
					break;
				default:
					return 1;
			}
		case NOP: break;
		case HALT:
			  simdata->cpu_state=CPU_HALTED;
			  break;
		case PUSH:
			  simdata->RAM[simdata->registers->SP  ]=simdata->registers->GPR[simdata->exec_data->in_op1->data]&0x000000FF;
			  simdata->RAM[simdata->registers->SP+1]=(simdata->registers->GPR[simdata->exec_data->in_op1->data]&0x0000FF00)>>8;
			  simdata->RAM[simdata->registers->SP+2]=(simdata->registers->GPR[simdata->exec_data->in_op1->data]&0x00FF0000)>>16;
			  simdata->registers->SP+=4;
			  break;
		case POP:
			  simdata->registers->SP-=4;
			  simdata->registers->GPR[simdata->exec_data->in_op1->data]=simdata->RAM[simdata->registers->SP];
			  simdata->registers->GPR[simdata->exec_data->in_op1->data]|=simdata->RAM[simdata->registers->SP+1]<<8;
			  simdata->registers->GPR[simdata->exec_data->in_op1->data]|=simdata->RAM[simdata->registers->SP+2]<<16;
			  break;
	}
	add_to_instr_list(&simdata->cpu_gui_hints->executing_list,simdata->exec_data->address);
	return 0;
}





int state=0;


/*
 * RETURN CODES:
 * 0 : success
 * 1 : error in execution stage
 * 2 : error in decode stage
 * 3 : error in fetch stage
 * 4 : internal error
 */
int cpu_cycle_clock(struct simdata_t *simdata){
	free_instr_list(&simdata->cpu_gui_hints->fetching_list);
	free_instr_list(&simdata->cpu_gui_hints->decoding_list);
	free_instr_list(&simdata->cpu_gui_hints->executing_list);
	switch(state){
		case 0:
			switch(fetch(simdata)){
				case 0:  break;
				default: return 3;
			}
			break;
		case 1:
			switch(decode(simdata)){
				case 0:  break;
				default: return 2;
			}
			break;
		case 2:
			switch(exec(simdata)){
				case 0:  break;
				case 2:  return 4;
				default: return 1;
			}
			break;
	}
	if(state==2)
		state=0;
	else
		state++;
	return 0;
}


int cpu_simdata_malloc(struct simdata_t *simdata){
	simdata->fetch_data=malloc_fetch_data();
	simdata->decode_data=malloc_decode_data();
	simdata->exec_data=malloc_exec_data();
	simdata->registers=malloc(sizeof(struct registers_t));
	simdata->registers->PC=0x00000000;
	simdata->registers->SP&=0x00FFFFFF; //intentionally leave it uninitialised but within the legal range
	if((simdata->fetch_data!=NULL)&&(simdata->decode_data!=NULL)&&(simdata->exec_data!=NULL))
		return 1;
	else
		return 0;
}

void cpu_simdata_free(struct simdata_t *simdata){
	free_fetch_data(simdata->fetch_data);
	free_decode_data(simdata->decode_data);
	free_exec_data(simdata->exec_data);
	free(simdata->registers);
}