//	instructions.c
//	Definition of all instruction functions, handling effect of instruction and flags.

#include"instructions.h"

/*	TO DO

!!!!!!!! CHECK THAT idata.value IS USED ACROSS ALL FUNCTIONS, NOT VAL !!!!!!!!!!!!!!
						~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Fix all functions before performing testing

*/


AddData idata;

// Load and Store Instructions

void fLDA(Addressing addr){
	acc = idata.value;
	SetFlagN(acc);
	SetFlagZ(acc);
}

void fLDX(Addressing addr){
	X = idata.value;
	SetFlagN(X);
	SetFlagZ(X);
}

void fLDY(Addressing addr){
	Y = idata.value;
	SetFlagN(Y);
	SetFlagZ(Y);
}

void fSTA(Addressing addr){
	SetMemory(idata.add, acc);
}

void fSTX(Addressing addr){
	SetMemory(idata.add, X);
}

void fSTY(Addressing addr){
	SetMemory(idata.add, Y);
}

// Arithmetic Instructions

void fADC(Addressing addr){
	byte buffer = acc + idata.value + getFlag(flag_C);

	SetFlagV(buffer, acc);

	SetFlag(flag_C, (buffer < acc) ? 1 : 0);
	
	acc = buffer;
	SetFlagN(acc);
	SetFlagZ(acc);
}

void fSBC(Addressing addr){
	byte buffer = acc - idata.value - !getFlag(flag_C);
        
	SetFlagV(buffer, acc);

	SetFlag(flag_C, (buffer > acc) ? 0 : 1);

	acc = buffer;
	
	SetFlagN(acc);
	SetFlagZ(acc);
}

//Increment and Decrement Instructions

void fINC(Addressing addr){
	byte a = idata.value;
	SetMemory(idata.add, ++a);
	SetFlagN(a);
	SetFlagZ(a);
}

void fINX(Addressing addr){
	X++;
	SetFlagN(X);
	SetFlagZ(X);
}

void fINY(Addressing addr){
	Y++;
	SetFlagN(Y);
	SetFlagZ(Y);
}

void fDEC(Addressing addr){
	byte a = idata.value;
	SetMemory(idata.add, --a);
	SetFlagN(a);
	SetFlagZ(a);
}

void fDEX(Addressing addr){
	X--;
	SetFlagN(X);
	SetFlagZ(X);
}

void fDEY(Addressing addr){
	Y--;
	SetFlagN(Y);
	SetFlagZ(Y);
}

// Logical Instructions

void fAND(Addressing addr){
	acc &= idata.value;
	SetFlagN(acc);
	SetFlagZ(acc);
}

void fORA(Addressing addr){
	acc |= idata.value;
	SetFlagN(acc);
	SetFlagZ(acc);
}

void fEOR(Addressing addr){
	acc ^= idata.value;
	SetFlagN(acc);
	SetFlagZ(acc);
}

// Jump, Branch, Compare, and Test Bits

void fJMP(Addressing addr){
	PC = idata.add - idata.length;
}

void fBCC(Addressing addr){
	if (getFlag(flag_C) == 0) PC += (char)idata.value; 
}

void fBCS(Addressing addr){
	if (getFlag(flag_C) == 1) PC += (char)idata.value;
}

void fBEQ(Addressing addr){
	if (getFlag(flag_Z) == 1) PC += (char)idata.value;
}

void fBNE(Addressing addr){
	if (getFlag(flag_Z) == 0) PC += (char)idata.value;
}

void fBMI(Addressing addr){
	if (getFlag(flag_N) == 1) PC += (char)idata.value;
}

void fBPL(Addressing addr){
	if (getFlag(flag_N) == 0) PC += (char)idata.value;
}

void fBVS(Addressing addr){
	if (getFlag(flag_V) == 1) PC += (char)idata.value;
}

void fBVC(Addressing addr){
	if (getFlag(flag_V) == 0) PC += (char)idata.value;
}

void fCMP(Addressing addr){
	SetFlag(flag_C, (acc >= idata.value) ? 1 : 0);
	SetFlag(flag_Z, (acc == idata.value) ? 1 : 0);
	SetFlag(flag_N, (acc  < idata.value) ? 1 : 0);
}

void fCPX(Addressing addr){
	SetFlag(flag_C, (X >= idata.value) ? 1 : 0);
	SetFlag(flag_Z, (X == idata.value) ? 1 : 0);
	SetFlag(flag_N, (X  < idata.value) ? 1 : 0);
}

void fCPY(Addressing addr){
	SetFlag(flag_C, (Y >= idata.value) ? 1 : 0);
	SetFlag(flag_Z, (Y == idata.value) ? 1 : 0);
	SetFlag(flag_N, (Y  < idata.value) ? 1 : 0);
}

void fBIT(Addressing addr){
	SetFlag(flag_N, ((idata.value & flag_N) != 0));
	SetFlag(flag_V, ((idata.value & flag_V) != 0));
	SetFlag(flag_Z, ((idata.value &    acc) == 0));
}

// Shift and Rotate Instructions

void fASL(Addressing addr){
	byte m = (addr == eAccumulator) ? acc : idata.value;
	
	SetFlag(flag_C, (m & 0b10000000));
	m = (m << 1) & 0b11111110;
	SetFlagN(m);
	SetFlagZ(m);
	
	(addr == eAccumulator)
	?	acc = m
	:	SetMemory(idata.add, m);
}

void fLSR(Addressing addr) {
	byte m = (addr == eAccumulator) ? acc : idata.value;
	
	SetFlag(flag_C, (m & 0b00000001));
	m = (m >> 1) & 0b01111111;
	SetFlagN(m);
	SetFlagZ(m);
	
	(addr == eAccumulator)
	?	acc = m
	:	SetMemory(idata.add, m);
}

void fROL(Addressing addr){
	byte m = (addr == eAccumulator) ? acc : idata.value;
	
	byte flag_store = (m & 0b10000000);
	//SetFlag(flag_C, (m & 0b10000000));
	m = (m << 1) & 0b11111110;
	m |= (getFlag(flag_C)) ? 0b00000001 : 0;
	SetFlag(flag_C, flag_store);
	SetFlagN(m);
	SetFlagZ(m);
	
	(addr == eAccumulator)
	?	acc = m
	:	SetMemory(idata.add, m);
}

void fROR(Addressing addr){
	byte m = (addr == eAccumulator) ? acc : idata.value;

	byte flag_store = (m & 0b00000001);
	//SetFlag(flag_C, (m & 0b00000001));
	m = (m >> 1) & 0b01111111;
	m |= (getFlag(flag_C)) ? 0b10000000 : 0;
	SetFlag(flag_C, flag_store);
	SetFlagN(m);
	SetFlagZ(m);
	
	
	(addr == eAccumulator)
	?	acc = m
	:	SetMemory(idata.add, m);
}

// Transfer Instructions

void fTAX(Addressing addr){
	X = acc;
	SetFlagN(X);
	SetFlagZ(X);
}

void fTAY(Addressing addr){
	Y = acc;
	SetFlagN(Y);
	SetFlagZ(Y);
}

void fTXA(Addressing addr){
	acc = X;
	SetFlagN(acc);
	SetFlagZ(acc);
}

void fTYA(Addressing addr){
	acc = Y;
	SetFlagN(acc);
	SetFlagZ(acc);
}

// Stack Instructions

void fTSX(Addressing addr){
	X = S;
}

void fTXS(Addressing addr){
	S = X;
}

void fPHA(Addressing addr){
	SetStack(acc);
}

void fPHP(Addressing addr){
	SetStack(P);
}

void fPLA(Addressing addr){
	acc = GetStack();
}

void fPLP(Addressing addr){
	P = GetStack();
}

// Subroutine Instructions

void fJSR(Addressing addr){
	SetStack ((PC+idata.length) >> 8);
	SetStack(((PC+idata.length) & 0x00FF) - 1);
	PC = idata.add;
	PC -= idata.length;
}

void fRTS(Addressing addr){
	PC  =  (address)(GetStack()) + 1;
	PC += ((address)(GetStack())) << 8;
	PC -= idata.length;
}

void fRTI(Addressing addr){
	P = GetStack(); //NEED TO FIX
	PC  = (address)(GetStack());
	PC += (address)(GetStack() << 8);
}

// Set/Reset Insutrctions

void fCLC(Addressing addr){
	SetFlag(flag_C, 0);
}

void fCLD(Addressing addr){
	SetFlag(flag_D, 0);
}

void fCLI(Addressing addr){
	SetFlag(flag_I, 0);
}

void fCLV(Addressing addr){
	SetFlag(flag_V, 0);
}

void fSEC(Addressing addr){
	SetFlag(flag_C, 1);
}

void fSED(Addressing addr){
	SetFlag(flag_D, 1);
}

void fSEI(Addressing addr){
	SetFlag(flag_I, 1);
}

// NOP/BRK Instructions

void fNOP(Addressing addr){
}

void fBRK(Addressing addr){
	SetStack((((PC+2) & 0xFF00) >> 8));
	SetStack((PC+2) & 0x00FF);
	SetStack(P);
	PC = (address)(GetMemory(0xFFFE));
	PC += ((address)(GetMemory(0xFFFF)) << 8);
}