/*
 *	wiiuse
 *
 *	Written By:
 *		Michal Wiedenbauer	< shagkur >
 *		Dave Murphy			< WinterMute >
 *		Hector Martin		< marcan >
 * 		Radu Andries		<admiral0>
 *
 *	Copyright 2009
 *
 *	This file is part of wiiuse and fWIIne.
 *
 *	This program is free software; you can redistribute it and/or modify
 *	it under the terms of the GNU General Public License as published by
 *	the Free Software Foundation; either version 3 of the License, or
 *	(at your option) any later version.
 *
 *	This program is distributed in the hope that it will be useful,
 *	but WITHOUT ANY WARRANTY; without even the implied warranty of
 *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *	GNU General Public License for more details.
 *
 *	You should have received a copy of the GNU General Public License
 *	along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *	$Header$
 *
 */

#include "motion_plus.h"

#include "dynamics.h" /* for calc_joystick_state, etc */
#include "events.h"   /* for disable_expansion */
#include "io.h"       /* for wiiuse_read */
#include "ir.h"       /* for wiiuse_set_ir_mode */
#include "nunchuk.h"  /* for nunchuk_pressed_buttons */

#include <math.h>   /* for fabs */
#include <string.h> /* for memset */

static void wiiuse_calibrate_motion_plus(struct motion_plus_t *mp);
static void calculate_gyro_rates(struct motion_plus_t *mp);

void wiiuse_probe_motion_plus(struct wiimote_t *wm)
{
    byte buf[MAX_PAYLOAD];
    unsigned id;

    wiiuse_read_data_sync(wm, 0, WM_EXP_MOTION_PLUS_IDENT, 6, buf);

    /* check error code */
    if ((buf[5] & 0x0f) == 0)
    {
        WIIUSE_DEBUG("No Motion+ available, stopping probe.");
        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT);
        return;
    }

    /* decode the id */
    id = from_big_endian_uint32_t(buf + 2);

    if (id != EXP_ID_CODE_INACTIVE_MOTION_PLUS && id != EXP_ID_CODE_INACTIVE_MOTION_PLUS_BUILTIN
        && id != EXP_ID_CODE_NLA_MOTION_PLUS && id != EXP_ID_CODE_NLA_MOTION_PLUS_NUNCHUK
        && id != EXP_ID_CODE_NLA_MOTION_PLUS_CLASSIC)
    {
        /* we have read something weird */
        WIIUSE_DEBUG("Motion+ ID doesn't match, probably not connected.");
        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT);
        return;
    }

    WIIUSE_DEBUG("Detected inactive Motion+!");
    WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT);

    /* init M+ */
    buf[0] = 0x55;
    wiiuse_write_data(wm, WM_EXP_MOTION_PLUS_INIT, buf, 1);

    /* Init whatever is hanging on the pass-through port */
    buf[0] = 0x55;
    wiiuse_write_data(wm, WM_EXP_MEM_ENABLE1, buf, 1);

    buf[0] = 0x00;
    wiiuse_write_data(wm, WM_EXP_MEM_ENABLE2, buf, 1);

    /* Init gyroscope data */
    wm->exp.mp.cal_gyro.roll      = 0;
    wm->exp.mp.cal_gyro.pitch     = 0;
    wm->exp.mp.cal_gyro.yaw       = 0;
    wm->exp.mp.orient.roll        = 0.0;
    wm->exp.mp.orient.pitch       = 0.0;
    wm->exp.mp.orient.yaw         = 0.0;
    wm->exp.mp.raw_gyro_threshold = 10;

    wm->exp.mp.nc         = &(wm->exp.nunchuk);
    wm->exp.mp.classic    = &(wm->exp.classic);
    wm->exp.nunchuk.flags = &wm->flags;

    wm->exp.mp.ext = 0;

    wiiuse_set_ir_mode(wm);
    wiiuse_set_report_type(wm);
}

void wiiuse_motion_plus_handshake(struct wiimote_t *wm, byte *data, unsigned short len)
{
    uint32_t val;
    if (data == NULL)
    {
        wiiuse_read_data_cb(wm, wiiuse_motion_plus_handshake, wm->motion_plus_id, WM_EXP_ID, 6);
    } else
    {
        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
        WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP); /* tell wiimote to include exp. data in reports */

        val = from_big_endian_uint32_t(data + 2);

        if (val == EXP_ID_CODE_MOTION_PLUS || val == EXP_ID_CODE_MOTION_PLUS_NUNCHUK
            || val == EXP_ID_CODE_MOTION_PLUS_CLASSIC)
        {
            /* handshake done */
            wm->event = WIIUSE_MOTION_PLUS_ACTIVATED;

            switch (val)
            {
            case EXP_ID_CODE_MOTION_PLUS:
                wm->exp.type = EXP_MOTION_PLUS;
                break;

            case EXP_ID_CODE_MOTION_PLUS_NUNCHUK:
                wm->exp.type = EXP_MOTION_PLUS_NUNCHUK;
                break;

            case EXP_ID_CODE_MOTION_PLUS_CLASSIC:
                wm->exp.type = EXP_MOTION_PLUS_CLASSIC;
                break;

            default:
                /* huh? */
                WIIUSE_WARNING("Unknown ID returned in Motion+ handshake %d\n", val);
                wm->exp.type = EXP_MOTION_PLUS;
                break;
            }

            WIIUSE_DEBUG("Motion plus connected");

            /* Init gyroscopes */
            wm->exp.mp.cal_gyro.roll      = 0;
            wm->exp.mp.cal_gyro.pitch     = 0;
            wm->exp.mp.cal_gyro.yaw       = 0;
            wm->exp.mp.orient.roll        = 0.0;
            wm->exp.mp.orient.pitch       = 0.0;
            wm->exp.mp.orient.yaw         = 0.0;
            wm->exp.mp.raw_gyro_threshold = 10;

            wm->exp.mp.nc         = &(wm->exp.nunchuk);
            wm->exp.mp.classic    = &(wm->exp.classic);
            wm->exp.nunchuk.flags = &wm->flags;

            wm->exp.mp.ext = 0;

            wiiuse_set_ir_mode(wm);
            wiiuse_set_report_type(wm);
        }
    }
}

/**
 *      @brief Enable/disable Motion+ expansion
 *
 *      @param wm        Pointer to the wiimote with Motion+
 *      @param status    0 - off, 1 - on, standalone, 2 - nunchuk pass-through
 *
 */
void wiiuse_set_motion_plus(struct wiimote_t *wm, int status)
{
    byte buf[MAX_PAYLOAD];
    byte val;
    int i;

    if (!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_MPLUS_PRESENT) || WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP_HANDSHAKE))
    {
        return;
    }

    if (status)
    {
        WIIUSE_DEBUG("Enabling Motion+\n");

        WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
        val = (status == 1) ? 0x04 : 0x05;
        wiiuse_write_data(wm, WM_EXP_MOTION_PLUS_ENABLE, &val, 1);

        wiiuse_millisleep(500); /* wait for M+ switch over */
        wiiuse_motion_plus_handshake(wm, NULL, 0);

    } else
    {
        WIIUSE_DEBUG("Disabling Motion+\n");

        disable_expansion(wm);
        val = 0x55;
        wiiuse_write_data(wm, WM_EXP_MEM_ENABLE1, &val, 1);
        wiiuse_millisleep(500); /* wait for M+ switch over */

        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
        wiiuse_set_ir_mode(wm);

        /*
         * try to ask for status 3 times, sometimes the first one(s) gives bad data
         * and doesn't show expansions - likely because the device didn't settle yet
         */
        for (i = 0; i < 3; ++i)
        {
            int rc = 0;

            WIIUSE_DEBUG("Asking for status, attempt %d ...\n", i);
            wm->event = WIIUSE_CONNECT;

            do
            {
                wiiuse_status(wm);
                rc = wiiuse_wait_report(wm, WM_RPT_CTRL_STATUS, buf, MAX_PAYLOAD, WIIUSE_READ_TIMEOUT);
            } while (rc < 0);

            if (buf[3] != 0)
                break;

            wiiuse_millisleep(500);
        }
        propagate_event(wm, WM_RPT_CTRL_STATUS, buf + 1);
    }
}

void motion_plus_disconnected(struct motion_plus_t *mp)
{
    WIIUSE_DEBUG("Motion plus disconnected");
    memset(mp, 0, sizeof(struct motion_plus_t));
}

void motion_plus_event(struct motion_plus_t *mp, int exp_type, byte *msg)
{
    /*
     * Pass-through modes interleave data from the gyro
     * with the expansion data. This extracts the tag
     * determining which is which
     */
    int isMPFrame = (1 << 1) & msg[5];
    mp->ext       = msg[4] & 0x1; /* extension attached to pass-through port? */

    if (mp->ext == 0 || isMPFrame)
    { /* reading gyro frame */
        /* Check if the gyroscope is in fast or slow mode (0 if rotating fast, 1 if slow or still) */
        mp->acc_mode = ((msg[4] & 0x2) << 1) | ((msg[3] & 0x1) << 1) | ((msg[3] & 0x2) >> 1);

        mp->raw_gyro.roll  = ((msg[4] & 0xFC) << 6) | msg[1];
        mp->raw_gyro.pitch = ((msg[5] & 0xFC) << 6) | msg[2];
        mp->raw_gyro.yaw   = ((msg[3] & 0xFC) << 6) | msg[0];

        /* First calibration */
        if ((mp->raw_gyro.roll > 5000) && (mp->raw_gyro.pitch > 5000) && (mp->raw_gyro.yaw > 5000)
            && (mp->raw_gyro.roll < 0x3fff) && (mp->raw_gyro.pitch < 0x3fff) && (mp->raw_gyro.yaw < 0x3fff)
            && !(mp->cal_gyro.roll) && !(mp->cal_gyro.pitch) && !(mp->cal_gyro.yaw))
        {
            wiiuse_calibrate_motion_plus(mp);
        }

        /* Calculate angular rates in deg/sec and performs some simple filtering */
        calculate_gyro_rates(mp);
    }

    else
    {
        /* expansion frame */
        if (exp_type == EXP_MOTION_PLUS_NUNCHUK)
        {
            /* ok, this is nunchuck, re-encode it as regular nunchuck packet */

            /* get button states */
            nunchuk_pressed_buttons(mp->nc, (msg[5] >> 2));

            /* calculate joystick state */
            calc_joystick_state(&(mp->nc->js), msg[0], msg[1]);

            /* calculate orientation */
            mp->nc->accel.x = msg[2];
            mp->nc->accel.y = msg[3];
            mp->nc->accel.z = (msg[4] & 0xFE) | ((msg[5] >> 5) & 0x04);

            calculate_orientation(&(mp->nc->accel_calib), &(mp->nc->accel), &(mp->nc->orient),
                                  NUNCHUK_IS_FLAG_SET(mp->nc, WIIUSE_SMOOTHING));

            calculate_gforce(&(mp->nc->accel_calib), &(mp->nc->accel), &(mp->nc->gforce));

        }

        else if (exp_type == EXP_MOTION_PLUS_CLASSIC)
        {
            WIIUSE_ERROR("Classic controller pass-through is not implemented!\n");
        }

        else
        {
            WIIUSE_ERROR("Unsupported mode passed to motion_plus_event() !\n");
        }
    }
}

/**
 *    @brief Calibrate the Motion Plus gyroscopes.
 *
 *    @param mp        Pointer to a motion_plus_t structure.
 *
 *  This should be called only after receiving the first values
 *    from the Motion Plus.
 */
void wiiuse_calibrate_motion_plus(struct motion_plus_t *mp)
{
    mp->cal_gyro.roll  = mp->raw_gyro.roll;
    mp->cal_gyro.pitch = mp->raw_gyro.pitch;
    mp->cal_gyro.yaw   = mp->raw_gyro.yaw;
    mp->orient.roll    = 0.0;
    mp->orient.pitch   = 0.0;
    mp->orient.yaw     = 0.0;
}

static void calculate_gyro_rates(struct motion_plus_t *mp)
{
    short int tmp_r, tmp_p, tmp_y;
    float tmp_roll, tmp_pitch, tmp_yaw;

    /* We consider calibration data */
    tmp_r = mp->raw_gyro.roll - mp->cal_gyro.roll;
    tmp_p = mp->raw_gyro.pitch - mp->cal_gyro.pitch;
    tmp_y = mp->raw_gyro.yaw - mp->cal_gyro.yaw;

    /* We convert to degree/sec according to fast/slow mode */
    if (mp->acc_mode & 0x04)
    {
        tmp_roll = (float)tmp_r / 20.0f;
    } else
    {
        tmp_roll = (float)tmp_r / 4.0f;
    }

    if (mp->acc_mode & 0x02)
    {
        tmp_pitch = (float)tmp_p / 20.0f;
    } else
    {
        tmp_pitch = (float)tmp_p / 4.0f;
    }

    if (mp->acc_mode & 0x01)
    {
        tmp_yaw = (float)tmp_y / 20.0f;
    } else
    {
        tmp_yaw = (float)tmp_y / 4.0f;
    }

    /* Simple filtering */
    if (fabs(tmp_roll) < 0.5f)
    {
        tmp_roll = 0.0f;
    }
    if (fabs(tmp_pitch) < 0.5f)
    {
        tmp_pitch = 0.0f;
    }
    if (fabs(tmp_yaw) < 0.5f)
    {
        tmp_yaw = 0.0f;
    }

    mp->angle_rate_gyro.roll  = tmp_roll;
    mp->angle_rate_gyro.pitch = tmp_pitch;
    mp->angle_rate_gyro.yaw   = tmp_yaw;
}