#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);

    status->malloc.lo_mem = 0x08100000 + 32768;
    status->malloc.hi_mem = 0x08100000 + 524288 - 16384;
    status->malloc.margin = 0x200;
    status->malloc.allocs = 0;
    status->malloc.mallocs = malloc(0);
}

int cpu_run(cpu_status_t* status){

}

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

        //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+524288){
        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 0;
    }
}

void cpu_write32(cpu_status_t* status, uint32_t addr, uint32_t data){
    if(addr >=0x08100000 && addr <= 0x08100000+524288){
        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;

        if(addr==0x0817daac) printf("\e[36mWRITE ALERT!\e[39m\n");
        
    }
    /*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;
        //display_update(status->display,status);
    }
    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+524288){
        status->ram[addr-0x08100000] = (data >> 8) & 0xFF;
        status->ram[addr-0x08100000+1] = data & 0xFF;
    }
    /*else if(addr >=0x00300200 && addr <= 0x00300200+status->program_size){
        memcpy(&status->rom[addr-0x00300200], &addr, 2);
    }*/
    else if(addr == 0xa4000102 || addr == 0xa4000118){

    }
    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+524288){
        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 if(addr == 0xb4000000){
        display_set_register_selector(status->display, data);
    }
    else if(addr == 0xb4010000){
        display_set_data_register(status->display, data,status);
    }
    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]
    );*/
    if(status->log){
        printf("pc: %8x %02x%02x 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,cpu_read8(status,status->pc),cpu_read8(status,status->pc+1), 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(cpu_read8(status,status->pc) == 0x7f && cpu_read8(status,status->pc+1) == 0x04) status->pc += 2;
    else if(nibble[0] == 0b1110) instruction_mov_imm_r(status);
    else if(nibble[0] == 0b0000 && nibble[2] == 0b0010 && nibble[3] == 0b1001) instruction_movt_r(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b1000) instruction_swapb_r_r(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b0111) instruction_mova_disp_pc_r0(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] == 0b0101) instruction_movw_arp_r(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b0001) instruction_movw_r0_disp_gbr(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b0001) instruction_movw_ar_r(status);
    else if(nibble[0] == 0b0000 && nibble[3] == 0b1101) instruction_movw_r0_r_r(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b0101) instruction_movw_disp_gbr_r0(status);
    else if(nibble[0] == 0b0000 && nibble[3] == 0b0101) instruction_movw_r_r0_r(status);
    // good
    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_movb_r0_disp_r(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b0000) instruction_movb_r0_disp_gbr(status);
    
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0010 && nibble[3] == 0b0100) instruction_roctl_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0010 && nibble[3] == 0b0001) instruction_shar_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0000 && nibble[3] == 0b1000) instruction_shll2_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0000 && nibble[3] == 0b0001) instruction_shlr_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0000 && nibble[3] == 0b1001) instruction_shlr2_r(status);
    else if(nibble[0] == 0b0100 && nibble[3] == 0b1101) instruction_shld_r_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0000 && nibble[3] == 0b0000) instruction_shll_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0010 && nibble[3] == 0b1000) instruction_shll16_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0001 && nibble[3] == 0b1000) instruction_shll8_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0010 && nibble[3] == 0b0101) instruction_rotcr_r(status);
    else if(nibble[0] == 0b0100 && nibble[3] == 0b1100) instruction_shad_r_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0010 && nibble[3] == 0b1001) instruction_shlr16_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0001 && nibble[3] == 0b1001) instruction_shlr8_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0000 && nibble[3] == 0b0101) instruction_rotr_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0000 && nibble[3] == 0b0100) instruction_rotl_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] == 0b0011 && nibble[3] == 0b0011) instruction_cmp_ge_r_r(status);
    else if(nibble[0] == 0b0000 && nibble[3] == 0b0111) instruction_mull_r_r(status);
    else if(nibble[0] == 0b0011 && nibble[3] == 0b0000) instruction_cmp_eq_r_r(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b1100) instruction_extub_r_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0001 && nibble[3] == 0b0000) instruction_dt_r(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b0111) instruction_div0s_r_r(status);
    else if(nibble[0] == 0b0011 && nibble[3] == 0b1110) instruction_addc_r_r(status);
    else if(nibble[0] == 0b0011 && nibble[3] == 0b1010) instruction_subc_r_r(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b1110) instruction_extsb_r_r(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b1011) instruction_neg_r_r(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b1100) instruction_cmp_str_r_r(status);
    else if(nibble[0] == 0b0000 && nibble[1] == 0b0000 && nibble[2] == 0b0001 && nibble[3] == 0b1001) instruction_div0u(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b1000) instruction_tst_imm_r0(status);
    else if(nibble[0] == 0b0011 && nibble[3] == 0b0101) instruction_dmulul_r_r(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b1010) instruction_negc_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[1] == 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] == 0b1000 && nibble[1] == 0b1111 ) instruction_bfs_lbl(status);
    else if(nibble[0] == 0b0000 && nibble[2] == 0b0010 && nibble[3] == 0b0011) instruction_braf_r(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 if(nibble[0] == 0b0100 && nibble[2] == 0b0001 && nibble[3] == 0b0110) instruction_ldsl_arp_macl(status);
    else if(nibble[0] == 0b0000 && nibble[2] == 0b0001 && nibble[3] == 0b1010) instruction_sts_macl_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0001 && nibble[3] == 0b1110) instruction_ldc_r_gbr(status);
    else if(nibble[0] == 0b0000 && nibble[2] == 0b0001 && nibble[3] == 0b0010) instruction_stc_gbr_r(status);
    else if(nibble[0] == 0b0100 && nibble[2] == 0b0000 && nibble[3] == 0b0110) instruction_ldsl_arm_mach(status);
    else if(nibble[0] == 0b0000 && nibble[2] == 0b0000 && nibble[3] == 0b1010) instruction_sts_mach_r(status);
    
    else if(nibble[0] == 0b0010 && nibble[3] == 0b1000) instruction_tst_r_r(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b1011) instruction_or_r_r(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b1001) instruction_and_r_r(status);
    else if(nibble[0] == 0b0010 && nibble[3] == 0b1010) instruction_xor_r_r(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b1001) instruction_and_imm_r0(status);
    else if(nibble[0] == 0b0110 && nibble[3] == 0b0111) instruction_not_r_r(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b1011) instruction_or_imm_r0(status);
    else if(nibble[0] == 0b1100 && nibble[1] == 0b1010) instruction_xor_imm_r0(status);

    else{ printf("\e[33mpc: %8x unkdown opcode, skipping...\e[39m\n",status->pc);status->pc += 2;exit(1);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){
    while (status->pc-0x00300200 < status->program_size || ( status->pc >= 0x08100000 && status->pc <= 0x08100000+524288 )){
        // TODO: better kerboard gestion
        SDL_PumpEvents();
        const unsigned char* key = SDL_GetKeyboardState(NULL);
        if (key[SDL_SCANCODE_ESCAPE]) {
            exit(0);
        }

        if(cpu_execute(status)){
            total_error++;
        }
        //total_executions++;
        /*if(total_executions > 100000){
            break;
        }*/
    }
    printf("excution terminated with %d not found opcodes\n",total_error);
    
}