/**
  Generated main.c file from MPLAB Code Configurator

  @Company
    MBARI

  @File Name
    main.c

  @Summary
    This is the edited main.c file for the Attitude/Orientation Sensor.
    The template was generated by MPLAB Code Configurator.

  @Description
    This code runs on the MBARI AOS PIC24 board to sense transducer pitch and roll
    during deployments of the Simrad WBT Mini sonar system. The sensor is a
    Bosch BMX160 9 DOF accelerometer/gyro/compass. Bosch has not published
    driver code for the BMX160, so drivers used here are for the similar BMI160.
    This requires the definition of BMI160_CHIP_ID in the file bmi160_defs.h to
    be changed from 0xD1 to 0xD8; this is the only change needed as the code will
    otherwise function with either chip. 
    
    Generation Information :
        Product Revision  :  PIC24 / dsPIC33 / PIC32MM MCUs - 1.171.0
        Device            :  PIC24FV32KA302
    The generated drivers are tested against the following:
        Compiler          :  XC16 v1.70
        MPLAB 	          :  MPLAB X v5.50
*/

/*
    (c) 2020 Microchip Technology Inc. and its subsidiaries. You may use this
    software and any derivatives exclusively with Microchip products.

    THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, WHETHER
    EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, INCLUDING ANY IMPLIED
    WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, AND FITNESS FOR A
    PARTICULAR PURPOSE, OR ITS INTERACTION WITH MICROCHIP PRODUCTS, COMBINATION
    WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION.

    IN NO EVENT WILL MICROCHIP BE LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE,
    INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND
    WHATSOEVER RELATED TO THE SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS
    BEEN ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE
    FULLEST EXTENT ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN
    ANY WAY RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY,
    THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE.

    MICROCHIP PROVIDES THIS SOFTWARE CONDITIONALLY UPON YOUR ACCEPTANCE OF THESE
    TERMS.
*/

// Constant definitions
#define FCY 4000000

/**
  Section: Included Files
*/
#include "mcc_generated_files/system.h"
#include "mcc_generated_files/i2c1.h"
#include <libpic30.h>
#include <stdio.h>
#include "BMI160_driver-master/bmi160.h"

// Function prototypes
int8_t bmi160_read(uint8_t dev_id, uint8_t address, uint8_t *pData, uint16_t nCount);
int8_t bmi160_write(uint8_t dev_id, uint8_t address, uint8_t *pData, uint16_t nCount);
void delay_millisecs(uint16_t time);

/*
                         Main application
 */
int main(void)
{
    int8_t rslt = BMI160_OK;
    struct bmi160_dev sensor;
    struct bmi160_sensor_data accel;
    //struct bmi160_sensor_data gyro;

    // initialize the PIC24
    // UART2 is initialized to 19200, 8, N, 1
    // I2C1 runs at 100 kHz
    SYSTEM_Initialize();

    // set up the BMI160 data structure and initialize the sensor
    sensor.id = BMI160_I2C_ADDR;
    sensor.intf = BMI160_I2C_INTF;
    sensor.read = (bmi160_read_fptr_t) &bmi160_read;
    sensor.write = (bmi160_write_fptr_t) &bmi160_write;
    sensor.delay_ms = (bmi160_delay_fptr_t) &delay_millisecs;
    rslt = bmi160_init(&sensor);    // initialize the BMI160 sensor
    printf("Init result = %d\r", rslt);
    
    printf("AOS PIC24 v1.0.0\r");

    while (1)
    {
        rslt = bmi160_get_sensor_data(BMI160_ACCEL_SEL | BMI160_TIME_SEL, &accel, NULL, &sensor);
        printf("r = %d, t = %ld: x = %d, y = %d, z = %d\n", rslt, accel.sensortime, accel.x, accel.y, accel.z);
//        rslt = bmi160_get_regs(BMI160_CHIP_ID_ADDR, &sensor.chip_id, 1, &sensor);
//        printf("r = %d, id = %02x\r", rslt, sensor.chip_id);
//        if ((rslt == BMI160_OK) && (sensor.chip_id == BMI160_CHIP_ID))
//        {
//            sensor.any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED;
//
//            /* Soft reset */
//            rslt = bmi160_soft_reset(&sensor);
//            printf("reset rslt = %d\r", rslt);
//        }
        //printf("Done\r");
        __delay_ms(1000);
    }

    return 1;
}

#define RETRY_MAX       50  // define the retry count
#define DEVICE_TIMEOUT  50  // define slave timeout 
int8_t bmi160_read(uint8_t dev_id, uint8_t address, uint8_t *pData, uint16_t nCount)
{
    I2C1_MESSAGE_STATUS status = I2C1_MESSAGE_PENDING;
    uint8_t     writeBuffer[3];
    uint16_t    retryTimeOut, slaveTimeOut;
    uint16_t    counter;
    uint8_t     *pD;

    pD = pData;

    for(counter = 0; counter < nCount; counter++) {

        // load the buffer with the address of the register to be read
        writeBuffer[0] = (uint8_t) address;

        // Now it is possible that the slave device will be slow.
        // As a work around on these slaves, the application can
        // retry sending the transaction
        retryTimeOut = 0;
        slaveTimeOut = 0;
        //printf("bmi160_read: counter = %d, writeBuffer[0]=0x%02x, status = %d\n",
        //        counter, writeBuffer[0], status);
        while(status != I2C1_MESSAGE_FAIL) {
            // write one byte to device
            I2C1_MasterWrite(writeBuffer, 1, dev_id, &status);

            // wait for the message to be sent or status has changed.
            while(status == I2C1_MESSAGE_PENDING) {
                // add some delay here
                __delay_ms(1);
                // timeout checking
                // check for max retry and skip this byte
                if(slaveTimeOut == DEVICE_TIMEOUT)
                    return(BMI160_E_COM_FAIL);
                else
                    slaveTimeOut++;
            } // end while
            //printf("write status = %d\n",status);
            if(status == I2C1_MESSAGE_COMPLETE) break;

            // if status is  I2C1_MESSAGE_ADDRESS_NO_ACK,
            //               or I2C1_DATA_NO_ACK,
            // The device may be busy and needs more time for the last
            // write so we can retry writing the data, this is why we
            // use a while loop here

            // check for max retry and skip this byte
            if(retryTimeOut == RETRY_MAX)
                break;
            else
                retryTimeOut++;
        } // end while
        //printf("end of write status = %d\n",status);
        if(status == I2C1_MESSAGE_COMPLETE) {

            // this portion will read the byte from the register location.
            retryTimeOut = 0;
            slaveTimeOut = 0;

            while(status != I2C1_MESSAGE_FAIL) {
                // read one byte from the device
                I2C1_MasterRead(pData, 1, dev_id, &status);

                // wait for the message to be sent or status has changed.
                while(status == I2C1_MESSAGE_PENDING) {
                    // add some delay here
                    __delay_ms(1);
                    // timeout checking
                    // check for max retry and skip this byte
                    if (slaveTimeOut == DEVICE_TIMEOUT)
                        return (BMI160_E_COM_FAIL);
                    else
                        slaveTimeOut++;
                } // end while
                //printf("read status = %d, data = 0x%02x\n", status, pData[0]);
                if(status == I2C1_MESSAGE_COMPLETE)
                    break;

                // if status is  I2C1_MESSAGE_ADDRESS_NO_ACK,
                //               or I2C1_DATA_NO_ACK,
                // The device may be busy and needs more time for the last
                // write so we can retry writing the data, this is why we
                // use a while loop here

                // check for max retry and skip this byte
                if(retryTimeOut == RETRY_MAX)
                    break;
                else
                    retryTimeOut++;
            } // end while
        } // end if

        // exit if the last transaction failed
        if(status == I2C1_MESSAGE_FAIL) {
            return(BMI160_E_COM_FAIL);
            break;
        } // end if

        pD++;
        address++;

    } // end for
    return(BMI160_OK);

} // end bmi160_read())


int8_t bmi160_write(uint8_t dev_id, uint8_t address, uint8_t *pData, uint16_t nCount)
{
    I2C1_MESSAGE_STATUS status = I2C1_MESSAGE_PENDING;
    uint8_t     writeBuffer[3];
    uint16_t    retryTimeOut, slaveTimeOut;
    uint16_t    counter;
    uint8_t     *pD;
    pD = pData;

    for(counter = 0; counter < nCount; counter++) {

        // load the buffer with the address of the register to be written
        writeBuffer[0] = (uint8_t)(address);

        //printf("bmi160_WRITE: counter = %d, writeBuffer[0]=%02x, value = %02x, Status = %d\n",
        //        counter,writeBuffer[0],writeBuffer[1],status);


        // Now it is possible that the slave device will be slow.
        // As a work around on these slaves, the application can
        // retry sending the transaction
        retryTimeOut = 0;
        slaveTimeOut = 0;

        while(status != I2C1_MESSAGE_FAIL) {
            // write one byte to device
            I2C1_MasterWrite(writeBuffer, 1, dev_id, &status);

            // wait for the message to be sent or status has changed.
            while(status == I2C1_MESSAGE_PENDING) {
                // add some delay here
                __delay_ms(1);
                // timeout checking
                // check for max retry and skip this byte
                if (slaveTimeOut == DEVICE_TIMEOUT)
                    return(BMI160_E_COM_FAIL);
                else
                    slaveTimeOut++;
            } // end while

            if(status == I2C1_MESSAGE_COMPLETE)
                break;

            // if status is  I2C1_MESSAGE_ADDRESS_NO_ACK,
            //               or I2C1_DATA_NO_ACK,
            // The device may be busy and needs more time for the last
            // write so we can retry writing the data, this is why we
            // use a while loop here

            // check for max retry and skip this byte
            if(retryTimeOut == RETRY_MAX)
                break;
            else
                retryTimeOut++;
        } // end while

        // exit if the last transaction failed
        if(status == I2C1_MESSAGE_FAIL) {
            return(BMI160_E_COM_FAIL);
            break;
        } // end if

        pD++;
        address++;

    } // end for
    return(BMI160_OK);
} // end bmi160_write())


// wrap the __delay_ms macro in a function so its pointer can be passed to the
//  bmi160 routines
void delay_millisecs(uint16_t time) {
    __delay_ms(time);
} // end delay_millisecs()

/**
 End of File
*/