#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <cpu.h>
#include <log.h>
#include <instructions/instructions.h>
#include <display.h>

uint32_t decode(unsigned char a,unsigned char b,unsigned char c,unsigned char d)
{
    return ((uint32_t)d << 24) | ((uint32_t)c << 16) | ((uint32_t)b << 8) | (uint32_t)a;
}

uint16_t decode16(unsigned char a,unsigned char b)
{
    return ((uint16_t)b << 8) | (uint16_t)a;
}

int cpu_setup_addin(cpu_status_t* status,char* _file){
    FILE * addin_file;
    addin_file = fopen(_file, "rb");
    if(addin_file == NULL){
        return 1;
    }

    printf("file: %s\n",_file);

    fseek(addin_file, 0L, SEEK_END);
    status->program_size = ftell(addin_file) - 0x200L;

    status->rom = malloc(status->program_size);
    fseek(addin_file, 0x200L,SEEK_SET);
    fread(status->rom, status->program_size, 1, addin_file);

    fclose(addin_file);    

    printf("%d bytes allocated\n",status->program_size);

    status->pc = 0x00300200;

    for(int i=0; i<32768; i++){
        status->ram[i] = 0x00;
    }

    status->display = malloc(sizeof(display_t));
    display_init(status->display);
    display_clear(status->display);
    display_update(status->display);

}

int cpu_run(cpu_status_t* status){

}

uint32_t cpu_read32(cpu_status_t* status, uint32_t addr){
    if(addr >=0x08100000 && addr <= 0x08100000+32768*8){
        uint32_t ret;
        ret = decode(status->ram[addr-0x08100000+3], status->ram[addr-0x08100000+2], status->ram[addr-0x08100000+1], status->ram[addr-0x08100000]);
        return ret;
    }
    else if(addr >=0x00300200 && addr <= 0x00300200+status->program_size){
        uint32_t ret;
        ret = decode(status->rom[addr-0x00300200+3], status->rom[addr-0x00300200+2], status->rom[addr-0x00300200+1], status->rom[addr-0x00300200]);
        return ret;
    }
    else{
        log_mem_read_error(status, addr);
        return 0;
    }
}

uint16_t cpu_read16(cpu_status_t* status, uint32_t addr){
    if(addr >=0x08100000 && addr <= 0x08100000+32768*8){
        uint16_t ret;
        //ret = (uint16_t)status->ram[addr-0x08100000];
        ret = status->ram[addr-0x08100000+1];
        ret <<= 8;
        ret |= status->ram[addr-0x08100000];
        return ret;
    }
    else if(addr >=0x00300200 && addr <= 0x00300200+status->program_size){
        uint16_t ret;
        ret = status->rom[addr-0x00300200+1];
        ret <<= 8;
        ret |= status->rom[addr-0x00300200];

        //memcpy(&ret,&status->ram[addr-0x00300000],1);
        //ret = (uint16_t)status->rom[addr-0x00300000];
        return ret;
    }
    else{
        log_mem_read_error(status, addr);
        return 0;
    }
}

uint8_t cpu_read8(cpu_status_t* status, uint32_t addr){
    if(addr >=0x08100000 && addr <= 0x08100000+32768*8){
        uint8_t ret;
        ret = status->ram[addr-0x08100000];
        return ret;
        
    }
    else if(addr >=0x00300200 && addr <= 0x00300200+status->program_size){
        uint8_t ret;
        ret = status->rom[addr-0x00300200];
        return ret;
    }
    else if(addr >=0x01100000 && addr <= 0x01100000+8192){
        uint8_t ret;
        ret = status->vram[addr-0x01100000];
        return ret;
    }
    else{
        log_mem_read_error(status, addr);
        return 8;
    }
}

void cpu_write32(cpu_status_t* status, uint32_t addr, uint32_t data){
    if(addr >=0x08100000 && addr <= 0x08100000+32768*8){
        unsigned char bytes[4];

        status->ram[addr-0x08100000] = (data >> 24) & 0xFF;
        status->ram[addr-0x08100000+1] = (data >> 16) & 0xFF;
        status->ram[addr-0x08100000+2] = (data >> 8) & 0xFF;
        status->ram[addr-0x08100000+3] = data & 0xFF;
    }
    else if(addr >=0x00300200 && addr <= 0x00300200+status->program_size){
        memcpy(&status->rom[addr-0x00300200], &addr, 4);
    }
    else if(addr >=0x01100000 && addr <= 0x01100000+8192){
        status->vram[addr-0x01100000] = (data >> 24) & 0xFF;
        status->vram[addr-0x01100000+1] = (data >> 16) & 0xFF;
        status->vram[addr-0x01100000+2] = (data >> 8) & 0xFF;
        status->vram[addr-0x01100000+3] = data & 0xFF;
    }
    else{
        log_mem_write_error(status, addr);
    }
}

void cpu_write16(cpu_status_t* status, uint32_t addr, uint16_t data){
    if(addr >=0x08100000 && addr <= 0x08100000+32768*8){
        memcpy(&status->ram[addr-0x08100000],&data, 2);
    }
    else if(addr >=0x00300200 && addr <= 0x00300200+status->program_size){
        memcpy(&status->rom[addr-0x00300200], &addr, 2);
    }
    else{
        log_mem_write_error(status, addr);
    }
}

void cpu_write8(cpu_status_t* status, uint32_t addr, uint8_t data){
    if(addr >=0x08100000 && addr <= 0x08100000+32768*8){
        status->ram[addr-0x08100000] = data;     
    }
    else if(addr >=0x00300200 && addr <= 0x00300200+status->program_size){
        status->rom[addr-0x00300200] = data;
    }
    else if(addr >=0x01100000 && addr <= 0x01100000+8192){
        status->vram[addr-0x01100000] = data;
    }
    else{
        log_mem_write_error(status, addr);
    }
}

int cpu_execute(cpu_status_t* status){
    char nibble[4] = {
        HI_NIBBLE(cpu_read8(status,status->pc)),
        LO_NIBBLE(cpu_read8(status,status->pc)),
        HI_NIBBLE(cpu_read8(status,status->pc+1)),
        LO_NIBBLE(cpu_read8(status,status->pc+1))
    };

    /*printf("pc: %8x pr: %8x r0: %8x r1: %8x r2: %8x r3: %8x r4: %8x r5: %8x r6: %8x r15: %08x\n",
        status->pc,status->pr,
        status->r[0],status->r[1],status->r[2],status->r[3],
        status->r[4],status->r[5],status->r[6],status->r[15]
    );*/
    printf("pc: %8x pr: %8x r0: %8x r1: %8x r2: %8x r3: %8x r4: %8x r5: %8x r6: %8x r7: %8x r8: %8x r9: %8x r15: %08x\n",
        status->pc,status->pr,
        status->r[0],status->r[1],status->r[2],status->r[3],
        status->r[4],status->r[5],status->r[6],
        status->r[7],status->r[8],status->r[9],
        status->r[15]
    );

    if(nibble[0] == 0b0110 && nibble[3] == 0b0011) instruction_mov_r_r(status);
    else if(nibble[0] == 0b1110) instruction_mov_imm_r(status);
    else if(nibble[0] == 0b1101) instruction_movl_disp_pc_r(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b0010) instruction_movl_ar_r(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b0010) instruction_movl_r_ar(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b0110) instruction_movl_arp_r(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b0110) instruction_movl_r_amr(status);
    else if(nibble[0] == 0b0101) instruction_movl_disp_r_r(status);
    else if(nibble[0] == 0b0001) instruction_movl_r_disp_r(status);
    else if(nibble[0] == 0b0000 && nibble[3] == 0b1110) instruction_movl_r0_r_r(status);
    else if(nibble[0] == 0b0000 && nibble[3] == 0b0110) instruction_movl_r_r0_r(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b0110) instruction_movl_disp_gbr_r0(status);

    else if(nibble[0] == 0b1000 && nibble[1] == 0b0001) instruction_movw_r0_disp_r(status);
    else if(nibble[0] == 0b1000 && nibble[1] == 0b0101) instruction_movw_disp_r_r0(status);
    else if(nibble[0] == 0b1001) instruction_movw_disp_pc_r0(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b0001) instruction_movw_r_ar(status);

    else if(nibble[0] == 0b0110 && nibble[3] == 0b0000) instruction_movb_ar_r(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b0000) instruction_movb_r_ar(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b0100) instruction_movb_arp_r(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b0100) instruction_movb_r_amr(status);
    else if(nibble[0] == 0b1000 && nibble[1] == 0b0100) instruction_movb_disp_r_r0(status);
    else if(nibble[0] == 0b0000 && nibble[3] == 0b1100) instruction_movb_r0_r_r(status);
    else if(nibble[0] == 0b0000 && nibble[3] == 0b0100) instruction_movb_r_r0_r(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b0100) instruction_movb_disp_gbr_r0(status);
    else if(nibble[0] == 0b1000 && nibble[1] == 0b0000) instruction_movw_r0_disp_r(status);

    else if(nibble[0] == 0b0100 && nibble[2] == 0b0010 && nibble[3] == 0b0100) instruction_roctl_r(status);

    else if(nibble[0] == 0b0011 && nibble[3] == 0b0100) instruction_div1_r_r(status);
    else if(nibble[0] == 0b0011 && nibble[3] == 0b1100) instruction_add_r_r(status);
    else if(nibble[0] == 0b0111) instruction_add_imm_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0001 && nibble[3] == 0b0001) instruction_cmp_pz_r(status);
    else if(nibble[0] == 0b0011 && nibble[3] == 0b0111) instruction_cmp_gt_r_r(status);
    else if(nibble[0] == 0b0011 && nibble[3] == 0b0010) instruction_cmp_hs_r_r(status);
    else if(nibble[0] == 0b0011 && nibble[3] == 0b1000) instruction_sub_r_r(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b1101) instruction_extuw_r_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0001 && nibble[3] == 0b0101) instruction_cmp_pl_r(status);
    else if(nibble[0] == 0b1000 && nibble[1] == 0b1000) instruction_cmp_eq_imm_r0(status);
    else if(nibble[0] == 0b0011 && nibble[3] == 0b0110) instruction_cmp_hi_r_r(status);

    else if(nibble[0] == 0b0100 && nibble[2] == 0b0010 && nibble[3] == 0b1011) instruction_jmp_r(status);
    else if(nibble[0] == 0b1011) instruction_bsr_lbl(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0000 && nibble[3] == 0b1011) instruction_jsr_ar(status);
    else if(nibble[0] == 0b0000 && nibble[2] == 0b0000 && nibble[2] == 0b0000 && nibble[3] == 0b1011) instruction_rts(status);
    else if(nibble[0] == 0b1000 && nibble[1] == 0b1011 ) instruction_bf_lbl(status);
    else if(nibble[0] == 0b1010) instruction_bra_lbl(status);
    else if(nibble[0] == 0b1000 && nibble[1] == 0b1001 ) instruction_bt_lbl(status);
    else if(nibble[0] == 0b1000 && nibble[1] == 0b1101 ) instruction_bts_lbl(status);

    else if(nibble[0] == 0b0000 && nibble[1] == 0b0000 && nibble[2] == 0b0000 && nibble[3] == 0b1001) instruction_nop(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0000 && nibble[3] == 0b0010) instruction_stsl_mash_amr(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0001 && nibble[3] == 0b0010) instruction_stsl_macl_amr(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0010 && nibble[3] == 0b0010) instruction_stsl_pr_amr(status);
    else if(nibble[0] == 0b0000 && nibble[1] == 0b0000 && nibble[2] == 0b0000 && nibble[3] == 0b1000) instruction_clrt(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0010 && nibble[3] == 0b0110) instruction_ldsl_arp_pr(status);

    else{status->pc += 2; printf("\e[33munkdown opcode, skipping...\e[39m\n");return 1;}
    return 0;
}

int cpu_run_from(cpu_status_t* status, uint32_t addr){
    status->pc = addr;
    int total_error = 0;
    int total_executions = 0;

    while (status->pc-0x00300200 < status->program_size){
        if(cpu_execute(status)){
            total_error++;
        }
        total_executions++;
        if(total_executions > 150000){
            break;
        }
    }
    printf("excution terminated with %d not found opcodes\n",total_error);
    
}