/////* ----------------------------------------------------------------------------
//// * Copyright (c) 2017 Semiconductor Components Industries, LLC (d/b/a
//// * ON Semiconductor), All Rights Reserved
//// *
//// * This code is the property of ON Semiconductor and may not be redistributed
//// * in any form without prior written permission from ON Semiconductor.
//// * The terms of use and warranty for this code are covered by contractual
//// * agreements between ON Semiconductor and the licensee.
//// *
//// * This is Reusable Code.
//// *
//// * ----------------------------------------------------------------------------
//// * app.c
//// * - Simple application using a DIO5 input to write and read data throught I2C
//// * - DIO6 is configured as output and connected to a LED
//// * - During communication DIO6 is ON
//// * - If data read from the slave is equal to the written data to the slave the
//// *   LED blinks at the end of transaction
//// * ----------------------------------------------------------------------------
//// * $Revision: 1.11 $
//// * $Date: 2017/12/05 16:19:51 $
//// * ------------------------------------------------------------------------- */
////#include <rsl10.h>
////#include <stdint.h>
////
/////* ----------------------------------------------------------------------------
//// * Define declaration
//// * ------------------------------------------------------------------------- */
////#define CONCAT(x, y)                    x##y
////#define DIO_SRC(x)                      CONCAT(DIO_SRC_DIO_, x)
////#define TX_DATA                         "RSL10 includes an I2C"
////#define BUFFER_SIZE                     (sizeof TX_DATA)
////#define I2C_SCL_DIO                     0
////#define I2C_SDA_DIO                     1
////#define BUTTON_DIO                      5
////#define LED_DIO                         6
////#define SLAVE_ADDRESS                   0b1101010
////
/////* DIO number that is used for easy re-flashing (recovery mode) */
////#define RECOVERY_DIO                    12
////
/////* ----------------------------------------------------------------------------
//// * Global variables declaration
//// * ------------------------------------------------------------------------- */
////enum state
////{
////    I2C_IDLE, I2C_START, I2C_WRITE_FINISH, I2C_READ_FINISH
////};
////volatile enum state i2c_state = I2C_IDLE;
////volatile uint8_t    tx_buffer[BUFFER_SIZE] = TX_DATA;
////volatile uint8_t    tx_buffer_index = 0;
////volatile uint8_t    rx_buffer[BUFFER_SIZE];
////volatile uint8_t    rx_buffer_index = 0;
////
/////* ----------------------------------------------------------------------------
//// * Interrupt handler declaration
//// * ------------------------------------------------------------------------- */
////extern void DIO0_IRQHandler(void);
////extern void I2C_IRQHandler(void);
////
/////* ----------------------------------------------------------------------------
//// * Forward declaration
//// * ------------------------------------------------------------------------- */
////void Initialize(void);
////
/////* ----------------------------------------------------------------------------
//// * Function      : void DIO0_IRQHandler(void)
//// * ----------------------------------------------------------------------------
//// * Description   : Start the transactions
//// * Inputs        : None
//// * Outputs       : None
//// * Assumptions   : None
//// * ------------------------------------------------------------------------- */
////void DIO0_IRQHandler(void)
////{
////    static uint8_t ignore_next_dio_int = 0;
////    if (ignore_next_dio_int == 1)
////    {
////        ignore_next_dio_int = 0;
////    }
////    else if (DIO_DATA->ALIAS[BUTTON_DIO] == 0)
////    {
////        /* Button is pressed: Ignore next interrupt.
////         * This is required to deal with the debounce circuit limitations. */
////        ignore_next_dio_int = 1;
////
////        /* Set the i2c_start status */
////        i2c_state = I2C_START;
////    }
////}
////
/////* ----------------------------------------------------------------------------
//// * Function      : void I2C_IRQHandler(void)
//// * ----------------------------------------------------------------------------
//// * Description   : Handle the interrupts of I2C
//// * Inputs        : None
//// * Outputs       : None
//// * Assumptions   : None
//// * ------------------------------------------------------------------------- */
////void I2C_IRQHandler(void)
////{
////    uint32_t i2c_status = Sys_I2C_Get_Status();
////
////    if ((i2c_status & (1 << I2C_STATUS_STOP_DETECT_Pos)) == I2C_STOP_DETECTED)
////    {
////        /* Stop detected, set the I2C state according to the current mode. */
////        if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
////        {
////            if (rx_buffer_index == BUFFER_SIZE)
////            {
////                i2c_state = I2C_READ_FINISH;
////            }
////            else
////            {
////                /* Slave did not respond with ACK to Read request return
////                 * to IDLE state. */
////                i2c_state = I2C_IDLE;
////            }
////        }
////        else
////        {
////            if (tx_buffer_index == BUFFER_SIZE)
////            {
////                i2c_state = I2C_WRITE_FINISH;
////            }
////            else
////            {
////                /* Slave did not respond with ACK to write requests return
////                 * to IDLE state. */
////                i2c_state = I2C_IDLE;
////            }
////        }
////    }
////    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
////    {
////        /* READ mode, If buffer full put a new data on RX buffer. When receive
////         * the number of byte expected, send NACK and Stop. */
////        if ((i2c_status & (1 << I2C_STATUS_BUFFER_FULL_Pos)) == I2C_BUFFER_FULL)
////        {
////            if (rx_buffer_index < (BUFFER_SIZE - 1))
////            {
////                Sys_I2C_ACK();
////                rx_buffer[rx_buffer_index++] = I2C->DATA;
////            }
////            else
////            {
////                Sys_I2C_NACKAndStop();
////                rx_buffer[rx_buffer_index++] = I2C->DATA;
////            }
////        }
////
////        /* If Data Event is set send an ACK to start the read */
////        else if ((i2c_status & (1 << I2C_STATUS_DATA_EVENT_Pos)) ==
////                 I2C_DATA_EVENT)
////        {
////            Sys_I2C_ACK();
////        }
////    }
////    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_WRITE)
////    {
////        /* If WRITE mode, send the next byte. If all positions of the
////         * buffer are sent set LAST_DATA */
////        if ((i2c_status & (1 << I2C_STATUS_ACK_STATUS_Pos)) == I2C_HAS_ACK)
////        {
////            if (tx_buffer_index < BUFFER_SIZE)
////            {
////                I2C->DATA = tx_buffer[tx_buffer_index++];
////            }
////            else
////            {
////                I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
////            }
////        }
////    }
////}
////
/////* ----------------------------------------------------------------------------
//// * Function      : void Initialize(void)
//// * ----------------------------------------------------------------------------
//// * Description   : Initialize the system by disabling interrupts, configuring
//// *                 the required DIOs for an I2C interface and enabling the
//// *                 interrupts
//// * Inputs        : None
//// * Outputs       : None
//// * Assumptions   : None
//// * ------------------------------------------------------------------------- */
////void Initialize(void)
////{
////    /* Mask all interrupts */
////    __set_PRIMASK(PRIMASK_DISABLE_INTERRUPTS);
////
////    /* Disable all existing interrupts, clearing all pending source */
////    Sys_NVIC_DisableAllInt();
////    Sys_NVIC_ClearAllPendingInt();
////
////    /* Test DIO12 to pause the program to make it easy to re-flash */
////    DIO->CFG[RECOVERY_DIO] = DIO_MODE_INPUT  | DIO_WEAK_PULL_UP |
////                             DIO_LPF_DISABLE | DIO_6X_DRIVE;
////    while (DIO_DATA->ALIAS[RECOVERY_DIO] == 0);
////
////    /* Setup DIO5 as a GPIO input with interrupts on transitions, DIO6 as a
////     * GPIO output. Use the integrated debounce circuit to ensure that only a
////     * single interrupt event occurs for each push of the pushbutton.
////     * The debounce circuit always has to be used in combination with the
////     * transition mode to deal with the debounce circuit limitations.
////     * A debounce filter time of 50 ms is used. */
////    Sys_DIO_Config(BUTTON_DIO, DIO_MODE_GPIO_IN_0 | DIO_WEAK_PULL_UP |
////                   DIO_LPF_DISABLE);
////    Sys_DIO_Config(LED_DIO, DIO_MODE_GPIO_OUT_0);
////    Sys_DIO_IntConfig(0, DIO_EVENT_TRANSITION | DIO_SRC(BUTTON_DIO) |
////                      DIO_DEBOUNCE_ENABLE,
////                      DIO_DEBOUNCE_SLOWCLK_DIV1024, 49);
////
////    /* Configure I2C as Master */
////    Sys_I2C_Config(I2C_MASTER_SPEED_120  | I2C_CONTROLLER_CM3   |
////                   I2C_STOP_INT_ENABLE   | I2C_AUTO_ACK_DISABLE |
////                   I2C_SAMPLE_CLK_ENABLE | I2C_SLAVE_DISABLE);
////
////    /* Configure the DIOs for I2C operation, strong pull-up used to drive the
////     * line,
////     * if external pull-up is used the DIO_NO_PULL can be used */
////    Sys_I2C_DIOConfig(DIO_6X_DRIVE | DIO_LPF_ENABLE | DIO_STRONG_PULL_UP,
////                      I2C_SCL_DIO,
////                      I2C_SDA_DIO);
////
////    /* Enable interrupts */
////    NVIC_EnableIRQ(DIO0_IRQn);
////    NVIC_EnableIRQ(I2C_IRQn);
////
////    /* Unmask all interrupts */
////    __set_PRIMASK(PRIMASK_ENABLE_INTERRUPTS);
////}
////
/////* ----------------------------------------------------------------------------
//// * Function      : int main(void)
//// * ----------------------------------------------------------------------------
//// * Description   : Initialize the system, then send an I2C frame controlled
//// *                 by DIO5 (press to send).
//// *                 Set the LED 0.5 s when a transmission start
//// * Inputs        : None
//// * Outputs       : None
//// * Assumptions   : None
//// * ------------------------------------------------------------------------- */
////int main(void)
////{
////    /* Initialize the system */
////    Initialize();
////
////    /* Spin loop */
////    while (1)
////    {
////        /* Refresh the watch-dog timer */
////        Sys_Watchdog_Refresh();
////
////        /* The I2C transaction starts when the Button is pressed. It starts
////         * with a write to the slave with the address 5.
////         * The LED is set for 0.5 s. */
////        if (i2c_state == I2C_START)
////        {
////            i2c_state = I2C_IDLE;
////            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
////                   I2C_BUS_FREE);
////            tx_buffer_index = 0;
////            Sys_I2C_StartWrite(SLAVE_ADDRESS);
////            Sys_GPIO_Set_High(LED_DIO);
////            Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
////            Sys_GPIO_Set_Low(LED_DIO);
////        }
////
////        /* I2C write has finished execution. Continue with an I2C Read command.
////         * */
////        else if (i2c_state == I2C_WRITE_FINISH)
////        {
////            i2c_state = I2C_IDLE;
////            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
////                   I2C_BUS_FREE);
////
////            rx_buffer_index = 0;
////            Sys_I2C_StartRead(SLAVE_ADDRESS);
////        }
////
////        /* The I2C Read command has finished execution. Check the data received.
////         * The LED is set for 0.1 s if the data received is equal to the data
////         * that was sent. */
////        else if (i2c_state == I2C_READ_FINISH)
////        {
////            i2c_state = I2C_IDLE;
////            uint8_t error = 0;
////            uint8_t i;
////            for (i = 0; i < BUFFER_SIZE; i++)
////            {
////                if (rx_buffer[i] != tx_buffer[i])
////                {
////                    error = 1;
////                    break;
////                }
////            }
////            if (error == 0)
////            {
////                /* Wait 0.1 s before setting the LED so the user can see
////                 * the blink */
////                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////                Sys_GPIO_Set_High(LED_DIO);
////                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////                Sys_GPIO_Set_Low(LED_DIO);
////            }
////        }
////    }
////}
//
///* ----------------------------------------------------------------------------
// * Copyright (c) 2017 Semiconductor Components Industries, LLC (d/b/a
// * ON Semiconductor), All Rights Reserved
// *
// * This code is the property of ON Semiconductor and may not be redistributed
// * in any form without prior written permission from ON Semiconductor.
// * The terms of use and warranty for this code are covered by contractual
// * agreements between ON Semiconductor and the licensee.
// *
// * This is Reusable Code.
// *
// * ----------------------------------------------------------------------------
// * app.c
// * - Simple application using a DIO5 input to write and read data throught I2C
// * - DIO6 is configured as output and connected to a LED
// * - During communication DIO6 is ON
// * - If data read from the slave is equal to the written data to the slave the
// *   LED blinks at the end of transaction
// * ----------------------------------------------------------------------------
// * $Revision: 1.11 $
// * $Date: 2017/12/05 16:19:51 $
// * ------------------------------------------------------------------------- */
//#include <rsl10.h>
//#include <stdint.h>
//#include <stdio.h>
//
///* ----------------------------------------------------------------------------
// * Define declaration
// * ------------------------------------------------------------------------- */
//#define CONCAT(x, y)                    x##y
//#define DIO_SRC(x)                      CONCAT(DIO_SRC_DIO_, x)
//#define TX_DATA                         "RSL10 includes an I2C"
//#define BUFFER_SIZE                     (sizeof TX_DATA)
//#define I2C_SCL_DIO                     0x0
//#define I2C_SDA_DIO                     0x1
//#define BUTTON_DIO                      5
//#define LED_DIO                         6
//#define SLAVE_ADDRESS				0b1010011
//
//
////volatile int I2C_ACK = 0;
////volatile int I2C_NACK = 1;
//
///* DIO number that is used for easy re-flashing (recovery mode) */
//#define RECOVERY_DIO                    12
//
///* ----------------------------------------------------------------------------
// * Global variables declaration
// * ------------------------------------------------------------------------- */
//enum state
//{
//    I2C_IDLE, I2C_START, I2C_WRITE_FINISH, I2C_READ_FINISH
//};
//volatile enum state i2c_state = I2C_IDLE;
//volatile uint8_t    tx_buffer[BUFFER_SIZE] = TX_DATA;
//volatile uint8_t    tx_buffer_index = 0;
//volatile uint8_t    rx_buffer[BUFFER_SIZE];
//volatile uint8_t    rx_buffer_index = 0;
//
///* ----------------------------------------------------------------------------
// * Interrupt handler declaration
// * ------------------------------------------------------------------------- */
//extern void DIO0_IRQHandler(void);
//extern void I2C_IRQHandler(void);
//
///* ----------------------------------------------------------------------------
// * Forward declaration
// * ------------------------------------------------------------------------- */
//void Initialize(void);
//
///* ----------------------------------------------------------------------------
// * Function      : void DIO0_IRQHandler(void)
// * ----------------------------------------------------------------------------
// * Description   : Start the transactions
// * Inputs        : None
// * Outputs       : None
// * Assumptions   : None
// * ------------------------------------------------------------------------- */
//void DIO0_IRQHandler(void)
//{
//    static uint8_t ignore_next_dio_int = 0;
//    if (ignore_next_dio_int == 1)
//    {
//        ignore_next_dio_int = 0;
//    }
//    else if (DIO_DATA->ALIAS[BUTTON_DIO] == 0)
//    {
//        /* Button is pressed: Ignore next interrupt.
//         * This is required to deal with the debounce circuit limitations. */
//        ignore_next_dio_int = 1;
//
//        /* Set the i2c_start status */
//        i2c_state = I2C_START;
//    }
//}
//
///* ----------------------------------------------------------------------------
// * Function      : void I2C_IRQHandler(void)
// * ----------------------------------------------------------------------------
// * Description   : Handle the interrupts of I2C
// * Inputs        : None
// * Outputs       : None
// * Assumptions   : None
// * ------------------------------------------------------------------------- */
//void I2C_IRQHandler(void)
//{
//    uint32_t i2c_status = Sys_I2C_Get_Status();
//
//    if ((i2c_status & (1 << I2C_STATUS_STOP_DETECT_Pos)) == I2C_STOP_DETECTED)
//    {
//        /* Stop detected, set the I2C state according to the current mode. */
//        if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
//        {
//            if (rx_buffer_index == BUFFER_SIZE)
//            {
//                i2c_state = I2C_READ_FINISH;
//            }
//            else
//            {
//                /* Slave did not respond with ACK to Read request return
//                 * to IDLE state. */
//                i2c_state = I2C_IDLE;
//            }
//        }
//        else
//        {
//            if (tx_buffer_index == BUFFER_SIZE)
//            {
//                i2c_state = I2C_WRITE_FINISH;
//            }
//            else
//            {
//                /* Slave did not respond with ACK to write requests return
//                 * to IDLE state. */
//                i2c_state = I2C_IDLE;
//            }
//        }
//    }
//    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
//    {
//        /* READ mode, If buffer full put a new data on RX buffer. When receive
//         * the number of byte expected, send NACK and Stop. */
//        if ((i2c_status & (1 << I2C_STATUS_BUFFER_FULL_Pos)) == I2C_BUFFER_FULL)
//        {
//            if (rx_buffer_index < (BUFFER_SIZE - 1))
//            {
//                Sys_I2C_ACK();
//                rx_buffer[rx_buffer_index++] = I2C->DATA;
//            }
//            else
//            {
//               Sys_I2C_NACKAndStop();
//            	//Sys_I2C_ACK();
//                rx_buffer[rx_buffer_index++] = I2C->DATA;
//            	//Sys_I2C_ACK();
//            }
//        }
//
//        /* If Data Event is set send an ACK to start the read */
//        else if ((i2c_status & (1 << I2C_STATUS_DATA_EVENT_Pos)) ==
//                 I2C_DATA_EVENT)
//        {
//            Sys_I2C_ACK();
//        }
//    }
//    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_WRITE)
//    {
//        /* If WRITE mode, send the next byte. If all positions of the
//         * buffer are sent set LAST_DATA */
//        if ((i2c_status & (1 << I2C_STATUS_ACK_STATUS_Pos)) == I2C_HAS_ACK)
//        {
//            if (tx_buffer_index < BUFFER_SIZE)
//            {
//               //I2C->DATA = tx_buffer[tx_buffer_index++];
//            	I2C->DATA = tx_buffer[BUFFER_SIZE];
//            }
//            else
//            {
//                I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
//            }
//        }
//    }
//}
//
///* ----------------------------------------------------------------------------
// * Function      : void Initialize(void)
// * ----------------------------------------------------------------------------
// * Description   : Initialize the system by disabling interrupts, configuring
// *                 the required DIOs for an I2C interface and enabling the
// *                 interrupts
// * Inputs        : None
// * Outputs       : None
// * Assumptions   : None
// * ------------------------------------------------------------------------- */
//void Initialize(void)
//{
//    /* Mask all interrupts */
//    __set_PRIMASK(PRIMASK_DISABLE_INTERRUPTS);
//
//    /* Disable all existing interrupts, clearing all pending source */
//    Sys_NVIC_DisableAllInt();
//    Sys_NVIC_ClearAllPendingInt();
//
//    /* Test DIO12 to pause the program to make it easy to re-flash */
//    DIO->CFG[RECOVERY_DIO] = DIO_MODE_INPUT  | DIO_WEAK_PULL_UP |
//                             DIO_LPF_DISABLE | DIO_6X_DRIVE;
//    while (DIO_DATA->ALIAS[RECOVERY_DIO] == 0);
//
//    /* Setup DIO5 as a GPIO input with interrupts on transitions, DIO6 as a
//     * GPIO output. Use the integrated debounce circuit to ensure that only a
//     * single interrupt event occurs for each push of the pushbutton.
//     * The debounce circuit always has to be used in combination with the
//     * transition mode to deal with the debounce circuit limitations.
//     * A debounce filter time of 50 ms is used. */
//    Sys_DIO_Config(BUTTON_DIO, DIO_MODE_GPIO_IN_0 | DIO_WEAK_PULL_UP |
//                   DIO_LPF_DISABLE);
//    Sys_DIO_Config(LED_DIO, DIO_MODE_GPIO_OUT_0);
//    Sys_DIO_IntConfig(0, DIO_EVENT_TRANSITION | DIO_SRC(BUTTON_DIO) |
//                      DIO_DEBOUNCE_ENABLE,
//                      DIO_DEBOUNCE_SLOWCLK_DIV1024, 49);
//
//    /* Configure I2C as Master */
//    Sys_I2C_Config(I2C_MASTER_SPEED_120  | I2C_CONTROLLER_CM3   |
//                   I2C_STOP_INT_ENABLE   | I2C_AUTO_ACK_DISABLE |
//                   I2C_SAMPLE_CLK_ENABLE | I2C_SLAVE_DISABLE);
//
//    /* Configure the DIOs for I2C operation, strong pull-up used to drive the
//     * line,
//     * if external pull-up is used the DIO_NO_PULL can be used */
//    Sys_I2C_DIOConfig(DIO_6X_DRIVE | DIO_LPF_ENABLE | DIO_STRONG_PULL_UP,
//                      I2C_SCL_DIO,
//                      I2C_SDA_DIO);
//
//    /* Enable interrupts */
//    NVIC_EnableIRQ(DIO0_IRQn);
//    NVIC_EnableIRQ(I2C_IRQn);
//
//    /* Unmask all interrupts */
//    __set_PRIMASK(PRIMASK_ENABLE_INTERRUPTS);
//}
//
///*void UART_TX_IRQHandler(void)
//{
//    nbr_tx_send++;
//    if (tx_buffer_index < BUFFER_SIZE)
//    {
//        UART->TX_DATA = tx_buffer[tx_buffer_index++];
//    }
//}*/
//
//void writeAccel(uint8_t addr, uint8_t dataW){
//
//	uint32_t statc = 0;
//	uint8_t store = 0;
//	uint8_t SADW = 0xD5;
//	uint8_t SAD = 0b1010011;
//
//	//statc = (Sys_I2C_Get_Status() & 0b0000000000010000);
//	//while(statc == 0b0000000000010000 ){
//		//statc = (Sys_I2C_Get_Status() & 0b0000000000010000);
//	//}
//
//	//Sys_GPIO_Set_Low(I2C_SDA_DIO);
//	Sys_I2C_StartWrite(SLAVE_ADDRESS);
//	//Sys_I2C_StartWrite(0b0010000);
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	tx_buffer[BUFFER_SIZE] = addr;
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	tx_buffer[BUFFER_SIZE] = dataW;
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
//	//Sys_GPIO_Set_High(I2C_SDA_DIO);
//
//}
//
//uint8_t readAccel(uint8_t addrW){
//
//	uint8_t acc = 0;
//	uint8_t acc2 = 0;
//	uint32_t statc = 0;
//	uint8_t SAD = 0b1010011;
//
//	//statc = (Sys_I2C_Get_Status() & 0b0000000000010000);
//	//while(statc == 0b0000000000010000 ){
//		//statc = (Sys_I2C_Get_Status() & 0b0000000000010000);
////	}
//	//Sys_GPIO_Set_Low(I2C_SDA_DIO);
//	Sys_I2C_StartWrite(SLAVE_ADDRESS);
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	tx_buffer[BUFFER_SIZE] = addrW;
//	//Sys_I2C_DATA_IS(0x00);
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK){
//		//I2C->DATA = tx_buffer[BUFFER_SIZE];
//	}
//	Sys_I2C_StartRead(SLAVE_ADDRESS);
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	// catch the data
//	acc = I2C->DATA;
//	Sys_I2C_ACK();
//	acc2 = rx_buffer[BUFFER_SIZE];
//	//Sys_I2C_NACKAndStop();
//	I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
//	//Sys_GPIO_Set_High(I2C_SDA_DIO);
//
//	return acc;
//
//}
//
//void configSEN(void){
//
//	uint8_t FIFOaddr = 0x2E;
//	uint8_t FIFOset = 0b11000000;
//	uint8_t REG6addr = 0x20;
//	uint8_t REG6set = 0b10010000;
//	uint8_t REG9addr = 0x23;
//	uint8_t REG9set = 0b00000010;
//
//	//Set FIFO-CTRL register for continuous mode operation
//	writeAccel(FIFOaddr, FIFOset);
//	//Set CTRL_REG6_XL to use accelerometer but not use gyroscope
//	writeAccel(REG6addr, REG6set);
//	//CRL_REG9_FIFO_EN to enable FIFO memory
//	writeAccel(REG9addr, REG9set);
//
//}
//
//
//
///* ----------------------------------------------------------------------------
// * Function      : int main(void)
// * ----------------------------------------------------------------------------
// * Description   : Initialize the system, then send an I2C frame controlled
// *                 by DIO5 (press to send).
// *                 Set the LED 0.5 s when a transmission start
// * Inputs        : None
// * Outputs       : None
// * Assumptions   : None
// * ------------------------------------------------------------------------- */
//int main(void)
//{
//    /*while(1){
//    	Sys_GPIO_Set_Low(I2C_SDA_DIO);
//    	Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//    	Sys_GPIO_Set_High(I2C_SDA_DIO);
//    }*/
//    /* Initialize the system */
//	//Sys_GPIO_Set_High(I2C_SDA_DIO);
//    Initialize();
//   // configSEN();
//    //Sys_GPIO_Set_High(I2C_SDA_DIO);
//    uint8_t status = 0x27;
//    uint8_t xvallo = 0x28;
//    uint8_t xvalhi = 0x29;
//    uint8_t yvallo = 0x2A;
//    uint8_t yvalhi = 0x2B;
//    uint8_t zvallo = 0x2C;
//    uint8_t zvalhi = 0x2D;
//    uint8_t catchxlo = 0;
//    uint8_t catchxhi = 0;
//    uint8_t catchylo = 0;
//    uint8_t catchyhi = 0;
//    uint8_t catchzlo = 0;
//    uint8_t catchzhi = 0;
//	uint8_t REG6addr = 0x20;
//	uint8_t REG6set = 0b10010000;
//    float statcheck = 0;
//
//
////    while(1){
////    	uint8_t hold = 0;
////    	writeAccel(0x05, 0xAA);
////    	hold = readAccel(0x05);
////    	if (hold == 0xAA){
////    		while(1){
////            Sys_GPIO_Set_High(LED_DIO);
////            Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////            Sys_GPIO_Set_Low(LED_DIO);
////    		}
////    	}else{
////    		hold = 0;
////    	}
////    }
////    /* Spin loop */
////    while (1)
////    {
////    	statcheck = (readAccel(status) & 0b00000001);
////    	if (statcheck == 0b00000001){
////    		//printf("Hello World \n");
////    		//putchar(64);
////    		catchxhi = readAccel(xvalhi);
////    		catchxlo = readAccel(xvallo);
////    		catchyhi = readAccel(yvalhi);
////    		catchylo = readAccel(yvallo);
////    		catchzhi = readAccel(zvalhi);
////    		catchzlo = readAccel(zvallo);
////    		/* Restart indexes */
////    	}else{
////    		statcheck = 0;
////    	}
////    }
//        //Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
//
//        /* Refresh the watch-dog timer */
//    while(1){
//       Sys_Watchdog_Refresh();
//
//        /* The I2C transaction starts when the Button is pressed. It starts
//         * with a write to the slave with the address 5.
//         * The LED is set for 0.5 s. */
//      // if (i2c_state == I2C_START)
//       //{
//           i2c_state = I2C_IDLE;
//           while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
//                  I2C_BUS_FREE);
//           tx_buffer_index = 0;
//           readAccel(0x32);
//           //writeAccel(0x38, 0b10000001);
//           Sys_GPIO_Set_High(LED_DIO);
//           Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
//           Sys_GPIO_Set_Low(LED_DIO);
//
//
//        /* I2C write has finished execution. Continue with an I2C Read command.
//         * */
////        else if (i2c_state == I2C_WRITE_FINISH)
////        {
////            i2c_state = I2C_IDLE;
////            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
////                   I2C_BUS_FREE);
////
////            rx_buffer_index = 0;
////            Sys_I2C_StartRead(SLAVE_ADDRESS);
////        }
//
//        /* The I2C Read command has finished execution. Check the data received.
//         * The LED is set for 0.1 s if the data received is equal to the data
//         * that was sent. */
////        else if (i2c_state == I2C_READ_FINISH)
////        {
////            i2c_state = I2C_IDLE;
////            uint8_t error = 0;
////            uint8_t i;
////            for (i = 0; i < BUFFER_SIZE; i++)
////            {
////                if (rx_buffer[i] != tx_buffer[i])
////                {
////                    error = 1;
////                    break;
////                }
////            }
////            if (error == 0)
////            {
////                /* Wait 0.1 s before setting the LED so the user can see
////                 * the blink */
////                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////                Sys_GPIO_Set_High(LED_DIO);
////                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////                Sys_GPIO_Set_Low(LED_DIO);
////            }
////        }
//        /* Put the new value on UART TX buffer */
//        //tx_buffer[tx_buffer_index++] = catch;
//       // UART->TX_DATA   = tx_buffer[tx_buffer_index];
//        //sprintf(catchmsb, "%.5f",catch);
//        //printf(catchmsb);
//
//        /* Remove flag */
//        //start_tx = 0;
//
//        /* Refresh the watch-dog timer */
//        /*Sys_Watchdog_Refresh();*/
//
//        /* The I2C transaction starts when the Button is pressed. It starts
//         * with a write to the slave with the address 5.
//         * The LED is set for 0.5 s. */
//        /*if (i2c_state == I2C_START)
//        {
//            i2c_state = I2C_IDLE;
//            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
//                   I2C_BUS_FREE);
//            tx_buffer_index = 0;
//            Sys_I2C_StartWrite(SLAVE_ADDRESS);
//            Sys_GPIO_Set_High(LED_DIO);
//            Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
//            Sys_GPIO_Set_Low(LED_DIO);
//        }
//
//        /* I2C write has finished execution. Continue with an I2C Read command.
//         * */
//       /* else if (i2c_state == I2C_WRITE_FINISH)
//        {
//            i2c_state = I2C_IDLE;
//            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
//                   I2C_BUS_FREE);
//
//            rx_buffer_index = 0;
//            Sys_I2C_StartRead(SLAVE_ADDRESS);
//        }
//
//        /* The I2C Read command has finished execution. Check the data received.
//         * The LED is set for 0.1 s if the data received is equal to the data
//         * that was sent. */
//        /*else if (i2c_state == I2C_READ_FINISH)
//        {
//            i2c_state = I2C_IDLE;
//            uint8_t error = 0;
//            uint8_t i;
//            for (i = 0; i < BUFFER_SIZE; i++)
//            {
//                if (rx_buffer[i] != tx_buffer[i])
//                {
//                    error = 1;
//                    break;
//                }
//            }
//            if (error == 0)
//            {
//                /* Wait 0.1 s before setting the LED so the user can see
//                 * the blink */
//               /* Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//                Sys_GPIO_Set_High(LED_DIO);
//                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//                Sys_GPIO_Set_Low(LED_DIO);
//            }
//        }*/
//    }
//}
//
//
//


/* ----------------------------------------------------------------------------
 * Copyright (c) 2017 Semiconductor Components Industries, LLC (d/b/a
 * ON Semiconductor), All Rights Reserved
 *
 * This code is the property of ON Semiconductor and may not be redistributed
 * in any form without prior written permission from ON Semiconductor.
 * The terms of use and warranty for this code are covered by contractual
 * agreements between ON Semiconductor and the licensee.
 *
 * This is Reusable Code.
 *
 * ----------------------------------------------------------------------------
 * app.c
 * - Simple application using a DIO5 input to write and read data throught I2C
 * - DIO6 is configured as output and connected to a LED
 * - During communication DIO6 is ON
 * - If data read from the slave is equal to the written data to the slave the
 *   LED blinks at the end of transaction
 * ----------------------------------------------------------------------------
 * $Revision: 1.11 $
 * $Date: 2017/12/05 16:19:51 $
 * ------------------------------------------------------------------------- */
#include <rsl10.h>
#include <stdint.h>
#include <stdio.h>
#include "I2C.h"
#include "app.h"

/* ----------------------------------------------------------------------------
 * Define declaration
 * ------------------------------------------------------------------------- */
#define CONCAT(x, y)                    x##y
#define DIO_SRC(x)                      CONCAT(DIO_SRC_DIO_, x)
#define TX_DATA                         "RSL10 includes an I2C"
#define BUFFER_START                    (sizeof TX_DATA)
#define I2C_SCL_DIO                     0x7 //0x7
#define I2C_SDA_DIO                     0xA //0xA
#define BUTTON_DIO                      5
#define LED_DIO                         6
#define SLAVE_ADDRESS				0b1010011


/* DIO number that is used for easy re-flashing (recovery mode) */
#define RECOVERY_DIO                    12

volatile uint8_t XYZ[6];
volatile uint8_t count = 0;


/* ----------------------------------------------------------------------------
 * Global variables declaration
 * ------------------------------------------------------------------------- */
enum state
{
    I2C_IDLE, I2C_START, I2C_WRITE_FINISH, I2C_READ_FINISH
};
volatile enum state i2c_state = I2C_IDLE;
volatile int BUFFER_SIZEtx = 0;
volatile int BUFFER_SIZErx = 0;
volatile uint8_t    tx_buffer[BUFFER_START] = TX_DATA;
volatile uint8_t    tx_buffer_index = 0;
volatile uint8_t    rx_buffer[BUFFER_START];
volatile uint8_t    rx_buffer_index = 0;
volatile uint8_t 	hold = 0;

/* ----------------------------------------------------------------------------
 * Interrupt handler declaration
 * ------------------------------------------------------------------------- */
extern void DIO0_IRQHandler(void);
extern void I2C_IRQHandler(void);

/* ----------------------------------------------------------------------------
 * Forward declaration
 * ------------------------------------------------------------------------- */
void InitializeI2C(void);

/* ----------------------------------------------------------------------------
 * Function      : void DIO0_IRQHandler(void)
 * ----------------------------------------------------------------------------
 * Description   : Start the transactions
 * Inputs        : None
 * Outputs       : None
 * Assumptions   : None
 * ------------------------------------------------------------------------- */
void DIO0_IRQHandler(void)
{
    static uint8_t ignore_next_dio_int = 0;
    if (ignore_next_dio_int == 1)
    {
        ignore_next_dio_int = 0;
    }
    else if (DIO_DATA->ALIAS[BUTTON_DIO] == 0)
    {
        /* Button is pressed: Ignore next interrupt.
         * This is required to deal with the debounce circuit limitations. */
        ignore_next_dio_int = 1;

        /* Set the i2c_start status */
        i2c_state = I2C_START;
    }
}

/* ----------------------------------------------------------------------------
 * Function      : void I2C_IRQHandler(void)
 * ----------------------------------------------------------------------------
 * Description   : Handle the interrupts of I2C
 * Inputs        : None
 * Outputs       : None
 * Assumptions   : None
 * ------------------------------------------------------------------------- */
void I2C_IRQHandler(void)
{
    uint32_t i2c_status = Sys_I2C_Get_Status();

    if ((i2c_status & (1 << I2C_STATUS_STOP_DETECT_Pos)) == I2C_STOP_DETECTED)
    {
        /* Stop detected, set the I2C state according to the current mode. */
        if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
        {
            if (rx_buffer_index == BUFFER_SIZErx)
            {
                i2c_state = I2C_READ_FINISH;
            }
            else
            {
                /* Slave did not respond with ACK to Read request return
                 * to IDLE state. */
                i2c_state = I2C_IDLE;
            }
        }
        else
        {
            if (tx_buffer_index == BUFFER_SIZEtx)
            {
                i2c_state = I2C_WRITE_FINISH;
                for(int aa; aa<tx_buffer_index; aa++) {
                	tx_buffer[aa] = 0;
                }

            }
            else
            {
                /* Slave did not respond with ACK to write requests return
                 * to IDLE state. */
                i2c_state = I2C_IDLE;
            }
        }
    }
    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
    {
        /* READ mode, If buffer full put a new data on RX buffer. When receive
         * the number of byte expected, send NACK and Stop. */
        if ((i2c_status & (1 << I2C_STATUS_BUFFER_FULL_Pos)) == I2C_BUFFER_FULL)
        {
            if (rx_buffer_index < (BUFFER_SIZErx - 1))
            {
                Sys_I2C_ACK();
                rx_buffer[rx_buffer_index++] = I2C->DATA;
                XYZ[count++] = I2C->DATA;

            }
            else
            {
               Sys_I2C_NACKAndStop();
               rx_buffer[rx_buffer_index++] = I2C->DATA;
               XYZ[count++] = I2C->DATA;
               //I2C_CTRL1->RESET_ALIAS = I2C_RESET_BITBAND;
            }
        }

        /* If Data Event is set send an ACK to start the read */
        else if ((i2c_status & (1 << I2C_STATUS_DATA_EVENT_Pos)) ==
                 I2C_DATA_EVENT)
        {
            Sys_I2C_ACK();
        }
    }
    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_WRITE)
    {
        /* If WRITE mode, send the next byte. If all positions of the
         * buffer are sent set LAST_DATA */
        if ((i2c_status & (1 << I2C_STATUS_ACK_STATUS_Pos)) == I2C_HAS_ACK)
        {
            if (tx_buffer_index < BUFFER_SIZEtx)
            {
                I2C->DATA_M = tx_buffer[tx_buffer_index++];
            }
            else
            {
//            	I2C->DATA_M = 0x00;
                I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
//                I2C->DATA_M = 0x00;
//                I2C_CTRL1->STOP_ALIAS = I2C_STOP_BITBAND;
                //I2C_CTRL1->RESET_ALIAS = I2C_RESET_BITBAND;
            }
        }
    }
}

/* ----------------------------------------------------------------------------
 * Function      : void Initialize(void)
 * ----------------------------------------------------------------------------
 * Description   : Initialize the system by disabling interrupts, configuring
 *                 the required DIOs for an I2C interface and enabling the
 *                 interrupts
 * Inputs        : None
 * Outputs       : None
 * Assumptions   : None
 * ------------------------------------------------------------------------- */
void InitializeI2C(void)
{
	NVIC_EnableIRQ(DIO0_IRQn);
	/* Mask all interrupts */
    __set_PRIMASK(PRIMASK_DISABLE_INTERRUPTS);

    /* Disable all existing interrupts, clearing all pending source */
    Sys_NVIC_DisableAllInt();
    Sys_NVIC_ClearAllPendingInt();

    /* Test DIO12 to pause the program to make it easy to re-flash */
    DIO->CFG[RECOVERY_DIO] = DIO_MODE_INPUT  | DIO_WEAK_PULL_UP |
                             DIO_LPF_DISABLE | DIO_6X_DRIVE;
    while (DIO_DATA->ALIAS[RECOVERY_DIO] == 0);

    /* Setup DIO5 as a GPIO input with interrupts on transitions, DIO6 as a
     * GPIO output. Use the integrated debounce circuit to ensure that only a
     * single interrupt event occurs for each push of the pushbutton.
     * The debounce circuit always has to be used in combination with the
     * transition mode to deal with the debounce circuit limitations.
     * A debounce filter time of 50 ms is used. */
    Sys_DIO_Config(BUTTON_DIO, DIO_MODE_GPIO_IN_0 | DIO_WEAK_PULL_UP |
                   DIO_LPF_DISABLE);
    Sys_DIO_Config(LED_DIO, DIO_MODE_GPIO_OUT_0);
	Sys_DIO_Config(0, DIO_MODE_GPIO_OUT_0);
	Sys_DIO_Config(1, DIO_MODE_GPIO_OUT_0);
	Sys_DIO_Config(9, DIO_MODE_GPIO_OUT_0);
	Sys_DIO_Config(2, DIO_MODE_GPIO_OUT_0);
	Sys_DIO_Config(3, DIO_MODE_GPIO_OUT_0);
    Sys_DIO_IntConfig(0, DIO_EVENT_TRANSITION | DIO_SRC(BUTTON_DIO) |
                      DIO_DEBOUNCE_ENABLE,
                      DIO_DEBOUNCE_SLOWCLK_DIV1024, 49);

    /* Configure I2C as Master */
    Sys_I2C_Config(I2C_MASTER_SPEED_120  | I2C_CONTROLLER_CM3   |
                   I2C_STOP_INT_ENABLE   | I2C_AUTO_ACK_DISABLE |
                   I2C_SAMPLE_CLK_ENABLE | I2C_SLAVE_DISABLE);

    /* Configure the DIOs for I2C operation, strong pull-up used to drive the
     * line,
     * if external pull-up is used the DIO_NO_PULL can be used */
    Sys_I2C_DIOConfig(DIO_6X_DRIVE | DIO_LPF_ENABLE | DIO_STRONG_PULL_UP,
                      I2C_SCL_DIO,
                      I2C_SDA_DIO);

    // From Bluetooth Code //
    /* Configure the current trim settings for VCC, VDDA */
        ACS_VCC_CTRL->ICH_TRIM_BYTE = VCC_ICHTRIM_16MA_BYTE;
        ACS_VDDA_CP_CTRL->PTRIM_BYTE = VDDA_PTRIM_16MA_BYTE;

        /* Start 48 MHz XTAL oscillator */
        ACS_VDDRF_CTRL->ENABLE_ALIAS = VDDRF_ENABLE_BITBAND;
        ACS_VDDRF_CTRL->CLAMP_ALIAS = VDDRF_DISABLE_HIZ_BITBAND;

        /* Wait until VDDRF supply has powered up */
        while (ACS_VDDRF_CTRL->READY_ALIAS != VDDRF_READY_BITBAND);

        ACS_VDDPA_CTRL->ENABLE_ALIAS = VDDPA_DISABLE_BITBAND;
        ACS_VDDPA_CTRL->VDDPA_SW_CTRL_ALIAS = VDDPA_SW_VDDRF_BITBAND;

        /* Enable RF power switches */
        SYSCTRL_RF_POWER_CFG->RF_POWER_ALIAS = RF_POWER_ENABLE_BITBAND;

        /* Remove RF isolation */
        SYSCTRL_RF_ACCESS_CFG->RF_ACCESS_ALIAS = RF_ACCESS_ENABLE_BITBAND;

        /* Set radio output power of RF */
        Sys_RFFE_SetTXPower(OUTPUT_POWER_DBM);

        /* Start the 48 MHz oscillator without changing the other register bits */
        RF->XTAL_CTRL = ((RF->XTAL_CTRL & ~XTAL_CTRL_DISABLE_OSCILLATOR) |
                         XTAL_CTRL_REG_VALUE_SEL_INTERNAL);

        /* Enable the 48 MHz oscillator divider using the desired prescale value */
        RF_REG2F->CK_DIV_1_6_CK_DIV_1_6_BYTE = CK_DIV_1_6_PRESCALE_6_BYTE;

        /* Wait until 48 MHz oscillator is started */
        while (RF_REG39->ANALOG_INFO_CLK_DIG_READY_ALIAS !=
               ANALOG_INFO_CLK_DIG_READY_BITBAND);

        /* Switch to (divided 48 MHz) oscillator clock */
        Sys_Clocks_SystemClkConfig(JTCK_PRESCALE_1   |
                                   EXTCLK_PRESCALE_1 |
                                   SYSCLK_CLKSRC_RFCLK);

        /* Configure clock dividers */
        CLK->DIV_CFG0 = (SLOWCLK_PRESCALE_8 | BBCLK_PRESCALE_1 |
                         USRCLK_PRESCALE_1);
        CLK->DIV_CFG2 = (CPCLK_PRESCALE_8 | DCCLK_PRESCALE_2);

        BBIF->CTRL = (BB_CLK_ENABLE | BBCLK_DIVIDER_8 | BB_WAKEUP);

        /* Configure ADC channel 0 to measure VBAT/2 */
        Sys_ADC_Set_Config(ADC_VBAT_DIV2_NORMAL | ADC_NORMAL |
                           ADC_PRESCALE_6400);
        Sys_ADC_InputSelectConfig(0,
                                  (ADC_NEG_INPUT_GND |
                                   ADC_POS_INPUT_VBAT_DIV2));


        /* Initialize UART interface */
        UART_Initialize();

        /* Initialize the baseband and BLE stack */
        BLE_Initialize();

        App_Env_Initialize();

    /* Enable interrupts */
    NVIC_EnableIRQ(DIO0_IRQn);
    NVIC_EnableIRQ(I2C_IRQn);

    /* Unmask all interrupts */
    __set_PRIMASK(PRIMASK_ENABLE_INTERRUPTS);
}

void writeAccel(uint8_t addr, uint8_t dataW){

	while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
			I2C_BUS_FREE);
	tx_buffer_index = 0;
	BUFFER_SIZEtx = 2;
	tx_buffer[BUFFER_SIZEtx-2] = addr;
	tx_buffer[BUFFER_SIZEtx-1] = dataW;
	Sys_I2C_StartWrite(SLAVE_ADDRESS);
	holdData = holdData + 1;
	// add in wait for stop, then clear buffers

}

void readAccel(uint8_t addrW, int bytes2Read){

    uint8_t Catch[6];
    while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
            I2C_BUS_FREE);
    tx_buffer_index = 0;
    rx_buffer_index = 0;
    uint8_t acc = 0;
    BUFFER_SIZEtx = 1;
    BUFFER_SIZErx = bytes2Read;
    tx_buffer[BUFFER_SIZEtx-1] = addrW;
    //I2C->DATA = addrW;
    Sys_I2C_StartWrite(SLAVE_ADDRESS);
    while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
                I2C_BUS_FREE);
    Sys_I2C_StartRead(SLAVE_ADDRESS);
    int i = 0;
	for ( i = 0; i < bytes2Read; i++) {
		Catch[i] = I2C->DATA;
	}
    while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
                I2C_BUS_FREE);
//    int i = 0;
    for ( i = 0; i < bytes2Read; i++) {
    	Catch[i] = XYZ[i];
    	//Catch = I2C->DATA;
    }
//    Catch[1] = Catch[0] + 1;
//    Catch[2] = Catch[1] + 1;
//    Catch[3] = Catch[2] + 1;
//    Catch[4] = Catch[3] + 1;
//    Catch[5] = Catch[4] + 1;

//    X1 = Catch[0];
//    X2 = Catch[1];
    X = ((int)Catch[1]<<8)|(int)Catch[0];
    Y = ((int)Catch[3]<<8)|(int)Catch[2];
    Z = ((int)Catch[5]<<8)|(int)Catch[4];
//    if(Catch[0] == Catch[1]){
//    	X1 = 0x69;
//    }
//    Y1 = Catch[2];
//    Y2 = Catch[3];
//    Z1 = Catch[4];
//    Z2 = Catch[5];
    count = 0;

//    X = X1 << 8;
//    Y = Y1 << 8;
//    Z = Z1 << 8;

}

void configADXL(void){
	uint8_t BWRateReg = 0x2C;
	uint8_t PwrCtrlReg = 0x2D;
	uint8_t DataFmtReg = 0x31;
	uint8_t FIFOCtrlReg = 0x38;
	uint8_t BWVal = 0x0A;
	uint8_t PowerVal = 0x08;
	uint8_t FormatVal = 0x02;
	uint8_t fifoVal = 0x81;
	// reg 2C, value 0A: bit rate of 100Hz
	writeAccel(BWRateReg,BWVal);
	// reg 2D, value 08: put into measure mode
	writeAccel(PwrCtrlReg,PowerVal);
	// reg 31, value 02: 10 bit resolution, right justified, +/- 8g
	writeAccel(DataFmtReg,FormatVal);
	// reg 38, value 81: stream mode, one sample for watermark
	writeAccel(FIFOCtrlReg, fifoVal);


}

void readOnce(void){
	int bytesRead2 = 6;
    uint8_t *catches;
    uint8_t holdd = 0;
    uint8_t firstDataReg = 0x32;
    readAccel(firstDataReg,bytesRead2);
    //holdd = readAccel(firstDataReg,bytesRead2);
//    Sys_Delay_ProgramROM(1000 * SystemCoreClock);

//	   uint8_t CatchXYZ[bytesRead2];
//	    for (int i=0; i<bytesRead2; i++) {
//	        CatchXYZ[i] = *(catches + i);
//	    }
//	    int XYZ[3];
//	    for (int j=0; j<3; j++) {
//	    }
//	    //holdData = CatchXYZ[0];
//	    //holdData = Y1;
}



/* ----------------------------------------------------------------------------
 * Function      : int main(void)
 * ----------------------------------------------------------------------------
 * Description   : Initialize the system, then send an I2C frame controlled
 *                 by DIO5 (press to send).
 *                 Set the LED 0.5 s when a transmission start
 * Inputs        : None
 * Outputs       : None
 * Assumptions   : None
 * ------------------------------------------------------------------------- */
//int main(void)
//{

//    InitializeI2C();
//    configADXL();
    //Sys_GPIO_Set_High(I2C_SDA_DIO);
//    uint8_t catchxlo = 0;
//    uint8_t catchxhi = 0;
//    uint8_t catchylo = 0;
//    uint8_t catchyhi = 0;
//    uint8_t catchzlo = 0;
//    uint8_t catchzhi = 0;
//    float statcheck = 0;

        /* Refresh the watch-dog timer */
  //  while(1){
      // Sys_Watchdog_Refresh();



      // int Demo6 = 1;


//
//	   int bytesRead = 1;
      // uint8_t firstDataReg = 0x32;




//       Sys_GPIO_Set_Low(LED_DIO);
//       Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
//       Sys_GPIO_Set_High(LED_DIO);
//	   readAccel(0x00,bytesRead);

//       if (Demo6==1) {
//    	   int bytesRead2 = 6;
//    	   catchzhi = readAccel(firstDataReg,bytesRead2);
//       }
//       else {
//    	   int bytesRead3 = 2;
//    	   catchxhi = readAccel(firstDataReg,bytesRead3);
//       }




   // }
//}

