#include <unistd.h>             // usleep
#include <wiiuse/wpad.h>        // Wiimote
#include <ogcsys.h>             // nanosleep
#include <ogc/lwp_watchdog.h>   // gettime

/**
 * Structure to hold the rumble data.
 */
typedef struct _rumble_data
{
    bool rumbeling;  /**< True if Wiimote is rumbeling, false otherwise. */
    u64 time2rumble; /**< Time to rumble in milisecond. */
} RUMBLE_DATA;

static RUMBLE_DATA Rumble_Info[WPAD_MAX_WIIMOTES];

/**
 * Set rumbeling time for a specific controller
 * @param[in] chan Controller ID
 * @param[in] rumble_time Time to rumble in milisecond
 */
void RUMBLE_Wiimote(s32 chan, int rumble_time)
{
    Rumble_Info[chan].time2rumble = ticks_to_millisecs(gettime()) + rumble_time;
    Rumble_Info[chan].rumbeling = true;
    WPAD_Rumble(chan, 1); // Rumble on
}

/**
 * Initialize rumbeling to false
 */
void RUMBLE_Init()
{
    int i;
    for(i = 0; i < WPAD_MAX_WIIMOTES; i++)
    {
        Rumble_Info[i].rumbeling = false;
        Rumble_Info[i].time2rumble = 0;
    }
}

/**
 * Stop rumbeling if time is elapsed
 */
void RUMBLE_Verify()
{
    if(Rumble_Info[WPAD_CHAN_0].rumbeling && ticks_to_millisecs(gettime()) > Rumble_Info[WPAD_CHAN_0].time2rumble)
    {
        WPAD_Rumble(WPAD_CHAN_0, 0); // Rumble off
        Rumble_Info[WPAD_CHAN_0].rumbeling = false;
    }
    if(Rumble_Info[WPAD_CHAN_1].rumbeling && ticks_to_millisecs(gettime()) > Rumble_Info[WPAD_CHAN_1].time2rumble)
    {
        WPAD_Rumble(WPAD_CHAN_1, 0); // Rumble off
        Rumble_Info[WPAD_CHAN_1].rumbeling = false;
    }
    if(Rumble_Info[WPAD_CHAN_2].rumbeling && ticks_to_millisecs(gettime()) > Rumble_Info[WPAD_CHAN_2].time2rumble)
    {
        WPAD_Rumble(WPAD_CHAN_2, 0); // Rumble off
        Rumble_Info[WPAD_CHAN_2].rumbeling = false;
    }
    if(Rumble_Info[WPAD_CHAN_3].rumbeling && ticks_to_millisecs(gettime()) > Rumble_Info[WPAD_CHAN_3].time2rumble)
    {
        WPAD_Rumble(WPAD_CHAN_3, 0); // Rumble off
        Rumble_Info[WPAD_CHAN_3].rumbeling = false;
    }
}

/**
 * Waits for an amount of time in msec.
 * @param[in] milisec Number of milliseconds to wait.
 */
void msleep(unsigned long milisec)
{
    struct timespec req = {0};
    time_t sec = (int)(milisec / 1000);
    milisec -= (sec * 1000);
    req.tv_sec = sec;
    req.tv_nsec = milisec * 1000000L;
    while(nanosleep(&req) == -1)
        continue;
}