#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 9999
#define CALIBRATING_LOOPS 10

static uint16_t j_up, j_down, j_left, j_right;

static uint16_t read_axis(uint16_t *x, uint16_t *y)
{
    uint16_t i, value;
    uint16_t mask = AXISX | AXISY;

    *x = *y = 0;

    disable();
    outportb(PORT, 0);
    for (i = 1; mask && i < TIMEOUT; i++)
    {
        value = inportb(PORT) ^ mask;
        if (value & AXISX)
        {
            *x = i;
            mask ^= AXISX;
        }
        if (value & AXISY)
        {
            *y = i;
            mask ^= AXISY;
        }
    }
    enable();

    return i;
}

static uint8_t calibrate_release(uint16_t *x, uint16_t *y)
{
    uint8_t i;
    uint16_t xacc, yacc;

    while (1)
    {
        if (keys[KEY_ESC])
        {
            while (keys[KEY_ESC])
                wait_vsync();
            return 0;
        }

        xacc = yacc = 0;
        for (i = 0; i < CALIBRATING_LOOPS; i++)
        {
            read_axis(x, y);
            xacc += *x;
            yacc += *y;
        }

        if (!(inportb(PORT) & BUTTON1))
        {
            while (!(inportb(PORT) & BUTTON1))
                wait_vsync();

            *x = xacc / CALIBRATING_LOOPS;
            *y = yacc / CALIBRATING_LOOPS;

            return 1;
        }
        wait_vsync();
    }
}

static uint8_t calibrate_circle(uint16_t joy_values[4])
{
    uint8_t i;
    uint16_t x, y;

    while (1)
    {
        if (keys[KEY_ESC])
        {
            while (keys[KEY_ESC])
                wait_vsync();
            return 0;
        }

        for (i = 0; i < CALIBRATING_LOOPS; i++)
        {
            read_axis(&x, &y);
            // up
            if (y < joy_values[0])
                joy_values[0] = y;
            // down
            if (y > joy_values[1])
                joy_values[1] = y;
            // left
            if (x < joy_values[2])
                joy_values[2] = x;
            // right
            if (x > joy_values[3])
                joy_values[3] = x;
        }

        if (!(inportb(PORT) & BUTTON1))
        {
            while (!(inportb(PORT) & BUTTON1))
                wait_vsync();

            return 1;
        }
        wait_vsync();
    }
}

uint8_t joy_detect()
{
    uint16_t xcenter, ycenter;

    // this read doesn't matter
    if (read_axis(&xcenter, &ycenter) == TIMEOUT)
        return 0;

    printf("Joystick detected!\n");

    printf("Please center the joystick and press BUTTON 1 (ESC to abort)\n");
    if (!calibrate_release(&xcenter, &ycenter))
        return 0;

    // up, down, left, right
    uint16_t joy_values[4] = { ycenter, ycenter, xcenter, xcenter };

    printf("Please move the joystick in circles and then press BUTTON 1 (ESC to abort)\n");
    if (!calibrate_circle(joy_values))
        return 0;

    printf("Please center the joystick and press BUTTON 1 (ESC to abort)\n");
    if (!calibrate_release(&xcenter, &ycenter))
        return 0;

    j_up = ycenter - ((ycenter - joy_values[0]) / 2);
    j_down = ycenter + ((joy_values[1] - ycenter) / 2);
    j_left = xcenter - ((xcenter - joy_values[2]) / 2);
    j_right = xcenter + ((joy_values[3] - xcenter) / 2);

    return 1;
}

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

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

    uint16_t x, y;

    read_axis(&x, &y);
    if (y < j_up)
        r |= CTL_UP;
    if (y > j_down)
        r |= CTL_DOWN;
    if (x < j_left)
        r |= CTL_LEFT;
    if (x > j_right)
        r |= CTL_RIGHT;

    return r;
}