/**
 *   @defgroup  eMPL
 *   @brief     Embedded Motion Processing Library
 *
 *   @{
 *       @file      motion_driver_test.c
 */
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>

#include "eMPL/inv_mpu.h"
#include "eMPL/inv_mpu_dmp_motion_driver.h"

#include "common.h"

/* Data requested by client. */
#define PRINT_ACCEL     (0x01)
#define PRINT_GYRO      (0x02)
#define PRINT_QUAT      (0x04)

#define ACCEL_ON        (0x01)
#define GYRO_ON         (0x02)

#define MOTION          (0)
#define NO_MOTION       (1)

/* Starting sampling rate. */
#define DEFAULT_MPU_HZ  (100)

#define FLASH_SIZE      (512)
#define FLASH_MEM_START ((void*)0x1800)

struct rx_s {
    unsigned char header[3];
    unsigned char cmd;
};
struct hal_s {
    unsigned char sensors;
    unsigned char dmp_on;
    unsigned char wait_for_tap;
    volatile unsigned char new_gyro;
    unsigned short report;
    unsigned short dmp_features;
    unsigned char motion_int_mode;
    struct rx_s rx;
};
static struct hal_s hal = {0};

/* USB RX binary semaphore. Actually, it's just a flag. Not included in struct
 * because it's declared extern elsewhere.
 */
volatile unsigned char rx_new;

/* The sensors can be mounted onto the board in any orientation. The mounting
 * matrix seen below tells the MPL how to rotate the raw data from thei
 * driver(s).
 * TODO: The following matrices refer to the configuration on an internal test
 * board at Invensense. If needed, please modify the matrices to match the
 * chip-to-body matrix for your particular set up.
 */
static signed char gyro_orientation[9] = {-1, 0, 0,
                                           0,-1, 0,
                                           0, 0, 1};
/* These next two functions converts the orientation matrix (see
 * gyro_orientation) to a scalar representation for use by the DMP.
 * NOTE: These functions are borrowed from Invensense's MPL.
 */
static inline unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}

static inline unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar;

    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}

static void tap_cb(unsigned char direction, unsigned char count)
{

}

static void android_orient_cb(unsigned char orientation)
{

}

static inline void run_self_test(void)
{
    int result;
    //char test_packet[4] = {0};
    long gyro[3], accel[3];

    result = mpu_run_self_test(gyro, accel);
    if (result == 0x7) {
        /* Test passed. We can trust the gyro data here, so let's push it down
         * to the DMP.
         */
        float sens;
        unsigned short accel_sens;
        mpu_get_gyro_sens(&sens);
        gyro[0] = (long)(gyro[0] * sens);
        gyro[1] = (long)(gyro[1] * sens);
        gyro[2] = (long)(gyro[2] * sens);
        dmp_set_gyro_bias(gyro);
        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);
    }

    /* Report results. */
    //test_packet[0] = 't';
    //test_packet[1] = result;
    //send_packet(PACKET_TYPE_MISC, test_packet);
}


/* Every time new gyro data is available, this function is called in an
 * ISR context. In this example, it sets a flag protecting the FIFO read
 * function.
 */
static void gyro_data_ready_cb(void)
{
	//printf("gyro_data_ready_cb\r\n");
    hal.new_gyro = 1;
}

bool mpu_cfg_init(void)
{
	int result;
	struct int_param_s int_param;

	int_param.cb = (void *)gyro_data_ready_cb;
	result = mpu_init(&int_param);
	if (result < 0) {
		//printf("mpu_init Fail:%d\r\n", result);
		return false;
	}
	return true;
}

void mpu_configure(unsigned char sensors, unsigned char fifo, unsigned short rate)
{
    mpu_set_sensors(sensors);//(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    mpu_configure_fifo(fifo);//(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    mpu_set_sample_rate(rate);//(DEFAULT_MPU_HZ);
}

int mpu_dmp_init(unsigned short dmp_features, unsigned short rate)
{
	int ret = dmp_load_motion_driver_firmware();
	if(ret < 0) {
		return ret;
	}
	ret = dmp_set_orientation(
		inv_orientation_matrix_to_scalar(gyro_orientation));
	if(ret < 0) {
		return ret - 100;
	}
	dmp_register_tap_cb(tap_cb);
	dmp_register_android_orient_cb(android_orient_cb);
	//hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
	//	DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
	//	DMP_FEATURE_GYRO_CAL;
	hal.dmp_features = dmp_features;
	dmp_enable_feature(hal.dmp_features);
	dmp_set_fifo_rate(rate);//(DEFAULT_MPU_HZ);
	mpu_set_dmp_state(1);
	hal.dmp_on = 1;

	return 0;
}

bool mpu_take_fifo(char *msg)
{
	unsigned char index = 0;

	msg[0] = '\0';
	if (hal.new_gyro && hal.dmp_on) {
		short gyro[3], accel[3], sensors;
		unsigned char more;
		long quat[4];

		dmp_read_fifo(gyro, accel, quat, NULL, &sensors,
			&more);
		if (!more)
			hal.new_gyro = 0;

		
		if (sensors & INV_XYZ_GYRO) {
			sprintf(&msg[index], "%04x %04x %04x\r\n", gyro[0],gyro[1],gyro[2]);
			index += 16;
		}
		if (sensors & INV_XYZ_ACCEL) {
			sprintf(&msg[index], "%04x %04x %04x\r\n", accel[0],accel[1],accel[2]);
			index += 16;
		}

		if (sensors & INV_WXYZ_QUAT) {
			sprintf(&msg[index], "%04x %04x %04x %04x", (uint16_t)quat[0],(uint16_t)quat[1],(uint16_t)quat[2],(uint16_t)quat[3]);
		}
		//printf("%d\r\n", index);
		return index > 0;
	}
	return false;
}