#include <stdint.h>
#include <stdio.h>
#include <pc.h>
#include <dos.h>

#include "vga.h"
#include "keyb.h"
#include "control.h"

#include "joy.h"

#define PORT 0x201

#define BUTTON1 0x10
#define BUTTON2 0x20
#define AXISX   1
#define AXISY   2

#define TIMEOUT 16000

static int16_t j_up, j_down, j_left, j_right;

static int16_t read_axis(uint8_t axis)
{
    uint16_t i;

    disable();
    outportb(PORT, 0xff);
    for (i = 0; i < TIMEOUT; i++)
        if (!(inportb(PORT) & axis))
            break;
    enable();

    return i;
}

static uint8_t calibrate(int16_t *v, int16_t *h)
{
    while (1)
    {
        if (keys[KEY_ESC])
        {
            while (keys[KEY_ESC])
                wait_vsync();
            return 0;
        }

        *v = read_axis(AXISY);
        *h = read_axis(AXISX);

        outportb(PORT, 0xff);
        if (!(inportb(PORT) & BUTTON1))
        {
            while (!(inportb(PORT) & BUTTON1))
                wait_vsync();
            return 1;
        }

        wait_vsync();
    }
}

uint8_t joy_detect()
{
    if (read_axis(AXISX) == TIMEOUT)
        return 0;

    printf("Joystick detected!\n");

    int16_t cx, cy;
    printf("Please center the joystick and press BUTTON 1 (ESC to abort)\n");
    if (!calibrate(&cy, &cx))
        return 0;
    printf("Please move UP + LEFT and press BUTTON 1 (ESC to abort)\n");
    if (!calibrate(&j_up, &j_left))
        return 0;
    printf("Please move DOWN + RIGHT and press BUTTON 1 (ESC to abort)\n");
    if (!calibrate(&j_down, &j_right))
        return 0;

#ifdef DEBUG
    printf("Joystick reads: up=%04d, down=%04d, left=%04d, right=%04d\n",
           j_up, j_down, j_left, j_right);
#endif

    j_up = cy - ((cy - j_up) / 2);
    j_down = cy + ((j_down - cy) / 2);
    j_left = cx - ((cx - j_left) / 2);
    j_right = cx + ((j_right - cx) / 2);

#ifdef DEBUG
    printf("      Adjusted: up=%04d, down=%04d, left=%04d, right=%04d\n",
           j_up, j_down, j_left, j_right);

    printf("(press X to continue)\n");
    while (!keys[KEY_X])
        wait_vsync();
#endif

    return 1;
}

uint8_t joy_read()
{
    uint8_t r = CTL_NONE;
    uint8_t b;
    int16_t v;

    outportb(PORT, 0xff);
    b = inportb(PORT);
    if (!(b & BUTTON1))
        r |= CTL_FIRE1;
    if (!(b & BUTTON2))
        r |= CTL_FIRE2;

    v = read_axis(AXISY);
    if (v < j_up)
        r |= CTL_UP;
    if (v > j_down)
        r |= CTL_DOWN;

    v = read_axis(AXISX);
    if (v < j_left)
        r |= CTL_LEFT;
    if (v > j_right)
        r |= CTL_RIGHT;

    return r;
}