#include "assembly.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

#define MAX_LABEL_SIZE 30
#define MAX_LABEL_SIZE_str "30"

// Max instruction width is 25 characters without the null char
//
// INSTRUCTIONS:
//    BYTE 3       BYTE 2       BYTE 1       BYTE 0
// +===================================================+
// |            |            |            |            |
// +===================================================+
//
//
// Exception:
//      The bytecode 0xFFFFFFFx is an invalid command
//    where x is any nibble
//
//
// INSTRUCTION FORMAT 0:
// +===================================================+
// | OPCODE     |      24 BIT IMMEDIATE ADDRESS        |
// +===================================================+
//
// INSTRUCTION FORMAT 1:
// +===================================================+
// |          OPCODE         | SOURCE REG | DEST REG   |
// +===================================================+
//
// INSTRUCTION FORMAT 2:
// +===================================================+
// |   OPCODE   | DEST REG   |    16 BIT IMMEDIATE     |
// +===================================================+
//
//
//
//  OPCODE FORMAT:
//  ______________________________________________________________________________________________________
//  |BIT | 0   | 1   | 2   | 3   | 4   | 5   | 6   | 7   | 8   | 9   | 10  | 11  | 12  | 13  | 14  | 15  |
//  +----+-----+-----+-----+-----+-----+-----+-----+-----+-----+-----+-----+-----+-----+-----+-----+-----+
//  |    | INSTR. FORMAT   |    8bit opcode num          :          16bit opcode num                     |
//  +----+-----------------+-----------------------------+-----------------------------------------------+
//
//
//  INSTRUCTION FORMAT 0 OPCODE NUM:
//  +-------+----------------------------------+--------------+
//  | NUM   | DESCRIPTION                      | AFFECT FLAGS |
//  +-------+----------------------------------+--------------+
//  | 5'h0  | NOP                              |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h1  | Unconditional Jump               |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h2  | Jump if zero flag is set         |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h3  | Jump if zero flag is not set     |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h4  | Jump if carry flag is set        |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h5  | Jump if carry flag is not set    |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h10 | Unconditional ret                |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h11 | Unconditional call               |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h12 | Call if zero flag is set         |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h13 | Call if zero flag is not set     |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h14 | Call if carry flag is set        |      NO      |
//  +-------+----------------------------------+--------------+
//  | 5'h15 | Call if carry flag is not set    |      NO      |
//
//
//  |       |                                  |              |
//  +-------+----------------------------------+--------------+
//  | 5'h0F | MOV immediate to SP              |              |
//  +-------+----------------------------------+--------------+
//  |       |                                  |              |
//
//
//  INSTRUCTION FORMAT 1 OPCODE NUM:
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | NUM   | DESCRIPTION                                 | AFFECT FLAGS | SOURCE REG |  DEST REG  |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 13'h0 | ADD                                         |      YES     |  operand 1 |  operand 2 |
//  +-------+---------------------------------------------+--------------|------------+------------+
//  | 13'h1 | SUBTRACT                                    |      YES     |  operand 1 |  operand 2 |
//  +-------+---------------------------------------------+--------------|------------+------------+
//  | 13'h2 | SHIFT LEFT                                  |      YES     |   operand  |            |
//  +-------+---------------------------------------------+--------------|------------+------------+
//  | 13'h3 | SHIFT RIGHT                                 |      YES     |   operand  |            |
//  +-------+---------------------------------------------+--------------|------------+------------+
//  | 13'h4 | COMPARE  (SUBTRACT WITHOUT SAVE)            |      YES     |  operand 1 |  operand 2 |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 13'h5 | HALT                                        |      NO      |            |            |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 13'h6 | PUSH                                        |      NO      |   operand  |            |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 13'h7 | POP                                         |      NO      |   operand  |            |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 13'h8 | MOV register to indirect register ( 32-bit )|      NO      |   operand  | (operand)  |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 13'h9 | MOV indirect register to register ( 32-bit )|      NO      |  (operand) |  operand   |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 14'hA | Floating point devision                     |      YES     |   operand  |  operand   |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 14'hB | Floating point Multiplication               |      YES     |  operand 1 |  operand 2 |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 14'hC | Floating point addition                     |      YES     |  operand 1 |  operand 2 |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 14'hD | Floating point subtraction                  |      YES     |  operand 1 |  operand 2 |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 14'hE | Calculate the sin of a register             |      YES     |   operand  |            |
//  +-------+---------------------------------------------+--------------+------------+------------+
//  | 14'hF | Calculate the cos of a register             |      YES     |   operand  |            |
//
//
//  INSTRUCTION FORMAT 2 OPCODE NUM:
//  +-------+----------------------------------------------+--------------+
//  | NUM   | DESCRIPTION                                  | AFFECT FLAGS |
//  +-------+----------------------------------------------+--------------+
//  | 5'd0 | MOV IMMEDIATE TO DEST LOW PART OF REGISTER    |      NO      |
//  +-------+----------------------------------------------+--------------+
//  | 5'd1 | MOV IMMEDIATE TO DEST HIGH PART OF REGISTER   |      NO      |

#define MAX_INSTRUCTION_LENGTH 26

char *disassemble(uint32_t opcode_be){
	uint32_t opcode=(opcode_be&0x000000FF)<<24|(opcode_be&0x0000FF00)<<8|(opcode_be&0x00FF0000)>>8|(opcode_be&0xFF000000)>>24;
	char *ret=malloc(MAX_INSTRUCTION_LENGTH);

	if (!ret)
		return NULL;

	//////// PARSE BITECODE //////////

	int instruction_format,opcode_num;
	uint32_t val;
	uint8_t val1;
	uint8_t val2;

	instruction_format=(opcode&0xE0000000)>>29;
	if(instruction_format==0){
		// 8bit opcode
		opcode_num=(opcode&0x1F000000)>>24;
		val=opcode&0x00FFFFFF;

		switch(opcode_num){
			case 0x00:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"NOP");
				break;
			case 0x10:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"RET");
				break;
			case 0x01:
			case 0x11:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"%s $%06X",(opcode_num&0x10)?"CALL":"JMP",val);
				break;
			case 0x02:
			case 0x03:
			case 0x12:
			case 0x13:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"%s,%s $%06X",(opcode_num&0x10)?"CALL":"JMP",opcode_num&1?"NZ":"Z",val);
				break;
			case 0x04:
			case 0x05:
			case 0x14:
			case 0x15:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"%s,%s $%06X",(opcode_num&0x10)?"CALL":"JMP",opcode_num&1?"NC":"C",val);
				break;
			case 0x0F:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"MOV $%06X,%%SP",val);
				break;
			default:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"UNRECOGNISED INSTR");
				break;
		}
	}else if(instruction_format==1){
		// 16bit opcode
		opcode_num=(opcode&0x1FFF0000)>>16;
		val1=(opcode&0x0000FF00)>>8;
		val2=opcode&0x000000FF;

		switch(opcode_num){
			case 0x00:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"ADD %%R%0d, %%R%0d",val1,val2);
				break;
			case 0x01:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"SUB %%R%0d, %%R%0d",val1,val2);
				break;
			case 0x02:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"SL %%R%0d",val1);
				break;
			case 0x03:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"SR %%R%0d",val1);
				break;
			case 0x04:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"CMP %%R%0d, %%R%0d",val1,val2);
				break;
			case 0x05:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"HALT");
				break;
			case 0x06:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"PUSH %%R%0d",val1);
				break;
			case 0x07:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"POP %%R%0d",val1);
				break;
			case 0x08:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"MOV %%R%0d,(%%R%0d)",val1,val2);
				break;
			case 0x09:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"MOV (%%R%0d),%%R%0d",val1,val2);
				break;
			case 0x0A:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"FDIV %%R%0d,%%R%0d",val1,val2);
				break;
			case 0x0B:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"FMUL %%R%0d,%%R%0d",val1,val2);
				break;
			case 0x0C:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"FADD %%R%0d,%%R%0d",val1,val2);
				break;
			case 0x0D:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"FSUB %%R%0d,%%R%0d",val1,val2);
				break;
			case 0x0E:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"SIN %%R%0d",val1);
				break;
			case 0x0F:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"COS %%R%0d",val1);
				break;
			default:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"UNRECOGNISED INSTRUCTION");
				break;
		}
	}else if(instruction_format==2){
		// 16bit opcode
		opcode_num=(opcode&0x1F000000)>>24;
		val1=(opcode&0x00FF0000)>>16;
		val=opcode&0x0000FFFF;

		switch(opcode_num){
			case 0x00:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"MOV $%04X, %%R%0dl",val,val1);
				break;
			case 0x01:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"MOV $%04X, %%R%0dh",val,val1);
				break;
			default:
				snprintf(ret,MAX_INSTRUCTION_LENGTH,"UNRECOGNISED INSTRUCTION");
				break;
		}
	}else
		snprintf(ret,MAX_INSTRUCTION_LENGTH,"UNRECOGNISED INSTRUCTION");

	return ret;
}

struct imm_ret_t{
	uint8_t status;
	uint32_t value;
	uint8_t size;
};

struct imm_ret_t *parse_immediate(char* str,struct assembler_context_t *assembler_context){
	struct imm_ret_t *imm_ret=malloc(sizeof(struct imm_ret_t));
	if(!imm_ret)
		return 0;
	imm_ret->size=0;
	if(*str=='0'){
		uint32_t addr;
		if(sscanf(str,"0x%08X",&addr)!=1){
			imm_ret->status=1;
			return imm_ret;
		}else{
			imm_ret->size+=2;
			if(str[0]=='0'&&str[1]=='x'){
				int i=2;
				while(1){
					if((str[i]>='0'&&str[i]<='9')||(str[i]>='a'&&str[i]<='f')||(str[i]>='A'&&str[i]<='F'))
						imm_ret->size++;
					else
						break;
					i++;
				}
			}else{
				imm_ret->status=1;
				return imm_ret;
			}
			imm_ret->value=addr;
			imm_ret->status=0;
			return imm_ret;
		}
	}else{
		int select=0;
		if(*str=='<'){
			select=1;
			imm_ret->size++;
			str++;
		}else if(*str=='>'){
			select=2;
			imm_ret->size++;
			str++;
		}

		// Get name size
		int len=0;
		for(int i=0;i<MAX_LABEL_SIZE+1;i++){
			if(i==MAX_LABEL_SIZE){
				imm_ret->status=2;
				return imm_ret;
			}
			if(str[i]==0||str[i]=='\n'||str[i]==' '||str[i]==','||str[i]==')'){
					break;
			}else
					len++;
		}
		imm_ret->size+=len;

		struct label_ll_t *search=assembler_context->label_ll;
		while(search){
			if(strncmp(str,search->name,len)==0){
				if(select==1)
					imm_ret->value=search->address>>16;
				else if(select==2)
					imm_ret->value=search->address&0x0000FFFF;
				else
					imm_ret->value=search->address;
				imm_ret->status=0;
				return imm_ret;
			}
			search=search->next;
		}

		imm_ret->status=2;
		return imm_ret;
	}
}

int64_t assemble_line(char *line, struct assembler_context_t *assembler_context){
	uint16_t opcode;
	int x=0;
	int call;
	while(line[x]==' ')x++;
	if(line[x]==0||line[x]=='#')
		return -1;//empty line or comment
	if( (strncmp(line,"JMP,",4)==0) || (strncmp(line,"JMP ",4)==0) || ((call=strncmp(line,"CALL ",5))==0) ){
		if(!call){
			x=4;
			opcode=0x10;
		}else{
			x=3;
			opcode=0x00;
		}
		if(line[x]==' '){
			opcode|=0x01;
		}else if(line[x]==','){
			x++;
			if(line[x]=='Z'){
				opcode|=0x02;
				x++;
			}else if(line[x]=='C'){
				opcode|=0x04;
				x++;
			}else if(line[x]=='N'){
				x++;
				if(line[x]=='Z'){
					opcode|=0x03;
					x++;
				}else if(line[x]=='C'){
					opcode|=0x09;
					x++;
				}else
					return -2;
			}else
				return -2;
		}else
			return -2;
		while(line[x]==' ')x++;
		if(line[x]=='$'){
			x++;
			struct imm_ret_t *imm=parse_immediate(line+x,assembler_context);
			uint32_t temp;
			if(!imm)
				return -2;
			if(imm->status){
				switch(imm->status){
					default:
						free(imm);
						return -2;
					case 2:
						free(imm);
						return -3;
				}
			}else{
				temp=imm->value;
				free(imm);
				return opcode<<24|(temp&0x00FFFFFF);
			}
		}else
			return -2;
	}else{
		uint8_t r1,r0,params;
		if(strncmp(line,"ADD ",4)==0){
			x=3;
			opcode=0x2000;
			params=2;
		}else if(strncmp(line,"SUB ",4)==0){
			x=3;
			opcode=0x2001;
			params=2;
		}else if(strncmp(line,"SL ",3)==0){
			x=2;
			opcode=0x2002;
			params=1;
		}else if(strncmp(line,"SR ",3)==0){
			x=2;
			opcode=0x2003;
			params=1;
		}else if(strncmp(line,"CMP ",4)==0){
			x=3;
			opcode=0x2004;
			params=2;
		}else if(strncmp(line,"FDIV ",5)==0){
			x=4;
			opcode=0x200A;
			params=2;
		}else if(strncmp(line,"FMUL ",5)==0){
			x=4;
			opcode=0x200B;
			params=2;
		}else if(strncmp(line,"FADD ",5)==0){
			x=4;
			opcode=0x200C;
			params=2;
		}else if(strncmp(line,"FSUB ",5)==0){
			x=4;
			opcode=0x200D;
			params=2;
		}else if(strncmp(line,"SIN ",4)==0){
			x=3;
			opcode=0x200E;
			params=1;
		}else if(strncmp(line,"COS ",4)==0){
			x=3;
			opcode=0x200F;
			params=1;
		}else
			params=0;
		if(params!=0){
			while(line[x]==' ')x++;
			if(line[x]=='%'){
				x++;
				if(line[x]=='R'){
					x++;
					if(line[x]>='0'&&line[x]<='7'){
						r0=line[x]-'0';
						if(params==2){
							x++;
							while(line[x]==' ')x++;
							if(line[x]==','){
								x++;
								while(line[x]==' ')x++;
								if(line[x]=='%'){
									x++;
									if(line[x]=='R'){
										x++;
										if(line[x]>='0'&&line[x]<='7'){
											r1=line[x]-'0';
											return (opcode<<16)|r0<<8|r1;
										}else
											return -2;
									}else
										return -2;
								}else
									return -2;
							}else
								return -2;
						}else
							return (opcode<<16)|r0<<8;
					}else
						return -2;
				}else
					return -2;
			}else
				return -2;
		}else if(strncmp(line,"MOV ",4)==0){
			uint32_t data;
			x=3;
			while(line[x]==' ')x++;
			if(line[x]=='$'){
				x++;
				struct imm_ret_t *imm=parse_immediate(line+x,assembler_context);
				if(!imm)
					return -2;
				if(imm->status){
					switch(imm->status){
						default:
							free(imm);
							return -2;
						case 2:
							free(imm);
							return -3;
					}
				}else{
					data=imm->value;
					x+=imm->size;
					free(imm);
				}
				while(line[x]==' ')x++;
				if(line[x]!=','){
					x+=2;
					while(line[x]==' ')x++;
				}
				if(line[x]==','){
					x++;
					while(line[x]==' ')x++;
					if(line[x]=='%'){
						x++;
						if(line[x]=='R'){
							x++;
							if(line[x]>='0'&&line[x]<='7'){
								r1=line[x]-'0';
								x++;
								if(line[x]=='l')
									return 0x40000000|r1<<16|(data&0xFFFF);
								else if(line[x]=='h')
									return 0x41000000|r1<<16|(data&0xFFFF);
								else
									return -2;
							}else
								return -2;
						}else if(line[x]=='S'){
							x++;
							if(line[x]=='P'){
								return 0x0F000000|(data&0x00FFFFFF);
							}else
								return -2;
						}else
							return -2;
					}else
						return -2;
				}else
					return -2;
			}else if(line[x]=='%'){
				x++;
				if(line[x]=='R'){
					x++;
					if(line[x]>='0'&&line[x]<='7'){
						r0=line[x]-'0';
						x++;
						while(line[x]==' ')x++;
						if(line[x]==','){
							x++;
							while(line[x]==' ')x++;
							if(line[x]=='('){
								x++;
								while(line[x]==' ')x++;
								if(line[x]=='%'){
									x++;
									if(line[x]=='R'){
										x++;
										if(line[x]>='0'&&line[x]<='7'){
											r1=line[x]-'0';
											x++;
											while(line[x]==' ')x++;
											if(line[x]==')'){
												return 0x20080000|(r0&0xFF)<<8|(r1&0xFF);
											}else
												return -2;
										}else
											return -2;
									}else
										return -2;
								}else
									return -2;
							}else
								return -2;
						}else
							return -2;
					}else
						return -2;
				}else
					return -2;
			}else if(line[x]=='('){
				while(line[x]==' ')x++;
				x++;
				if(line[x]=='%'){
					x++;
					if(line[x]=='R'){
						x++;
						if(line[x]>='0'&&line[x]<='7'){
							r0=line[x]-'0';
							x++;
							while(line[x]==' ')x++;
							if(line[x]==')'){
								x++;
								while(line[x]==' ')x++;
								if(line[x]==','){
									x++;
									while(line[x]==' ')x++;
									if(line[x]=='%'){
										x++;
										if(line[x]=='R'){
											x++;
											if(line[x]>='0'&&line[x]<='7'){
												r1=line[x]-'0';
												return 0x20090000|(r0&0xFF)<<8|(r1&0xFF);
											}else
												return -2;
										}else
											return -2;
									}else
										return -2;
								}else
									return -2;
							}else
								return -2;
						}else
							return -2;
					}else
						return -2;
				}else
					return -2;
			}else
				return -2;
		}else if(strncmp(line,"HALT ",5)==0||strncmp(line,"HALT\0",5)==0){
			x=4;
			while(line[x]==' ')x++;
			if(line[x]==0)
				return 0x20050000;
			else
				return -2;
		}else if(strncmp(line,"RET ",4)==0||strncmp(line,"RET\0",4)==0){
			return 0x10000000;
		}else if(*line==':'){
			return -1;
		}else if(strncmp(line,"PUSH ",5)==0){
			x=4;
			while(line[x]==' ')x++;
			if(line[x]=='%'){
				x++;
				if(line[x]=='R'){
					x++;
					if(line[x]>='0'&&line[x]<='7')
						return 0x20060000|(line[x]-'0')<<8;
					else
						return -2;
				}
			}else
				return -2;
		}else if(strncmp(line,"POP ",4)==0){
			x=3;
			while(line[x]==' ')x++;
			if(line[x]=='%'){
				x++;
				if(line[x]=='R'){
					x++;
					if(line[x]>='0'&&line[x]<='7')
						return 0x20070000|(line[x]-'0')<<8;
					else
						return -2 ;
				}
			}else
				return -2;
		}else if(strncmp(line,"DDW ",4)==0){
			x=4;
			while(line[x]==' ')x++;
			if(line[x]=='$'){
				x++;
				struct imm_ret_t *imm=parse_immediate(line+x,assembler_context);
				uint32_t temp;
				if(!imm)
					return -2;
				if(imm->status){
					switch(imm->status){
						default:
							free(imm);
							return -2;
						case 2:
							free(imm);
							return -3;
					}
				}else{
					temp=imm->value;
					free(imm);
					return (temp&0xFF000000)>>24|(temp&0x00FF0000)>>8|(temp&0x0000FF00)<<8|(temp&0x000000FF>>24);
				}
			}else
				return -2;
		}
		return -2;//TODO: figure out which statemnt falls through to this
	}
}

int assembler_context_process(char* line,struct assembler_context_t *assembler_context){
	int x=0;
	switch(line[0]){
		case ':':
			if(line[1]==0)
				return 1;
			else{
				char* label=malloc(MAX_LABEL_SIZE+1);
				sscanf(line+1,"%"MAX_LABEL_SIZE_str"s",label);
				struct label_ll_t **current;
				current=&(assembler_context->label_ll);
				while(*current)
					current=&((*current)->next);
				*current=malloc(sizeof(struct label_ll_t));
				(*current)->name=label;
				(*current)->address=assembler_context->current_location;
				(*current)->next=NULL;
			}
			break;
		case 0:
			return 0;
		case ' ':
			while(line[x]==' ')x++;
			if(line[x]==0)
				return 0;
			else
				return 1;
		case '#':
			return 0;
		default:
			assembler_context->current_location+=4;

	}
	return 0;
}

struct assembler_context_t *malloc_assembler_context(){
	struct assembler_context_t *ret;
	ret=malloc(sizeof(struct assembler_context_t));
	ret->current_location=0x00000000;
	ret->label_ll=NULL;
	return ret;
}

void free_assembler_context(struct assembler_context_t *tofree){
	struct label_ll_t *tofree_ll=tofree->label_ll;
	while(tofree_ll){
		struct label_ll_t *temp=tofree_ll;
		tofree_ll=tofree_ll->next;
		free(temp->name);
		free(temp);
	}
	free(tofree);
}