#include "core_methods.h"

#define BCI_CORE_REGISTER_CHECK(var) if(var > BCI_CORE_NUM_REG) return 1

// #1 -> $0
char bci_cm_ldi(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);

	uint16_t arg;
	char res;
	res = machine_state_t_get_word(state, state->program_counter + 1, &arg);
	if(res)
	{
		return res;
	}

	state->data_reg[small_arg] = arg;

	return machine_state_t_default_afterexec(state, 1);
}

// @($1) -> $0
char bci_cm_ld(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);

	uint16_t arg;
	char res;
	res = machine_state_t_get_word(state, state->program_counter + 1, &arg);
	if(res)
	{
		return res;
	}

	BCI_CORE_REGISTER_CHECK(arg);
	arg = state->data_reg[arg];

	res = machine_state_t_get_mem(state, arg, &arg);
	if(res)
	{
		return res;
	}
	state->data_reg[small_arg] = arg;

	return machine_state_t_default_afterexec(state, 1);
}

// $0 -> @($1)
char bci_cm_st(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);
	char res;

	uint16_t arg;
	res = machine_state_t_get_word(state, state->program_counter + 1, &arg);
	if(res)
	{
		return res;
	}

	BCI_CORE_REGISTER_CHECK(arg);
	arg = state->data_reg[arg];

	res = machine_state_t_set_mem(state, arg, state->data_reg[small_arg]);
	if(res)
	{
		return res;
	}

	return machine_state_t_default_afterexec(state, 1);
}

// $0 + 1 -> $0
char bci_cm_inc(uint8_t small_arg, machine_state_t state)
{
	int32_t result;
	BCI_CORE_REGISTER_CHECK(small_arg);
	result = state->data_reg[small_arg] + 1;
	state->data_reg[small_arg] = result & 0xffff;
	state->status_reg = result >> 16;
	return machine_state_t_default_afterexec(state, 0);
}

// $0 - 1 -> $0
char bci_cm_dec(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);
	int32_t result = state->data_reg[small_arg] - 1;
	state->data_reg[small_arg] = result & 0xffff;
	state->status_reg = result >> 16;
	return machine_state_t_default_afterexec(state, 0);
}

// $0 + $1 -> $0
char bci_cm_add(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);
	uint16_t arg;
	char res;
	res = machine_state_t_get_word(state, state->program_counter + 1, &arg);
	if(res)
	{
		return res;
	}

	BCI_CORE_REGISTER_CHECK(arg);
	int32_t result = state->data_reg[small_arg] + state->data_reg[arg];
	state->data_reg[small_arg] = result & 0xffff;
	state->status_reg = result >> 16;
	
	return machine_state_t_default_afterexec(state, 1);
}


// $0 - $1 -> $0
char bci_cm_sub(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);
	uint16_t arg;
	char res;
	res = machine_state_t_get_word(state, state->program_counter + 1, &arg);
	if(res)
	{
		return res;
	}

	BCI_CORE_REGISTER_CHECK(arg);
	int32_t result = state->data_reg[small_arg] - state->data_reg[arg];
	
	state->data_reg[small_arg] = result & 0xffff;
	state->status_reg = result >> 16;

	return machine_state_t_default_afterexec(state, 1);
}


// $0 * $1 -> $0
char bci_cm_mul(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);
	uint16_t arg;
	char res;
	res = machine_state_t_get_word(state, state->program_counter + 1, &arg);
	if(res)
	{
		return res;
	}

	BCI_CORE_REGISTER_CHECK(arg);
	int32_t result = state->data_reg[small_arg] * state->data_reg[arg];
	state->data_reg[small_arg] = result & 0xffff;
	state->status_reg = result >> 16;
	return machine_state_t_default_afterexec(state, 1);
}

// $0 / $1 -> $0
char bci_cm_div(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);
	uint16_t arg;
	char res;
	res = machine_state_t_get_word(state, state->program_counter + 1, &arg);
	if(res)
	{
		return res;
	}

	BCI_CORE_REGISTER_CHECK(arg);
	int32_t result = state->data_reg[small_arg] / state->data_reg[arg];
	state->data_reg[small_arg] = result & 0xffff;
	state->status_reg = result >> 16;
	return machine_state_t_default_afterexec(state, 1);
}


// if($0 > 0): 1 -> $st_reg
// else: 0 -> $st_reg
char bci_cm_gt(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);

	if(state->data_reg[small_arg] > 0)
	{
		state->status_reg = 1;
	}
	else
	{
		state->status_reg = 0;
	}
	return machine_state_t_default_afterexec(state, 0);
}

// if($0 >= 0): 1 -> $st_reg
// else: 0 -> $st_reg
char bci_cm_ge(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);

	if(state->data_reg[small_arg] >= 0)
	{
		state->status_reg = 1;
	}
	else
	{
		state->status_reg = 0;
	}
	return machine_state_t_default_afterexec(state, 0);
}
// if($0 < 0): 1 -> $st_reg
// else: 0 -> $st_reg
char bci_cm_lt(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);

	if(state->data_reg[small_arg] < 0)
	{
		state->status_reg = 1;
	}
	else
	{
		state->status_reg = 0;
	}
	return machine_state_t_default_afterexec(state, 0);
}
// if($0 <= 0): 1 -> $st_reg
// else: 0 -> $st_reg
char bci_cm_le(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);

	if(state->data_reg[small_arg] <= 0)
	{
		state->status_reg = 1;
	}
	else
	{
		state->status_reg = 0;
	}
	return machine_state_t_default_afterexec(state, 0);
}
// if($0 == 0): 1 -> $st_reg
// else: 0 -> $st_reg
char bci_cm_eq(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);

	if(state->data_reg[small_arg] == 0)
	{
		state->status_reg = 1;
	}
	else
	{
		state->status_reg = 0;
	}
	return machine_state_t_default_afterexec(state, 0);
}

// $0 -> $pc; 0 -> $st_reg
char bci_cm_jmp(uint8_t small_arg, machine_state_t state)
{
	BCI_CORE_REGISTER_CHECK(small_arg);
	uint16_t arg = state->data_reg[arg];

	state->status_reg = 0;
	state->program_counter = arg;
	return 0;
}

// if($st_reg): $0 -> $pc; 0 -> $st_reg
// else: do nothing
char bci_cm_cjmp(uint8_t small_arg, machine_state_t state)
{
	uint16_t arg = small_arg;
	BCI_CORE_REGISTER_CHECK(small_arg);
#ifdef DEBUG
	printf("MSG: cjmp: arg=0x%x (=0x%x)\n", arg, state->data_reg[arg]);
#endif
	arg = state->data_reg[arg];

	if(state->status_reg)
	{
		state->program_counter = arg;
		state->status_reg = 0;
		return 0;
	}

	return machine_state_t_default_afterexec(state, 0);
}
// $pc -> @stack
// $0 -> $pc
char bci_cm_call(uint8_t small_arg, machine_state_t state)
{
	uint16_t arg;
	BCI_CORE_REGISTER_CHECK(small_arg);
	arg = state->data_reg[arg];

	char res = bci_stack_t_push(&(state->stack), state->program_counter);
	if(res)
	{
		return res;
	}
	state->program_counter = arg;
	return 0;
}
// if($st_reg): $pc -> @stack; $0 -> $pc; 0 -> $st_reg
// else: do nothing
char bci_cm_ccall(uint8_t small_arg, machine_state_t state)
{
	uint16_t arg;
	char res;
	BCI_CORE_REGISTER_CHECK(small_arg);
	arg = state->data_reg[small_arg];

	if(state->status_reg)
	{
		state->program_counter = arg;
		res = bci_stack_t_push(&(state->stack), state->program_counter);
		if(res)
		{
			return res;
		}
		return 0;
	}

	state->status_reg = 0;
	return machine_state_t_default_afterexec(state, 0);
}
// @stack -> $pc
char bci_cm_ret(uint8_t small_arg, machine_state_t state)
{
	uint16_t result;
	char res = bci_stack_t_pop(&(state->stack), &result);
	if(res)
	{
		return res;
	}

	state->program_counter = result;
	return 0;
}

// 0 -> $st_reg
char bci_cm_cl(uint8_t small_arg, machine_state_t state)
{
	state->status_reg = 0;
	return machine_state_t_default_afterexec(state, 0);
}

// 1 -> $su_reg
char bci_cm_stop(uint8_t small_arg, machine_state_t state)
{
	state->shutdown_reg = 1;
	return machine_state_t_default_afterexec(state, 0);
}

// if($st_reg): 0 -> $st_reg
// else: 1 -> $st_reg
char bci_cm_not(uint8_t small_arg, machine_state_t state)
{
	if(state->status_reg)
	{
		state->status_reg = 0;
	}
	else
	{
		state->status_reg = 1;
	}
	return machine_state_t_default_afterexec(state, 0);
}