/*
 * 86Box    A hypervisor and IBM PC system emulator that specializes in
 *          running old operating systems and software designed for IBM
 *          PC systems and compatibles from 1981 through fairly recent
 *          system designs based on the PCI bus.
 *
 *          This file is part of the 86Box distribution.
 *
 *          Implementation of the Teledisk floppy image format.
 *
 * Authors: Milodrag Milanovic,
 *          Haruhiko OKUMURA,
 *          Haruyasu YOSHIZAKI,
 *          Kenji RIKITAKE,
 *          Miran Grca, <mgrca8@gmail.com>
 *          Fred N. van Kempen, <decwiz@yahoo.com>
 *
 *          Based on Japanese version 29-NOV-1988
 *          LZSS coded by Haruhiko OKUMURA
 *          Adaptive Huffman Coding coded by Haruyasu YOSHIZAKI
 *          Edited and translated to English by Kenji RIKITAKE
 *
 *          Copyright 2013-2019 Milodrag Milanovic.
 *          Copyright 1988-2019 Haruhiko OKUMURA.
 *          Copyright 1988-2019 Haruyasu YOSHIZAKI.
 *          Copyright 1988-2019 Kenji RIKITAKE.
 *          Copyright 2016-2019 Miran Grca.
 */
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <wchar.h>
#define HAVE_STDARG_H
#include <86box/86box.h>
#include <86box/timer.h>
#include <86box/plat.h>
#include <86box/fdd.h>
#include <86box/fdd_86f.h>
#include <86box/fdd_td0.h>
#include <86box/fdc.h>
#include "lzw/lzw.h"

#define BUFSZ         512 /* new input buffer */
#define TD0_MAX_BUFSZ (1024UL * 1024UL * 4UL)

/* LZSS Parameters */
#define N         4096 /* Size of string buffer */
#define F         60   /* Size of look-ahead buffer */
#define THRESHOLD 2
#define NIL       N /* End of tree's node  */

/* Huffman coding parameters */
#define N_CHAR   (256 - THRESHOLD + F) /* code (= 0..N_CHAR-1) */
#define T        (N_CHAR * 2 - 1)      /* Size of table */
#define R        (T - 1)               /* root position */
#define MAX_FREQ 0x8000
/* update when cumulative frequency */
/* reaches to this value */

typedef struct tdlzhuf_t {
    uint16_t r;
    uint16_t bufcnt;      /* string buffer */
    uint16_t bufndx;      /* string buffer */
    uint16_t bufpos;      /* string buffer */
                          /* the following to allow block reads
                             from input in next_word() */
    uint16_t ibufcnt;     /* input buffer counters */
    uint16_t ibufndx;     /* input buffer counters */
    uint8_t inbuf[BUFSZ]; /* input buffer */
} tdlzhuf;

typedef struct td0dsk_t {
    FILE *fdd_file;
    off_t fdd_file_offset;

    tdlzhuf  tdctl;
    uint8_t  text_buf[N + F - 1];
    uint16_t freq[T + 1]; /* cumulative freq table */

    /*
     * pointing parent nodes.
     * area [T..(T + N_CHAR - 1)] are pointers for leaves
     */
    int16_t prnt[T + N_CHAR];

    /* pointing children nodes (son[], son[] + 1)*/
    int16_t son[T];

    uint16_t getbuf;
    uint8_t  getlen;
} td0dsk_t;

typedef struct td0_sector_t {
    uint8_t  track;
    uint8_t  head;
    uint8_t  sector;
    uint8_t  size;
    uint8_t  flags;
    uint8_t  fm;
    uint8_t *data;
} td0_sector_t;

typedef struct td0_t {
    FILE *fp;

    int          tracks;
    int          track_width;
    int          sides;
    uint16_t     disk_flags;
    uint16_t     default_track_flags;
    uint16_t     side_flags[256][2];
    uint8_t      max_sector_size;
    uint8_t      track_in_file[256][2];
    td0_sector_t sects[256][2][256];
    uint8_t      track_spt[256][2];
    uint8_t      gap3_len;
    uint16_t     current_side_flags[2];
    int          track;
    int          current_sector_index[2];
    uint8_t      calculated_gap3_lengths[256][2];
    uint8_t      xdf_ordered_pos[256][2];
    uint8_t      interleave_ordered_pos[256][2];

    uint8_t *lzw_buf;
    uint8_t *imagebuf;

    uint8_t *processed_buf;
} td0_t;

/*
 * Tables for encoding/decoding upper 6 bits of
 * sliding dictionary pointer
 */
static const uint8_t d_code[256] = {
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
    0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
    0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02,
    0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02,
    0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03,
    0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03,
    0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04,
    0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
    0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
    0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
    0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
    0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09,
    0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A,
    0x0B, 0x0B, 0x0B, 0x0B, 0x0B, 0x0B, 0x0B, 0x0B,
    0x0C, 0x0C, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D, 0x0D,
    0x0E, 0x0E, 0x0E, 0x0E, 0x0F, 0x0F, 0x0F, 0x0F,
    0x10, 0x10, 0x10, 0x10, 0x11, 0x11, 0x11, 0x11,
    0x12, 0x12, 0x12, 0x12, 0x13, 0x13, 0x13, 0x13,
    0x14, 0x14, 0x14, 0x14, 0x15, 0x15, 0x15, 0x15,
    0x16, 0x16, 0x16, 0x16, 0x17, 0x17, 0x17, 0x17,
    0x18, 0x18, 0x19, 0x19, 0x1A, 0x1A, 0x1B, 0x1B,
    0x1C, 0x1C, 0x1D, 0x1D, 0x1E, 0x1E, 0x1F, 0x1F,
    0x20, 0x20, 0x21, 0x21, 0x22, 0x22, 0x23, 0x23,
    0x24, 0x24, 0x25, 0x25, 0x26, 0x26, 0x27, 0x27,
    0x28, 0x28, 0x29, 0x29, 0x2A, 0x2A, 0x2B, 0x2B,
    0x2C, 0x2C, 0x2D, 0x2D, 0x2E, 0x2E, 0x2F, 0x2F,
    0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,
    0x38, 0x39, 0x3A, 0x3B, 0x3C, 0x3D, 0x3E, 0x3F,
};

static const uint8_t d_len[256] = {
    0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03,
    0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03,
    0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03,
    0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03,
    0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04,
    0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04,
    0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04,
    0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04,
    0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04,
    0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04,
    0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
    0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
    0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
    0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
    0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
    0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
    0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
    0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05,
    0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
    0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
    0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
    0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
    0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
    0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
    0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
    0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
    0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
    0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
    0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
    0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
    0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
    0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
};

static td0_t *td0[FDD_NUM];

#ifdef ENABLE_TD0_LOG
int td0_do_log = ENABLE_TD0_LOG;

static void
td0_log(const char *fmt, ...)
{
    va_list ap;

    if (td0_do_log) {
        va_start(ap, fmt);
        pclog_ex(fmt, ap);
        va_end(ap);
    }
}
#else
#    define td0_log(fmt, ...)
#endif

static void
fdd_image_read(int drive, char *buffer, uint32_t offset, uint32_t len)
{
    td0_t *dev = td0[drive];

    if (fseek(dev->fp, offset, SEEK_SET) == -1)
        fatal("fdd_image_read(): Error seeking to the beginning of the file\n");
    if (fread(buffer, 1, len, dev->fp) != len)
        fatal("fdd_image_read(): Error reading data\n");
}

static int
dsk_identify(int drive)
{
    char header[2];

    fdd_image_read(drive, header, 0, 2);
    if (header[0] == 'T' && header[1] == 'D')
        return 1;
    else if (header[0] == 't' && header[1] == 'd')
        return 1;

    return 0;
}

static int
state_data_read(td0dsk_t *state, uint8_t *buf, uint16_t size)
{
    uint32_t image_size = 0;

    fseek(state->fdd_file, 0, SEEK_END);
    image_size = ftell(state->fdd_file);
    if (size > image_size - state->fdd_file_offset)
        size = (image_size - state->fdd_file_offset) & 0xffff;
    if (fseek(state->fdd_file, state->fdd_file_offset, SEEK_SET) == -1)
        fatal("TD0: Failed to seek in state_data_read()\n");
    if (fread(buf, 1, size, state->fdd_file) != size)
        fatal("TD0: Error reading data in state_data_read()\n");
    state->fdd_file_offset += size;

    return size;
}

static int
state_next_word(td0dsk_t *state)
{
    if (state->tdctl.ibufndx >= state->tdctl.ibufcnt) {
        state->tdctl.ibufndx = 0;
        state->tdctl.ibufcnt = state_data_read(state, state->tdctl.inbuf, BUFSZ);
        if (state->tdctl.ibufcnt == 0)
            return (-1);
    }

    while (state->getlen <= 8) { /* typically reads a word at a time */
        state->getbuf |= state->tdctl.inbuf[state->tdctl.ibufndx++] << (8 - state->getlen);
        state->getlen += 8;
    }

    return 0;
}

/* get one bit */
static int
state_GetBit(td0dsk_t *state)
{
    int16_t i;

    if (state_next_word(state) < 0)
        return (-1);

    i = state->getbuf;
    state->getbuf <<= 1;
    state->getlen--;
    if (i < 0)
        return 1;

    return 0;
}

/* get a byte */
static int
state_GetByte(td0dsk_t *state)
{
    uint16_t i;

    if (state_next_word(state) != 0)
        return -1;

    i = state->getbuf;
    state->getbuf <<= 8;
    state->getlen -= 8;
    i = i >> 8;

    return ((int) i);
}

/* initialize freq tree */
static void
state_StartHuff(td0dsk_t *state)
{
    int i;
    int j;

    for (i = 0; i < N_CHAR; i++) {
        state->freq[i]     = 1;
        state->son[i]      = i + T;
        state->prnt[i + T] = i;
    }
    i = 0;
    j = N_CHAR;
    while (j <= R) {
        state->freq[j] = state->freq[i] + state->freq[i + 1];
        state->son[j]  = i;
        state->prnt[i] = state->prnt[i + 1] = j;
        i += 2;
        j++;
    }
    state->freq[T] = 0xffff;
    state->prnt[R] = 0;
}

/* reconstruct freq tree */
static void
state_reconst(td0dsk_t *state)
{
    int16_t  i;
    int16_t  j;
    int16_t  k;
    uint16_t f;
    uint16_t l;

    /* halven cumulative freq for leaf nodes */
    j = 0;
    for (i = 0; i < T; i++) {
        if (state->son[i] >= T) {
            state->freq[j] = (state->freq[i] + 1) / 2;
            state->son[j]  = state->son[i];
            j++;
        }
    }

    /* make a tree : first, connect children nodes */
    for (i = 0, j = N_CHAR; j < T; i += 2, j++) {
        k = i + 1;
        f = state->freq[j] = state->freq[i] + state->freq[k];
        for (k = j - 1; f < state->freq[k]; k--) { }
        k++;
        l = (j - k) * 2;

        /* These *HAVE* to be memmove's as destination and source
           can overlap, which memcpy can't handle. */
        memmove(&state->freq[k + 1], &state->freq[k], l);
        state->freq[k] = f;
        memmove(&state->son[k + 1], &state->son[k], l);
        state->son[k] = i;
    }

    /* connect parent nodes */
    for (i = 0; i < T; i++) {
        if ((k = state->son[i]) >= T)
            state->prnt[k] = i;
        else
            state->prnt[k] = state->prnt[k + 1] = i;
    }
}

/* update freq tree */
static void
state_update(td0dsk_t *state, int c)
{
    int i;
    int j;
    int k;
    int l;

    if (state->freq[R] == MAX_FREQ)
        state_reconst(state);

    c = state->prnt[c + T];

    /* do it until reaching the root */
    do {
        k = ++state->freq[c];

        /* swap nodes to keep the tree freq-ordered */
        if (k > state->freq[l = c + 1]) {
            while (k > state->freq[++l]) { }
            l--;
            state->freq[c] = state->freq[l];
            state->freq[l] = k;

            i              = state->son[c];
            state->prnt[i] = l;
            if (i < T)
                state->prnt[i + 1] = l;

            j             = state->son[l];
            state->son[l] = i;

            state->prnt[j] = c;
            if (j < T)
                state->prnt[j + 1] = c;
            state->son[c] = j;

            c = l;
        }
    } while ((c = state->prnt[c]) != 0);
}

static int16_t
state_DecodeChar(td0dsk_t *state)
{
    int      ret;
    uint16_t c;

    c = state->son[R];

    /*
     * start searching tree from the root to leaves.
     * choose node #(son[]) if input bit == 0
     * else choose #(son[]+1) (input bit == 1)
     */
    while (c < T) {
        if ((ret = state_GetBit(state)) < 0)
            return (-1);
        c += (unsigned) ret;
        c = state->son[c];
    }
    c -= T;

    state_update(state, c);

    return c;
}

static int16_t
state_DecodePosition(td0dsk_t *state)
{
    int16_t  bit;
    uint16_t i;
    uint16_t j;
    uint16_t c;

    /* decode upper 6 bits from given table */
    if ((bit = state_GetByte(state)) < 0)
        return (-1);

    i = (uint16_t) bit;
    c = (uint16_t) d_code[i] << 6;
    j = d_len[i];

    /* input lower 6 bits directly */
    j -= 2;
    while (j--) {
        if ((bit = state_GetBit(state)) < 0)
            return (-1);
        i = (i << 1) + bit;
    }

    return (c | (i & 0x3f));
}

/* DeCompression - split out initialization code to init_Decode() */
static void
state_init_Decode(td0dsk_t *state)
{
    state->getbuf        = 0;
    state->getlen        = 0;
    state->tdctl.ibufcnt = state->tdctl.ibufndx = 0; /* input buffer is empty */
    state->tdctl.bufcnt                         = 0;

    state_StartHuff(state);
    for (uint16_t i = 0; i < N - F; i++)
        state->text_buf[i] = ' ';

    state->tdctl.r = N - F;
}

/* Decoding/Uncompressing */
static int
state_Decode(td0dsk_t *state, uint8_t *buf, int len)
{
    int16_t c;
    int16_t pos;
    int     count; /* was an unsigned long, seems unnecessary */

    for (count = 0; count < len;) {
        if (state->tdctl.bufcnt == 0) {
            if ((c = state_DecodeChar(state)) < 0)
                return count; /* fatal error */
            if (c < 256) {
                *(buf++)                          = c & 0xff;
                state->text_buf[state->tdctl.r++] = c & 0xff;
                state->tdctl.r &= (N - 1);
                count++;
            } else {
                if ((pos = state_DecodePosition(state)) < 0)
                    return count; /* fatal error */
                state->tdctl.bufpos = (state->tdctl.r - pos - 1) & (N - 1);
                state->tdctl.bufcnt = c - 255 + THRESHOLD;
                state->tdctl.bufndx = 0;
            }
        } else {
            /* still chars from last string */
            while (state->tdctl.bufndx < state->tdctl.bufcnt && count < len) {
                c        = state->text_buf[(state->tdctl.bufpos + state->tdctl.bufndx) & (N - 1)];
                *(buf++) = c & 0xff;
                state->tdctl.bufndx++;
                state->text_buf[state->tdctl.r++] = c & 0xff;
                state->tdctl.r &= (N - 1);
                count++;
            }

            /* reset bufcnt after copy string from text_buf[] */
            if (state->tdctl.bufndx >= state->tdctl.bufcnt)
                state->tdctl.bufndx = state->tdctl.bufcnt = 0;
        }
    }

    return count; /* count == len, success */
}

static uint32_t
get_raw_tsize(int side_flags, int slower_rpm)
{
    uint32_t size;

    switch (side_flags & 0x27) {
        case 0x22:
            size = slower_rpm ? 5314 : 5208;
            break;

        default:
        case 0x02:
        case 0x21:
            size = slower_rpm ? 6375 : 6250;
            break;

        case 0x01:
            size = slower_rpm ? 7650 : 7500;
            break;

        case 0x20:
            size = slower_rpm ? 10629 : 10416;
            break;

        case 0x00:
            size = slower_rpm ? 12750 : 12500;
            break;

        case 0x23:
            size = slower_rpm ? 21258 : 20833;
            break;

        case 0x03:
            size = slower_rpm ? 25500 : 25000;
            break;

        case 0x25:
            size = slower_rpm ? 42517 : 41666;
            break;

        case 0x05:
            size = slower_rpm ? 51000 : 50000;
            break;
    }

    return size;
}

static int
td0_initialize(int drive)
{
    td0_t         *dev = td0[drive];
    uint8_t        header[12];
    int            fm;
    int            head;
    int            track;
    int            track_count = 0;
    int            head_count  = 0;
    int            track_spt;
    int            track_spt_adjusted;
    int            offset    = 0;
    int            density   = 0;
    int            temp_rate = 0;
    uint32_t       file_size;
    uint16_t       len;
    uint16_t       rep;
    td0dsk_t       disk_decode;
    const uint8_t *hs;
    uint16_t       size;
    uint8_t       *dbuf         = dev->processed_buf;
    uint32_t       total_size   = 0;
    uint32_t       id_field     = 0;
    uint32_t       pre_sector   = 0;
    int32_t        track_size   = 0;
    int32_t        raw_tsize    = 0;
    uint32_t       minimum_gap3 = 0;
    uint32_t       minimum_gap4 = 0;
    int            i;
    int            j;
    int            k;
    int            size_diff;
    int            gap_sum;

    if (dev->fp == NULL) {
        td0_log("TD0: Attempted to initialize without loading a file first\n");
        return 0;
    }

    fseek(dev->fp, 0, SEEK_END);
    file_size = ftell(dev->fp);

    if (file_size < 12) {
        td0_log("TD0: File is too small to even contain the header\n");
        return 0;
    }

    if (file_size > TD0_MAX_BUFSZ) {
        td0_log("TD0: File exceeds the maximum size\n");
        return 0;
    }

    fseek(dev->fp, 0, SEEK_SET);
    (void) !fread(header, 1, 12, dev->fp);
    head_count = header[9];

    if (header[0] == 't') {
        if (((header[4] / 10) % 10) == 2) {
            td0_log("TD0: File is compressed (TeleDisk 2.x, LZHUF)\n");
            disk_decode.fdd_file = dev->fp;
            state_init_Decode(&disk_decode);
            disk_decode.fdd_file_offset = 12;
            state_Decode(&disk_decode, dev->imagebuf, TD0_MAX_BUFSZ);
        } else {
            td0_log("TD0: File is compressed (TeleDisk 1.x, LZW)\n");
            if (fseek(dev->fp, 12, SEEK_SET) == -1)
                fatal("td0_initialize(): Error seeking to offet 12\n");
            if (fread(dev->lzw_buf, 1, file_size - 12, dev->fp) != (file_size - 12))
                fatal("td0_initialize(): Error reading LZW-encoded buffer\n");
            LZWDecodeFile((char *) dev->imagebuf, (char *) dev->lzw_buf, NULL, file_size - 12);
        }
    } else {
        td0_log("TD0: File is uncompressed\n");
        if (fseek(dev->fp, 12, SEEK_SET) == -1)
            fatal("td0_initialize(): Error seeking to offet 12\n");
        if (fread(dev->imagebuf, 1, file_size - 12, dev->fp) != (file_size - 12))
            fatal("td0_initialize(): Error reading image buffer\n");
    }

    if (header[7] & 0x80)
        offset = 10 + dev->imagebuf[2] + (dev->imagebuf[3] << 8);

    track_spt = dev->imagebuf[offset];
    if (track_spt == 255) {
        /* Empty file? */
        td0_log("TD0: File has no tracks\n");
        return 0;
    }

    density = (header[5] >> 1) & 3;

    if (density == 3) {
        td0_log("TD0: Unknown density\n");
        return 0;
    }

    /*
     * We determine RPM from the drive type as well as we possibly can.
     * This byte is actually the BIOS floppy drive type read by Teledisk
     * from the CMOS.
     */
    switch (header[6]) {
        case 0: /* 5.25" 360k in 1.2M drive: 360 rpm
                   CMOS Drive type: None, value probably
                   reused by Teledisk */
        case 2: /* 5.25" 1.2M: 360 rpm */
        case 5: /* 8"/5.25"/3.5" 1.25M: 360 rpm */
            dev->default_track_flags = (density == 1) ? 0x20 : 0x21;
            dev->max_sector_size     = (density == 1) ? 6 : 5; /* 8192 or 4096 bytes. */
            break;

        case 1: /* 5.25" 360k: 300 rpm */
        case 3: /* 3.5" 720k: 300 rpm */
            dev->default_track_flags = 0x02;
            dev->max_sector_size     = 5; /* 4096 bytes. */
            break;

        case 4: /* 3.5" 1.44M: 300 rpm */
            dev->default_track_flags = (density == 1) ? 0x00 : 0x02;
            dev->max_sector_size     = (density == 1) ? 6 : 5; /* 8192 or 4096 bytes. */
            break;

        case 6: /* 3.5" 2.88M: 300 rpm */
            dev->default_track_flags = (density == 1) ? 0x00 : ((density == 2) ? 0x03 : 0x02);
            dev->max_sector_size     = (density == 1) ? 6 : ((density == 2) ? 7 : 5); /* 16384, 8192, or 4096 bytes. */
            break;

        default:
            break;
    }

    dev->disk_flags = header[5] & 0x06;

    dev->track_width = (header[7] & 1) ^ 1;

    for (i = 0; i < 256; i++) {
        memset(dev->side_flags[i], 0, 4);
        memset(dev->track_in_file[i], 0, 2);
        memset(dev->calculated_gap3_lengths[i], 0, 2);
        for (j = 0; j < 2; j++)
            memset(dev->sects[i][j], 0, sizeof(td0_sector_t));
    }

    while (track_spt != 255) {
        track_spt_adjusted = track_spt;

        track                           = dev->imagebuf[offset + 1];
        head                            = dev->imagebuf[offset + 2] & 1;
        fm                              = (header[5] & 0x80) || (dev->imagebuf[offset + 2] & 0x80); /* ? */
        dev->side_flags[track][head]    = dev->default_track_flags | (fm ? 0 : 8);
        dev->track_in_file[track][head] = 1;
        offset += 4;
        track_size = fm ? 73 : 146;
        if (density == 2)
            id_field = fm ? 54 : 63;
        else
            id_field = fm ? 35 : 44;
        pre_sector = id_field + (fm ? 7 : 16);

        for (i = 0; i < track_spt; i++) {
            hs = &dev->imagebuf[offset];
            offset += 6;

            dev->sects[track][head][i].track  = hs[0];
            dev->sects[track][head][i].head   = hs[1];
            dev->sects[track][head][i].sector = hs[2];
            dev->sects[track][head][i].size   = hs[3];
            dev->sects[track][head][i].flags  = hs[4];
            dev->sects[track][head][i].fm     = !!fm;
            dev->sects[track][head][i].data   = dbuf;

            size = 128 << hs[3];
            if ((total_size + size) >= TD0_MAX_BUFSZ) {
                td0_log("TD0: Processed buffer overflow\n");
                return 0;
            }

            if (hs[4] & 0x30)
                memset(dbuf, (hs[4] & 0x10) ? 0xf6 : 0x00, size);
            else {
                offset += 3;
                switch (hs[8]) {
                    default:
                        td0_log("TD0: Image uses an unsupported sector data encoding: %i\n", hs[8]);
                        return 0;

                    case 0:
                        memcpy(dbuf, &dev->imagebuf[offset], size);
                        offset += size;
                        break;

                    case 1:
                        offset += 4;
                        k = (hs[9] + (hs[10] << 8)) * 2;
                        k = (k <= size) ? k : size;
                        for (j = 0; j < k; j += 2) {
                            dbuf[j]     = hs[11];
                            dbuf[j + 1] = hs[12];
                        }
                        if (k < size)
                            memset(&(dbuf[k]), 0, size - k);
                        break;

                    case 2:
                        k = 0;
                        while (k < size) {
                            len = dev->imagebuf[offset];
                            rep = dev->imagebuf[offset + 1];
                            offset += 2;
                            if (!len) {
                                memcpy(&(dbuf[k]), &dev->imagebuf[offset], rep);
                                offset += rep;
                                k += rep;
                            } else {
                                len = (1 << len);
                                rep = len * rep;
                                rep = ((rep + k) <= size) ? rep : (size - k);
                                for (j = 0; j < rep; j += len)
                                    memcpy(&(dbuf[j + k]), &dev->imagebuf[offset], len);
                                k += rep;
                                offset += len;
                            }
                        }
                        break;
                }
            }

            dbuf += size;
            total_size += size;

            if (hs[4] & 0x20) {
                track_size += id_field;
                track_spt_adjusted--;
            } else if (hs[4] & 0x40)
                track_size += (pre_sector - id_field + 3);
            else {
                if ((hs[4] & 0x02) || (hs[3] > (dev->max_sector_size - fm)))
                    track_size += (pre_sector + 3);
                else
                    track_size += (pre_sector + size + 2);
            }
        }

        if (track > track_count)
            track_count = track;

        if (track_spt != 255) {
            dev->track_spt[track][head] = track_spt;

            if ((dev->track_spt[track][head] == 8) && (dev->sects[track][head][0].size == 3))
                dev->side_flags[track][head] = (dev->side_flags[track][head] & ~0x67) | 0x20;

            raw_tsize    = get_raw_tsize(dev->side_flags[track][head], 0);
            minimum_gap3 = 12 * track_spt_adjusted;
            size_diff    = raw_tsize - track_size;
            gap_sum      = minimum_gap3 + minimum_gap4;
            if (size_diff < gap_sum) {
                /* If we can't fit the sectors with a reasonable minimum gap at perfect RPM, let's try 2% slower. */
                raw_tsize = get_raw_tsize(dev->side_flags[track][head], 1);
                /* Set disk flags so that rotation speed is 2% slower. */
                dev->disk_flags |= (3 << 5);
                size_diff = raw_tsize - track_size;
                if ((size_diff < gap_sum) && !fdd_get_turbo(drive)) {
                    /* If we can't fit the sectors with a reasonable minimum gap even at 2% slower RPM, abort. */
                    td0_log("TD0: Unable to fit the %i sectors into drive %i, track %i, side %i\n", track_spt_adjusted, drive, track, head);
                    return 0;
                }
            }
            dev->calculated_gap3_lengths[track][head] = (size_diff - minimum_gap4) / track_spt_adjusted;

            track_spt = dev->imagebuf[offset];
        }
    }

    if ((dev->disk_flags & 0x60) == 0x60)
        td0_log("TD0: Disk will rotate 2% below perfect RPM\n");

    dev->tracks = track_count + 1;

    temp_rate = dev->default_track_flags & 7;
    if ((dev->default_track_flags & 0x27) == 0x20)
        temp_rate = 4;
    dev->gap3_len = gap3_sizes[temp_rate][dev->sects[0][0][0].size][dev->track_spt[0][0]];
    if (!dev->gap3_len)
        dev->gap3_len = dev->calculated_gap3_lengths[0][0]; /* If we can't determine the GAP3 length, assume the smallest one we possibly know of. */

    if (head_count == 2)
        dev->disk_flags |= 8; /* 2 sides */

    if (dev->tracks <= 43)
        dev->track_width &= ~1;

    dev->sides = head_count;

    dev->current_side_flags[0] = dev->side_flags[0][0];
    dev->current_side_flags[1] = dev->side_flags[0][1];

    td0_log("TD0: File loaded: %i tracks, %i sides, disk flags: %02X, side flags: %02X, %02X, GAP3 length: %02X\n", dev->tracks, dev->sides, dev->disk_flags, dev->current_side_flags[0], dev->current_side_flags[1], dev->gap3_len);

    return 1;
}

static uint16_t
disk_flags(int drive)
{
    const td0_t *dev = td0[drive];

    return (dev->disk_flags);
}

static uint16_t
side_flags(int drive)
{
    const td0_t   *dev    = td0[drive];
    int            side   = 0;
    uint16_t       sflags = 0;

    side   = fdd_get_head(drive);
    sflags = dev->current_side_flags[side];

    return sflags;
}

static void
set_sector(int drive, int side, uint8_t c, uint8_t h, uint8_t r, uint8_t n)
{
    td0_t *dev = td0[drive];
    int    cyl = c;

    dev->current_sector_index[side] = 0;
    if (cyl != dev->track)
        return;
    for (uint8_t i = 0; i < dev->track_spt[cyl][side]; i++) {
        if ((dev->sects[cyl][side][i].track == c) && (dev->sects[cyl][side][i].head == h) && (dev->sects[cyl][side][i].sector == r) && (dev->sects[cyl][side][i].size == n)) {
            dev->current_sector_index[side] = i;
        }
    }
}

static uint8_t
poll_read_data(int drive, int side, uint16_t pos)
{
    const td0_t *dev = td0[drive];

    return (dev->sects[dev->track][side][dev->current_sector_index[side]].data[pos]);
}

static int
track_is_xdf(int drive, int side, int track)
{
    td0_t  *dev   = td0[drive];
    uint8_t id[4] = { 0, 0, 0, 0 };
    int     i;
    int     effective_sectors;
    int     xdf_sectors;
    int     high_sectors;
    int     low_sectors;
    int     max_high_id;
    int     expected_high_count;
    int     expected_low_count;

    effective_sectors = xdf_sectors = high_sectors = low_sectors = 0;

    memset(dev->xdf_ordered_pos[side], 0, 256);

    if (!track) {
        if ((dev->track_spt[track][side] == 16) || (dev->track_spt[track][side] == 19)) {
            if (!side) {
                max_high_id         = (dev->track_spt[track][side] == 19) ? 0x8B : 0x88;
                expected_high_count = (dev->track_spt[track][side] == 19) ? 0x0B : 0x08;
                expected_low_count  = 8;
            } else {
                max_high_id         = (dev->track_spt[track][side] == 19) ? 0x93 : 0x90;
                expected_high_count = (dev->track_spt[track][side] == 19) ? 0x13 : 0x10;
                expected_low_count  = 0;
            }

            for (i = 0; i < dev->track_spt[track][side]; i++) {
                id[0] = dev->sects[track][side][i].track;
                id[1] = dev->sects[track][side][i].head;
                id[2] = dev->sects[track][side][i].sector;
                id[3] = dev->sects[track][side][i].size;
                if (!(id[0]) && (id[1] == side) && (id[3] == 2)) {
                    if ((id[2] >= 0x81) && (id[2] <= max_high_id)) {
                        high_sectors++;
                        dev->xdf_ordered_pos[id[2]][side] = i;
                    }

                    if ((id[2] >= 0x01) && (id[2] <= 0x08)) {
                        low_sectors++;
                        dev->xdf_ordered_pos[id[2]][side] = i;
                    }
                }
            }

            if ((high_sectors == expected_high_count) && (low_sectors == expected_low_count)) {
                dev->current_side_flags[side] = (dev->track_spt[track][side] == 19) ? 0x08 : 0x28;
                return ((dev->track_spt[track][side] == 19) ? 2 : 1);
            }
        }
    } else {
        for (i = 0; i < dev->track_spt[track][side]; i++) {
            id[0] = dev->sects[track][side][i].track;
            id[1] = dev->sects[track][side][i].head;
            id[2] = dev->sects[track][side][i].sector;
            id[3] = dev->sects[track][side][i].size;
            effective_sectors++;
            if ((id[0] == track) && (id[1] == side) && !(id[2]) && !(id[3])) {
                effective_sectors--;
            }
            if ((id[0] == track) && (id[1] == side) && (id[2] == (id[3] | 0x80))) {
                xdf_sectors++;
                dev->xdf_ordered_pos[id[2]][side] = i;
            }
        }

        if ((effective_sectors == 3) && (xdf_sectors == 3)) {
            dev->current_side_flags[side] = 0x28;
            return 1; /* 5.25" 2HD XDF */
        }

        if ((effective_sectors == 4) && (xdf_sectors == 4)) {
            dev->current_side_flags[side] = 0x08;
            return 2; /* 3.5" 2HD XDF */
        }
    }

    return 0;
}

static int
track_is_interleave(int drive, int side, int track)
{
    td0_t *dev = td0[drive];
    int    i;
    int    effective_sectors;
    int    track_spt;

    effective_sectors = 0;

    for (i = 0; i < 256; i++)
        dev->interleave_ordered_pos[i][side] = 0;

    track_spt = dev->track_spt[track][side];

    if (track_spt != 21)
        return 0;

    for (i = 0; i < track_spt; i++) {
        if ((dev->sects[track][side][i].track == track) && (dev->sects[track][side][i].head == side) && (dev->sects[track][side][i].sector >= 1) && (dev->sects[track][side][i].sector <= track_spt) && (dev->sects[track][side][i].size == 2)) {
            effective_sectors++;
            dev->interleave_ordered_pos[dev->sects[track][side][i].sector][side] = i;
        }
    }

    if (effective_sectors == track_spt)
        return 1;

    return 0;
}

static void
td0_seek(int drive, int track)
{
    td0_t  *dev = td0[drive];
    uint8_t id[4] = { 0, 0, 0, 0 };
    int     sector;
    int     current_pos;
    int     ssize           = 512;
    int     track_rate      = 0;
    int     track_gap2      = 22;
    int     track_gap3      = 12;
    int     xdf_type        = 0;
    int     interleave_type = 0;
    int     is_trackx       = 0;
    int     xdf_spt         = 0;
    int     xdf_sector      = 0;
    int     ordered_pos     = 0;
    int     real_sector     = 0;
    int     actual_sector   = 0;
    int     fm;
    int     sector_adjusted;

    if (dev->fp == NULL)
        return;

    if (!dev->track_width && fdd_doublestep_40(drive))
        track /= 2;

    d86f_set_cur_track(drive, track);

    is_trackx  = (track == 0) ? 0 : 1;
    dev->track = track;

    dev->current_side_flags[0] = dev->side_flags[track][0];
    dev->current_side_flags[1] = dev->side_flags[track][1];

    d86f_reset_index_hole_pos(drive, 0);
    d86f_reset_index_hole_pos(drive, 1);

    d86f_destroy_linked_lists(drive, 0);
    d86f_destroy_linked_lists(drive, 1);

    if (track > dev->tracks) {
        d86f_zero_track(drive);
        return;
    }

    for (int side = 0; side < dev->sides; side++) {
        track_rate = dev->current_side_flags[side] & 7;
        /* Make sure 300 kbps @ 360 rpm is treated the same as 250 kbps @ 300 rpm. */
        if (!track_rate && (dev->current_side_flags[side] & 0x20))
            track_rate = 4;
        if ((dev->current_side_flags[side] & 0x27) == 0x21)
            track_rate = 2;
        track_gap3 = gap3_sizes[track_rate][dev->sects[track][side][0].size][dev->track_spt[track][side]];
        if (!track_gap3)
            track_gap3 = dev->calculated_gap3_lengths[track][side];

        track_gap2 = ((dev->current_side_flags[side] & 7) >= 3) ? 41 : 22;

        xdf_type = track_is_xdf(drive, side, track);

        interleave_type = track_is_interleave(drive, side, track);

        current_pos     = d86f_prepare_pretrack(drive, side, 0);
        sector_adjusted = 0;

        if (!xdf_type) {
            for (sector = 0; sector < dev->track_spt[track][side]; sector++) {
                if (interleave_type == 0) {
                    real_sector   = dev->sects[track][side][sector].sector;
                    actual_sector = sector;
                } else {
                    real_sector   = dmf_r[sector];
                    actual_sector = dev->interleave_ordered_pos[real_sector][side];
                }

                id[0] = dev->sects[track][side][actual_sector].track;
                id[1] = dev->sects[track][side][actual_sector].head;
                id[2] = real_sector;
                id[3] = dev->sects[track][side][actual_sector].size;
                td0_log("track %i, side %i, %i,%i,%i,%i %i\n", track, side, id[0], id[1], id[2], id[3], dev->sects[track][side][actual_sector].flags);
                fm = dev->sects[track][side][actual_sector].fm;
                if (((dev->sects[track][side][actual_sector].flags & 0x42) || (id[3] > (dev->max_sector_size - fm))) && !fdd_get_turbo(drive))
                    ssize = 3;
                else
                    ssize = 128 << ((uint32_t) id[3]);
                current_pos = d86f_prepare_sector(drive, side, current_pos, id, dev->sects[track][side][actual_sector].data, ssize, track_gap2, track_gap3, dev->sects[track][side][actual_sector].flags);

                if (sector_adjusted == 0)
                    d86f_initialize_last_sector_id(drive, id[0], id[1], id[2], id[3]);

                if (!(dev->sects[track][side][actual_sector].flags & 0x40))
                    sector_adjusted++;
            }
        } else {
            xdf_type--;
            xdf_spt = xdf_physical_sectors[xdf_type][is_trackx];
            for (sector = 0; sector < xdf_spt; sector++) {
                xdf_sector  = (side * xdf_spt) + sector;
                id[0]       = track;
                id[1]       = side;
                id[2]       = xdf_disk_layout[xdf_type][is_trackx][xdf_sector].id.r;
                id[3]       = is_trackx ? (id[2] & 7) : 2;
                ordered_pos = dev->xdf_ordered_pos[id[2]][side];
                fm          = dev->sects[track][side][ordered_pos].fm;
                if (((dev->sects[track][side][ordered_pos].flags & 0x42) || (id[3] > (dev->max_sector_size - fm))) && !fdd_get_turbo(drive))
                    ssize = 3;
                else
                    ssize = 128 << ((uint32_t) id[3]);
                if (is_trackx)
                    current_pos = d86f_prepare_sector(drive, side, xdf_trackx_spos[xdf_type][xdf_sector], id, dev->sects[track][side][ordered_pos].data, ssize, track_gap2, xdf_gap3_sizes[xdf_type][is_trackx], dev->sects[track][side][ordered_pos].flags);
                else
                    current_pos = d86f_prepare_sector(drive, side, current_pos, id, dev->sects[track][side][ordered_pos].data, ssize, track_gap2, xdf_gap3_sizes[xdf_type][is_trackx], dev->sects[track][side][ordered_pos].flags);

                if (sector_adjusted == 0)
                    d86f_initialize_last_sector_id(drive, id[0], id[1], id[2], id[3]);

                if (!(dev->sects[track][side][ordered_pos].flags & 0x40))
                    sector_adjusted++;
            }
        }
    }
}

void
td0_init(void)
{
    memset(td0, 0x00, sizeof(td0));
}

void
td0_abort(int drive)
{
    td0_t *dev = td0[drive];

    if (dev->imagebuf)
        free(dev->imagebuf);
    if (dev->processed_buf)
        free(dev->processed_buf);
    if (dev->fp)
        fclose(dev->fp);
    memset(floppyfns[drive], 0, sizeof(floppyfns[drive]));
    free(dev);
    td0[drive] = NULL;
}

void
td0_load(int drive, char *fn)
{
    td0_t   *dev;
    uint32_t i;

    d86f_unregister(drive);

    writeprot[drive] = 1;

    dev = (td0_t *) calloc(1, sizeof(td0_t));
    td0[drive] = dev;

    dev->fp = plat_fopen(fn, "rb");
    if (dev->fp == NULL) {
        memset(floppyfns[drive], 0, sizeof(floppyfns[drive]));
        return;
    }

    fwriteprot[drive] = writeprot[drive];

    if (!dsk_identify(drive)) {
        td0_log("TD0: Not a valid Teledisk image\n");
        td0_abort(drive);
        return;
    } else {
        td0_log("TD0: Valid Teledisk image\n");
    }

    /* Allocate the processing buffers. */
    i             = 1024UL * 1024UL * 4UL;
    dev->lzw_buf = (uint8_t *) calloc(1, i);
    dev->imagebuf = (uint8_t *) calloc(1, i);
    dev->processed_buf = (uint8_t *) calloc(1, i);

    if (!td0_initialize(drive)) {
        td0_log("TD0: Failed to initialize\n");
        td0_abort(drive);
        return;
    } else {
        td0_log("TD0: Initialized successfully\n");
    }

    /* Attach this format to the D86F engine. */
    d86f_handler[drive].disk_flags        = disk_flags;
    d86f_handler[drive].side_flags        = side_flags;
    d86f_handler[drive].writeback         = null_writeback;
    d86f_handler[drive].set_sector        = set_sector;
    d86f_handler[drive].read_data         = poll_read_data;
    d86f_handler[drive].write_data        = null_write_data;
    d86f_handler[drive].format_conditions = null_format_conditions;
    d86f_handler[drive].extra_bit_cells   = null_extra_bit_cells;
    d86f_handler[drive].encoded_data      = common_encoded_data;
    d86f_handler[drive].read_revolution   = common_read_revolution;
    d86f_handler[drive].index_hole_pos    = null_index_hole_pos;
    d86f_handler[drive].get_raw_size      = common_get_raw_size;
    d86f_handler[drive].check_crc         = 1;
    d86f_set_version(drive, 0x0063);

    drives[drive].seek = td0_seek;

    d86f_common_handlers(drive);
}

void
td0_close(int drive)
{
    td0_t *dev = td0[drive];

    if (dev == NULL)
        return;

    d86f_unregister(drive);

    if (dev->lzw_buf)
        free(dev->lzw_buf);
    if (dev->imagebuf)
        free(dev->imagebuf);
    if (dev->processed_buf)
        free(dev->processed_buf);

    for (uint16_t i = 0; i < 256; i++) {
        for (uint8_t j = 0; j < 2; j++) {
            for (uint16_t k = 0; k < 256; k++)
                dev->sects[i][j][k].data = NULL;
        }
    }

    for (uint16_t i = 0; i < 256; i++) {
        memset(dev->side_flags[i], 0, 4);
        memset(dev->track_in_file[i], 0, 2);
        memset(dev->calculated_gap3_lengths[i], 0, 2);
        for (uint8_t j = 0; j < 2; j++)
            memset(dev->sects[i][j], 0, sizeof(td0_sector_t));
    }

    if (dev->fp != NULL)
        fclose(dev->fp);

    /* Release resources. */
    free(dev);
    td0[drive] = NULL;
}
