#include "i2c_control.h"

#include "nrf_drv_gpiote.h"
#include "sdk_macros.h"
#include "boards.h"
#include "common_decl.h"
#include "timer_user.h"

extern uint32_t system_sleep_counter; /**< System sleep counter used in main for power managment */

// Accl related flags
uint8_t accl_int1_flag = RESET; /**< Accl intterupt flag */
uint8_t accl_read_flag = RESET; /**< Flag to tell it's time to read Accl */


/**@brief Accelerometer Interupt handler.
 *
 * @details This gets called when accelerometer raises hardware interrupt
 * 
 * @param pin Which pin raised interrupr
 * 
 * @param action Polarity of the Pin
 */
void acclIntHandler (nrf_drv_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
{
	// Clear interrupt flags
	NRF_GPIOTE->EVENTS_PORT = RESET;
	NRF_GPIOTE->EVENTS_IN[0] = RESET;
	NRF_GPIOTE->EVENTS_IN[1] = RESET;
	NRF_GPIOTE->EVENTS_IN[2] = RESET;
	NRF_GPIOTE->EVENTS_IN[3] = RESET;

	accl_int1_flag = SET;
	return;
}


/**@brief Chec if motion happening in Accelerometer
 *
 * @details Reads Accelerometer and gets the flags in Accelerometer 
 *					which tells wether acceleration is crossing threhhold
 * 
 * @return 1: Acceleration is crossing threhhold, 0: Acceleration not crossing threhhold
 * 
 */
uint8_t accl_check_motion_happening (void)
{
	uint8_t sub_addr_data[2];
	sub_addr_data[0] = 0x16;
	/*ret_code_t twi_write_read(uint8_t addr, uint8_t subaddr[], 
															uint8_t subaddr_len, uint8_t data_read[], 
															uint8_t data_len)
	*/
	twi_write_read (ACCL_SLAVE_ADDR, &sub_addr_data[0], 1, &sub_addr_data[1], 1);
	return (sub_addr_data[1] & 0x80);
}


/**@brief Watch Accelerometer
 *
 * @details Controls the main program by using *system_sleep_counter*
 *					according to status of *accl_int1_flag* *accl_read_flag* and *system_sleep_counter*
 * 
 */
void watch_accl (void)
{
	/* 
		When system_sleep_counter is 0, and interrupt is rasied by accelerometer
		system_sleep_counter will be set to SYSTEM_UP_DURATION which will wake up 
		the system. If system_sleep_counter goes to 0 the system goes to sleep
	*/
	if (accl_int1_flag && (!system_sleep_counter))
	{
		accl_int1_flag = RESET;
		system_sleep_counter = SYSTEM_UP_DURATION;
	}
	/*
		When is wakeup mode system_sleep_counter will be non zero.
		In wakeup mode at every ACCL_READ_FREQ the accelerometer's register will be read
		and if motion is going on, the system_sleep_counter will be set to SYSTEM_UP_DURATION
	*/
	if (system_sleep_counter)
	{
		if ((accl_read_flag) && (accl_check_motion_happening()))
		{
			accl_read_flag = RESET;
			system_sleep_counter = SYSTEM_UP_DURATION;
		}
	}
	return;
}


/**@brief Enable external interupt used by Accelerometer
 *
 * @details Enable external interupt used by Accelerometer and the callback function
 *
 * @return 32-bit success/error code
 * 
 */
uint32_t enableAcclExtInt (void)
{
	uint32_t err_code;
	// Configure the device INT1 Pin Input as Interrupt
	nrf_drv_gpiote_init ();
	nrf_drv_gpiote_in_config_t config = GPIOTE_RAW_CONFIG_IN_SENSE_HITOLO(false);
	config.pull = NRF_GPIO_PIN_PULLUP;
	config.skip_gpio_setup = false;
	err_code = nrf_drv_gpiote_in_init(ACCL_INT_1_PIN, &config, acclIntHandler);
	VERIFY_SUCCESS(err_code);
	nrf_drv_gpiote_in_event_enable (ACCL_INT_1_PIN, true);
	return err_code;
}


/**@brief Enable motion detection in Accelerometer
 *
 * @details Enable motion detection and configure Accelerometer for power saving 
 *					by wrting into it's registers over I2C.
 *
 * @return 32-bit success/error code
 * 
 */
uint32_t enableAcclMOtionDetection (void)
{
	uint8_t send_array [3];
	uint32_t err_code;
	
	// Put Module in Standby mode do the settings
	// And set Data Rate and auto_sleep to 12.5 Hz
	// Auto Wakeup Sleep frrequency to 12.5 Hz
	// Normal mode for LNOis and Fast Read
	send_array[0] = 0x2A; // Register adress CTRL_REG1
	send_array[1] = 0xF8; // Data to be written in the register
	err_code = twi_write (ACCL_SLAVE_ADDR, send_array, 2);
	VERIFY_SUCCESS(err_code);
	
	
	// Auto Sleep counter
	send_array[0] = 0x29; // Register adress FF_MT_CFG
	send_array[1] = 0x00; // Data to be written in the register
	err_code = twi_write (ACCL_SLAVE_ADDR, send_array, 2);
	VERIFY_SUCCESS(err_code);
	
	
	// Setting to detect motion, X and Y axis will be considered
	// only X and Y is considered beacause Z will always be 1g
	// and status registers will reset after event is over
	send_array[0] = 0x15; // Register adress FF_MT_CFG
	send_array[1] = 0xD8; // Data to be written in the register
	err_code = twi_write (ACCL_SLAVE_ADDR, send_array, 2);
	VERIFY_SUCCESS(err_code);
	
	// Setting to set threshhold to 1.25g 
	// The step count is 0.063g/ count• 1.25/0.063g = 19.4; //Round up to 20
	// DBCNTM bit is a logic ‘1’, the debounce counter is cleared to 0 
	// whenever the inertial event of interest is no longer true
	send_array[0] = 0x17; // Register adress FF_MT_THS
	send_array[1] = 0x82; // Data to be written in the register
	err_code = twi_write (ACCL_SLAVE_ADDR, send_array, 2);
	VERIFY_SUCCESS(err_code);
	
	// Debouce counter to avoid false reading
	// Set to 10 Counts
	send_array[0] = 0x18; // Register adress FF_MT_COUNT
	send_array[1] = 0x0A; // Data to be written in the register
	err_code = twi_write (ACCL_SLAVE_ADDR, send_array, 2);
	VERIFY_SUCCESS(err_code);
	
	// Enable Motion Interrupt Function in the System
	send_array[0] = 0x2D; // Register adress CTRL_REG4
	send_array[1] = 0x04; // Data to be written in the register
	err_code = twi_write (ACCL_SLAVE_ADDR, send_array, 2);
	VERIFY_SUCCESS(err_code);
	
	// Route the Motion Interrupt Function to INT1 hardware pin
	send_array[0] = 0x2E; // Register adress CTRL_REG5
	send_array[1] = 0x04; // Data to be written in the register
	err_code = twi_write (ACCL_SLAVE_ADDR, send_array, 2);
	VERIFY_SUCCESS(err_code);
		
	// Self test disabled, Reset at 0 
	// Sleep mode and normal mode power profile is se to Low power
	// Auto sleep enabled
	send_array[0] = 0x2B; // Register adress CTRL_REG2
	send_array[1] = 0x1F; // Data to be written in the register
	err_code = twi_write (ACCL_SLAVE_ADDR, send_array, 2);
	VERIFY_SUCCESS(err_code);
	
	// Activate the device aftr settings are done
	// And set Data Rate to 12.5 Hz
	// Auto Wakeup Sleep frrequency to 12.5 Hz
	// Normal mode for LNOis and Fast Read
	send_array[0] = 0x2A; // Register adress CTRL_REG1
	send_array[1] = 0xF9; // Data to be written in the register
	err_code = twi_write (ACCL_SLAVE_ADDR, send_array, 2);
	VERIFY_SUCCESS(err_code);
	
	err_code = enableAcclExtInt ();
	
	return err_code;
}