/**
 * Copyright (c) 2023 Bosch Sensortec GmbH. All rights reserved.
 *
 * BSD-3-Clause
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * @file    quaternion.c
 * @brief   Quaternion data stream example for the BHI260/BHA260
 *
 */

#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include "bhi3.h"

#include "bhy2.h"
#include "bhy2_parse.h"
#include "common.h"
#include <math.h>

/*#define UPLOAD_FIRMWARE_TO_FLASH */

#ifdef UPLOAD_FIRMWARE_TO_FLASH
#include "bhi260ap/BHI260AP_aux_BMM150-flash.fw.h"
#else
//#include "bhi260ap/BHI260AP_aux_BMM150.fw.h"
#include "bhi360/BHI360_BMM150.fw.h"
//#include "bhi380/BHI380.fw.h"
#endif

#define WORK_BUFFER_SIZE  2048

#define QUAT_SENSOR_ID    BHY2_SENSOR_ID_RV

//static void parse_anymotion(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref);

static void parse_quaternion(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref);
static void parse_accel_xyz(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref);
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width);
static void parse_meta_event(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref);
static void print_api_error(int8_t rslt, struct bhy2_dev *dev);
static void upload_firmware(uint8_t boot_stat, struct bhy2_dev *dev);

enum bhy2_intf intf;
float sample_rate = 100.0; /* Read out data measured at 100Hz */
uint32_t report_latency_ms = 0; /* Report immediately */
struct bhy2_dev bhy2;
float x;
float y;
float z;
float w;

int main(void)
{
    uint8_t product_id = 0;
    uint16_t version = 0;
    int8_t rslt;
    
    uint8_t work_buffer[WORK_BUFFER_SIZE];
    uint8_t hintr_ctrl, hif_ctrl, boot_status;

#ifdef BHY2_USE_I2C
    intf = BHY2_I2C_INTERFACE;
#else
    intf = BHY2_SPI_INTERFACE;
#endif

    setup_interfaces(true, intf); /* Perform a power on reset */

#ifdef BHY2_USE_I2C
    rslt = bhy2_init(BHY2_I2C_INTERFACE, bhy2_i2c_read, bhy2_i2c_write, bhy2_delay_us, BHY2_RD_WR_LEN, NULL, &bhy2);
#else
    rslt = bhy2_init(BHY2_SPI_INTERFACE, bhy2_spi_read, bhy2_spi_write, bhy2_delay_us, BHY2_RD_WR_LEN, NULL, &bhy2);
#endif
    print_api_error(rslt, &bhy2);

    rslt = bhy2_soft_reset(&bhy2);
    print_api_error(rslt, &bhy2);

    rslt = bhy2_get_product_id(&product_id, &bhy2);
    print_api_error(rslt, &bhy2);

    /* Check for a valid product ID */
    if (product_id != BHY2_PRODUCT_ID)
    {
        printf("Product ID read %X. Expected %X\r\n", product_id, BHY2_PRODUCT_ID);
    }
    else
    {
        printf("BHI260/BHA260 found. Product ID read %X\r\n", product_id);
    }

    /* Check the interrupt pin and FIFO configurations. Disable status and debug */
    hintr_ctrl = BHY2_ICTL_DISABLE_STATUS_FIFO | BHY2_ICTL_DISABLE_DEBUG;

    rslt = bhy2_set_host_interrupt_ctrl(hintr_ctrl, &bhy2);
    print_api_error(rslt, &bhy2);
    rslt = bhy2_get_host_interrupt_ctrl(&hintr_ctrl, &bhy2);
    print_api_error(rslt, &bhy2);

    printf("Host interrupt control\r\n");
    printf("    Wake up FIFO %s.\r\n", (hintr_ctrl & BHY2_ICTL_DISABLE_FIFO_W) ? "disabled" : "enabled");
    printf("    Non wake up FIFO %s.\r\n", (hintr_ctrl & BHY2_ICTL_DISABLE_FIFO_NW) ? "disabled" : "enabled");
    printf("    Status FIFO %s.\r\n", (hintr_ctrl & BHY2_ICTL_DISABLE_STATUS_FIFO) ? "disabled" : "enabled");
    printf("    Debugging %s.\r\n", (hintr_ctrl & BHY2_ICTL_DISABLE_DEBUG) ? "disabled" : "enabled");
    printf("    Fault %s.\r\n", (hintr_ctrl & BHY2_ICTL_DISABLE_FAULT) ? "disabled" : "enabled");
    printf("    Interrupt is %s.\r\n", (hintr_ctrl & BHY2_ICTL_ACTIVE_LOW) ? "active low" : "active high");
    printf("    Interrupt is %s triggered.\r\n", (hintr_ctrl & BHY2_ICTL_EDGE) ? "pulse" : "level");
    printf("    Interrupt pin drive is %s.\r\n", (hintr_ctrl & BHY2_ICTL_OPEN_DRAIN) ? "open drain" : "push-pull");

    /* Configure the host interface */
    hif_ctrl = 0;
    rslt = bhy2_set_host_intf_ctrl(hif_ctrl, &bhy2);
    print_api_error(rslt, &bhy2);

    /* Check if the sensor is ready to load firmware */
    rslt = bhy2_get_boot_status(&boot_status, &bhy2);
    print_api_error(rslt, &bhy2);

    if (boot_status & BHY2_BST_HOST_INTERFACE_READY)
    {
        upload_firmware(boot_status, &bhy2);

        rslt = bhy2_get_kernel_version(&version, &bhy2);
        print_api_error(rslt, &bhy2);
        if ((rslt == BHY2_OK) && (version != 0))
        {
            printf("Boot successful. Kernel version %u.\r\n", version);
        }

        rslt = bhy2_register_fifo_parse_callback(BHY2_SYS_ID_META_EVENT, parse_meta_event, NULL, &bhy2);
        print_api_error(rslt, &bhy2);
        rslt = bhy2_register_fifo_parse_callback(BHY2_SYS_ID_META_EVENT_WU, parse_meta_event, NULL, &bhy2);
        print_api_error(rslt, &bhy2);
        
        rslt = bhy2_register_fifo_parse_callback(BHY2_SENSOR_ID_LACC , parse_accel_xyz, NULL, &bhy2);
        print_api_error(rslt, &bhy2);
        //rslt = bhy2_register_fifo_parse_callback(BHY2_SENSOR_ID_ACC_RAW , parse_accel_xyz, NULL, &bhy2);
        //print_api_error(rslt, &bhy2);

        rslt = bhy2_register_fifo_parse_callback(QUAT_SENSOR_ID, parse_quaternion, NULL, &bhy2);
        print_api_error(rslt, &bhy2);

        rslt = bhy2_get_and_process_fifo(work_buffer, WORK_BUFFER_SIZE, &bhy2);
        print_api_error(rslt, &bhy2);
    }
    else
    {
        printf("Host interface not ready. Exiting\r\n");

        close_interfaces(intf);

        return 0;
    }

    /* Update the callback table to enable parsing of sensor data */
    rslt = bhy2_update_virtual_sensor_list(&bhy2);
    print_api_error(rslt, &bhy2);

    rslt = bhy2_set_virt_sensor_cfg(BHY2_SENSOR_ID_LACC , sample_rate, report_latency_ms, &bhy2);
    print_api_error(rslt, &bhy2);
    printf("Enable %s at %.2fHz.\r\n", get_sensor_name(BHY2_SENSOR_ID_LACC ), sample_rate);


    //rslt = bhy2_set_virt_sensor_cfg(BHY2_SENSOR_ID_ACC_RAW  , sample_rate, report_latency_ms, &bhy2);
    //print_api_error(rslt, &bhy2);
    //printf("Enable %s at %.2fHz.\r\n", get_sensor_name(BHY2_SENSOR_ID_ACC_RAW  ), sample_rate);

    float sample_rate = 100.0; /* Read out data measured at 100Hz */
    uint32_t report_latency_ms = 0; /* Report immediately */
    rslt = bhy2_set_virt_sensor_cfg(QUAT_SENSOR_ID, sample_rate, report_latency_ms, &bhy2);
    print_api_error(rslt, &bhy2);
    printf("Enable %s at %.2fHz.\r\n", get_sensor_name(QUAT_SENSOR_ID), sample_rate);

    while (rslt == BHY2_OK)
    {
        if (get_interrupt_status())
        {
            /* Data from the FIFO is read and the relevant callbacks if registered are called */
            rslt = bhy2_get_and_process_fifo(work_buffer, WORK_BUFFER_SIZE, &bhy2);
            print_api_error(rslt, &bhy2);
        }
    }

    close_interfaces(intf);
    printf("reading");

    return rslt;
}

static void parse_accel_xyz(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref)
{
    struct bhy2_data_xyz data;
	(void)callback_ref;
	uint32_t s, ns;
    float sensitivity, acel_x, acel_y, acel_z, acel_x_oriented, acel_y_oriented, acel_z_oriented, time;
    static float max_still_x = 0, max_still_y = 0, max_still_z = 0;
    static bool initiating = true;
    static uint64_t past_time = 0 ;
    static uint64_t current_time = 0;
    static float vel_x = 0, vel_y = 0, vel_z = 0, dist_x = 0, dist_y = 0, dist_z = 0, distance = 0;
    static int count = 0;

    if (callback_info->data_size != 7) /* Check for a valid payload size. Includes sensor ID */
    {
        return;
    }

    bhy2_parse_xyz(callback_info->data_ptr, &data);
    sensitivity = 4096;

    //Run 1000 cycles to find the max acceleration when the sensor is still
    //Intention is to apply a lowpass filter of acceleration readings cutoff at the highest reading during still
    if (initiating)
    {
        acel_x = lsb_to_mps2(data.x, 8, 16);
        acel_y = lsb_to_mps2(data.y, 8, 16);
        acel_z = lsb_to_mps2(data.z, 8, 16);

        if (max_still_x < acel_x)
        {
            max_still_x = acel_x;
        } 
        if (max_still_y < acel_y)
        {
            max_still_y = acel_y;
        }
        if (max_still_z < acel_z)
        {
            max_still_z = acel_z;
        }
        printf("%d\n", count);
        
        count++;

        if(count >= 1000)
        {
            initiating = false;
            printf("initialized\n");
            printf("x: %f; y: %f; z: %f\n", max_still_x, max_still_y, max_still_z);
        }
    }
    else
    {
        uint64_t timestamp = *callback_info->time_stamp; /* Store the last timestamp */
        timestamp = timestamp * 15625; /* Timestamp is now in nanoseconds */
        s = (uint32_t)(timestamp / UINT64_C(1000000000));
        ns = (uint32_t)(timestamp - (s * UINT64_C(1000000000)));
        current_time = timestamp;

        /*
        acel_x = -((float)data.x/sensitivity)*9.8; 
        acel_y = -((float)data.y/sensitivity)*9.8;
        acel_z = -((float)data.z/sensitivity)*9.8;
        */

        //read acceleration reading as m/s^2
        acel_x = lsb_to_mps2(data.x, 8, 16);
        acel_y = lsb_to_mps2(data.y, 8, 16);
        acel_z = lsb_to_mps2(data.z, 8, 16);
        
        //if acceleration in lower than when the sensor is still, the acceleration is 0
        if (abs(acel_x) < max_still_x){
            acel_x = 0;
        }

        if (abs(acel_y) < max_still_y){
            acel_y = 0;
        }

        if (abs(acel_z) < max_still_z){
            acel_z = 0;
        }     
        
        //Reversing quaternion variables in order to reference back to original frame
        x = -x;
        y = -y;
        z = -z;

        //Math from https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
        //The acceleration reading is referenced back to original frame
        acel_x_oriented = w*w*acel_x + 2*y*w*acel_z - 2*z*w*acel_y + x*x*acel_x + 2*y*x*acel_y + 2*z*x*acel_z - z*z*acel_x - y*y*acel_x;
        acel_y_oriented = 2*x*y*acel_x + y*y*acel_y + 2*z*y*acel_z + 2*w*z*acel_x - z*z*acel_y + w*w*acel_y - 2*x*w*acel_z - x*x*acel_y;
        acel_z_oriented = 2*x*z*acel_x + 2*y*z*acel_y + z*z*acel_z- 2*w*y*acel_x - y*y*acel_z + 2*w*x*acel_y - x*x*acel_z + w*w*acel_z;

        time = 1/sample_rate;

        //calculate velocity
        vel_x += acel_x_oriented * time;
        vel_y += acel_y_oriented * time;
        vel_z += acel_z_oriented * time;

        //calculate displacement from original position
        dist_x += vel_x + 0.5 * acel_x_oriented * time * time;
        dist_y += vel_y + 0.5 * acel_y_oriented * time * time;
        dist_z += vel_z + 0.5 * acel_z_oriented * time * time;
        distance = sqrt(pow(dist_x,2) + pow(dist_y,2) + pow(dist_z,2));


            printf("SID: %u; T: %u.%09u; x: %f, y: %f, z: %f, vx: %f, vy: %f, vz: %f, dist: %f\r\n",
                callback_info->sensor_id,
                s,
                ns, 
                acel_x ,
                acel_y ,
                acel_z ,
                vel_x  ,
                vel_y  ,
                vel_z  ,
                distance);       
        
        past_time = timestamp;   
    }

}

static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
{
float half_scale = ((float)(1 << bit_width) / 2.0f);

return (9.8 * val * g_range) / half_scale;
}

static void parse_quaternion(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref)
{
    (void)callback_ref;
    struct bhy2_data_quaternion data;
    uint32_t s, ns;
    if (callback_info->data_size != 11) /* Check for a valid payload size. Includes sensor ID */
    {
        return;
    }

    bhy2_parse_quaternion(callback_info->data_ptr, &data);

    uint64_t timestamp = *callback_info->time_stamp; /* Store the last timestamp */

    timestamp = timestamp * 15625; /* Timestamp is now in nanoseconds */
    s = (uint32_t)(timestamp / UINT64_C(1000000000));
    ns = (uint32_t)(timestamp - (s * UINT64_C(1000000000)));

    x = data.x / 16384.0f;
    y = data.y / 16384.0f;
    z = data.z / 16384.0f;
    w = data.w / 16384.0f;
    /*
    printf("SID: %u; T: %u.%09u; x: %f, y: %f, z: %f, w: %f; acc: %.2f\r\n",
           callback_info->sensor_id,
           s,
           ns,
           data.x / 16384.0f,
           data.y / 16384.0f,
           data.z / 16384.0f,
           data.w / 16384.0f,
           ((data.accuracy * 180.0f) / 16384.0f) / 3.141592653589793f);
    */
}



static void parse_meta_event(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref)
{
    (void)callback_ref;
    uint8_t meta_event_type = callback_info->data_ptr[0];
    uint8_t byte1 = callback_info->data_ptr[1];
    uint8_t byte2 = callback_info->data_ptr[2];
    char *event_text;

    if (callback_info->sensor_id == BHY2_SYS_ID_META_EVENT)
    {
        event_text = "[META EVENT]";
    }
    else if (callback_info->sensor_id == BHY2_SYS_ID_META_EVENT_WU)
    {
        event_text = "[META EVENT WAKE UP]";
    }
    else
    {
        return;
    }

    switch (meta_event_type)
    {
        case BHY2_META_EVENT_FLUSH_COMPLETE:
            printf("%s Flush complete for sensor id %u\r\n", event_text, byte1);
            break;
        case BHY2_META_EVENT_SAMPLE_RATE_CHANGED:
            printf("%s Sample rate changed for sensor id %u\r\n", event_text, byte1);
            break;
        case BHY2_META_EVENT_POWER_MODE_CHANGED:
            printf("%s Power mode changed for sensor id %u\r\n", event_text, byte1);
            break;
        case BHY2_META_EVENT_ALGORITHM_EVENTS:
            printf("%s Algorithm event\r\n", event_text);
            break;
        case BHY2_META_EVENT_SENSOR_STATUS:
            printf("%s Accuracy for sensor id %u changed to %u\r\n", event_text, byte1, byte2);
            break;
        case BHY2_META_EVENT_BSX_DO_STEPS_MAIN:
            printf("%s BSX event (do steps main)\r\n", event_text);
            break;
        case BHY2_META_EVENT_BSX_DO_STEPS_CALIB:
            printf("%s BSX event (do steps calib)\r\n", event_text);
            break;
        case BHY2_META_EVENT_BSX_GET_OUTPUT_SIGNAL:
            printf("%s BSX event (get output signal)\r\n", event_text);
            break;
        case BHY2_META_EVENT_SENSOR_ERROR:
            printf("%s Sensor id %u reported error 0x%02X\r\n", event_text, byte1, byte2);
            break;
        case BHY2_META_EVENT_FIFO_OVERFLOW:
            printf("%s FIFO overflow\r\n", event_text);
            break;
        case BHY2_META_EVENT_DYNAMIC_RANGE_CHANGED:
            printf("%s Dynamic range changed for sensor id %u\r\n", event_text, byte1);
            break;
        case BHY2_META_EVENT_FIFO_WATERMARK:
            printf("%s FIFO watermark reached\r\n", event_text);
            break;
        case BHY2_META_EVENT_INITIALIZED:
            printf("%s Firmware initialized. Firmware version %u\r\n", event_text, ((uint16_t)byte2 << 8) | byte1);
            break;
        case BHY2_META_TRANSFER_CAUSE:
            printf("%s Transfer cause for sensor id %u\r\n", event_text, byte1);
            break;
        case BHY2_META_EVENT_SENSOR_FRAMEWORK:
            printf("%s Sensor framework event for sensor id %u\r\n", event_text, byte1);
            break;
        case BHY2_META_EVENT_RESET:
            printf("%s Reset event\r\n", event_text);
            break;
        case BHY2_META_EVENT_SPACER:
            break;
        default:
            printf("%s Unknown meta event with id: %u\r\n", event_text, meta_event_type);
            break;
    }
}

static void print_api_error(int8_t rslt, struct bhy2_dev *dev)
{
    if (rslt != BHY2_OK)
    {
        printf("%s\r\n", get_api_error(rslt));
        if ((rslt == BHY2_E_IO) && (dev != NULL))
        {
            printf("%s\r\n", get_coines_error(dev->hif.intf_rslt));
            dev->hif.intf_rslt = BHY2_INTF_RET_SUCCESS;
        }

        exit(0);
    }
}

static void upload_firmware(uint8_t boot_stat, struct bhy2_dev *dev)
{
    uint8_t sensor_error;
    int8_t temp_rslt;
    int8_t rslt = BHY2_OK;

#ifdef UPLOAD_FIRMWARE_TO_FLASH
    if (boot_stat & BHY2_BST_FLASH_DETECTED)
    {
        uint32_t start_addr = BHY2_FLASH_SECTOR_START_ADDR;
        uint32_t end_addr = start_addr + sizeof(bhy2_firmware_image);
        printf("Flash detected. Erasing flash to upload firmware\r\n");

        rslt = bhy2_erase_flash(start_addr, end_addr, dev);
        print_api_error(rslt, dev);
    }
    else
    {
        printf("Flash not detected\r\n");

        rslt = BHY2_E_IO;
        print_api_error(rslt, dev);
    }

    printf("Loading firmware into FLASH.\r\n");
    rslt = bhy2_upload_firmware_to_flash(bhy2_firmware_image, sizeof(bhy2_firmware_image), dev);
#else
    printf("Loading firmware into RAM.\r\n");
    rslt = bhy2_upload_firmware_to_ram(bhy2_firmware_image, sizeof(bhy2_firmware_image), dev);
#endif
    temp_rslt = bhy2_get_error_value(&sensor_error, dev);
    if (sensor_error)
    {
        printf("%s\r\n", get_sensor_error_text(sensor_error));
    }

    print_api_error(rslt, dev);
    print_api_error(temp_rslt, dev);

#ifdef UPLOAD_FIRMWARE_TO_FLASH
    printf("Booting from FLASH.\r\n");
    rslt = bhy2_boot_from_flash(dev);
#else
    printf("Booting from RAM.\r\n");
    rslt = bhy2_boot_from_ram(dev);
#endif

    temp_rslt = bhy2_get_error_value(&sensor_error, dev);
    if (sensor_error)
    {
        printf("%s\r\n", get_sensor_error_text(sensor_error));
    }

    print_api_error(rslt, dev);
    print_api_error(temp_rslt, dev);
}