/*
 $License:
    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
 $
 */

/*******************************************************************************
 *
 * $Id:$
 *
 ******************************************************************************/

/*
    Includes, Defines, and Macros
*/

#undef MPL_LOG_NDEBUG
#define MPL_LOG_NDEBUG 0 /* turn to 0 to enable verbose logging */

#include "log.h"
#undef MPL_LOG_TAG
#define MPL_LOG_TAG "MPL-playback"

#include "and_constructor.h"
#include "mlos.h"
#include "invensense.h"
#include "invensense_adv.h"

/*
    Typedef
*/
struct inv_construct_t {
    int product; /**< Gyro Product Number */
    int debug_mode;
    int last_mode;
    FILE *file;
    int dmp_version;
    int left_in_buffer;
#define FIFO_READ_SIZE 100
    unsigned char fifo_data[FIFO_READ_SIZE];
    int gyro_enable;
    int accel_enable;
    int compass_enable;
    int quat_enable;
};

/*
    Globals
*/
static struct inv_construct_t inv_construct = {0};
static void (*s_func_cb)(void);
static char playback_filename[101] = "/data/playback.bin";
struct fifo_dmp_config fifo_dmp_cfg = {0};

/*
    Functions
*/
void inv_set_playback_filename(char *filename, int length)
{
    if (length > 100) {
        MPL_LOGE("Error : file name and path too long, 100 characters limit\n");
        return;
    }
    strncpy(playback_filename, filename, length);
}

inv_error_t inv_constructor_setup(void)
{
    unsigned short orient;
    extern signed char g_gyro_orientation[9];
    extern signed char g_accel_orientation[9];
    extern signed char g_compass_orientation[9];
    float scale = 2.f;
    long sens;

    // gyro setup
    orient = inv_orientation_matrix_to_scalar(g_gyro_orientation);
    inv_set_gyro_orientation_and_scale(orient, 2000L << 15);

    // accel setup
    orient = inv_orientation_matrix_to_scalar(g_accel_orientation);
    scale = 2.f;
    sens = (long)(scale * (1L << 15));
    inv_set_accel_orientation_and_scale(orient, sens);

    // compass setup
    orient = inv_orientation_matrix_to_scalar(g_compass_orientation);
    // scale is the max value of the compass in micro Tesla.
    scale = 5000.f;
    sens = (long)(scale * (1L << 15));
    inv_set_compass_orientation_and_scale(orient, sens);

    return INV_SUCCESS;
}

inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void))
{
    s_func_cb = func_cb;
    return INV_SUCCESS;
}

void int32_to_long(int32_t in[], long out[], int length)
{
    int ii;
    for (ii = 0; ii < length; ii++)
        out[ii] = (long)in[ii];
}

inv_error_t inv_playback(void)
{
    inv_rd_dbg_states type;
    inv_time_t ts;
    int32_t buffer[4];
    short gyro[3];
    size_t r = 1;
    int32_t orientation;
    int32_t sensitivity, sample_rate_us = 0;

    // Check to make sure we were request to playback
    if (inv_construct.debug_mode != RD_PLAYBACK) {
        MPL_LOGE("%s|%s|%d error: debug_mode != RD_PLAYBACK\n",
                 __FILE__, __func__, __LINE__);
        return INV_ERROR;
    }

    if (inv_construct.file == NULL) {
        inv_construct.file = fopen(playback_filename, "rb");
        if (!inv_construct.file) {
            MPL_LOGE("Error : cannot find or open playback file '%s'\n",
                     playback_filename);
            return INV_ERROR_FILE_OPEN;
        }
    }

    while (1) {
        r = fread(&type, sizeof(type), 1, inv_construct.file);
        if (r == 0) {
            MPL_LOGV("read 0 bytes, PLAYBACK file closed\n");
            inv_construct.debug_mode = RD_NO_DEBUG;
            fclose(inv_construct.file);
            break;
        }
        //MPL_LOGV("TYPE : %d, %d\n", type);
        switch (type) {
        case PLAYBACK_DBG_TYPE_GYRO:
            r = fread(gyro, sizeof(gyro[0]), 3, inv_construct.file);
            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
            inv_build_gyro(gyro, ts);
            MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO, %+d, %+d, %+d, %+lld\n",
                     gyro[0], gyro[1], gyro[2], ts);
            break;
        case PLAYBACK_DBG_TYPE_ACCEL:
        {
            long accel[3];
            r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file);
            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
            int32_to_long(buffer, accel, 3);
            inv_build_accel(accel, 0, ts);
            MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL, %+d, %+d, %+d, %lld\n",
                     buffer[0], buffer[1], buffer[2], ts);
            break;
        }
        case PLAYBACK_DBG_TYPE_COMPASS:
        {
            long compass[3];
            r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file);
            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
            int32_to_long(buffer, compass, 3);
            inv_build_compass(compass, 0, ts);
            MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS, %+d, %+d, %+d, %lld\n",
                     buffer[0], buffer[1], buffer[2], ts);
            break;
        }
        case PLAYBACK_DBG_TYPE_TEMPERATURE:
            r = fread(buffer, sizeof(buffer[0]), 1, inv_construct.file);
            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
            inv_build_temp(buffer[0], ts);
            MPL_LOGV("PLAYBACK_DBG_TYPE_TEMPERATURE, %+d, %lld\n",
                     buffer[0], ts);
            break;
        case PLAYBACK_DBG_TYPE_QUAT:
        {
            long quat[4];
            r = fread(buffer, sizeof(buffer[0]), 4, inv_construct.file);
            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
            int32_to_long(buffer, quat, 4);
            inv_build_quat(quat, INV_BIAS_APPLIED, ts);
            MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT, %+d, %+d, %+d, %+d, %lld\n",
                     buffer[0], buffer[1], buffer[2], buffer[3], ts);
            break;
        }
        case PLAYBACK_DBG_TYPE_EXECUTE:
            MPL_LOGV("PLAYBACK_DBG_TYPE_EXECUTE\n");
            inv_execute_on_data();
            if (s_func_cb)
                s_func_cb();
            //done = 1;
            break;

        case PLAYBACK_DBG_TYPE_G_ORIENT:
            MPL_LOGV("PLAYBACK_DBG_TYPE_G_ORIENT\n");
            r = fread(&orientation, sizeof(orientation), 1, inv_construct.file);
            r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file);
            inv_set_gyro_orientation_and_scale(orientation, sensitivity);
            break;
        case PLAYBACK_DBG_TYPE_A_ORIENT:
            MPL_LOGV("PLAYBACK_DBG_TYPE_A_ORIENT\n");
            r = fread(&orientation, sizeof(orientation), 1, inv_construct.file);
            r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file);
            inv_set_accel_orientation_and_scale(orientation, sensitivity);
            break;
        case PLAYBACK_DBG_TYPE_C_ORIENT:
            MPL_LOGV("PLAYBACK_DBG_TYPE_C_ORIENT\n");
            r = fread(&orientation, sizeof(orientation), 1, inv_construct.file);
            r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file);
            inv_set_compass_orientation_and_scale(orientation, sensitivity);
            break;

        case PLAYBACK_DBG_TYPE_G_SAMPLE_RATE:
            r = fread(&sample_rate_us, sizeof(sample_rate_us), 
                      1, inv_construct.file);
            inv_set_gyro_sample_rate(sample_rate_us);
            MPL_LOGV("PLAYBACK_DBG_TYPE_G_SAMPLE_RATE => %d\n",
                     sample_rate_us);
            break;
        case PLAYBACK_DBG_TYPE_A_SAMPLE_RATE:
            r = fread(&sample_rate_us, sizeof(sample_rate_us), 
                      1, inv_construct.file);
            inv_set_accel_sample_rate(sample_rate_us);
            MPL_LOGV("PLAYBACK_DBG_TYPE_A_SAMPLE_RATE => %d\n",
                     sample_rate_us);
            break;
        case PLAYBACK_DBG_TYPE_C_SAMPLE_RATE:
            r = fread(&sample_rate_us, sizeof(sample_rate_us), 
                      1, inv_construct.file);
            inv_set_compass_sample_rate(sample_rate_us);
            MPL_LOGV("PLAYBACK_DBG_TYPE_C_SAMPLE_RATE => %d\n",
                     sample_rate_us);
            break;

        case PLAYBACK_DBG_TYPE_GYRO_OFF:
            MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO_OFF\n");
            inv_gyro_was_turned_off();
            break;
        case PLAYBACK_DBG_TYPE_ACCEL_OFF:
            MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL_OFF\n");
            inv_accel_was_turned_off();
            break;
        case PLAYBACK_DBG_TYPE_COMPASS_OFF:
            MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS_OFF\n");
            inv_compass_was_turned_off();
            break;
        case PLAYBACK_DBG_TYPE_QUAT_OFF:
            MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT_OFF\n");
            inv_quaternion_sensor_was_turned_off();
            break;

        case PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE:
            MPL_LOGV("PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE\n");
            r = fread(&sample_rate_us, sizeof(sample_rate_us), 
                      1, inv_construct.file);
            inv_set_quat_sample_rate(sample_rate_us);
            break;
        default:
            //MPL_LOGV("PLAYBACK file closed\n");
            fclose(inv_construct.file);
            MPL_LOGE("%s|%s|%d error: unrecognized log type '%d', "
                     "PLAYBACK file closed\n",
                     __FILE__, __func__, __LINE__, type);
            return INV_ERROR;
        }
    }
    msleep(1);

    inv_construct.debug_mode = RD_NO_DEBUG;
    fclose(inv_construct.file);

    return INV_SUCCESS;
}

/** Turns on/off playback and record modes
* @param mode Turn on recording mode with RD_RECORD and turn off recording mode with
*             RD_NO_DBG. Turn on playback mode with RD_PLAYBACK.
*/
void inv_set_debug_mode(rd_dbg_mode mode)
{
#ifdef INV_PLAYBACK_DBG
    inv_construct.debug_mode = mode;
#endif
}

inv_error_t inv_constructor_start(void)
{
    inv_error_t result;
    unsigned char divider;
    //int gest_enabled = inv_get_gesture_enable();

    // start the software
    result = inv_start_mpl();
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    
    /*
    if (inv_construct.dmp_version == WIN8_DMP_VERSION) {
        int fifo_divider;
        divider = 4; // 4 means 200Hz DMP
        fifo_divider = 3;
        // Set Gyro Sample Rate in MPL in micro seconds
        inv_set_gyro_sample_rate(1000L*(divider+1)*(fifo_divider+1));

        // Set Gyro Sample Rate in MPL in micro seconds
        inv_set_quat_sample_rate(1000L*(divider+1)*(fifo_divider+1));

        // Set Compass Sample Rate in MPL in micro seconds
        inv_set_compass_sample_rate(1000L*(divider+1)*(fifo_divider+1));

        // Set Accel Sample Rate in MPL in micro seconds
        inv_set_accel_sample_rate(1000L*(divider+1)*(fifo_divider+1));
    } else if (gest_enabled) {
        int fifo_divider;
        unsigned char mpl_divider;

        inv_send_interrupt_word();
        inv_send_sensor_data(INV_ALL & INV_GYRO_ACC_MASK);
        inv_send_quaternion();

        divider = fifo_dmp_cfg.sample_divider;
        mpl_divider = fifo_dmp_cfg.mpl_divider;

        // Set Gyro Sample Rate in MPL in micro seconds
        inv_set_gyro_sample_rate(1000L*(mpl_divider+1));

        // Set Gyro Sample Rate in MPL in micro seconds
        inv_set_quat_sample_rate(1000L*(mpl_divider+1));

        // Set Compass Sample Rate in MPL in micro seconds
        inv_set_compass_sample_rate(1000L*(mpl_divider+1));

        // Set Accel Sample Rate in MPL in micro seconds
        inv_set_accel_sample_rate(1000L*(mpl_divider+1));
    } else 
    */
    {
        divider = 9;
        // set gyro sample sate in MPL in micro seconds
        inv_set_gyro_sample_rate(1000L*(divider+1));
        // set compass sample rate in MPL in micro seconds
        inv_set_compass_sample_rate(1000L*(divider+1));
        // set accel sample rate in MPL in micro seconds
        inv_set_accel_sample_rate(1000L*(divider+1));
    }

    // setup the scale factors and orientations and other parameters
    result = inv_constructor_setup();

    return result;
}

inv_error_t inv_constructor_default_enable()
{
    INV_ERROR_CHECK(inv_enable_quaternion());
    INV_ERROR_CHECK(inv_enable_fast_nomot());
    INV_ERROR_CHECK(inv_enable_heading_from_gyro());
    INV_ERROR_CHECK(inv_enable_compass_bias_w_gyro());
    INV_ERROR_CHECK(inv_enable_hal_outputs());
    INV_ERROR_CHECK(inv_enable_vector_compass_cal());
    INV_ERROR_CHECK(inv_enable_9x_sensor_fusion());
    INV_ERROR_CHECK(inv_enable_gyro_tc());
    INV_ERROR_CHECK(inv_enable_no_gyro_fusion());
    INV_ERROR_CHECK(inv_enable_in_use_auto_calibration());
    INV_ERROR_CHECK(inv_enable_magnetic_disturbance());
    return INV_SUCCESS;
}

/**
 * @}
 */