/* Copyright (c) 2010-2011 mbed.org, MIT License
*
* 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 "MMA8451Q.h"



MMA8451Q::MMA8451Q(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) 
{
    
    m_i2c.frequency(4750000);
    
    data_ready = false;
}

MMA8451Q::~MMA8451Q() { }

void MMA8451Q::init_100Hz_logger()
{
    writeRegs(REG_CTRL_REG1, 0x18); //CRTL_REG1: 100 Hz mode DR = 011, Active = 0, F_Read = 0
    writeRegs(REG_F_SETUP_FIFO, 0x80); //FIFO Set to Fill Mode (stops accepting samples when overflowed)
    writeRegs(REG_CTRL_REG4, 0x40); //Enable the interrupt Pin for the FIFO
    writeRegs(REG_CTRL_REG5, 0x40); //Set the interrupt to route to INT1
    writeRegs(REG_XYZ_DATA_CFG, 0x01); //HPF_OUT not in use, 4g mode
    uint8_t CRTL_REG1_Data;             //Active Mode
    readRegs(REG_CTRL_REG1, &CRTL_REG1_Data, 1);
    CRTL_REG1_Data = CRTL_REG1_Data | 0x01;
    writeRegs(REG_CTRL_REG1,CRTL_REG1_Data);
}

void MMA8451Q::init_400Hz_logger(void)
{
// Power Minimization Example: Data Logger Collecting 14-bit HPF Data 400 Hz
// Go to Standby Mode, set Sample Rate to 400 Hz, Standby (Active = 0),14-bit (F_READ = 0),
    writeRegs(REG_CTRL_REG1, 0x08); //CRTL_REG1: 400 Hz mode DR = 001, Active = 0,F_Read = 0
    writeRegs(REG_F_SETUP_FIFO, 0x80);  // Set the FIFO to Fill Buffer Mode (stops accepting new samples when overflowed)
    writeRegs(REG_CTRL_REG4, 0x40);     //Set the FIFO Interrupt Pin to Int1 to flush the data every 32 samples when the overflow flag asserts
    writeRegs(REG_CTRL_REG5, 0x40);      //Enable the interrupt Pin for the FIFO
    writeRegs(REG_F_SETUP_FIFO, 0x80);  // Set the FIFO to Fill Buffer Mode (stops accepting new samples when overflowed)
    //writeRegs(REG_F_SETUP_FIFO, 0x8A);  // Set the FIFO to Fill Mode (wmrk 10)
    writeRegs(REG_CTRL_REG4, 0x40);     //Set the FIFO Interrupt Pin to Int1 to flush the data every 32 samples when the overflow flag asserts
    writeRegs(REG_CTRL_REG5, 0x40);      //Enable the interrupt Pin for the FIFO
    writeRegs(REG_XYZ_DATA_CFG, 0x00); //HPF_OUT not in use, 2g mode  //for 4g mode write 0x01
    writeRegs(REG_CTRL_REG1, 0x09);      //Go to Active Mode
}

void MMA8451Q::init_800Hz_logger(void)
{
// Power Minimization Example: Data Logger Collecting 14-bit HPF Data 800 Hz, 4g
    writeRegs(REG_CTRL_REG1, 0x00);     // standby Mode, set Sample Rate to 800 Hz, Standby (Active = 0),14-bit (F_READ = 0)
    writeRegs(REG_F_SETUP_FIFO, 0x80);      // Set the FIFO to Fill Buffer Mode (stops accepting new samples when overflowed)
    writeRegs(REG_CTRL_REG4, 0x40);     //Set the FIFO Interrupt Pin to Int1 to flush the data every 32 samples when the overflow flag asserts
    writeRegs(REG_CTRL_REG5, 0x40);     //Enable the interrupt Pin for the FIFO
    writeRegs(REG_XYZ_DATA_CFG, 0x01);  //HPF_OUT not in use, 4g mode
    writeRegs(REG_CTRL_REG1, 0x01);              //Go to Active Mode
}


void MMA8451Q::init_tap(void)
{
    //Event Detection Trigger on a Tap Event to Flush the Data for Further Analysis
    //800 Hz ODR, 8g Mode, 14-bit data
    writeRegs(REG_CTRL_REG1, 0x00);     // standby Mode, set Sample Rate to 800 Hz, Standby (Active = 0),14-bit (F_READ = 0)
    writeRegs(REG_F_SETUP_FIFO, 0xD4);      //FIFO in Trigger Buffer Mode in Register 0x09 F_Setup/ watermar to 20
    writeRegs(REG_TRIG_CFG, 0x08);          //Step 3: Set the Trigger for Tap Detection (pulse interrupt trigger)
    writeRegs(REG_PULSE_CFG , 0x15);         //Enable the Tap to Trigger the FIFO / single pulse event on x,y,z axises
    //Note: Every step is 0.063g, 1.575g/ 0.063g = 25 counts, 2.52g/0.063g = 40 counts
    writeRegs(REG_PULSE_THSX, 0x19);            //Set Threshold 1.575g on X and Y
    writeRegs(REG_PULSE_THSY, 0x19);            
    writeRegs(REG_PULSE_THSZ, 0x05);            //and 2.52g on Z
    //Note: 800 Hz ODR, Time step is 0.625 ms per step, 50 ms/0.625 ms = 80 counts
    writeRegs(REG_PULSE_TMLT, 0x50);          //Set Time Limit for Tap Detection to 50 ms No LPF Enabled 
    //Note: 800 Hz ODR, Time step is 1.25 ms per step, 300 ms/1.25 ms = 240 counts
    writeRegs(REG_PULSE_LTCY, 0xF0);        //Set Latency Timer to 300 ms
    writeRegs(REG_CTRL_REG4, 0x08);         //Enable the interrupt Pin Tap (pulse)
    writeRegs(REG_CTRL_REG5, 0x08);         //Set the interrupt to route to INT1
    writeRegs(REG_XYZ_DATA_CFG, 0x02);      //8g, HPF not in use
    writeRegs(REG_CTRL_REG1, 0x01);         //Active Mode
}



uint8_t MMA8451Q::getWhoAmI() 
{
    uint8_t who_am_i = 0;
    readRegs(REG_WHO_AM_I, &who_am_i, 1);
    return who_am_i;
}

void MMA8451Q::readRegs(int addr, uint8_t * data, int len) 
{
    char t[1] = {addr};
    m_i2c.write(m_addr, t, 1, true);
    m_i2c.read(m_addr, (char *)data, len);
}

void MMA8451Q::writeRegs(uint8_t * data, int len) 
{
    m_i2c.write(m_addr, (char *)data, len);
}
void MMA8451Q::writeRegs(uint8_t adr1, uint8_t adr2) 
{
    uint8_t data[2] = {adr1, adr2};
    m_i2c.write(m_addr, (char *)data, 2);
}
extern Serial pc;
extern Timer txTim;

void MMA8451Q::isr(void)
{
    
    uint8_t data;
    readRegs(0x0C, &data, 1);           //Determine the source of the interrupt by reading the system interrupt  
    if(data & 0x40)                     //FIFO interrupt source 
    {
        readRegs(0x00, &data, 1);       //read the status register to clear all the flags and find out interrupt cause
        if(data & 0x40)                 //FIFO watermark flag    
             readRegs(0x01,(uint8_t *) xyz, 192);           // get samples from FIFO
        if((data & 0x80) && (testX == 0))                 // FIFO overflow flag  
        {
            test = 1;
            readRegs(0x01, buffer[ind1], 192);           // get samples from FIFO
            ind1++;
            pc.printf("index1: %d", ind1);
            if(ind1 == 10)
            {
                testX = 1;
                ind1 = 0;
            }
        }
     data_ready = true;   
    }            
    
    if(data & 0x08)     // tap event occured
    {
        readRegs(0x00, &data, 1);           //clear the flag
        readRegs(0x01, (uint8_t *) xyz, 192);   //get data from FIFO
        data_ready = true;
    }
    //ptrEnd->actual[192];
    test = 0;   
}