#include "bma.h"

#define DEBUGPORT Serial
#ifdef DEBUGPORT
#define DEBUG(...)      DEBUGPORT.printf(__VA_ARGS__)
#else
#define DEBUG(...)
#endif

BMA423::BMA423()
{
    __readRegisterFptr = nullptr;
    __writeRegisterFptr = nullptr;
    __delayCallBlackFptr = nullptr;
    __init = false;
}

BMA423::~BMA423()
{

}

bool BMA423::begin(bma4_com_fptr_t readCallBlack,
                   bma4_com_fptr_t writeCallBlack,
                   bma4_delay_fptr_t delayCallBlack,
                   uint8_t address)
{

    if (__init ||
            readCallBlack == nullptr ||
            writeCallBlack == nullptr ||
            delayCallBlack == nullptr) {
        return true;
    }

    __readRegisterFptr = readCallBlack;
    __writeRegisterFptr = writeCallBlack;
    __delayCallBlackFptr = delayCallBlack;

    __devFptr.dev_addr        = address;
    __devFptr.interface       = BMA4_I2C_INTERFACE;
    __devFptr.bus_read        = readCallBlack;
    __devFptr.bus_write       = writeCallBlack;
    __devFptr.delay           = delayCallBlack;
    __devFptr.read_write_len  = 8;
    __devFptr.resolution      = 12;
    __devFptr.feature_len     = BMA423_FEATURE_SIZE;

    softReset();

    __delayCallBlackFptr(20);

    if (bma423_init(&__devFptr) != BMA4_OK) {
        DEBUG("BMA423 FAIL\n");
        return false;
    }

    if (bma423_write_config_file(&__devFptr) != BMA4_OK) {
        DEBUG("BMA423 Write Config FAIL\n");
        return false;
    }

    __init = true;

    struct bma4_int_pin_config config ;
    config.edge_ctrl = BMA4_LEVEL_TRIGGER;
    config.lvl = BMA4_ACTIVE_HIGH;
    config.od = BMA4_PUSH_PULL;
    config.output_en = BMA4_OUTPUT_ENABLE;
    config.input_en = BMA4_INPUT_DISABLE;


    if (bma4_set_int_pin_config(&config, BMA4_INTR1_MAP, &__devFptr) != BMA4_OK) {
        DEBUG("BMA423 SET INT FAIL\n");
        return false;
    }
    return true;
}

void BMA423::softReset()
{
    uint8_t reg = BMA4_RESET_ADDR;
    __writeRegisterFptr(BMA4_I2C_ADDR_PRIMARY, BMA4_RESET_SET_MASK, &reg, 1);
}

void BMA423::shutDown()
{
    bma4_set_advance_power_save(BMA4_DISABLE,  &__devFptr);
}

void BMA423::wakeUp()
{
    bma4_set_advance_power_save(BMA4_ENABLE, &__devFptr);
}

uint16_t BMA423::getErrorCode()
{
    struct bma4_err_reg err;
    uint16_t rslt = bma4_get_error_status(&err, &__devFptr);
    return rslt;
}

uint16_t BMA423::getStatus()
{
    uint8_t status;
    bma4_get_status(&status, &__devFptr);
    return status;
}

uint32_t BMA423::getSensorTime()
{
    uint32_t ms;
    bma4_get_sensor_time(&ms, &__devFptr);
    return ms;
}

bool BMA423::selfTest()
{
    return (BMA4_OK == bma4_selftest_config(BMA4_ACCEL_SELFTEST_ENABLE_MSK, &__devFptr));
}

uint8_t BMA423::getDirection()
{
    Accel acc;
    if (bma4_read_accel_xyz(&acc, &__devFptr) != BMA4_OK) {
        return 0;
    }
    uint16_t absX = abs(acc.x);
    uint16_t absY = abs(acc.y);
    uint16_t absZ = abs(acc.z);

    if ((absZ > absX) && (absZ > absY)) {
        if (acc.z > 0) {
            return  DIRECTION_DISP_DOWN;
        } else {
            return DIRECTION_DISP_UP;
        }
    } else if ((absY > absX) && (absY > absZ)) {
        if (acc.y > 0) {
            return DIRECTION_RIGHT_EDGE;
        } else {
            return  DIRECTION_LEFT_EDGE;
        }
    } else {
        if (acc.x < 0) {
            return  DIRECTION_BOTTOM_EDGE;
        } else {
            return DIRECTION_TOP_EDGE;
        }
    }
}

float BMA423::readTemperature()
{
    int32_t data = 0;
    bma4_get_temperature(&data, BMA4_DEG, &__devFptr);
    float res = (float)data / (float)BMA4_SCALE_TEMP;
    /* 0x80 - temp read from the register and 23 is the ambient temp added.
     * If the temp read from register is 0x80, it means no valid
     * information is available */
    if (((data - 23) / BMA4_SCALE_TEMP) == 0x80) {
        res = 0;
    }
    return res;
}


float BMA423::readTemperatureF()
{
    float temp = readTemperature();
    if (temp != 0) {
        temp = temp * 1.8 + 32.0;
    }
    return (temp);
}

bool BMA423::getAccel(Accel &acc)
{
    memset(&acc, 0, sizeof(acc));
    if (bma4_read_accel_xyz(&acc, &__devFptr) != BMA4_OK) {
        return false;
    }
    return true;
}

bool BMA423::getAccelEnable()
{
    uint8_t en;
    bma4_get_accel_enable(&en, &__devFptr);
    return (en & BMA4_ACCEL_ENABLE_POS) == BMA4_ACCEL_ENABLE_POS;
}

bool BMA423::disableAccel()
{
    return enableAccel(false);
}

bool BMA423::enableAccel(bool en)
{
    return (BMA4_OK == bma4_set_accel_enable(en ? BMA4_ENABLE : BMA4_DISABLE, &__devFptr));
}

bool BMA423::setAccelConfig(Acfg &cfg)
{
    return (BMA4_OK == bma4_set_accel_config(&cfg, &__devFptr));
}

bool BMA423::getAccelConfig(Acfg &cfg)
{
    return (BMA4_OK == bma4_get_accel_config(&cfg, &__devFptr));
}

bool BMA423::setRemapAxes(struct bma423_axes_remap *remap_data)
{
    return (BMA4_OK == bma423_set_remap_axes(remap_data, &__devFptr));
}

bool BMA423::resetStepCounter()
{
    return  BMA4_OK == bma423_reset_step_counter(&__devFptr) ;
}

uint32_t BMA423::getCounter()
{
    uint32_t stepCount;
    if (bma423_step_counter_output(&stepCount, &__devFptr) == BMA4_OK) {
        return stepCount;
    }
    return 0;
}

bool BMA423::setINTPinConfig(struct bma4_int_pin_config config, uint8_t pinMap)
{
    return BMA4_OK == bma4_set_int_pin_config(&config, pinMap, &__devFptr);
}

bool BMA423::getINT()
{
    return bma423_read_int_status(&__IRQ_MASK, &__devFptr) == BMA4_OK;
}

uint8_t BMA423::getIRQMASK()
{
    return __IRQ_MASK;
}

bool BMA423::disableIRQ(uint16_t int_map)
{
    return (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, int_map, BMA4_DISABLE, &__devFptr));
}

bool BMA423::enableIRQ(uint16_t int_map)
{
    return (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, int_map, BMA4_ENABLE, &__devFptr));
}

bool BMA423::enableFeature(uint8_t feature, uint8_t enable)
{
    if ((feature & BMA423_STEP_CNTR) == BMA423_STEP_CNTR) {
        bma423_step_detector_enable(enable ? BMA4_ENABLE : BMA4_DISABLE, &__devFptr);
    }
    return (BMA4_OK == bma423_feature_enable(feature, enable, &__devFptr));
}

bool BMA423::isStepCounter()
{
    return (bool)(BMA423_STEP_CNTR_INT & __IRQ_MASK);
}

bool BMA423::isDoubleClick()
{
    return (bool)(BMA423_WAKEUP_INT & __IRQ_MASK);
}

bool BMA423::isTilt()
{
    return (bool)(BMA423_TILT_INT & __IRQ_MASK);
}

bool BMA423::isActivity()
{
    return (bool)(BMA423_ACTIVITY_INT & __IRQ_MASK);
}

bool BMA423::isAnyNoMotion()
{
    return (bool)(BMA423_ANY_NO_MOTION_INT & __IRQ_MASK);
}

bool BMA423::enableStepCountInterrupt(bool en)
{
    return  (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP,  BMA423_STEP_CNTR_INT, en, &__devFptr));
}

bool BMA423::enableTiltInterrupt(bool en)
{
    return  (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_TILT_INT, en, &__devFptr));
}

bool BMA423::enableWakeupInterrupt(bool en)
{
    return  (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_WAKEUP_INT, en, &__devFptr));
}

bool BMA423::enableAnyNoMotionInterrupt(bool en)
{
    return  (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_ANY_NO_MOTION_INT, en, &__devFptr));
}

bool BMA423::enableActivityInterrupt(bool en)
{
    return  (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_ACTIVITY_INT, en, &__devFptr));
}

const char *BMA423::getActivity()
{
    uint8_t activity;
    bma423_activity_output(&activity, &__devFptr);
    if (activity & BMA423_USER_STATIONARY) {
        return "BMA423_USER_STATIONARY";
    } else if (activity & BMA423_USER_WALKING) {
        return "BMA423_USER_WALKING";
    } else if (activity & BMA423_USER_RUNNING) {
        return "BMA423_USER_RUNNING";
    } else if (activity & BMA423_STATE_INVALID) {
        return "BMA423_STATE_INVALID";
    }
    return "None";
}