/*
 * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
 * Copyright (c) 2014 Intel Corporation.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include <iostream>
#include <string>
#include <stdexcept>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <pthread.h>
#include <math.h>

#include "mma7455.h"

using namespace upm;

MMA7455::MMA7455 (int bus, int devAddr) : m_i2ControlCtx(bus) {
    unsigned char data   = 0;
    int           nBytes = 0;

    m_name = "MMA7455";

    m_controlAddr = devAddr;
    m_bus = bus;

    mraa::Result error = m_i2ControlCtx.address(m_controlAddr);
    if (error != mraa::SUCCESS) {
        throw std::runtime_error(std::string(__FUNCTION__) +
                                 ": mraa_i2c_address() failed");
        return;
    }

    // setting GLVL 0x1 (64LSB/g) and MODE 0x1 (Measurement Mode)
    data = (BIT (MMA7455_GLVL0) | BIT (MMA7455_MODE0));
    error = i2cWriteReg (MMA7455_MCTL, &data, 0x1);
    if (error != mraa::SUCCESS) {
        throw std::runtime_error(std::string(__FUNCTION__) +
                                 ": writing mode register failed");
        return;
    }

    if (mraa::SUCCESS != calibrate ()) {
        throw std::runtime_error(std::string(__FUNCTION__) +
                                 ": calibrate() failed");
        return;
    }
}

mraa::Result
MMA7455::calibrate () {
    mraa::Result error = mraa::SUCCESS;
    int i = 0;

    accelData xyz;
    xyz.value.x = xyz.value.y = xyz.value.z = 0;

    do {
        error = readData (&xyz.value.x, &xyz.value.y, &xyz.value.z);
        if (mraa::SUCCESS != error) {
            return error;
        }

        xyz.value.x += 2 * -xyz.value.x;
        xyz.value.y += 2 * -xyz.value.y;
        xyz.value.z += 2 * -(xyz.value.z - 64);

        error = i2cWriteReg (MMA7455_XOFFL,  (unsigned char *) &xyz, 0x6);
        if (error != mraa::SUCCESS) {
            return error;
        }

    } while ( ++i < 3 );

    return error;
}

mraa::Result
MMA7455::readData (short * ptrX, short * ptrY, short * ptrZ) {
    accelData xyz;
    unsigned char data = 0;
    int nBytes = 0;

    /*do {
        nBytes = i2cReadReg (MMA7455_STATUS, &data, 0x1);
    } while ( !(data & MMA7455_DRDY) && nBytes == mraa::SUCCESS);

    if (nBytes == mraa::SUCCESS) {
        std::cout << "NO_GDB :: 1" << std::endl;
        return mraa::SUCCESS;
    }*/

    nBytes = i2cReadReg (MMA7455_XOUTL, (unsigned char *) &xyz, 0x6);
    if (nBytes == 0) {
        std::cout << "NO_GDB :: 2" << std::endl;
        return mraa::ERROR_UNSPECIFIED;
    }

    if (xyz.reg.x_msb & 0x02) {
        xyz.reg.x_msb |= 0xFC;
    }

    if (xyz.reg.y_msb & 0x02) {
        xyz.reg.y_msb |= 0xFC;
    }

    if (xyz.reg.z_msb & 0x02) {
        xyz.reg.z_msb |= 0xFC;
    }

    // The result is the g-force in units of 64 per 'g'.
    *ptrX = xyz.value.x;
    *ptrY = xyz.value.y;
    *ptrZ = xyz.value.z;

    return mraa::SUCCESS;
}

#ifdef SWIGJAVA
short *MMA7455::readData() {
    short *v = new short[3];
    readData(&v[0], &v[1], &v[2]);
    return v;
}
#endif

int
MMA7455::i2cReadReg (unsigned char reg, uint8_t *buffer, int len) {
    if (mraa::SUCCESS != m_i2ControlCtx.address(m_controlAddr)) {
        throw std::runtime_error(std::string(__FUNCTION__) +
                                 ": mraa_i2c_address() failed");
        return 0;
    }

    if (mraa::SUCCESS != m_i2ControlCtx.writeByte(reg)) {
        throw std::runtime_error(std::string(__FUNCTION__) +
                                 ": mraa_i2c_write_byte() failed");
        return 0;
    }

    return (int) m_i2ControlCtx.read(buffer, len);
}

mraa::Result
MMA7455::i2cWriteReg (unsigned char reg, uint8_t *buffer, int len) {
    mraa::Result error = mraa::SUCCESS;

    uint8_t data[len + 1];
    data[0] = reg;
    memcpy(&data[1], buffer, len);

    error = m_i2ControlCtx.address (m_controlAddr);
    if (error != mraa::SUCCESS) {
        throw std::runtime_error(std::string(__FUNCTION__) +
                                 ": mraa_i2c_address() failed");
        return error;
    }
    error = m_i2ControlCtx.write (data, len + 1);
    if (error != mraa::SUCCESS) {
        throw std::runtime_error(std::string(__FUNCTION__) +
                                 ": mraa_i2c_write() failed");
        return error;
    }

    return error;
}