/**
  * @brief  This function handles TIM4 global interrupt request.
  * @param  None
  * @retval None
  */
void TIM4_IRQHandler(void)
{
#if 0
   uint8_t clickreg = 0;
  if (AudioPlayStart != 0x00)
  {
    /* Read click status register */
    LIS302DL_Read(&clickreg, LIS302DL_CLICK_SRC_REG_ADDR, 1); 
    LIS302DL_Read(Buffer, LIS302DL_STATUS_REG_ADDR, 6);
  }

  /* Checks whether the TIM interrupt has occurred */
  if (TIM_GetITStatus(TIM4, TIM_IT_CC1) != RESET)
  {
    TIM_ClearITPendingBit(TIM4, TIM_IT_CC1);
    if( LED_Toggle == 3)
    {
      /* LED3 Orange toggling */
      STM_EVAL_LEDToggle(LED3);
      STM_EVAL_LEDOff(LED6);
      STM_EVAL_LEDOff(LED4);
    }
    else if( LED_Toggle == 4)
    {
      /* LED4 Green toggling */
      STM_EVAL_LEDToggle(LED4);
      STM_EVAL_LEDOff(LED6);
      STM_EVAL_LEDOff(LED3);
    }
    else if( LED_Toggle == 6)
    {
      /* LED6 Blue toggling */
      STM_EVAL_LEDOff(LED3);
      STM_EVAL_LEDOff(LED4);
      STM_EVAL_LEDToggle(LED6);
    }
    else if (LED_Toggle ==0)
    {
      /* LED6 Blue On to signal Pause */
      STM_EVAL_LEDOn(LED6);
    }
    else if (LED_Toggle == 7)
    {
      /* LED4 toggling with frequency = 439.4 Hz */
      STM_EVAL_LEDOff(LED3);
      STM_EVAL_LEDOff(LED4);
      STM_EVAL_LEDOff(LED5);
      STM_EVAL_LEDOff(LED6);
    }
    capture = TIM_GetCapture1(TIM4);
    TIM_SetCompare1(TIM4, capture + CCR_Val);
  }
#endif
}

/**
 * @brief Reads data from the accelerometer and returns an accel_data.
 *
 * @note	Reads data from the accelerometer and returns three signed
 *				integers contained within an accel_data packet. It starts
 *				reading at address 0x29, which corresponds to Out_X, and
 *				reads the next five registers that represent the three axes.
 *				The results must be casted to a signed integer, and then
 *				multiplied by 18 as each LSB represents 18mgs.
 *
 * @return the pitch and roll in degrees.
 */
angle_data read_Accelerometer(void){
	uint8_t Buffer[5];
	accel_data returned_vals;
	angle_data angle_vals;
	
	//0x29 is the first register for Out_X. Followed
	//by Out_Y and Out_Z, each separated by a "junk" register.
	LIS302DL_Read(Buffer, 0x29, 5);
	returned_vals.x = (int8_t)(Buffer[0]) * 18;
	returned_vals.y = (int8_t)(Buffer[2]) * 18;
	returned_vals.z = (int8_t)(Buffer[4]) * 18;
	
	returned_vals.x += 8.1;
	returned_vals.y += 29.3;
	returned_vals.z += -10.2;
	
	returned_vals.x = filter_Value(&Filter_Structure_X, returned_vals.x);
	returned_vals.y = filter_Value(&Filter_Structure_Y, returned_vals.y);
	returned_vals.z = filter_Value(&Filter_Structure_Z, returned_vals.z);
	
	angle_vals.pitch = (atan(returned_vals.x/sqrt(pow(returned_vals.y,2) + pow(returned_vals.z, 2))))*(180/3.14);
	angle_vals.roll = (atan(returned_vals.y/sqrt(pow(returned_vals.x,2) + pow(returned_vals.z, 2))))*(180/3.14);
	
	return angle_vals;
}

/**
*	@brief handles when the external interrupt happens
*/
void EXTI0_IRQHandler () {
	
	tick  = 1;
	uint8_t crtl;
	LIS302DL_Read(&crtl, LIS302DL_CLICK_SRC_REG_ADDR , 1);
	EXTI_ClearFlag(LIS302DL_SPI_INT1_EXTI_LINE);//reset the interrupt flag
}

/**
  * @brief  This function handles TIM4 global interrupt request.
  * @param  None
  * @retval None
  */
void TIM4_IRQHandler(void)
{
  static __IO uint8_t servo1TurningRight = 1;
  static __IO uint8_t servo2TurningRight = 1;

  int16_t incrementX = 0;
  int16_t incrementY = 0;

  /* TIM_Update */
  if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
  {
    TIM_ClearITPendingBit(TIM4, TIM_IT_Update);

	/* Read MEMS values */
    Buffer[0] = 0;
    Buffer[2] = 0;

    LIS302DL_Read(Buffer, LIS302DL_OUT_X_ADDR, 4);
    /* Remove the offsets values from data */
    Buffer[0] -= XOffset;
    Buffer[2] -= YOffset;

	incrementX = Buffer[0] * LIS302DL_SENSITIVITY_2_3G;
	servo1Pulse = incrementX + 1500;
		
	incrementY = Buffer[2] * LIS302DL_SENSITIVITY_2_3G;
	servo2Pulse = incrementY + 1500;

    TIM_SetCompare1(TIM4, servo1Pulse);
    TIM_SetCompare2(TIM4, servo1Pulse);
    TIM_SetCompare3(TIM4, servo2Pulse);
    TIM_SetCompare4(TIM4, servo2Pulse);
  }

}

int main(void) {
  
  unsigned char welcome_str[] = "xxyyzz\r\n";
  u8 loop = 1;
  
  initPA15();
  init_USART1(BT_BAUD);
  init_LIS302DL();
    
//  setPA15On();
//  togglePA15();

  while(loop){
    //Read and print the accelerometer values
    LIS302DL_Read(Buffer, LIS302DL_OUT_X_ADDR, 6);
    printf("%d, %d ,%d\n",Buffer[0],Buffer[2],Buffer[4]);
    
    //Send data through the bluetooth communication
    UARTSend(welcome_str, sizeof(welcome_str));
    
    //Wait some time befor ending the loop
    Delay(10000000);
  }
  
    /* Disable SPI1 used to drive the MEMS accelerometre */
    SPI_Cmd(LIS302DL_SPI, DISABLE);
  
    /* Disable the UART connection */
    USART_Cmd(USART1, DISABLE);
}

void LIS302DL_Reboot(SPI_TypeDef* spi) {
	uint8_t tmpreg;
	
	// Read the CTRL_REG2, enable the reboot memory flag and write it back
	LIS302DL_Read(spi, &tmpreg, LIS302DL_CTRL_REG2_ADDR, 1);
	tmpreg |= (uint8_t)LIS302DL_BIT1;
	LIS302DL_Write(spi, &tmpreg, LIS302DL_CTRL_REG2_ADDR, 1);
}

void LIS302DL_ReadACCY(int32_t* out) {
    u8 buffer[6];
    LIS302DL_Read(0x2B, buffer, 6);

    for(int i=0; i<3; i++) {
      *out =(int32_t)(72 * (int8_t)buffer[2*i]);
      out++;
    }
}

void acceleration_task() {
	vTaskDelay(60);
	uint8_t buffer[6];
	LIS302DL_Read(buffer, LIS302DL_OUT_X_ADDR, 6);
	for (;;) {
		signed char result;
		xQueueReceive(queue, &result, portMAX_DELAY);
		display_bar(-128, 128, result);
	}
}

void LIS302DL_GetGlobalInterruptConfiguration(SPI_TypeDef* spi, LIS302DL_GlobalInterruptConfig* pConfig) {
	uint8_t cnf = 0x00;
	LIS302DL_Read(&cnf, LIS302DL_CTRL_REG2_ADDR, 1);
	
	// Map received data into the configuration; Data is delivered in BigEndian
	pConfig->SendData = cnf & (uint8_t)LIS302DL_BIT3 ? 0x01 : 0x00;
	pConfig->Interrupt_1 = cnf & (uint8_t)LIS302DL_BIT4 ? 0x01 : 0x00;
	pConfig->Interrupt_2 = cnf & (uint8_t)LIS302DL_BIT5 ? 0x01 : 0x00;
	pConfig->CutOffFrequency = cnf & (uint8_t)~(LIS302DL_BIT0 | LIS302DL_BIT1 | LIS302DL_BIT2 | LIS302DL_BIT3 | LIS302DL_BIT4 | LIS302DL_BIT5);
}

void LIS302D_ChangePowerControl(SPI_TypeDef* spi, bool enableActiveMode) {
	uint8_t tmpreg;
	
	// Read the CTRL_REG1, enable/disable the PD memory flag and write it back
	LIS302DL_Read(spi, &tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);
	tmpreg &= (uint8_t)~LIS302DL_BIT1; // Unset the FS bit
	if (enableActiveMode) { // If the bit is set, we are in active mode, else in power down control
		tmpreg |= (uint8_t)LIS302DL_BIT1;
	}
	LIS302DL_Write(spi, &tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);
}

void LIS302D_ChangeDataRate(SPI_TypeDef* spi, bool enableHighSpeed) {
	uint8_t tmpreg;
	
	// Read the CTRL_REG1, enable/disable the DR memory flag and write it back
	LIS302DL_Read(spi, &tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);
	tmpreg &= (uint8_t)~LIS302DL_BIT0; // Unset the FS bit
	if (!enableHighSpeed) { // If the bit is set, we are on 400Hz, otherwise on 100Hz
		tmpreg |= (uint8_t)LIS302DL_BIT0;
	}
	LIS302DL_Write(spi, &tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);
}

void LIS302DL_ChangeScaleMode(SPI_TypeDef* spi, bool fullScaleEnable) {
	uint8_t tmpreg;
	
	// Read the CTRL_REG1, enable/disable the FS memory flag and write it back
	LIS302DL_Read(spi, &tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);
	tmpreg &= (uint8_t)~LIS302DL_BIT2; // Unset the FS bit
	if (!fullScaleEnable) { // If the bit is not set, we are in ±2.3g mode (full scale)
		tmpreg |= (uint8_t)LIS302DL_BIT2;
	}
	LIS302DL_Write(spi, &tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);
}

int main(void) {
    init();

    u8 id;

    LIS302DL_Read(0x0F, &id, 1);

    while(1) {
        loop();
    }
}

/**
 * @brief  Reboot memory content of LIS302DL
 * @param  None
 * @retval None
 */
void LIS302DL_RebootCmd(void) {
    uint8_t tmpreg;
    /* Read CTRL_REG2 register */
    LIS302DL_Read(&tmpreg, LIS302DL_CTRL_REG2_ADDR, 1);

    /* Enable or Disable the reboot memory */
    tmpreg |= LIS302DL_BOOT_REBOOTMEMORY;

    /* Write value to MEMS CTRL_REG2 regsister */
    LIS302DL_Write(&tmpreg, LIS302DL_CTRL_REG2_ADDR, 1);
}

void LIS302DL_InterruptConfig(LIS302DL_InterruptConfigTypeDef *LIS302DL_IntConfigStruct)
{
uint8_t ctrl = 0x00;
/* Read CLICK_CFG register */
LIS302DL_Read(&ctrl, LIS302DL_CLICK_CFG_REG_ADDR, 1);
/* Configure latch Interrupt request, click interrupts and double click interrupts */
ctrl = (uint8_t)(LIS302DL_IntConfigStruct->Latch_Request| \
LIS302DL_IntConfigStruct->SingleClick_Axes | \
LIS302DL_IntConfigStruct->DoubleClick_Axes);
/* Write value to MEMS CLICK_CFG register */
LIS302DL_Write(&ctrl, LIS302DL_CLICK_CFG_REG_ADDR, 1);
}

/**
 * @brief  Data Rate command 
 * @param  DataRateValue: Data rate value
 *   This parameter can be one of the following values:
 *     @arg LIS302DL_DATARATE_100: 100 Hz output data rate 
 *     @arg LIS302DL_DATARATE_400: 400 Hz output data rate    
 * @retval None
 */
void LIS302DL_DataRateCmd(uint8_t DataRateValue) {
    uint8_t tmpreg;

    /* Read CTRL_REG1 register */
    LIS302DL_Read(&tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);

    /* Set new Data rate configuration */
    tmpreg &= (uint8_t) ~LIS302DL_DATARATE_400;
    tmpreg |= DataRateValue;

    /* Write value to MEMS CTRL_REG1 regsister */
    LIS302DL_Write(&tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);
}

/**
 * @brief  Change the Full Scale of LIS302DL
 * @param  FS_value: new full scale value. 
 *   This parameter can be one of the following values:
 *     @arg LIS302DL_FULLSCALE_2_3: +-2.3g
 *     @arg LIS302DL_FULLSCALE_9_2: +-9.2g   
 * @retval None
 */
void LIS302DL_FullScaleCmd(uint8_t FS_value) {
    uint8_t tmpreg;

    /* Read CTRL_REG1 register */
    LIS302DL_Read(&tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);

    /* Set new full scale configuration */
    tmpreg &= (uint8_t) ~LIS302DL_FULLSCALE_9_2;
    tmpreg |= FS_value;

    /* Write value to MEMS CTRL_REG1 regsister */
    LIS302DL_Write(&tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);
}

void LIS302DL_GetWakeupInterruptData(SPI_TypeDef* spi, LIS302DL_WakeupInterruptData* pData, uint8_t intNum) {
	uint8_t tmpreg = 0x00;
	LIS302DL_Read(&tmpreg, ((intNum & LIS302DL_INTERRUPT_NUM_2) ? LIS302DL_FF_WU_SRC2_REG_ADDR : LIS302DL_FF_WU_SRC1_REG_ADDR), 1);
	
	// Map received data into the configuration; Data is delivered in BigEndian
	pData->Active = tmpreg & (uint8_t)LIS302DL_BIT1 ? 0x01 : 0x00;
	pData->Z_High = tmpreg & (uint8_t)LIS302DL_BIT2 ? 0x01 : 0x00;
	pData->Z_Low = tmpreg & (uint8_t)LIS302DL_BIT3 ? 0x01 : 0x00;
	pData->Y_High = tmpreg & (uint8_t)LIS302DL_BIT4 ? 0x01 : 0x00;
	pData->Y_Low = tmpreg & (uint8_t)LIS302DL_BIT5 ? 0x01 : 0x00;
	pData->X_High = tmpreg & (uint8_t)LIS302DL_BIT6 ? 0x01 : 0x00;
	pData->X_Low = tmpreg & (uint8_t)LIS302DL_BIT7 ? 0x01 : 0x00;
}

/**
 * @brief  Change the lowpower mode for LIS302DL
 * @param  LowPowerMode: new state for the lowpower mode.
 *   This parameter can be one of the following values:
 *     @arg LIS302DL_LOWPOWERMODE_POWERDOWN: Power down mode
 *     @arg LIS302DL_LOWPOWERMODE_ACTIVE: Active mode  
 * @retval None
 */
void LIS302DL_LowpowerCmd(uint8_t LowPowerMode) {
    uint8_t tmpreg;

    /* Read CTRL_REG1 register */
    LIS302DL_Read(&tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);

    /* Set new low power mode configuration */
    tmpreg &= (uint8_t) ~LIS302DL_LOWPOWERMODE_ACTIVE;
    tmpreg |= LowPowerMode;

    /* Write value to MEMS CTRL_REG1 regsister */
    LIS302DL_Write(&tmpreg, LIS302DL_CTRL_REG1_ADDR, 1);
}

void LIS302DL_GetClickInterruptConfiguration(SPI_TypeDef* spi, LIS302DL_ClickInterruptConfig* pConfig) {
	uint8_t cnf = 0x00;
	LIS302DL_Read(&cnf, LIS302DL_CLICK_CFG_REG_ADDR, 1);
	
	// Map received data into the configuration; Data is delivered in BigEndian
	pConfig->Latched = cnf & (uint8_t)LIS302DL_BIT1 ? 0x01 : 0x00;
	pConfig->DoubleClick_Z = cnf & (uint8_t)LIS302DL_BIT2 ? 0x01 : 0x00;
	pConfig->SingleClick_Z = cnf & (uint8_t)LIS302DL_BIT3 ? 0x01 : 0x00;
	pConfig->DoubleClick_Y = cnf & (uint8_t)LIS302DL_BIT4 ? 0x01 : 0x00;
	pConfig->SingleClick_Y = cnf & (uint8_t)LIS302DL_BIT5 ? 0x01 : 0x00;
	pConfig->DoubleClick_X = cnf & (uint8_t)LIS302DL_BIT6 ? 0x01 : 0x00;
	pConfig->DoubleClick_X = cnf & (uint8_t)LIS302DL_BIT7 ? 0x01 : 0x00;
}