#include <ogc/pad.h>
#include <stdio.h>
#include <unistd.h>
#include <wiiuse/wpad.h>

#include "controller.h"
#include "dopios.h"
#include "tools.h"

#define MAX_WIIMOTES    4

int ScanPads(u32 *button) {
    *button = 0;
    u32 pressed;
    u32 held, held2;
    u16 pressedGC;

    WPAD_ScanPads();
    pressed = WPAD_ButtonsDown(WPAD_CHAN_0) | WPAD_ButtonsDown(WPAD_CHAN_1)
            | WPAD_ButtonsDown(WPAD_CHAN_2) | WPAD_ButtonsDown(WPAD_CHAN_3);
    held = WPAD_ButtonsHeld(WPAD_CHAN_0) | WPAD_ButtonsHeld(WPAD_CHAN_1)
            | WPAD_ButtonsHeld(WPAD_CHAN_2) | WPAD_ButtonsHeld(WPAD_CHAN_3);

    if (held) {
        if (held & WPAD_BUTTON_UP) {
            pressed |= WPAD_BUTTON_UP;
        } else if (held & WPAD_BUTTON_DOWN) {
            pressed |= WPAD_BUTTON_DOWN;
        }
        if (held & WPAD_BUTTON_LEFT) {
            pressed |= WPAD_BUTTON_LEFT;
        } else if (held & WPAD_BUTTON_RIGHT) {
            pressed |= WPAD_BUTTON_RIGHT;
        }
        usleep(4000);
        WPAD_ScanPads();
        held2 = WPAD_ButtonsHeld(WPAD_CHAN_0) | WPAD_ButtonsHeld(WPAD_CHAN_1)
                | WPAD_ButtonsHeld(WPAD_CHAN_2) | WPAD_ButtonsHeld(WPAD_CHAN_3);
        if( held != held2 ) {
            held = 0;
        }
        usleep(260000);
    }
    PAD_ScanPads();
    pressedGC = PAD_ButtonsDown(PAD_CHAN0) | PAD_ButtonsDown(PAD_CHAN1)
            | PAD_ButtonsDown(PAD_CHAN2) | PAD_ButtonsDown(PAD_CHAN3);

    if (pressed || pressedGC) {
        if (pressedGC) usleep(20000);
        if (pressed & WPAD_BUTTON_A || pressed & WPAD_CLASSIC_BUTTON_A
                || pressedGC & PAD_BUTTON_A) *button = WPAD_BUTTON_A;
        if (pressed & WPAD_BUTTON_B || pressed & WPAD_CLASSIC_BUTTON_B
                || pressedGC & PAD_BUTTON_B) *button = WPAD_BUTTON_B;
        if (pressed & WPAD_BUTTON_LEFT || pressed & WPAD_CLASSIC_BUTTON_LEFT
                || pressedGC & PAD_BUTTON_LEFT) *button = WPAD_BUTTON_LEFT;
        if (pressed & WPAD_BUTTON_RIGHT || pressed & WPAD_CLASSIC_BUTTON_RIGHT
                || pressedGC & PAD_BUTTON_RIGHT) *button = WPAD_BUTTON_RIGHT;
        if (pressed & WPAD_BUTTON_UP || pressed & WPAD_CLASSIC_BUTTON_UP
                || pressedGC & PAD_BUTTON_UP) *button = WPAD_BUTTON_UP;
        if (pressed & WPAD_BUTTON_DOWN || pressed & WPAD_CLASSIC_BUTTON_DOWN
                || pressedGC & PAD_BUTTON_DOWN) *button = WPAD_BUTTON_DOWN;
        if (pressed & WPAD_BUTTON_HOME || pressed & WPAD_CLASSIC_BUTTON_HOME
                || pressedGC & PAD_BUTTON_START) *button = WPAD_BUTTON_HOME;
        if (pressed & WPAD_BUTTON_MINUS || pressed & WPAD_CLASSIC_BUTTON_MINUS
                || pressedGC & PAD_TRIGGER_L) *button = WPAD_BUTTON_MINUS;
        if (pressed & WPAD_BUTTON_PLUS || pressed & WPAD_CLASSIC_BUTTON_PLUS
                || pressedGC & PAD_TRIGGER_R) *button = WPAD_BUTTON_PLUS;
        if (pressed & WPAD_BUTTON_1 || pressed & WPAD_CLASSIC_BUTTON_FULL_L) *button
                = WPAD_BUTTON_1;
        if (pressed & WPAD_BUTTON_2 || pressed & WPAD_CLASSIC_BUTTON_FULL_R) *button
                = WPAD_BUTTON_2;
    }

    return *button;
}

void WaitAnyKey() {
    u32 button = 0;
    while (!button && !ExitMainThread) {
        button = WaitButtons();
    }
}

u32 WaitKey(u32 button) {
    u32 pressed = 0;
    
    while (!(pressed & button)) {
        if (ExitMainThread) return 0;
        VIDEO_WaitVSync();
        ScanPads(&pressed);
        if (pressed & WPAD_BUTTON_HOME) ReturnToLoader();
    }

    return pressed;
}

u32 Wpad_GetButtons(void) {
    u32 buttons = 0, cnt;

    /* Scan pads */
    WPAD_ScanPads();

    /* Get pressed buttons */
    for (cnt = 0; cnt < MAX_WIIMOTES; cnt++)
        buttons |= WPAD_ButtonsDown(cnt);

    return buttons;
}

u32 Wpad_GetButtonsHeld(void) {
    u32 buttons = 0, cnt;

    /* Scan pads */
    WPAD_ScanPads();

    /* Get pressed buttons */
    for (cnt = 0; cnt < MAX_WIIMOTES; cnt++)
        buttons |= WPAD_ButtonsHeld(cnt);

    return buttons;
}

u32 Pad_GetButtons(void) {
    u32 buttons = 0, cnt;

    /* Scan pads */
    PAD_ScanPads();

    /* Get pressed buttons */
    //for (cnt = 0; cnt < MAX_WIIMOTES; cnt++)
    for (cnt = 0; cnt < 4; cnt++)
        buttons |= PAD_ButtonsDown(cnt);

    return buttons;
}

// Routine to wait for a button from either the Wiimote or a gamecube
// controller. The return value will mimic the WPAD buttons to minimize
// the amount of changes to the original code, that is expecting only
// Wiimote button presses. Note that the "HOME" button on the Wiimote
// is mapped to the "SELECT" button on the Gamecube Ctrl. (wiiNinja 5/15/2009)
u32 WaitButtons(void) {
    u32 buttons = 0;
    u32 buttonsHeld = 0;
    u32 buttonsGC = 0;
    
    /* Wait for button pressing */
    while (!buttons && !buttonsGC && !buttonsHeld && !ExitMainThread) {
        // GC buttons
        buttonsGC = Pad_GetButtons();
        
        // Wii buttons
        buttons = Wpad_GetButtons();
        
        buttonsHeld = Wpad_GetButtonsHeld();
        if (!(buttonsHeld & WPAD_BUTTON_UP)
                && !(buttonsHeld & WPAD_BUTTON_DOWN) && !(buttonsHeld
                & WPAD_BUTTON_LEFT) && !(buttonsHeld & WPAD_BUTTON_RIGHT)) {
            buttonsHeld = 0;
        }
        VIDEO_WaitVSync();
    }

    if (buttonsHeld) {
        if (buttonsHeld & WPAD_BUTTON_UP) {
            buttons |= WPAD_BUTTON_UP;
        } else if (buttonsHeld & WPAD_BUTTON_DOWN) {
            buttons |= WPAD_BUTTON_DOWN;
        }
        if (buttonsHeld & WPAD_BUTTON_LEFT) {
            buttons |= WPAD_BUTTON_LEFT;
        } else if (buttonsHeld & WPAD_BUTTON_RIGHT) {
            buttons |= WPAD_BUTTON_RIGHT;
        }
        usleep(120000);
    }

    if (buttons & WPAD_CLASSIC_BUTTON_A) {
        buttons |= WPAD_BUTTON_A;
    } else if (buttons & WPAD_CLASSIC_BUTTON_B) {
        buttons |= WPAD_BUTTON_B;
    } else if (buttons & WPAD_CLASSIC_BUTTON_LEFT) {
        buttons |= WPAD_BUTTON_LEFT;
    } else if (buttons & WPAD_CLASSIC_BUTTON_RIGHT) {
        buttons |= WPAD_BUTTON_RIGHT;
    } else if (buttons & WPAD_CLASSIC_BUTTON_UP) {
        buttons |= WPAD_BUTTON_UP;
    } else if (buttons & WPAD_CLASSIC_BUTTON_DOWN) {
        buttons |= WPAD_BUTTON_DOWN;
    } else if (buttons & WPAD_CLASSIC_BUTTON_HOME) {
        buttons |= WPAD_BUTTON_HOME;
    } else if (buttons & WPAD_CLASSIC_BUTTON_MINUS) {
        buttons |= WPAD_BUTTON_MINUS;
    } else if (buttons & WPAD_CLASSIC_BUTTON_PLUS) {
        buttons |= WPAD_BUTTON_PLUS;
    } else if (buttons & WPAD_CLASSIC_BUTTON_FULL_L) {
        buttons |= WPAD_BUTTON_1;
    } else if (buttons & WPAD_CLASSIC_BUTTON_FULL_R) {
        buttons |= WPAD_BUTTON_2;
    }

    if (buttonsGC) {
        if (buttonsGC & PAD_BUTTON_A) {
            buttons |= WPAD_BUTTON_A;
        } else if (buttonsGC & PAD_BUTTON_B) {
            buttons |= WPAD_BUTTON_B;
        } else if (buttonsGC & PAD_BUTTON_LEFT) {
            buttons |= WPAD_BUTTON_LEFT;
        } else if (buttonsGC & PAD_BUTTON_RIGHT) {
            buttons |= WPAD_BUTTON_RIGHT;
        } else if (buttonsGC & PAD_BUTTON_DOWN) {
            buttons |= WPAD_BUTTON_DOWN;
        } else if (buttonsGC & PAD_BUTTON_UP) {
            buttons |= WPAD_BUTTON_UP;
        } else if (buttonsGC & PAD_BUTTON_START) {
            buttons |= WPAD_BUTTON_HOME;
        } else if (buttonsGC & PAD_TRIGGER_L) {
            buttons |= WPAD_BUTTON_MINUS;
        } else if (buttonsGC & PAD_TRIGGER_R) {
            buttons |= WPAD_BUTTON_PLUS;
        } else if (buttonsGC & PAD_BUTTON_Y) {
            buttons |= WPAD_BUTTON_1;
        } else if (buttonsGC & PAD_BUTTON_X) {
            buttons |= WPAD_BUTTON_2;
        }
    }
    
    return buttons;
} // WaitButtons
