/*******************************************
*   Sample sketch that configures an MPU6000
*   and reads back the three axis of accel,
*   temperature, three axis of gyro data
*******************************************/

#include <AP_Common.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>

/* register #defines */
#include "MPU6000.h"

// debug only:
#include <avr/io.h>

const AP_HAL_AVR::HAL_AVR& hal = AP_HAL_AVR_APM2;

const uint8_t _cs_pin = 53;
const uint8_t _baro_cs_pin = 40;

static void register_write(uint8_t reg, uint8_t val) {
    hal.gpio->write(_cs_pin, 0);
    hal.spi->transfer(reg);
    hal.spi->transfer(val);
    hal.gpio->write(_cs_pin, 1);
}

static uint8_t register_read(uint8_t reg) {
    /* set most significant bit to read register */
    uint8_t regaddr = reg | 0x80;
    hal.gpio->write(_cs_pin, 0);
    hal.spi->transfer(regaddr);
    uint8_t val = hal.spi->transfer(0);
    hal.gpio->write(_cs_pin, 1);
    return val;
}

static uint16_t spi_read_16(void) {
    uint8_t byte_h, byte_l;
    byte_h = hal.spi->transfer(0);
    byte_l = hal.spi->transfer(0);
    return (((int16_t) byte_h)<< 8) | byte_l;
}


static void mpu6k_init(void) {
    // chip reset
    register_write(MPUREG_PWR_MGMT_1, BIT_H_RESET);
    hal.scheduler->delay(100);
    // Wake up device and select GyroZ clock (better performance)
    register_write(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
    hal.scheduler->delay(1);
    register_write(MPUREG_PWR_MGMT_2, 0);
    hal.scheduler->delay(1);
    // Disable I2C bus (recommended on datasheet)
    register_write(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
    hal.scheduler->delay(1);
    // SAMPLE RATE
    //// Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
    register_write(MPUREG_SMPLRT_DIV,0x04);
    hal.scheduler->delay(1);
    // FS & DLPF   FS=2000º/s, DLPF = 98Hz (low pass filter)
    register_write(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
    hal.scheduler->delay(1);
    register_write(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS);  // Gyro scale 2000º/s
    hal.scheduler->delay(1);

     // read the product ID rev c has 1/2 the sensitivity of rev d
    uint8_t _product_id = register_read(MPUREG_PRODUCT_ID);

    //Serial.printf("Product_ID= 0x%x\n", (unsigned) _product_id);

    if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5)
      ||(_product_id == MPU6000_REV_C4)   || (_product_id == MPU6000_REV_C5)){
        // Accel scale 8g (4096 LSB/g)
        // Rev C has different scaling than rev D
        register_write(MPUREG_ACCEL_CONFIG,1<<3);
    } else {
        // Accel scale 8g (4096 LSB/g)
        register_write(MPUREG_ACCEL_CONFIG,2<<3);
    }
    hal.scheduler->delay(1);

    // INT CFG => Interrupt on Data Ready
    //// INT: Raw data ready
    register_write(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN);
    hal.scheduler->delay(1);
    // INT: Clear on any read
    register_write(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR); 
    hal.scheduler->delay(1);

}

static void mpu6k_read(int16_t* data) {

    hal.gpio->write(_cs_pin, 0);
    hal.spi->transfer( MPUREG_ACCEL_XOUT_H | 0x80 );
    for (int i = 0; i < 7; i++) {
        data[i] = spi_read_16();
    }
    hal.gpio->write(_cs_pin, 1);
}

static void setup() {
    hal.console->printf_P(PSTR("Initializing MPU6000\r\n"));

    /* Setup the barometer cs to stop it from holding the bus */
    hal.gpio->pinMode(_baro_cs_pin, GPIO_OUTPUT);
    hal.gpio->write(_baro_cs_pin, 1);

    /* Setup CS pin hardware */
    hal.gpio->pinMode(_cs_pin, GPIO_OUTPUT);
    hal.gpio->write(_cs_pin, 1);
    hal.scheduler->delay(1);

    uint8_t spcr = SPCR;
    uint8_t spsr = SPSR;
    hal.console->printf_P(PSTR("SPCR 0x%x, SPSR 0x%x\r\n"),
            (int)spcr, (int)spsr);

    mpu6k_init();
}

static void loop() {
    int16_t sensors[7];
    mpu6k_read(sensors);
    hal.console->printf_P(PSTR("mpu6k: %d %d %d %d %d %d %d\r\n"),
        sensors[0], sensors[1], sensors[2],
        sensors[3], sensors[4], sensors[5], sensors[6]);
    hal.scheduler->delay(10);
}




extern "C" {
int main (void) {
    hal.init(NULL);
    setup();
    for(;;) loop();
    return 0;
}
}