/**@file
 * @defgroup Accelerometer access through bit-bang SPI bus
 * @{
 * @ingroup nrf_ant_bicycle_power
 *
 * @brief The accelerometer sensor for cadence measurements
 */

#include "nrf_delay.h"
#include "nrf_gpio.h"
#include "spi_master.h"
#include "acc_lis2.h"
#include "nordic_common.h"
#include "app_util_platform.h"

#define ACCHRMODE (0)														// Turn on High Resolution 12bit output normal mode +/-4g
#define HW_SPI

#define ACC_WHO_AM_I 							0x0F
#define ACC_CTRL_REG1 						0x20
#define ACC_CTRL_REG2 						0x21
#define ACC_CTRL_REG3 						0x22
#define ACC_CTRL_REG4 						0x23
#define ACC_CTRL_REG5 						0x24
#define ACC_CTRL_REG6 						0x25
#define ACC_STATUS_REG 						0x27
#define ACC_OUT_X 								0x28
#define ACC_OUT_Y 								0x2A
#define ACC_OUT_Z 								0x2C
#define ACC_FIFO_CTRL_REG 				0x2E
#define ACC_FIFO_SRC_REG 					0x2F


#define ACC_INT1_CFG 							0x30
#define ACC_INT1_SRC 							0x31
#define ACC_INT1_THS 							0x32
#define ACC_INT1_DURATION 				0x33
#define ACC_OUT_TEMP_L 						0x0C
#define ACC_OUT_TEMP_H 						0x0D
#define ACC_TEMP_CFG_REG 					0x1F 

// Accelerometer

#define AINT_PIN									0				// Interrupt pin
#define VCCA           						9    		// Accelerometer power pin
#define SPIS_CSN_PIN							11    	// SPI CSN signal = CS (Enable)
#define SPIS_SCK_PIN							3    		// SPI SCK signal = SPC (Clock)
#define SPIS_MISO_PIN							15     	// SPI MISO signal = SDI (Input)
#define SPIS_MOSI_PIN							12    	// SPI MOSI signal = SDO (Output)



static bool init_done = false;

static uint8_t m_tx_data_spi[5]; /**< SPI master TX buffer. */
static uint8_t m_rx_data_spi[26]; /**< SPI master RX buffer. */
static int16_t count;

static volatile bool m_transfer_completed = false;

/**@brief Handler for SPI1 master events.
 *
 * @param[in] spi_master_evt    SPI master event.
 */
void spi_master_event_handler(spi_master_evt_t spi_master_evt){
	switch(spi_master_evt.evt_type){
		case SPI_MASTER_EVT_TRANSFER_COMPLETED:
			//Transmission done.
            //log("EVENT\n");
            //spi_master_close(SPI_MASTER_0);
			m_transfer_completed = true;
			
        
			//rx_buffer must have received data now.
		break;
		default:
			// no implementation is required for now.
		 
		break;
	}
}

void spia_init(void)
{
	// Turn off accelerometer
	
	nrf_gpio_cfg_output(VCCA);
	nrf_gpio_pin_clear(VCCA);
  nrf_gpio_cfg_input(AINT_PIN, NRF_GPIO_PIN_PULLDOWN);
	// Set up SPI bus
	//Structure for SPI master configuration, initialized by default values.
	
	spi_master_config_t spi_instance =  SPI_MASTER_INIT_DEFAULT;
	
	spi_instance.SPI_Pin_SCK = SPIS_SCK_PIN;	
	spi_instance.SPI_Pin_MOSI = SPIS_MOSI_PIN;
	spi_instance.SPI_Pin_MISO = SPIS_MISO_PIN;
	spi_instance.SPI_Pin_SS = SPIS_CSN_PIN;

	spi_instance.SPI_CONFIG_CPOL = SPI_CONFIG_CPOL_ActiveLow;
	spi_instance.SPI_CONFIG_CPHA = SPI_CONFIG_CPHA_Trailing;

	spi_instance.SPI_CONFIG_ORDER = SPI_CONFIG_ORDER_MsbFirst;
	spi_instance.SPI_Freq = SPI_FREQUENCY_FREQUENCY_K250; // 250 kHz.

	spi_instance.SPI_PriorityIRQ = NRF_APP_PRIORITY_HIGH;	//with softdevice enabled.

	spi_master_open(SPI_MASTER_0, &spi_instance);// init the SPI0 Instance.
	
	//Register SPI master event handler.
  spi_master_evt_handler_reg( SPI_MASTER_0, spi_master_event_handler);


	// Turn on accelerometer
	nrf_gpio_pin_set(VCCA);

    
}

void send_recv_spi0_data(uint8_t rx_bytes, uint8_t tx_bytes){
    
	m_transfer_completed = false;
	//any random data for now for demonstration.
	//m_tx_data_spi[0] = spi0_data;

	uint32_t err_code = spi_master_send_recv(SPI_MASTER_0, m_tx_data_spi, tx_bytes, m_rx_data_spi, rx_bytes);
	if (err_code != NRF_SUCCESS)
		{
				//Data transmission failed.				
		}
	else{
		//UART somethning
        
		//while(!m_transfer_completed);//wait for the data to transfer complete.


			
		}
}


static uint8_t spia_rd8(uint8_t reg)
{	
       // nrf_gpio_pin_clear(SPIS_CSN_PIN); 	// Set CS low
        m_rx_data_spi[0] = 0;
        m_rx_data_spi[1] = 0;
        m_rx_data_spi[2] = 0;
        
        m_tx_data_spi[0] = (0x80 | reg);
        m_tx_data_spi[1] = 0;
        send_recv_spi0_data(2,2);
        while(!m_transfer_completed);//wait for the data to transfer complete.
     //   nrf_gpio_pin_set(SPIS_CSN_PIN); 		// Set CS high
        return m_rx_data_spi[1];
}

static uint16_t spia_rd16(uint8_t reg)
{	

	  // nrf_gpio_pin_clear(SPIS_CSN_PIN); 	// Set CS low
		m_rx_data_spi[0] = 0;
		m_rx_data_spi[1] = 0;
		m_rx_data_spi[2] = 0;
		
		uint16_t rd;
		
		m_tx_data_spi[0] = (0xC0 | reg);
		m_tx_data_spi[1] = 0;
		m_tx_data_spi[2] = 0;
		send_recv_spi0_data(3,3);
		while(!m_transfer_completed);//wait for the data to transfer complete.
		rd = m_rx_data_spi[1] | (m_rx_data_spi[2] << 8); 
	  // nrf_gpio_pin_set(SPIS_CSN_PIN); 		// Set CS high
    return rd;
    
}

static void spia_wr8(uint8_t reg, uint8_t data)
{	
	//nrf_gpio_pin_clear(SPIS_CSN_PIN); 	// Set CS low
	m_tx_data_spi[0] = (0x7F & reg);
	m_tx_data_spi[1] = data;
	send_recv_spi0_data(2,2);
	while(!m_transfer_completed);//wait for the data to transfer complete.
	//nrf_gpio_pin_set(SPIS_CSN_PIN); 		// Set CS high
 
}



/**@brief Function for initializing the module 
 *
 * Sets up the GPIO's (and power cycle the accelerometer)
 */
void acc_init(void)
{
	spia_init();

  nrf_delay_ms(20);
    
	// Wait for boot of the accelerometer (should be ready 5-20ms after power on)

	acc_set_400Hz_lp_mode();
	acc_set_400Hz_lp_mode();
	acc_set_400Hz_lp_mode();
	acc_set_400Hz_lp_mode();
  
	while ((acc_get_status() & 0x08) == 0)	// Magic wait for first data that get the things going
	{
	}

	acc_set_400Hz_lp_mode();
	spia_wr8(ACC_CTRL_REG2, 0x00); 		// Set default values
	spia_wr8(ACC_CTRL_REG3, 0x00); 		// Set default values
		
	#if ACCHRMODE
			spia_wr8(ACC_CTRL_REG4, 0x18); 	// Set default values + 4G + High Resolution mode
	#else
			spia_wr8(ACC_CTRL_REG4, 0x00); 	// Set default values
	#endif
	
spia_wr8(ACC_CTRL_REG5, 0x00); 		// Set default values
	spia_wr8(ACC_CTRL_REG6, 0x00); 		// Set default values
	spia_wr8(ACC_INT1_CFG,  0x00); 		// Set default values
	//spia_wr8(FIFO_CTRL_REG, 0x00); 		// Set default values
	spia_wr8(ACC_CTRL_REG5, 0x40); 		// Set FIFO enable 
	spia_wr8(ACC_FIFO_CTRL_REG, 0x80);// Set FIFO stream mode
  
	init_done = true;
 
}

#if 0
/**@brief Function for setting up the accelerometer to generate interrupt at 100Hz with 400Hz sampling
 *
 * Init the LIS2DH acc chip with 400Hz and FIFO
 */
void xacc_init_fifo_400Hz(void)
{
    while(!m_transfer_completed);//wait for the data to transfer complete.
	spia_init();
	
	// Wait for boot of the accelerometer (should be ready 5ms after power on)
	acc_set_400Hz_lp_mode();
	acc_set_400Hz_lp_mode();
	acc_set_400Hz_lp_mode();
	acc_set_400Hz_lp_mode();
	acc_set_400Hz_lp_mode();
	while ((acc_get_status() & 0x08) == 0)	// Magic wait for first data that get the things going
		;
	acc_set_400Hz_lp_mode();

	#if ACCHRMODE
		spia_wr8(ACC_CTRL_REG1, 0x77); // CTRL_REG1 set 400Hz + normal mode + X+Y+Zen
		//spia_wr8(ACC_CTRL_REG1, 0x57); // CTRL_REG1 set 100Hz + normal mode + X+Y+Zen
	#else
		spia_wr8(ACC_CTRL_REG1, 0x7F); // CTRL_REG1 set LR/400Hz + LP mode + X+Y+Zen
	#endif

	spia_wr8(ACC_CTRL_REG2, 0x00); 		// Set default values
	spia_wr8(ACC_CTRL_REG3, 0x04); 		// Set FIFO watermark interrupt
//spia_wr8(ACC_CTRL_REG3, 0x02); 		// Set FIFO overrun interrupt
	#if ACCHRMODE
		spia_wr8(ACC_CTRL_REG4, 0x18); 	// Set default values + 4G + High Resolution mode
		//spia_wr8(ACC_CTRL_REG4, 0x98); 	// Set default values + 4G + High Resolution mode + BDU
	#else
		spia_wr8(ACC_CTRL_REG4, 0x00); 	// Set default values
	#endif
	spia_wr8(ACC_CTRL_REG5, 0x08); 		// Set latch on INT1

	spia_wr8(ACC_CTRL_REG6, 0x40); 		// Set INT1 enable
	spia_wr8(ACC_INT1_CFG,  0x00); 		// Set default values

	// Set up FIFO
	spia_wr8(ACC_CTRL_REG5, 0x48); 		// Set FIFO enable + Latch INT1
	spia_wr8(ACC_FIFO_CTRL_REG, 0x84); // Set FIFO stream mode + Threshold 4 (watermark)
//spia_wr8(FIFO_CTRL_REG, 0x44); 		// Set FIFO mode + Trigger on INT1 + Threshold 4 (watermark)
//spia_wr8(FIFO_CTRL_REG, 0x40); 		// Set FIFO mode + Trigger on INT1
}
#endif

/**@brief Function for setting up the accelerometer to generate interrupt when moved (rotated)
 *
 * Initialization
 */
void acc_init_w_int(bool up, uint8_t x, uint8_t y, uint8_t z, uint8_t ig_cfg1)
{
	acc_init();														// Power cycle and basic setup
	
	spia_wr8(ACC_CTRL_REG3, 0x40); 			// Set I1_AOI1 to route interrupt on INT1 pin
	// spia_wr8(ACC_CTRL_REG5, 0x0C); 	// Set default values + INT1 latch enable + 4D
	// spia_wr8(ACC_CTRL_REG5, 0x04); 	// Set default values + 4D
	// spia_wr8(ACC_CTRL_REG4, 0x80); 	// Set default values + BDU ??
	spia_wr8(ACC_CTRL_REG4, 0x00); 			// Set default values

	// spia_wr8(ACC_CTRL_REG5, 0x00); 	// Set default values
	spia_wr8(ACC_CTRL_REG5, 0x08); 			// Set default values + INT1 latch enable
	spia_wr8(ACC_CTRL_REG6, 0x40); 			// Set default values + INT1 enable
	spia_wr8(ACC_FIFO_CTRL_REG, 0x00); 	// Set default values

	spia_wr8(ACC_INT1_THS, 32); 				// Set high/low threshold (32*16mg ~0.5g)
	spia_wr8(ACC_INT1_DURATION, 2); 		// Set duration for interrupt (10 => 10/100 = 1/10s)

	#if defined(XACCISZ)
		// spia_wr8(ACC_INT1_CFG, 0x50); 	// Set 6D movement detection "01" + ZLO-event
		// spia_wr8(ACC_INT1_CFG, 0x20); 	
		if (up) 
		{
			spia_wr8(ACC_INT1_CFG, 0x60); 	// Set 6D movement detection "01" + ZHI-event
		} else {
			spia_wr8(ACC_INT1_CFG, 0x50); 	// Set 6D movement detection "01" + ZLO-event 	
		}
	#else
		spia_wr8(ACC_INT1_CFG, 0x41); 		// Set 6D movement detection "01" + XLO-event
	#endif

}	

int16_t acc_get_x(void)
{
	return (int16_t) spia_rd16(ACC_OUT_X);
}


int8_t acc_get_xyzreg(uint8_t reg)
{
	#if ACCHRMODE
		int16_t acc;
	
			acc = (int16_t)spia_rd16(reg);
			acc = acc >> 8;																	// Switch from 12bit (high adjusted) to 8bit (1g = 32)
			return (int8_t) acc;
	#else
		return (int8_t) spia_rd8(reg+1);
	#endif
}



/**@brief Function for reading X/Y/Z from accelerometer
 *
 * start the Reading of 4 values in 12bit mode from the FIFO and averages them
 *
 */
void acc_start_xyz(void){
	
	count = (acc_get_fifo_src() & 0x1F);	// Find how many values to read
    if (count > 4) count = 4;

	if (count == 0)
		return; 														// Return if nothing to do

	//nrf_gpio_pin_clear(SPIS_CSN_PIN); 		// Set CS low
	
  m_tx_data_spi[0] = (0xC0 | ACC_OUT_X);  // Read + increment

  send_recv_spi0_data((count*6)+1, 1);
}

/**@brief Function for reading X/Y/Z from accelerometer
 *
 * Reads all available values in 12/14bit mode from the FIFO and averages them
 *
 * @return Average of x, y, z values (32 = 1G)
 */
uint8_t acc_get_xyz(int16_t *x, int16_t *y, int16_t *z)
{
	uint16_t rd, i;
	int16_t x1, y1, z1;

	
	if (!init_done) 
	{
		*x = -77;
		*y = -77;
		*z = -77;
		return 0; 														// Return if not ready yet
	}

	x1 = 0;
	y1 = 0;
	z1 = 0;

    if(m_transfer_completed == true){
        for (i = 0; i<count; i++)
        {
            rd = m_rx_data_spi[(i*6)+1];								// Read low byte
            rd = rd | (m_rx_data_spi[(i*6)+2] << 8);		// Read high byte
            x1 += ((int16_t)rd) >> 4;

            rd = m_rx_data_spi[(i*6)+3]; 								// Read low byte
            rd = rd | (m_rx_data_spi[(i*6)+4] << 8);		// Read high byte
            y1 += ((int16_t)rd) >> 4;

            rd = m_rx_data_spi[(i*6)+5]; 								// Read low byte
            rd = rd | (m_rx_data_spi[(i*6)+6] << 8);		// Read high byte
            z1 += ((int16_t)rd) >> 4;
        }

        *x = (x1 / count + 8) >> 4;						// Average and scale/round to 1G = 32
        *y = (y1 / count + 8) >> 4;
        *z = (z1 / count + 8) >> 4;
        
        
        //log("Count: %d\n", count);
        return 1;
        
    } else return 0;
}


/**@brief Function for reading X/Y/Z from accelerometer
 *
 * Reads all available values in 12bit mode from the FIFO return a 8 bit values
 *
 * @return x, y, z values (32 = 1G)
 */
void acc_get_xyz8(int8_t *x, int8_t *y, int8_t *z)
{
	int16_t x16, y16, z16;
	
	x16 = spia_rd16(ACC_OUT_X);
	x16 = x16>>8;
	y16 = spia_rd16(ACC_OUT_Y);
	y16 = y16>>8;
	z16 = spia_rd16(ACC_OUT_Z);
	z16 = z16>>8;
	
	*x = (int8_t)x16;
	*y = (int8_t)y16;
	*z = (int8_t)z16;
}

/**@brief Function for reading X+Y from accelerometer
 *
 * Reads one set of values in 8bit mode
 *
 * @return x, y values (32 = 1G)
 */
void acc_get_xy(int8_t *x, int8_t *y)
{
	int8_t x1, y1, z1;
	
	x1 = spia_rd8(ACC_OUT_X);
	x1 = spia_rd8(ACC_OUT_X+1);
	y1 = spia_rd8(ACC_OUT_Y);
	y1 = spia_rd8(ACC_OUT_Y+1);
	z1 = spia_rd8(ACC_OUT_Z);
	z1 = spia_rd8(ACC_OUT_Z+1);
	
	#if defined(XACCISZ)
		*x = z1;
		*y = y1;
		UNUSED_VARIABLE(x1);
	#else
		*x = x1;
		#if defined(YACCISZ)
			*y = z1;
			UNUSED_VARIABLE(y1);
		#else
			*y = y1;
			UNUSED_VARIABLE(z1);
		#endif
	#endif
}

/**@brief Set up accelerometer for 100Hz low power 8 bit updates
 *
 * Enable X + Y (but may use Z as X/Y for those hardware orientations)
 */
void acc_set_100Hz_lp_mode_xy(void)
{
	spia_wr8(ACC_CTRL_REG1, 0x5B); // CTRL_REG1 set LR/100Hz + LP mode + X+Yen
}

void acc_set_100Hz_lp_mode_xz(void)
{
	spia_wr8(ACC_CTRL_REG1, 0x5D); // CTRL_REG1 set LR/100Hz + LP mode + X+Zen
}

void acc_set_100Hz_lp_mode_yz(void)
{
	spia_wr8(ACC_CTRL_REG1, 0x5E); // CTRL_REG1 set LR/100Hz + LP mode + Y+Zen
}

/**@brief Set up accelerometer for 100Hz low power 8 bit updates
 *
 * Enable X + Y (but may use Z as X/Y for those hardware orientations)
 */
void acc_set_100Hz_lp_mode(void)
{
	#if defined(XACCISZ)
		acc_set_100Hz_lp_mode_yz();
	#else
		#if defined(YACCISZ)
			acc_set_100Hz_lp_mode_xz();
		#else
			acc_set_100Hz_lp_mode_xy();
		#endif
	#endif
}
	
void acc_set_400Hz_lp_mode_xy(void)
{
	#if ACCHRMODE
		spia_wr8(ACC_CTRL_REG1, 0x73); // CTRL_REG1 set 400Hz + normal mode + X+Yen
	#else
		spia_wr8(ACC_CTRL_REG1, 0x7B); // CTRL_REG1 set LR/400Hz + LP mode + X+Yen
	#endif
}

void acc_set_400Hz_lp_mode_xz(void)
{
	#if ACCHRMODE
		spia_wr8(ACC_CTRL_REG1, 0x75); // CTRL_REG1 set 400Hz + normal mode + X+Zen
	#else
		spia_wr8(ACC_CTRL_REG1, 0x7D); // CTRL_REG1 set LR/400Hz + LP mode + X+Zen
	#endif
}

void acc_set_400Hz_lp_mode_yz(void)
{
	#if ACCHRMODE
		spia_wr8(ACC_CTRL_REG1, 0x76); // CTRL_REG1 set 400Hz + normal mode + Y+Zen
	#else
		spia_wr8(ACC_CTRL_REG1, 0x7E); // CTRL_REG1 set LR/400Hz + LP mode + Y+Zen
	#endif
}

/**@brief Set up accelerometer for 400Hz low power 8 bit updates
 *
 * Enable X + Y (but may use Z as X/Y for those hardware orientations)
 */
void acc_set_400Hz_lp_mode(void)
{
	/*
	#if defined(XACCISZ)
		acc_set_400Hz_lp_mode_yz();
	#else
		#if defined(YACCISZ)
			acc_set_400Hz_lp_mode_xz();
		#else
			acc_set_400Hz_lp_mode_xy();
		#endif
	#endif
	*/
	spia_wr8(ACC_CTRL_REG1, 0x77); // CTRL_REG1 set 400Hz + normal mode + X+Y+Zen

}
	
/**@brief Function for reading X-acceleration
 *
 * @return X-acceleration (8 bit)
 */
int8_t acc_get_x8(void)
{
	#if ACCHRMODE
		int16_t acc;
	
		#if defined(XACCISZ)
			acc = (int16_t)spia_rd16(ACC_OUT_Z);
			acc = acc >> 8;																	// Switch from 12bit (high adjusted) to 8bit (1g = 32)
			return (int8_t) acc;
		#else
			acc = spia_rd16(ACC_OUT_X);
			acc = acc >> 8;																	// Switch from 12bit (high adjusted) to 8bit (1g = 32)
			return (int8_t) acc;
		#endif
	#else
		#if defined(XACCISZ)
			return (int8_t) spia_rd8(ACC_OUT_Z+1);
		#else
			return (int8_t) spia_rd8(ACC_OUT_X+1);
		#endif
	#endif
}

/**@brief Function for reading Y-acceleration
 *
 * @return Y-acceleration (8 bit)
 */
int8_t acc_get_y8(void)
{
	#if ACCHRMODE
		int16_t acc;
	
		#if defined(YACCISZ)
			acc = spia_rd16(ACC_OUT_Z);
			acc = acc >> 8;																	// Switch from 12bit (high adjusted) to 8bit (1g = 32)
			return (int8_t) acc;
		#else
			acc = spia_rd16(ACC_OUT_Y);
			acc = acc >> 8;																	// Switch from 12bit (high adjusted) to 8bit (1g = 32)
			return (int8_t) acc;
		#endif
	#else
		#if defined(YACCISZ)
			return (int8_t) spia_rd8(ACC_OUT_Z+1);
		#else
			return (int8_t) spia_rd8(ACC_OUT_Y+1);
		#endif
	#endif
}

/**@brief Function for reading Z-acceleration
 *
 * @return Z-acceleration (8 bit)
 */
int8_t acc_get_z8(void)
{
	#if ACCHRMODE
		int16_t acc;
	
		acc = spia_rd16(ACC_OUT_Z);
		acc = acc >> 8;																	// Switch from 12bit (high adjusted) to 8bit (1g = 32)
		return (int8_t) acc;
	#else
		return (int8_t) spia_rd8(ACC_OUT_Z+1);
	#endif
}

/**@brief Function for clearing the FIFO
 *
 * Empty the FIFO
 */
void acc_reset_fifo(void)
{
	spia_wr8(ACC_FIFO_CTRL_REG, 0x04); 		// Set Bypass mode + Trigger on INT1 + Threshold 4 (watermark)
	spia_wr8(ACC_FIFO_CTRL_REG, 0x44); 		// Set FIFO mode + Trigger on INT1 + Threshold 4 (watermark)
}

/**@brief Function for reading status register in accelerometer
 *
 * @return Status register
 */
int8_t acc_get_status(void)
{
	return (int8_t) spia_rd8(ACC_STATUS_REG);
}

int8_t acc_get_who_am_i(void)
{
	return (int8_t) spia_rd8(ACC_WHO_AM_I);
}





/**@brief Function for reading interrupt 1 source register in accelerometer
 *
 * Read interrupt source (and clear interrupt)
 *
 * @return INT1_SRC register
 */
int8_t acc_get_int1_src(void)
{
		return (int8_t) spia_rd8(ACC_INT1_SRC);
}

/**@brief Function for reading interrupt 1 configuration register in accelerometer
 *
 * @return INT1_CFG register
 */
int8_t acc_get_int1_cfg(void)
{
		return (int8_t) spia_rd8(ACC_INT1_CFG);
}

/**@brief Function for reading the FIFO interrupt source register in accelerometer
 *
 * @return FIFO_SRC_REG register
 */
int8_t acc_get_fifo_src(void)
{
	return (int8_t) spia_rd8(ACC_FIFO_SRC_REG);
}

/**@brief Function for reading the FIFO watermark flag in accelerometer
 *
 * @return True if the FIFO watermark flag is set
 */
bool acc_get_fifo_wtm(void)
{
	return ((spia_rd8(ACC_FIFO_SRC_REG) & 0x80) != 0);
}

