#include "GyroAttr.h"
#include "mbed_trace.h"

#define TRACE_GROUP  "Gyro"

GyroAttr::GyroAttr(BMI160* _imu) :
    imu(_imu),
    gyro(NULL),
    x(NULL),
    y(NULL),
    z(NULL),
    interval_ms(0),
    thread(osPriorityNormal, GYRO_SENSOR_UPDATE_THREAD_STACK_SIZE)
{
    init_attr();
}

GyroAttr::~GyroAttr()
{
    if (gyro) {
        cJSON_Delete(gyro);
    }
}

int GyroAttr::sensor_init()
{
    int ret = 0;

    ret = imu->setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL);
    if (ret != BMI160::RTN_NO_ERROR) {
        tr_error("Sensor power mode set failed: %d", ret);
        return ret;
    }
    ThisThread::sleep_for(100);

    ret = imu->setSensorPowerMode(BMI160::ACC, BMI160::NORMAL);
    if (ret != BMI160::RTN_NO_ERROR) {
        tr_error("Sensor power mode set failed: %d", ret);
        return ret;
    }
    ThisThread::sleep_for(100);

    ret = imu->getSensorConfig(acc_config);
    if (ret != BMI160::RTN_NO_ERROR) {
        tr_error("Sensor get config failed: %d", ret);
        return ret;
    }

    acc_config.range = BMI160::SENS_4G;
    acc_config.us = BMI160::ACC_US_OFF;
    acc_config.bwp = BMI160::ACC_BWP_2;
    acc_config.odr = BMI160::ACC_ODR_8;
    ret = imu->setSensorConfig(acc_config);
    if (ret != BMI160::RTN_NO_ERROR) {
        tr_error("Sensor set config failed: %d", ret);
        return ret;
    }

    ret = imu->getSensorConfig(gyro_config);
    if (ret != BMI160::RTN_NO_ERROR) {
        tr_error("Sensor get config failed: %d", ret);
        return ret;
    }
    ThisThread::sleep_for(100);

    return BMI160::RTN_NO_ERROR;
}

void GyroAttr::init_attr()
{
    gyro = cJSON_CreateObject();
    x = cJSON_AddNumberToObject(gyro, "x", 0);
    y = cJSON_AddNumberToObject(gyro, "y", 0);
    z = cJSON_AddNumberToObject(gyro, "z", 0);
}

cJSON* GyroAttr::get_gyro()
{
    return gyro;
}

int GyroAttr::start_sensor_update(int _interval_ms)
{
    osStatus ret;

    interval_ms = _interval_ms;

    ret = thread.start(callback(this, &GyroAttr::sensor_update_main_loop));

    if (ret != osOK) {
        tr_error("Start thread failed with osStatus: %d", ret);
        return -1;
    }
    return 0;
}


void GyroAttr::sensor_update_main_loop()
{
    BMI160::SensorData acc_data;
    BMI160::SensorData gyro_data;
    BMI160::SensorTime sensor_time;

    while(1) {
        imu->getGyroAccXYZandSensorTime(acc_data, gyro_data, sensor_time,
            acc_config.range, gyro_config.range);

        x->valueint = gyro_data.xAxis.raw;
        y->valueint = gyro_data.yAxis.raw;
        z->valueint = gyro_data.zAxis.raw;

        tr_debug("Read Gyro RAW Date:x=%d, y=%d, z=%d",
            gyro_data.xAxis.raw,
            gyro_data.yAxis.raw,
            gyro_data.zAxis.raw);

        ThisThread::sleep_for(interval_ms);
    }
}