#include "zx_disk.h"
#include "z80.h"
#include <stdio.h>

#define ZX_FDC_MODE_IDLE        0
#define ZX_FDC_MODE_COMMAND     1
#define ZX_FDC_MODE_EXECUTION   2
#define ZX_FDC_MODE_RESULT      3

#define ZX_FDC_COMMAND_MASK     0x1F
#define ZX_FDC_COMMAND_SPECIFY  0x03
#define ZX_FDC_COMMAND_SENSE_DRIVE_STATUS  0x04

#define ZX_FDC_MAIN_DRIVE0_BUSY 1
#define ZX_FDC_MAIN_DRIVE1_BUSY 2
#define ZX_FDC_MAIN_DRIVE2_BUSY 4
#define ZX_FDC_MAIN_DRIVE3_BUSY 8
#define ZX_FDC_MAIN_BUSY        16
#define ZX_FDC_MAIN_EXEC_MODE   32
#define ZX_FDC_MAIN_DATA_DIR    64 // 0: CPU->FDC   1: FDC->CPU
#define ZX_FDC_MAIN_DATA_READY  128


namespace zx_disk
{
    uint8_t mode = ZX_FDC_MODE_IDLE;
    uint8_t call_count = 0;

    uint8_t main_status_register = 0x80;
    uint8_t st0 = 0x00;
    uint8_t st1 = 0x00;
    uint8_t st2 = 0x00;
    uint8_t st3 = 0x00;

    uint8_t srt, hlt, hut, nd;

    uint8_t current_head = 0;
    uint8_t current_drive = 0;

    int zx_fdc_main_status_port_in(int port);
    int zx_fdc_data_port_in(int port);
    void zx_fdc_data_port_out(int port, int val);

    void init()
    {
        z80::connect_port(0x2ffd, 0xf002, zx_fdc_main_status_port_in, nullptr);
        z80::connect_port(0x3ffd, 0xf002, zx_fdc_data_port_in, zx_fdc_data_port_out);
    }

    void reset()
    {

    }

    int zx_fdc_main_status_port_in(int port)
    {
        printf("FDC port 0x2ffd IN\n");
        return main_status_register;
    }

    void start_command(uint8_t command);

    uint8_t (*process_current_command)(uint8_t) = nullptr;
    uint8_t process_command_specify(uint8_t command);
    uint8_t process_command_sense_drive_status(uint8_t command);

    int zx_fdc_data_port_in(int port)
    {
        printf("FDC port 0x3ffd IN\n");
        if (process_current_command) return process_current_command(0);
        return 0;
    }

    void zx_fdc_data_port_out(int port, int val)
    {
        printf("FDC port 0x3ffd OUT: 0x%02x\n", val);
        if (process_current_command)
            process_current_command(val);
        else
            start_command(val & ZX_FDC_COMMAND_MASK);
    }

    void start_command(uint8_t command)
    {
        main_status_register |= ZX_FDC_MAIN_BUSY;
        mode = ZX_FDC_MODE_COMMAND;
        call_count = 1;
        switch(command)
        {
            case ZX_FDC_COMMAND_SPECIFY:
                process_current_command = process_command_specify;
                break;
            case ZX_FDC_COMMAND_SENSE_DRIVE_STATUS:
                process_current_command = process_command_sense_drive_status;
                break;
            default:
            {
                mode = ZX_FDC_MODE_RESULT;
                call_count = 0;
            }
        }
    }

    // ===================================================================
    // FDC COMMAND: SPECIFY (0x03)
    // ===================================================================
    uint8_t process_command_specify(uint8_t command)
    {
        switch (call_count) {
            case 1:
                srt = (command & 0xf0) >> 4;
                hut = command & 0x0f;
                call_count++;
                break;
            case 2:
                hlt = (command & 0xfe) >> 1;
                nd = command & 0x01;
                call_count=0;
                process_current_command = nullptr;
                mode = ZX_FDC_MODE_IDLE;
                main_status_register &= ~ZX_FDC_MAIN_BUSY;
                break;
        }
        return 0;
    }

    // ===================================================================
    // FDC COMMAND: SENSE DRIVE STATUS (0x04)
    // ===================================================================
    uint8_t process_command_sense_drive_status(uint8_t command)
    {
        switch (mode)
        {
            case ZX_FDC_MODE_COMMAND:
                current_head = (command & 0x4)>>2;
                current_drive = command & 0x3;
                call_count = 0;
                mode = ZX_FDC_MODE_RESULT;
                break;
            case ZX_FDC_MODE_RESULT:
                process_current_command = nullptr;
                mode = ZX_FDC_MODE_IDLE;
                main_status_register &= ~ZX_FDC_MAIN_BUSY;
                return current_drive | (current_head<<2) | 0x38; // ST3
        }
        return 0;
    }

}