#include "asmmon.h"

#define AM_ADDR (AM_ZM | AM_ZMX | AM_ZMY | \
		 AM_IND | AM_INDX | AM_INDY | \
		 AM_ABS | AM_ABX | AM_ABY | \
		 AM_AIND | AM_AINDX | AM_AINDY | \
		 AM_INDX2 | AM_ZM2)

static const uint64_t mem_size = 0x04000000; /* Size of address space. */

token *tok_global;

uint8_t isexpr(uint8_t type, uint8_t dbg) {
	switch (type) {
		case EXPR_PLUS:
		case EXPR_MINUS:
		case EXPR_LOW:
		case EXPR_HIGH:
		case EXPR_OR:
		case EXPR_LSHFT:
		case EXPR_RSHFT:
			return 1;
		default:
			return 0;
	}
}

uint8_t get_rs(token *t, uint8_t dbg) {
	if (t->id == TOK_RS) {
		return t->type;
	} else {
		return 0xFF;
	}
}

uint8_t get_of(token *t, uint8_t dbg) {
	if (t->id == TOK_OF) {
		return t->type;
	} else {
		return 0xFF;
	}
}

uint8_t get_ind(uint8_t mne, uint8_t am, uint8_t dbg) {
	uint8_t base_idx = 0;
	uint8_t offset = 0;
	switch (mne) {
		case CMP: base_idx = CMP_IND; break;
		case CPB: base_idx = CPB_IND; break;
		case JMP: base_idx = JMP_IND; break;
		case JSR: base_idx = JSR_IND; break;
		case LDA: base_idx = LDA_IND; break;
		case LDB: base_idx = LDB_IND; break;
		case LDX: base_idx = LDX_IND; break;
		case LDY: base_idx = LDY_IND; break;
		case STA: base_idx = STA_IND; break;
		case STB: base_idx = STB_IND; break;
		case STX: base_idx = STX_IND; break;
		case STY: base_idx = STY_IND; break;
	}
	switch (am) {
		case IND : offset  = 0; break;
		case INDY: offset += 1; break;
		case INDX: offset += 2; break;
	}
	return base_idx + offset;
}

uint8_t get_eind(uint8_t mne, uint8_t dbg) {
	switch (mne) {
		case DEC: return DEC_EIND;
		case INC: return INC_EIND;
		case STY: return STY_EIND;
		case STA: return STA_EIND;
		case STB: return STB_EIND;
		case LDX: return LDX_EIND;
		case STX: return STX_EIND;
		case CPB: return CPB_EIND;
		case CPX: return CPX_EIND;
		case CPY: return CPY_EIND;
	}
	return 0xFF;
}

uint8_t get_ext_ortho(uint8_t mne, uint8_t dbg) {
	switch (mne) {
		case LEA: return OP_LEA;
		case PEA: return OP_PEA;
		case ADD: return OP_ADD;
		case SUB: return OP_SUB;
		case NOT: return OP_NOT;
		case CLZ: return OP_CLZ;
		case CLO: return OP_CLO;
		case SWP: return OP_SWP;
		case PCN: return OP_PCN;
	}
	return 0xFF;
}

static void write_value(uint64_t value, uint64_t address, uint8_t size) {
	if (address < mem_size) {
		size = (size > 7) ? 7 : size;
		#if 1
		if (size < 7) {
			uint64_t mask = (-(uint64_t)1 >> ((7 - size) * 8));
			*(uint64_t *)(addr+address) = (*(uint64_t *)(addr+address) & ~mask) | (value & mask);
		} else {
			*(uint64_t *)(addr+address) = value;
		}
		#else
		memcpy(addr+address, &value, size+1);
		#endif
	}
}

uint64_t get_val(token *t, uint64_t addr, uint8_t size, uint8_t end_expr, uint8_t stop_comma, uint8_t dbg) {
	uint64_t value = 0;
	uint64_t tmp_val = 0;
	uint8_t type = EXPR_NONE;
	uint8_t isstart = 1;
	int done = 0;
	do {
		if (t->id == TOK_EXPR) {
			type = t->type;
			t = t->next;
		}
		if (stop_comma && t->subtype == TOK_CSV) {
			done = 1;
		}
		if (stop_comma && t->id == TOK_REG) {
			break;
		}
		switch (t->id) {
			case TOK_HEX:
			case TOK_DEC:
			case TOK_BIN:
			case TOK_CHAR: tmp_val = t->qword; t = t->next; break;
			case TOK_SYM:
			case TOK_LABEL:
				for (; t->sym && t->sym->isstruct && t->next && t->next->id == TOK_SYM; t = t->next);
				tmp_val = (t->sym) ? t->sym->val : addr;
				t = t->next;
				break;
			default: tmp_val = 0;
		}
		if (end_expr != 0xFF && type == end_expr) {
			break;
		}
		switch (type) {
			case EXPR_PLUS : (isstart) ? (value =  tmp_val) : (value += tmp_val); break;
			case EXPR_MINUS: (isstart) ? (value = -tmp_val) : (value -= tmp_val); break;
			case EXPR_OR   : value  |= tmp_val; break;
			case EXPR_LSHFT: value <<= tmp_val; break;
			case EXPR_RSHFT: value >>= tmp_val; break;
			case EXPR_LOW  :
				value = tmp_val;
				switch (size) {
					default:
					case 2 : value &= 0xFFFFFFFF; break;
					case 1 : value &= 0x0000FFFF; break;
					case 0 : value &= 0x000000FF; break;
				}
				break;
			case EXPR_HIGH :
				value = tmp_val;
				switch (size) {
					default:
					case 2 : value >>= 0x20; break;
					case 1 : value >>= 0x10; break;
					case 0 : value >>= 0x08; break;
				}
				break;
			case EXPR_NONE : value  = tmp_val; break;
		}
		isstart = 0;
		if (dbg) {
			printf("get_val(): Value: $%"PRIX64", Expression type: $%X, Expression Value: $%"PRIX64".\n", value, type, tmp_val);
		}
	} while (!done && t && t->id == TOK_EXPR && isexpr(t->type, dbg));
	return value;
}

token *skip_expr(token *t, uint8_t end_expr, uint8_t stop_comma, uint8_t dbg) {
	int done = 0;
	int is_reg = 0;
	do {
		is_reg = (t && t->next && t->next->id == TOK_REG);
		t = (t->id == TOK_EXPR && !is_reg) ? t->next : t;
		if (is_reg || (stop_comma && (t->subtype == TOK_CSV))) {
			done = 1;
		}
		switch (t->id) {
			case TOK_HEX  :
			case TOK_DEC  :
			case TOK_BIN  :
			case TOK_CHAR :
			case TOK_SYM  :
			case TOK_LABEL: t = t->next; break;
		}
		if (end_expr != 0xFF && t->id == TOK_EXPR && t->type == end_expr) {
			break;
		}
	} while (!done && t && t->id == TOK_EXPR && isexpr(t->type, dbg));
	return t;
}



uint8_t get_directivesize(uint8_t type, uint8_t dbg) {
	switch (type) {
		case DIR_QWORD: return 3;
		case DIR_DWORD: return 2;
		case DIR_WORD : return 1;
		case DIR_BYTE : return 0;
	}
	return 0;
}

uint16_t handle_struct(line **ln, uint64_t address, uint16_t offset, uint8_t dbg) {
	uint8_t is_struct = 0;
	uint8_t done = 0;
	uint8_t ismember = 0;
	uint16_t size = 0;
	uint16_t member_size = 0;
	line *l = *ln;
	symbol *strct = NULL;
	token *tok = l->tok;

	for (uint8_t found = 0; tok && !found; tok = tok->next) {
		switch (tok->id) {
			case TOK_DIR:
				is_struct = (tok->type == DIR_STRUCT);
				found = (tok->type == DIR_STRUCT || tok->type == DIR_UNION);
				break;
			case TOK_STRUCT: is_struct = 1;
			case TOK_UNION : found = 1; break;
		}
	}
	if (tok != NULL) {
		strct = tok->sym;
	}

	if (l && l->next) {
		l = l->next;
	}

	for (; l && !done; l = l->next) {
		token *t = l->tok;
		token *start = t;
		symbol *member;
		for (; t && !done; t = t->next) {
			switch (t->id) {
				case TOK_MEMBER: ismember = 1; member = t->sym; break;
				case TOK_DIR   :
					ismember = (t->type == DIR_STRUCT || t->type == DIR_UNION) ? 1 : ismember;
					done = ((is_struct && t->type == DIR_ENDSTRUCT) || (!is_struct && t->type == DIR_ENDUNION));
					if (!done && ismember) {
						switch (t->type) {
							case DIR_BYTE  : member_size = 1; break;
							case DIR_WORD  : member_size = 2; break;
							case DIR_DWORD : member_size = 4; break;
							case DIR_QWORD : member_size = 8; break;
							case DIR_UNION :
							case DIR_STRUCT: member_size = handle_struct(&l, address, offset, dbg); break;
							case DIR_RES   : member_size = get_val(t, address, 3, 0xFF, 0, dbg); t = skip_expr(t, 0xFF, 0, dbg); break;
						}
						if (member && t->type != DIR_UNION && t->type != DIR_STRUCT) {
							member->val = offset;
						}
						if (is_struct) {
							size += member_size;
							offset += member_size;
						} else if (size < member_size) {
							size = member_size;
						}
					}
					ismember = 0;
					break;
			}
		}
		if (done) {
			break;
		}
	}
	*ln = l;
	if (strct != NULL) {
		strct->val = size;
	}
	return size;
}

uint64_t handle_directive(token *t, bytecount *bc, uint8_t isasm, uint64_t address, uint8_t dbg) {
	union reg val;
	uint8_t c = 0;
	uint8_t tmp = 0;
	uint8_t type = t->type;
	uint64_t tmpaddr = address;
	t = t->next;
	for (; t; t = t->next) {
		tmp = 0;
		switch (t->id) {
			case TOK_HEX:
			case TOK_DEC:
			case TOK_BIN:
			case TOK_CHAR:
			case TOK_SYM:
			case TOK_LABEL:
				val.u64 = get_val(t, tmpaddr, get_directivesize(type, dbg), 0xFF, 0, dbg);
				switch (type) {
					case DIR_QWORD: tmp = 8; break;
					case DIR_DWORD: tmp = 4; break;
					case DIR_WORD : tmp = 2; break;
					case DIR_BYTE : tmp = 1; break;
				}
				write_value(val.u64, tmpaddr, tmp-1);
				tmpaddr += tmp;
				bc->datasize += tmp;
				if (t->next && t->next->id == TOK_EXPR && isexpr(t->next->type, dbg)) {
					t = skip_expr(t, 0xFF, 0, dbg);
				}
				break;
			case TOK_STRING:
				if (type == DIR_BYTE) {
					for (uint16_t k = 0; t->str[k] != '\0'; k++) {
						switch (t->str[k]) {
							case '\\':
								switch (t->str[k+1]) {
									case 'n' : c = '\n'; break;
									case 'r' : c = '\r'; break;
									case 't' : c = '\t'; break;
									case '\"': c = '\"'; break;
									case '\'': c = '\''; break;
									case '\\': c = '\\'; break;
									case '0' : c = '\0'; break;
								}
								k++;
								break;
							default: c = t->str[k]; break;
						}
						if (isasm) {
							addr[tmpaddr] = c;
						}
						tmpaddr++;
						bc->datasize++;
					}
					if (isasm) {
						addr[tmpaddr] = '\0';
					}
					tmpaddr++;
					bc->datasize++;
				}
				break;
		}
		if (t == NULL) {
			break;
		}
	}
	return tmpaddr;
}

static uint8_t write_inst(uint8_t prefix, uint8_t ext_prefix, uint8_t opcode, operand *op, uint64_t address, uint8_t *op_size, uint8_t isasm, uint8_t dbg) {
	uint8_t inst_size = 0;
	uint8_t op_ins_size[2];
	union reg ins;
	union reg op_ins[2];
	memset(op_ins_size, 0, sizeof(op_ins_size));
	memset(op_ins, 0, sizeof(op_ins_size));
	if (prefix & 3) {
		ins.u8[inst_size++] = prefix;
	}
	if ((ext_prefix & 0x0D) == 0x0D) {
		ins.u8[inst_size++] = ext_prefix;
	}
	ins.u8[inst_size++] = opcode;
	if (ext_prefix == 0x1D) {
		for (int i = 0; i < 2 && op[i].type != 0xFF; i++) {
			ins.u8[inst_size] |= (op[i].id << (!i*4));
		}
		inst_size++;
		for (int i = 0; i < 2 && op[i].type != 0xFF; i++) {
			int is_sib = 0;
			if (op[i].type == 1) {
				int reg = 0;
				switch (op[i].id) {
					case MEM_SIB  : op_ins[i].u8[op_ins_size[i]++] = op[i].scale; /* Falls through. */
					case MEM_ABSR :
					case MEM_ARIND:
					case MEM_AINDR:
					case MEM_ZMR  :
					case MEM_ZRIND:
					case MEM_ZINDR:
					case MEM_RIND :
						op_ins[i].u8[op_ins_size[i]] = op[i].rind[reg];
						reg = (op[i].rind[1] != 0xFF);
						op_ins[i].u8[op_ins_size[i]++] |= op[i].rind[reg] << 4;
						break;
				}
			}
		}
	}
	if (isasm) {
		if (dbg) {
			printf("$%04"PRIX64":\t", address);
			for (int i = 0; i < inst_size; i++) {
				printf("%02X", ins.u8[i]);
				if (i < inst_size-1) {
					putchar(' ');
				}
			}
		}
		write_value(ins.u64, address, inst_size-1);
	}
	for (int i = 0; i < 2 && op[i].type != 0xFF; i++) {
		if (op[i].type == 1) {
			if (ext_prefix == 0x1D && op_ins_size[i]) {
				if (isasm) {
					if (dbg) {
						putchar(' ');
						for (int j = 0; j < op_ins_size[i]; j++) {
							printf("%02X", op_ins[i].u8[j]);
							if (j < op_ins_size[i]-1) {
								putchar(' ');
							}
						}
					}
					write_value(op_ins[i].u64, address+inst_size, op_ins_size[i]-1);
				}
				inst_size += op_ins_size[i];
			}
			if (op_size[i] && (ext_prefix != 0x1D || (ext_prefix == 0x1D && op[i].id != MEM_SIB && op[i].id != MEM_RIND))) {
				if (isasm) {
					if (dbg) {
						uint8_t *tmp = (uint8_t *)&op[i].value;
						putchar(' ');
						for (int j = 0; j < op_size[i]; j++) {
							printf("%02X", tmp[j]);
							if (j < op_size[i]-1) {
								putchar(' ');
							}
						}
					}
					write_value(op[i].value, address+inst_size, op_size[i]-1);
				}
				inst_size += op_size[i];
			}
		}
	}
	if (isasm && dbg) {
		putchar('\n');
	}
	return inst_size;
}

token *get_operands(token *t, operand *op, uint64_t address, uint8_t rs, uint8_t dbg) {
	uint8_t op_type;
	uint8_t op_inst;
	/*switch (t->id) {
		case TOK_OPCODE:
		case TOK_EXTOP :
		case TOK_ORTHO :
			op_type = t->id;
			op_inst = t->byte;
			t = t->next;
			break;
	}*/
	int i = 0;
	int old_i = -1;
	uint8_t expr_type = 0xFF;
	uint8_t stop_comma = 0;
	uint64_t value = 0;
	uint8_t reg = 0;
	uint8_t got_value = 0;
	uint8_t is_sib = 0;
	uint8_t brack_done = 0;
	uint8_t is_comma = 0;
	token *tmp = t;
	/*for (; tmp; tmp = tmp->next) {
		printf("t: %p, t->id: $%X, t->id: %s, t->subtype: $%X, t->subtype: %s\n", tmp, tmp->id, (tmp->id <= TOK_MEMBER) ? lex_tok[tmp->id] : "TOK_NONE", tmp->subtype, (tmp->subtype == TOK_IND || tmp->subtype == TOK_CSV) ? lex_tok[tmp->subtype] : "TOK_NONE");
	}*/

	for (; t && i < 2; t = t->next) {
		if (t->subtype == TOK_IND) {
			brack_done = 1;
		}
		switch (t->id) {
			case TOK_HEX  :
			case TOK_DEC  :
			case TOK_BIN  :
			case TOK_SYM  :
			case TOK_CHAR :
			case TOK_LABEL:
				if (!got_value) {
					expr_type = (expr_type == 0xFF && t->next && t->next->id == TOK_EXPR) ? t->next->type : expr_type;
					switch (expr_type) {
						default		: stop_comma = 1; break;
						case EXPR_MUL	: stop_comma = 0; break;
					}
					is_sib = (!stop_comma && op[i].type && op[i].id == MEM_IND);
					value = get_val(t, address, (rs != 0xFF) ? rs : 0, (!stop_comma) ? expr_type : 0xFF, stop_comma, dbg);
					op[i].value = (!is_sib) ? value : op[i].value;
					got_value = 1;
				} else {
					op[i].value = (!is_sib) ? value : op[i].value;
					got_value = 0;
				}
				if ((op[i].type == 1 && op[i].id == MEM_RIND) || (!op[i].type)) {
					op[i].is_ind = (op[i].type == 1 && op[i].id == MEM_RIND);
					op[i].type = 1;
					op[i].rind[0] = (op[i].rind[0] == 0xFF) ? op[i].id : op[i].rind[0];
					op[i].id = MEM_ZMR;
				}
				is_comma += (t && (t->subtype == TOK_CSV || (t->next && t->next->subtype == TOK_CSV)));
				is_comma = (is_comma >= 2) ? 0 : is_comma;
				/* Falls Through. */
			case TOK_MEM:
				is_comma = (t->id == TOK_MEM) ? 0 : is_comma;
				if (old_i != i) {
					op[i].type = 1;	/* Set type to memory. */
					op[i].id = (t->id == TOK_MEM) ? t->type : 0xFF;
					op[i].id = (is_sib) ? MEM_SIB : op[i].id;
					op[i].scale = (is_sib) ? value : op[i].scale;
					old_i = i;
				} else {
					if (!op[i].type && !is_sib) {
						op[i].type = 1;
						op[i].id = MEM_ZMR;
					}
				}
				if (got_value && !is_comma) {
					if (t && t->subtype != TOK_CSV) {
						t = skip_expr(t, (!stop_comma) ? expr_type : 0xFF, stop_comma, dbg);
					}
				}
				i += is_comma;
				if (old_i != i) {
					got_value = 0;
				}
				break;
			case TOK_REG:
				if (old_i != i) {
					op[i].type = 0;	/* Set type to register. */
					op[i].id = t->type;
					old_i = i;
				} else {
					if (op[i].type == 1) {
						if (op[i].id == MEM_IND) {
							op[i].id = (!brack_done && got_value) ? MEM_ZRIND : MEM_RIND;
							op[i].id = (brack_done && got_value) ? MEM_ZINDR : op[i].id;
							brack_done = 0;
						} else {
							op[i].id = (got_value) ? MEM_ZMR : op[i].id;
						}
						op[i].rind[reg] = t->type;
						reg++;
						reg = (reg > 1) ? 0 : reg;
					}
				}
				is_comma += (t && (t->subtype == TOK_CSV || (t->next && t->next->subtype == TOK_CSV)));
				is_comma = (is_comma >= 2) ? 0 : is_comma;
				i += is_comma;
				break;
			case TOK_EXPR:
				expr_type = t->type;
				switch (expr_type) {
					default		: stop_comma = 1; break;
					case EXPR_MUL	: stop_comma = 0; break;
				}
				if (!got_value) {
					if (t->next && t->next->id != TOK_REG) {
						value = get_val(t, address, (rs != 0xFF) ? rs : 0, (!stop_comma) ? expr_type : 0xFF, stop_comma, dbg);
						got_value = 1;
					}
				} else {
					got_value = 0;
				}
				break;
		}
		if (!t) {
			break;
		}
	}
	return t;
}

uint64_t handle_opcode(token *t, bytecount *bc, uint8_t isasm, uint64_t address, uint8_t dbg) {
	union reg val;
	uint8_t opsize;
	uint8_t id;
	uint8_t instr;
	uint8_t opcode;
	uint8_t ext_prefix = 0;
	uint8_t type;
	uint32_t am = 0;
	uint8_t op_size[2];
	uint8_t prefix = 0;
	uint8_t rs = 0;
	uint8_t of = 0;
	uint8_t tmp_prefix = 0;
	uint8_t inst_size = 0;
	int is_ortho = 0;
	val.u64 = 0;
	instruction ins;
	operand op[2];
	memset(op, 0xFF, sizeof(op));
	op_size[0] = 0;
	op_size[1] = 0;
	if (t) {
		if (t->id == TOK_OPCODE || t->id == TOK_EXTOP || t->id == TOK_ORTHO) {
			id = t->id;
			instr = t->byte;
			type = t->type;
		} else {
			return address;
		}
		/*tmp = 0;*/
		opsize = 1;
		opcode = 0;
		if (t->next) {
			rs = get_rs(t->next, dbg);
			t = (rs != 0xFF) ? t->next : t;
			if (t->next) {
				is_ortho = (t->next->id == TOK_OS);
				t = (is_ortho) ? t->next : t;
				if (t->next) {
					of = get_of(t->next, dbg);
					t = (of != 0xFF) ? t->next : t;
				}
			}
		}
		get_operands(t, op, address, rs, dbg);
		if (dbg) {
			for (int i = 0; i < 2 && op[i].type != 0xFF; i++) {
				printf("%i: op.type: %u, op.id: $%X, op.scale; $%X, op.rind[0]: $%X, op.rind[1]: $%X, op.value: $%"PRIX64"\n", i, op[i].type, op[i].id, op[i].scale, op[i].rind[0], op[i].rind[1], op[i].value);
			}
		}
		uint8_t is_eind = (op[0].type == 1 && op[0].id == MEM_RIND && op[0].rind[0] == REG_E);
		uint8_t is_mem = (op[0].type == 1 && op[0].id != MEM_IMM);
		uint8_t is_idx = (is_mem && !op[1].type && (op[1].id == REG_X || op[1].id == REG_Y));
		of = (is_mem && (op[0].id == MEM_RIND || op[0].id == MEM_ZMR) && op[0].rind[0] == REG_SP) ? 1 : of;
		if (rs != 0xFF || of != 0xFF) {
			tmp_prefix  = (rs != 0xFF) ? (rs << 0) : tmp_prefix;
			tmp_prefix |= (of != 0xFF) ? (of << 2) : tmp_prefix;
		}
		prefix = (tmp_prefix) ? ((tmp_prefix << 4) | 3) : 0;
		uint8_t isincdec = (instr == INC || instr == DEC);
		uint8_t isimplied = (op[0].type == 0xFF);
		switch (id) {
			case TOK_OPCODE: ins = 	     inst[instr]; break;
			case TOK_EXTOP : ins =   ext_inst[instr]; break;
			case TOK_ORTHO : ins = ortho_inst[instr]; break;
		}
		am = ins.am;
		if (id == TOK_EXTOP || (id == TOK_OPCODE && is_eind)) {
			ext_prefix = 0x0D;
		}
		if ((am & AM_IMPL) && isimplied) {
			type = IMPL;
		} else {
			if (ins.am & AM_REL) {
				type = REL;
			}
		}
		opcode = ins.op;
		uint64_t saveaddr = address;
		uint64_t max_val = 0;
		uint8_t i = 0;
		uint8_t j = 1;
		uint8_t type2 = 0xFF;
		if (!is_ortho) {
			is_ortho = (id == TOK_ORTHO);
		}
		int is_valid = 1;
		if (!is_ortho && (is_mem || is_idx)) {
			switch (op[0].id) {
				case MEM_AINDR:
				case MEM_ZINDR:
				case MEM_ARIND:
				case MEM_ZRIND:
				case MEM_SIB  : is_valid = 0; break;
				case MEM_ZMR  :
				case MEM_ABSR :
				case MEM_RIND :
				default       :
					if (is_mem && !op[1].type && op[1].id != REG_X && op[1].id != REG_Y) {
						is_valid = 0;
					} else if (op[0].id == MEM_ZMR || op[0].id == MEM_ABSR || op[0].id == MEM_RIND) {
						is_valid = (of != 0xFF || op[0].rind[0] == REG_SP);
					} else if (is_mem && op[1].type == 1) {
						is_valid = 0;
					}
					break;
			}
		} else if (is_ortho) {
			is_valid = 0;
		}
		if (type == IMPL && (am & AM_IMPL)) {
			if (id == TOK_OPCODE && instr == CPS) {
				rs = 0;
			}
			opcode = opcode;
			is_ortho = 0;
		} else if (type == REL) {
			rs = (rs != 0xFF) ? rs : 0;
			op_size[0] = (1 << rs);
			uint64_t max_sign = 0;
			uint8_t offset = 1;
			uint64_t tmp_val;
			tmp_val = op[0].value;
			offset += (prefix != 0);
			tmp_val -= offset+op_size[0];
			tmp_val -= address;
			switch (rs) {
				default: max_sign = (int8_t )(1 <<  7); break;
				case 1 : max_sign = (int16_t)(1 << 15); break;
				case 2 : max_sign = (int32_t)(1 << 31); break;
				case 3 : max_sign = (int64_t)((uint64_t)1 << 63); break;
			}
			if ((int64_t)tmp_val > ~(int64_t)max_sign || (int64_t)tmp_val < (int64_t)max_sign) {
				offset += (!rs);
				rs += (rs <= 3);
				op_size[0] = (1 << rs);
				tmp_val = op[i].value;
				tmp_val -= offset+op_size[0];
				tmp_val -= address;
				prefix = ((rs << 4) | 3);
			}
			op[0].value = tmp_val;
			is_ortho = 0;
		} else if (id != TOK_ORTHO) {
			if (!is_eind && is_valid && (is_mem || is_idx)) {
				if (of != 0xFF) {
					i = 8;
					for (; i <= 64; i += 8, j++) {
						max_val |= ((uint64_t)1 << (i-1));
						if ((int64_t)op[0].value >= ~(int64_t)(max_val) || (int64_t)op[0].value <= (int64_t)(max_val)) {
							opsize = j;
							break;
						}
					}
				} else {
					for (; i <= 64; i += 8, j++) {
						max_val |= (0xFF << i);
						if (op[0].value <= max_val) {
							opsize = j;
							break;
						}
					}
				}
				if (is_idx) {
					switch (op[1].id) {
						case REG_X: type = (op[0].id == MEM_IND || (of == 1 && op[0].id == MEM_ZMR && op[0].is_ind)) ? INDX : ZMX; break;
						case REG_Y: type = (op[0].id == MEM_IND || (of == 1 && op[0].id == MEM_ZMR && op[0].is_ind)) ? INDY : ZMY; break;
					}
				} else {
					type = (op[0].id == MEM_IND || (of == 1 && op[0].id == MEM_ZMR && op[0].is_ind)) ? IND : 0xFF;
				}
				type2 = type;
				if (type == 0xFF || (id == TOK_EXTOP && type2 != 0xFF)) {
					switch (opsize-1) {
						case 0: case 2: case 5: case 3: type = ZM ; break;
						case 1: case 4: case 6: case 7: type = ABS; break;
					}
				}
				switch (type2) {
					case ZMX : type = (type == ABS) ? ABSX	: type2; break;
					case ZMY : type = (type == ABS) ? ABSY	: type2; break;
					case IND : type = (type == ABS) ? AIND	: type2; break;
					case INDX: type = (type == ABS) ? AINDX	: type2; break;
					case INDY: type = (type == ABS) ? AINDY	: type2; break;
				}
				if (opsize) {
					uint8_t is_abs = 0;
					switch (type) {
						case ABS  :
						case ABSX :
						case ABSY :
						case AIND :
						case AINDX:
						case AINDY: is_abs = 1; break;

					}
					if (!is_abs || (type2 != 0xFF && type == type2)) {
						switch (opsize) {
							case 2: opsize = 3; break;
							case 5: opsize = 6; break;
						}
					}
					prefix |= amp[opsize-1];
				}
				if (am & AM_ADDR) {
					switch (type) {
						case ZM:
							if (am & AM_ZM) {
								opcode += 0x04;
							} else if (am & AM_ZM2) {
								opcode += 0x20;
							} else {
								is_ortho = 1;
							}
							break;
						case ZMX:
							if (am & AM_ZMX) {
								opcode += (id == TOK_OPCODE) ? 0x06 : 0x54;
							} else {
								is_ortho = 1;
							}
							break;
						case ZMY:
							if (am & AM_ZMY) {
								opcode += 0x14;
							} else {
								is_ortho = 1;
							}
							break;
						case INDX:
							if (am & AM_INDX) {
								opcode += (id == TOK_OPCODE) ? 0x16 : 0x94;
								break;
							} else {
								is_ortho = 1;
							}
							/* Falls Through. */
						case IND:
						case INDY:
							if ((id == TOK_OPCODE) && (am & (AM_IND|AM_INDY|AM_INDX2))) {
								opcode = ind_ops[get_ind(instr, type, dbg)];
							} else if (id = TOK_EXTOP) {
								opcode += (type == IND) ? 0x44 : 0x84;
							} else {
								is_ortho = 1;
							}
							break;
						case ABS:
							if (am & AM_ABS) {
								opcode += 0x10;
							} else {
								is_ortho = 1;
							}
							break;
						case ABSX:
							if (am & AM_ABX) {
								opcode += 0x50;
							} else {
								is_ortho = 1;
							}
							break;
						case ABSY:
							if (am & AM_ABY) {
								opcode += 0x00;
							} else {
								is_ortho = 1;
							}
							break;
						case AIND:
							if (am & AM_AIND) {
								opcode += 0x40;
							} else {
								is_ortho = 1;
							}
							break;
						case AINDX:
							if (am & AM_AINDX) {
								opcode += 0x90;
							} else {
								is_ortho = 1;
							}
							break;
						case AINDY:
							if (am & AM_AINDY) {
								opcode += 0x80;
							} else {
								is_ortho = 1;
							}
							break;

					}
					op_size[0] = opsize;
				}

			} else if (op[1].type == 0xFF) {
				if (!op[0].type) {
					if (op[0].id == REG_B && (am & AM_BREG)) {
						opcode += 0x14;
					} else if (op[0].id == REG_A && (am & AM_IMPL)) {
						opcode = opcode;
					} else {
						is_ortho = 1;
					}
				} else {
					if (is_eind && (am & AM_EIND|AM_EIND2)) {
						int eind_type = ((am & AM_EIND2) != 0);
						switch (eind_type) {
							case 0: opcode = (id == TOK_EXTOP) ? opcode+0x14 : opcode+0x10; break;
							case 1: opcode = (id == TOK_EXTOP) ? opcode+0x01 : eind_base_ops[get_eind(instr, dbg)]; break;
						}
					} else if ((op[0].id == MEM_IMM) && (am & AM_IMM)) {
						rs = (rs != 0xFF) ? rs : 0;
						op_size[0] = (1 << rs);
					} else {
						is_ortho = 1;
					}
				}
			} else {
				is_ortho = 1;
			}
		}
		if (is_ortho) {
			ext_prefix = 0x1D;
			if (id == TOK_EXTOP) {
				opcode = ext_ortho_ops[get_ext_ortho(instr, dbg)];
			}
			for (int i = 0; i < 2 && op[i].type != 0xFF; i++) {
				int i2 = 0;
				int j = 0;
				(op[i].type == 1) ? (opcode |= (1 << (3+!i))) : (opcode &= ~(1 << (3+!i)));
				if (op[i].type == 1 && (am & AM_ORTHO|AM_ORTHO2)) {
					switch (op[i].id) {
						case MEM_RIND: break;
						case MEM_IMM:
							rs = (rs != 0xFF) ? rs : 0;
							op_size[i] = (1 << rs);
							break;
						case MEM_ZRIND:
						case MEM_ZINDR:
						case MEM_ZMR:
						case MEM_IND:
						default:
							if (of != 0xFF) {
								max_val = 0;
								for (i2 = 8, j = 1; i2 <= 64; i2 += 8, j++) {
									max_val |= ((uint64_t)1 << (i2-1));
									if ((int64_t)op[i].value >= ~(int64_t)(max_val) || (int64_t)op[i].value <= (int64_t)(max_val)) {
										opsize = j;
										break;
									}
								}
							} else {
								max_val = 0;
								for (i2 = 0, j = 1; i2 <= 64; i2 += 8, j++) {
									max_val |= (0xFF << i2);
									if (op[i].value <= max_val) {
										opsize = j;
										break;
									}
								}
							}
							switch (opsize-1) {
								case 0: case 2: case 5: case 3: type = ZM ; break;
								case 1: case 4: case 6: case 7: type = ABS; break;
							}
							switch (op[i].id) {
								case MEM_IND  : op[i].id = (type == ABS) ? MEM_AIND  : op[i].id; break;
								case MEM_ZMR  : op[i].id = (type == ABS) ? MEM_ABSR  : op[i].id; break;
								case MEM_ZRIND: op[i].id = (type == ABS) ? MEM_ARIND : op[i].id; break;
								case MEM_ZINDR: op[i].id = (type == ABS) ? MEM_AINDR : op[i].id; break;
								case 0xFF     : op[i].id = (type == ABS) ? MEM_ABS   :   MEM_ZM; break;
							}
							if (opsize) {
								uint8_t is_abs = (type == ABS);
								if (!is_abs) {
									switch (opsize) {
										case 2: opsize = 3; break;
										case 5: opsize = 6; break;
									}
								}
								prefix |= amp[opsize-1];
							}
							op_size[i] = opsize;
							if (isasm /*&& dbg*/) {
								printf("op_size[%i]: %i, opsize: %u\n", i, op_size[i], opsize);
							}
							break;
					}
				}
			}
		}
		inst_size = write_inst(prefix, ext_prefix, opcode, op, address, op_size, isasm, /*dbg*/isasm);
		address += inst_size;
		bc->progsize += inst_size;
		if (isasm /*&& dbg*/) {
			printf("inst_size: $%X, bc->progsize: $%"PRIX64"\n", inst_size, bc->progsize);
		}
	}
	return address;
}

uint64_t parse_tokens(token *t, line **l, bytecount *bc, uint8_t isasm, uint64_t address, uint8_t dbg) {
	for (; t; t = t->next) {
		switch (t->id) {
			case TOK_DIR:
				switch (t->type) {
					case DIR_STRUCT:
					case DIR_UNION : handle_struct(l, address, 0, dbg); break;
					case DIR_RES: t = t->next; address += get_val(t, address, 3, 0xFF, 0, dbg); break;
					case DIR_ORG: t = t->next; address  = get_val(t, address, 3, 0xFF, 0, dbg); break;
					case DIR_BYTE:
					case DIR_WORD:
					case DIR_DWORD:
					case DIR_QWORD: address = handle_directive(t, bc, isasm, address, dbg); break;
				}
				break;
			case TOK_ORTHO :
			case TOK_EXTOP :
			case TOK_OPCODE: address = handle_opcode(t, bc, isasm, address, dbg); break;
			case TOK_COMMENT: break;
		}
	}
	return address;
}

token *make_token(uint8_t id, uint8_t type, uint8_t space, uint8_t tab, uint64_t value, char *str, symbol *s) {
	token *new_tok = malloc(sizeof(token));
	(last_tok) ? (last_tok->next = new_tok) : (tokens = new_tok);

	new_tok->id = id;
	new_tok->type = type;
	new_tok->subtype = 0xFF;

	new_tok->tab = tab;
	new_tok->space = space;

	new_tok->subtab = 0;
	new_tok->subspace = 0;

	new_tok->digits = 0;

	if (s) {
		new_tok->sym = s;
	} else if (str[0]) {
		new_tok->str = str;
	} else {
		new_tok->qword = value;
	}
	new_tok->next = NULL;
	last_tok = new_tok;
	return new_tok;
}

void assemble(line *ln, bytecount *bc, uint8_t dbg) {
	uint64_t address = 0;
	line *l = ln;
	for (int i = 0; i < 2; i++) {
		l = ln;
		for (; l; l = l->next) {
			l->addr = address;
			token *t = l->tok;
			address = parse_tokens(t, &l, bc, 0, address, dbg);
		}
		l = ln;
		for (; l; l = l->next) {
			address = l->addr;
			token *t = l->tok;
			for (; t; t = t->next) {
				if (t->id == TOK_LABEL && t->sym->val != address) {
					t->sym->val = l->addr;
				}
			}
		}
	}
	l = ln;
	bc->progsize = 0;
	bc->datasize = 0;
	for (; l; l = l->next) {
		address = parse_tokens(l->tok, &l, bc, 1, address, dbg);
	}
}

static void find_dupsym() {
	symbol *root = symbols;
	symbol *s = symbols;
	for (; s; s = s->next) {
		root = symbols;
		for (int i = 0; root; root = root->next) {
			if (root == s) {
				i++;
			}
			if (i > 1) {
				printf("Found duplicate symbol, s->name: %s, root->name: %s\n", s->name, root->name);
				i = 0;
			}
		}
	}
}

static symbol *find_fixup(token *t) {
	fixup* f = fixups;
	for (; f && t != f->t; f = f->next);
	return (f && t == f->t) ? f->s : NULL;
}


static void print_symval(symbol *s) {
	if (s) {
		if (s->down) {
			print_symval(s->down);
		}
		if (s->name) {
			printf("s->name: %s, s->val: $%"PRIX64"\n", s->name, s->val);
		}
		print_symval(s->next);
	}
}

static void print_symtree(symbol *s, int depth) {
	if (s) {
		if (s->name != NULL) {
			for (int i = depth; i; i--) {
				printf("|%s", (i > 1) ? "  " : "--");
			}
			printf("%s: $%"PRIX64"\n", s->name, s->val);
		}
		if (s->down != NULL) {
			print_symtree(s->down, depth+1);
		}
		print_symtree(s->next, depth);
	}
}

void fix_symtree(line *l) {
	symbol *s = symbols;
	symbol *cur_sym = NULL;
	symbol *sym_struct = NULL;
	symbols = NULL;
	last_sym = NULL;
	int islocal = 0;
	int isanon = 0;
	int is_struct = 0;
	int is_structsym = 0;
	for (; l; l = l->next) {
		token *t = l->tok;
		token *lt = NULL;
		for (; t; t = t->next) {
			int ismember = (t->id == TOK_MEMBER);
			switch (t->id) {
				case TOK_STRUCT:
				case TOK_UNION : islocal = !(is_struct == 1 && lt && lt->id == TOK_DIR);
				case TOK_SYM   :
						if (t->id == TOK_SYM && t != l->tok) {
							break;
						}
				case TOK_MEMBER:
				case TOK_LABEL :
					if (symbols) {
						(!islocal && s && !s->up) ? (last_sym = s) : (last_loc = s);
					}
					if (((t->type == 1 || ismember) && !islocal) || (islocal && ismember && is_structsym)) {
						is_structsym = 0;
						last_loc = NULL;
						islocal = 1;
						cur_sym = s;
						s->down = t->sym;
						s->down->up = s;
						s = s->down;
						if (s) {
							s->next = NULL;
							s->prev = NULL;
							s->down = NULL;
						}
						locals = s;
					} else if ((islocal || t->type == 0)) {
						if (t->type == 0 && !is_struct && islocal && !ismember) {
							islocal = 0;
							if (s) {
								s->up->down = locals;
								s = s->up;
							}
						}
						symbol *tmp = s;
						s = t->sym;
						if (s) {
							s->prev = (tmp && tmp != s) ? tmp : NULL;
							s->up = (s->prev) ? s->prev->up : s->up;
						}
						if (s && s->next) {
							s->next = NULL;
						}
					}
					if (!islocal) {
						last_loc = NULL;
						(last_sym) ? (last_sym->next = s) : (symbols = s);
						cur_sym = s;
						if (last_sym) {
							last_sym->next->prev = last_sym;
							last_sym->next->up = last_sym->up;
							last_sym->next->down = NULL;
						}
					} else {
						(last_loc) ? (last_loc->next = s) : (locals = s);
						if (last_loc) {
							last_loc->next->prev = last_loc;
							last_loc->next->up = last_loc->up;
							last_loc->next->down = NULL;
						} else {
							locals->prev = NULL;
							locals->down = NULL;
						}
					}
					break;
				case TOK_DIR:
					if (t->type == DIR_STRUCT || t->type == DIR_UNION) {
						is_struct++;
						is_structsym = (t->next && (t->next->id == TOK_STRUCT || t->next->id == TOK_UNION));
						if ((!is_structsym) || (isanon && is_structsym)) {
							isanon++;
						}
					}
					break;
			}
		}
	}
}

static inline void free_tokens(token *t) {
	token *tok;
	if (t != NULL) {
		tok = t;
		t = t->next;
		free(tok);
		free_tokens(t);
	}
}

void free_lines(line *l) {
	line *ln;
	if (l != NULL) {
		free_tokens(l->tok);
		ln = l;
		l = l->next;
		free(ln);
		free_lines(l);
	}
}

static void free_symbols(symbol *s) {
	symbol *sym;
	sym = s;
	if (sym != NULL) {
		if (s && s->down) {
			free_symbols(s->down);
		}
		if (sym->name != NULL) {
			free(sym->name);
			sym->name = NULL;
		}
		s = s->next;
		free(sym);
		sym = NULL;
		free_symbols(s);
	}
}

static inline void free_fixups(fixup *f) {
	fixup *fix;
	if (f != NULL) {
		fix = f;
		f = f->next;
		free(fix);
		fix = NULL;
		free_fixups(f);
	}
}

uint64_t get_tokmem(token *t) {
	uint64_t i = 0;
	for (; t; t = t->next, i++);
	return i*sizeof(token);
}

void get_linemem(line *l) {
	uint64_t i = 0;
	uint64_t j = 0;
	for (; l; j += get_tokmem(l->tok), l = l->next, i++);
	printf("Bytes per line: %"PRIu64", Bytes per token: %"PRIu64", Total size of line table in bytes: %"PRIu64"\n", sizeof(line), sizeof(token), j+(i*sizeof(line)));
}

void cleanup() {
	uint16_t i;
	if (lines) {
		/*get_linemem(lines);*/
		/*fix_symtree(lines);*/
		free_lines(lines);
		lines = NULL;
	}
	if (symbols) {
		/*print_symtree(symbols, 0);*/
		free_symbols(symbols);
		symbols = NULL;
	}
	if (fixups) {
		free_fixups(fixups);
		fixups = NULL;
	}
	while (i < stridx || i < comidx) {
		if (i < stridx && string[i]) {
			free(string[i]);
			string[i] = NULL;
		}
		if (i < comidx && comment[i]) {
			free(comment[i]);
			comment[i] = NULL;
		}
		i++;
	}
}