/**
 * \file
 *
 * \brief Empty user application template
 *
 */

/*
 * Include header files for all drivers that have been imported from
 * AVR Software Framework (ASF).
 */
#include <stdio.h>
#include <asf.h>
#include <string.h>
// platform specific
#include "bsp/twim.h"
#include "bsp/stdio_usb.h"
#include "packet.h"
#include "testFunctions.h"
#include "umplCalClient.h"

//#define VERBOSE_INTERR
#define VERBOSE_DATALOG 1

//#define TESTING
#ifdef TESTING
#include "umpl-tests.h"
#endif

#include "log.h"
#undef MPL_LOG_TAG
#define MPL_LOG_TAG "uMPL-main"

// MPL interface
#include "ml.h"
#include "mlFIFO.h"
#include "temp_comp.h"
#include "mldl.h"
#include "ustore_manager.h"
#include "ustore_manager_io.h"
#include "mlmath.h"
#include "compass.h"

// umpl state machine
#include "umpl-states.h"

#define BUTTON_MODE_MPUPOWER  (0)
#define BUTTON_MODE_MOUSE     (1)
static int button_mode = BUTTON_MODE_MPUPOWER;
static int loop_delay = 10;

// prototypes
void quatSend(void);
void umplAppRun(void);
int waitForUmplAppBegin(void);
#ifdef TESTING
int testingMain(void);
#endif

// definitions
void umplAppRun(void)
{
    unsigned short display_counter = 0;
    unsigned short debounce_counter = 0;
    inv_error_t result = INV_SUCCESS;

    MPL_LOGI(INV_VERSION"\n");

    result = umplInit(NULL);
    if (result != INV_SUCCESS)
        MPL_LOGE("umplInit Error %d\n",result);

    result = umplStartMPU();
    if (result != INV_SUCCESS)
        MPL_LOGE("umplStartMPU Error %d\n",result);

    /* --------------------  L  O  O  P  --------------------- */
    for(;;) {
        display_counter += loop_delay;
        debounce_counter += loop_delay;
        delay_ms (loop_delay);

        /* Button push: Demonstrates umpStartMPU and umplStop
         * Disabled by BUTTON_MODE_MOUSE (i.e. teapot app mode)
         */
        if (button_mode == BUTTON_MODE_MPUPOWER
           && gpio_get_pin_value(GPIO_PUSH_BUTTON_0) == GPIO_PUSH_BUTTON_0_PRESSED
           && debounce_counter > 250)
        {
            debounce_counter = 0;
            if (umplGetState() == UMPL_RUN ||
                umplGetState() == UMPL_ACCEL_ONLY ||
                umplGetState() == UMPL_LPACCEL_ONLY) {
                MPL_LOGI("umplStop\n");
                umplStop();
            } else if (umplGetState() == UMPL_STOP) {
                MPL_LOGI("umplStartMPU\n");
                umplStartMPU();
            }
        }

#ifdef UMPL_ENABLE_EXTRA_POWERMODE_BUTTONS
        if (gpio_get_pin_value(GPIO_PUSH_BUTTON_1) == GPIO_PUSH_BUTTON_1_PRESSED
            && debounce_counter > 250){
            debounce_counter = 0;
             if (umplGetState() !=  UMPL_ACCEL_ONLY ||
                 umplGetState() != UMPL_UNINIT) {
                MPL_LOGI("umplStartAccelOnly\n");
                umplStartAccelOnly(50.0);
            }
        }
        if (gpio_get_pin_value(GPIO_PUSH_BUTTON_2) == GPIO_PUSH_BUTTON_2_PRESSED
            && debounce_counter > 250){
            debounce_counter = 0;
             if (umplGetState() != UMPL_LPACCEL_ONLY || 
                 umplGetState() != UMPL_UNINIT) {
                 MPL_LOGI("umplStartAccelOnly (LP)\n");
                umplStartAccelOnlyLowPower(40.0);
            }
        }
        if (gpio_get_pin_value(GPIO_PUSH_BUTTON_3) == GPIO_PUSH_BUTTON_3_PRESSED
            && debounce_counter > 250){
            debounce_counter = 0;
             if (umplGetState() == UMPL_ACCEL_ONLY) {
                 MPL_LOGI("umplStartMPU from accel only mode\n");
                umplStartMPU();
            }
            else if (umplGetState() == UMPL_LPACCEL_ONLY) {
                 MPL_LOGI("umplStartMPU from low power mode\n");
                umplStartMPU();
            }
        }
        if (gpio_get_pin_value(GPIO_PUSH_BUTTON_4) == GPIO_PUSH_BUTTON_4_PRESSED
            && debounce_counter > 250){
            debounce_counter = 0;
            testLowPowerState();
            //inv_clear_nvram();
        }
#endif /* UMPL_ENABLE_EXTRA_POWERMODE_BUTTONS */

#ifdef CONFIG_MPU_SENSORS_MPU3050

        /* IMU-3000 Axillary Accelerometer Interrupt.
         * At this time, the interrupt will not be triggered
         * and has no effect. It will be used in the future to
         * support low power accel mode.
         */
        if (gpio_get_pin_value(GPIO_INERTIAL2_INT_ACCEL)
            == GPIO_INERTIAL2_INT_ACCEL_ACTIVE) {
            umplNotifyInterrupt(INTSRC_AUX1);
        #ifdef VERBOSE_INTERR
            MPL_LOGD("INT accel\n");
        #endif
        }

        /* IMU-3000 Interrupt
         * In this sample app, we check the interrupt in the main loop.
         * However, you can configure your microcontroller to trigger
         * this interrupt asynchronously.
         */
        if (gpio_get_pin_value(GPIO_INERTIAL2_INT_MPU)
            == GPIO_INERTIAL2_INT_MPU_ACTIVE) {
            umplNotifyInterrupt(INTSRC_MPU);
        #ifdef VERBOSE_INTERR
            MPL_LOGD("INT MPU\n");
        #endif
        }

#else /* #ifdef CONFIG_MPU_SENSORS_MPU3050 */

        /* MPU-6000 Interrupt
         * In this sample app, we check the interrupt in the main loop.
         * However, you can configure your microcontroller to trigger
         * this interrupt asynchronously.
         */
        if (gpio_get_pin_value(GPIO_INERTIAL3_INT_MPU)
            == GPIO_INERTIAL3_INT_MPU_ACTIVE) {
            umplNotifyInterrupt(INTSRC_MPU);
        #ifdef VERBOSE_INTERR
            MPL_LOGD("INT MANTIS\n");
        #endif
        }

#endif /* #ifdef CONFIG_MPU_SENSORS_MPU3050 */

        /* umplOnTimer:
         * This should be run either at a fixed rate corresponding to
         * the FIFO Rate (see umpl-setup / inv_set_fifo_rate) or each time
         * an MPU interrupt has occurred.
         * It is a time-consuming call, so it is advised not to run it from
         * an ISR.
         */
        umplOnTimer();

        /* quatSend:
         * Sends a quaternion packet over the serial port. Used to support
         * the cube demo in the Debug console, and other apps. (mouse, teapot, etc).
         */
        if (umplGetState() == UMPL_RUN
           || umplGetState() == UMPL_ACCEL_ONLY
           || umplGetState() == UMPL_LPACCEL_ONLY) {

            quatSend();

            /* display at intervals:
             * used for testing, evaluation, etc.
             * disabled by DISPLAY_MODE_SILENT (i.e. teapot app mode)
             */
            if (VERBOSE_DATALOG && display_counter >= 1000) {
                int mot_flag;
                mot_flag = inv_get_motion_state();
#ifdef UMPL_NINE_AXIS
                inv_error_t result;
                float accel[3];
                float temp;
                float gyros[3];
                float gyro_bias[3];
                float temp_slope[3];
                long ext_data[6];
                long compass[3];
                float mag[3];
                float mag_bias[3];
                float heading;

                inv_get_accel_float(accel);
                inv_get_gyro_float(gyros);
                inv_get_temperature_float(&temp);
                inv_get_gyro_bias_float(gyro_bias);
#ifdef UMPL_ENABLE_TEMPCOMP
                inv_get_gyro_temp_slope_float(temp_slope);
#endif
                result = inv_get_external_sensor_data(ext_data,6);
                inv_get_compass_data(compass);
                inv_get_magnetometer_float(mag);
                inv_get_mag_bias_float(mag_bias);
                inv_get_heading_float(&heading);

                MPL_LOGI("temp: %+4.2f\n", temp);
                MPL_LOGI("gyros: %+4.2f %+4.2f %+4.2f\n",
                         gyros[0], gyros[1], gyros[2]);
                MPL_LOGI("gyro bias: %+4.2f %+4.2f %+4.2f\n",
                         gyro_bias[0], gyro_bias[1], gyro_bias[2]);
#ifdef UMPL_ENABLE_TEMPCOMP
                MPL_LOGI("gyro temp slope: %+4.2f %+4.2f %+4.2f\n",
                         temp_slope[0], temp_slope[1], temp_slope[2]);
#endif
                MPL_LOGI("accel: %+4.2f %+4.2f %+4.2f\n",
                         accel[0], accel[1], accel[2]);
                MPL_LOGI("ext result : %d data: %ld %ld %ld %ld %ld %ld\n"
                        , result
                        , ext_data[0], ext_data[1], ext_data[2]
                        , ext_data[3], ext_data[4], ext_data[5]
                        );
                MPL_LOGI("compass: %d %d %d\n",compass[0], compass[1], compass[2]);

                MPL_LOGI("magnetometer: %+4.2f %+4.2f %+4.2f\n",
                          mag[0],mag[1],mag[2]);
                MPL_LOGI("mag bias    : %+4.2f %+4.2f %+4.2f\n",
                          mag_bias[0],mag_bias[1], mag_bias[2]);
                MPL_LOGI("heading: %+3.1f\n",heading);
#else /* Non UMPL_NINE_AXIS debugging routines. */
                float quat[4];
                float gyros[3];
                float gyro_bias[3];
                float temp_slope[3];
                float accel[3];
                float temp;

                inv_get_quaternion_float(quat);
                inv_get_gyro_float(gyros);
                inv_get_accel_float(accel);
                inv_get_gyro_bias_float(gyro_bias);
#ifdef UMPL_ENABLE_TEMPCOMP
                inv_get_gyro_temp_slope_float(temp_slope);
#endif
                inv_get_temperature_float(&temp);

                MPL_LOGI("Q: %+6.4f %+6.4f %+6.4f %+6.4f (%+6.4f)\n",
                         quat[0], quat[1], quat[2], quat[3],
                         sqrtf(
                            quat[0] * quat[0] + quat[1] * quat[1] +
                            quat[2] * quat[2] + quat[3] * quat[3]
                         ));
                MPL_LOGI("GC: %+4.2f %+4.2f %+4.2f\n",
                         gyros[0], gyros[1], gyros[2]);
                MPL_LOGI("AC: %+4.2f %+4.2f %+4.2f\n",
                         accel[0], accel[1], accel[2]);
                MPL_LOGI("T: %+4.2f\n", temp);
                MPL_LOGI("gb: %+4.2f %+4.2f %+4.2f\n",
                         gyro_bias[0], gyro_bias[1], gyro_bias[2]);
#ifdef UMPL_ENABLE_TEMPCOMP
                MPL_LOGI("gts: %+4.2f %+4.2f %+4.2f\n",
                         temp_slope[0], temp_slope[1], temp_slope[2]);
#endif
#endif /* End non UMPL_NINE_AXIS */
                display_counter = 0;
            }
        }   /* umplGetState() == UMPL_RUN */
    }       /* for (;;) */
}

void quatSend (void)
{
    static unsigned char packetCount;
    inv_error_t result;
    long longquat[4];
    unsigned char buttons;

    /* longquat: send a fixed point representation of the sensor fusion output.
     * The quaternion behaves well when less significant bits are truncated.
     * We get the 32 bit element representation with inv_get_quaternion,
     * and send the most significant 16 bits of each element in the packet.
     */
    result = inv_get_quaternion(longquat);

    /* buttons:
     * least significant bit corresponds to UC3 Push Button 0.
     * This is a 1-byte bitfield in the quaternion packet.
     * Some demo apps can use more buttons, which are added as additional bits.
     * Disabled by MOUSE_MODE_MPUSTOP (i.e. standard debugging mode)
     */
    if (button_mode == BUTTON_MODE_MOUSE) {
        buttons = (gpio_get_pin_value(GPIO_PUSH_BUTTON_0) ==
                     GPIO_PUSH_BUTTON_0_PRESSED) ? (1 << 0) : 0;
    } else {
        buttons = 0;
    }

    if (INV_SUCCESS == result) {
        unsigned char out[10];

        // Send the most significant two bytes of each quaternion component.
        out[0] = (unsigned char) (longquat[0] >> 24);
        out[1] = (unsigned char) ((uint16_t)(longquat[0] >> 16) % 256);
        out[2] = (unsigned char) (longquat[1] >> 24);
        out[3] = (unsigned char) ((uint16_t)(longquat[1] >> 16) % 256);
        out[4] = (unsigned char) (longquat[2] >> 24);
        out[5] = (unsigned char) ((uint16_t)(longquat[2] >> 16) % 256);
        out[6] = (unsigned char) (longquat[3] >> 24);
        out[7] = (unsigned char) ((uint16_t)(longquat[3] >> 16) % 256);
        out[8] = buttons;
        out[9] = packetCount++;
        result = sendPacket(PACKET_TYPE_QUATERNION, out);
    }
}

int waitForUmplAppBegin(void)
{
    char control;
    control = getchar(); // wait for host PC to send character at start of connection.

    /* teapot app mouse button functionality : */
    if (control == 't') {
        button_mode = BUTTON_MODE_MOUSE;
    }

    umplAppRun();    // does not return
    return 0;
}

static void sio_init (void)
{
    //
    // Start and attach USB CDC device interface for devices with
    // integrated USB interfaces.
    //
    stdio_usb_init (&CONFIG_USART_IF);
    //
    // Specify that stdout and stdin should not be buffered.
    //
    setbuf (stdout, NULL);
    setbuf (stdin,  NULL);
}

static inline void platform_init (void)
{
    sysclk_init();
    board_init();
    sio_init();
    delay_init(sysclk_get_cpu_hz());
    twim_init(&CONFIG_TWI_IF, CONFIG_TWI_SPEED);
}

#ifdef TESTING
int testingMain(void)
{
    bool done = false;
    int c, result;
    unsigned char data[3];

    //delay_ms (350);
    while(!done) {
        printf("\n\n\n\n\n\n\n\rEnter a number to start a Test or 'q' to quit.\n");
        scanf("%d", &c);
        switch (c) {
            case 1:
                selfTest();
                done = true;
                break;
            case 2:
                whoamitest();
                done = true;
                break;
            //case 3:
                //result = mpuTest();
                //printf("\rMpuTest returned %d.\n", result);
                //done = true;
                //break;
            //case 4:
                //result = testGyro();
                //done = true;
                //break;

            case 'q':
                done = true;
                break;

            default:
                printf("Invalid choice '%c'.\n", c);
                break;
        }
    }

    return 1;
}
#endif /* TESTING */

int main(void)
{
    platform_init();
    delay_ms (500); // Allow USB time to connect with PC

#ifdef TESTING
    testingMain();
#else
    waitForUmplAppBegin();
#endif
}

