uint8_t PCA9536_REG_NX_WRITE(uint8_t bit, NX_VALUE_TypeDef value)
{
  uint8_t tmp = 0;   
  
  /* Get all the Pins direction */
  tmp = I2C_ReadDeviceRegister(PCA9536_ADDRESS, PCA9536_REG_NX);
  
  if (value != INVERTED)
  {
    tmp |= (uint8_t)bit;
  }  
  else 
  {
    tmp &= ~(uint8_t)bit;
  }
  
  /* Write the register new value */
  I2C_WriteDeviceRegister(PCA9536_ADDRESS, PCA9536_REG_NX, tmp);
  
  /* If all OK return OK */
  return OK;      
}

/**
  * @brief  Configures the selected pin direction (to be an input or an output)
  * @param  DeviceAddr: The address of the IOExpander, could be : IOE_1_ADDR
  *         or IOE_2_ADDR.
  * @param  IO_Pin: IO_Pin_x: Where x can be from 0 to 7.   
  * @param  Direction: could be Direction_IN or Direction_OUT.      
  * @retval IOE_OK: if all initializations are OK. Other value if error.
  */
uint8_t IOE_IOPinConfig(uint8_t DeviceAddr, uint8_t IO_Pin, uint8_t Direction)
{
  uint8_t tmp = 0;   
  
  /* Get all the Pins direction */
  tmp = I2C_ReadDeviceRegister(DeviceAddr, IOE_REG_GPIO_DIR);
  
  if (Direction != Direction_IN)
  {
    tmp |= (uint8_t)IO_Pin;
  }  
  else 
  {
    tmp &= ~(uint8_t)IO_Pin;
  }
  
  /* Write the register new value */
  I2C_WriteDeviceRegister(DeviceAddr, IOE_REG_GPIO_DIR, tmp);
  
  /* If all OK return IOE_OK */
  return IOE_OK;      
}

/**
  * @brief  Configures the selected pins to generate an interrupt or not.
  * @param  DeviceAddr: The address of the IOExpander, could be : IOE_1_ADDR
  *         or IOE_2_ADDR.
  * @param  IO_IT: The IO interrupt to be configured. This parameter could be any
  *         combination of the following values:
  *   @arg  IO_IT_x: where x can be from 0 to 7.
  * @param  NewState: could be ENABLE or DISABLE.
  * @retval IOE_OK: if all initializations are OK. Other value if error.
  */
uint8_t IOE_IOITConfig(uint8_t DeviceAddr, uint8_t IO_IT, FunctionalState NewState)
{
  uint8_t tmp = 0;

  tmp = I2C_ReadDeviceRegister(DeviceAddr, IOE_REG_GPIO_INT_EN);

  if (NewState != DISABLE)
  {
    /* Set the interrupts to be Enabled */
    tmp |= (uint8_t)IO_IT;
  }
  else
  {
    /* Set the interrupts to be Disabled */
    tmp &= ~(uint8_t)IO_IT;
  }

  /* Set the register */
  I2C_WriteDeviceRegister(DeviceAddr, IOE_REG_GPIO_INT_EN, tmp);

  /* If all OK return IOE_OK */
  return IOE_OK;
}

/**
  * @brief  Configures the selected source to generate or not a global interrupt
  * @param Global_IT: the interrupt source to be configured, could be:
  *   @arg  Global_IT_ADC : ADC interrupt     
  *   @arg  Global_IT_FE : Touch Panel Controller FIFO Error interrupt
  *   @arg  Global_IT_FF : Touch Panel Controller FIFO Full interrupt      
  *   @arg  Global_IT_FOV : Touch Panel Controller FIFO Overrun interrupt     
  *   @arg  Global_IT_FTH : Touch Panel Controller FIFO Threshold interrupt   
  *   @arg  Global_IT_TOUCH : Touch Panel Controller Touch Detected interrupt 
  * @param  NewState: can be ENABLE pr DISABLE   
  * @retval IOE_OK: if all initializations are OK. Other value if error.
  */
uint8_t IOE_GITConfig(uint8_t Global_IT, FunctionalState NewState)
{
  uint8_t tmp = 0;
  
  /* Get the current value of the INT_EN register */
  tmp = I2C_ReadDeviceRegister(IOE_REG_INT_EN);
  
  if (NewState != DISABLE)
  {
    /* Set the interrupts to be Enabled */    
    tmp |= (uint8_t)Global_IT;  
  }
  else
  {
    /* Set the interrupts to be Disabled */    
    tmp &= ~(uint8_t)Global_IT;
  }
  /* Set the register */
  I2C_WriteDeviceRegister(IOE_REG_INT_EN, tmp);
  
  /* If all OK return IOE_OK */
  return IOE_OK;  
}

/**
  * @brief  Configures the selected pin to be in Alternate function or not
  * @param  DeviceAddr: The address of the IOExpander, could be : IOE_1_ADDR
  *         or IOE_2_ADDR.
  * @param  IO_Pin: IO_Pin_x, Where x can be from 0 to 7.
  * @param  NewState: State of the AF for the selected pin, could be
  *         ENABLE or DISABLE.
  * @retval IOE_OK: if all initializations are OK. Other value if error.
  */
uint8_t IOE_IOAFConfig(uint8_t DeviceAddr, uint8_t IO_Pin, FunctionalState NewState)
{
  uint8_t tmp = 0;

  /* Get the current state of the GPIO_AF register */
  tmp = I2C_ReadDeviceRegister(DeviceAddr, IOE_REG_GPIO_AF);

  if (NewState != DISABLE)
  {
    /* Enable the selected pins alternate function */
    tmp |= (uint8_t)IO_Pin;
  }
  else
  {
    /* Disable the selected pins alternate function */
    tmp &= ~(uint8_t)IO_Pin;
  }

  /* Write back the new valu in GPIO_AF register */
  I2C_WriteDeviceRegister(DeviceAddr, IOE_REG_GPIO_AF, tmp);

  /* If all OK return IOE_OK */
  return IOE_OK;
}

/**
  * @brief  Enables or disables the Global interrupt.
  * @param  DeviceAddr: The address of the IOExpander, could be :I OE_1_ADDR
  *         or IOE_2_ADDR.
  * @param  NewState: could be ENABLE or DISABLE.
  * @retval IOE_OK: if all initializations are OK. Other value if error.
  */
uint8_t IOE_GITCmd(uint8_t DeviceAddr, FunctionalState NewState)
{
  uint8_t tmp = 0;

  /* Read the Interrupt Control register  */
  I2C_ReadDeviceRegister(DeviceAddr, IOE_REG_INT_CTRL);

  if (NewState != DISABLE)
  {
    /* Set the global interrupts to be Enabled */
    tmp |= (uint8_t)IOE_GIT_EN;
  }
  else
  {
    /* Set the global interrupts to be Disabled */
    tmp &= ~(uint8_t)IOE_GIT_EN;
  }

  /* Write Back the Interrupt Control register */
  I2C_WriteDeviceRegister(DeviceAddr, IOE_REG_INT_CTRL, tmp);

  /* If all OK return IOE_OK */
  return IOE_OK;
}

/**
  * @brief  Configures the selcted IO Expander functionalities.
  * @param  DeviceAddr: The address of the IOExpander, could be : IOE_1_ADDR
  *         or IOE_2_ADDR.
  * @param  IOE_TEMPSENS_FCT: the functions to be configured. could be any
  *         combination of the following values:
  *   @arg  IOE_IO_FCT : IO function
  *   @arg  IOE_TS_FCT : Touch Screen function
  *   @arg  IOE_ADC_FCT : ADC function
  *   @arg  IOE_TEMPSENS_FCT : Tempreature Sensor function
  * @retval IOE_OK: if all initializations are OK. Other value if error.
  */
uint8_t IOE_FnctCmd(uint8_t DeviceAddr, uint8_t Fct, FunctionalState NewState)
{
  uint8_t tmp = 0;

  /* Get the register value */
  tmp = I2C_ReadDeviceRegister(DeviceAddr, IOE_REG_SYS_CTRL2);

  if (NewState != DISABLE)
  {
    /* Set the Functionalities to be Enabled */
    tmp &= ~(uint8_t)Fct;
  }
  else
  {
    /* Set the Functionalities to be Disabled */
    tmp |= (uint8_t)Fct;
  }

  /* Set the register value */
  I2C_WriteDeviceRegister(DeviceAddr, IOE_REG_SYS_CTRL2, tmp);

  /* If all OK return IOE_OK */
  return IOE_OK;
}

/**
  * @brief  Configures the touch Screen Controller (Single point detection)
  * @param  None
  * @retval IOE_OK if all initializations are OK. Other value if error.
  */
uint8_t IOE_TS_Config(void)
{
  uint8_t tmp = 0;

  /* Enable TSC Fct: already done in IOE_Config */
  tmp = I2C_ReadDeviceRegister(IOE_1_ADDR, IOE_REG_SYS_CTRL2);
  tmp &= ~(uint32_t)(IOE_TS_FCT | IOE_ADC_FCT);
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_SYS_CTRL2, tmp);

  /* Enable the TSC gloabl interrupts */
  tmp = I2C_ReadDeviceRegister(IOE_1_ADDR, IOE_REG_INT_EN);
  tmp |= (uint32_t)(IOE_GIT_TOUCH | IOE_GIT_FTH | IOE_GIT_FOV);
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_INT_EN, tmp);

  /* Select Sample Time, bit number and ADC Reference */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_ADC_CTRL1, 0x49);

  /* Wait for ~20 ms */
  _delay_(2);

  /* Select the ADC clock speed: 3.25 MHz */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_ADC_CTRL2, 0x01);

  /* Select TSC pins in non default mode */
  tmp = I2C_ReadDeviceRegister(IOE_1_ADDR, IOE_REG_GPIO_AF);
  tmp &= ~(uint8_t)TOUCH_IO_ALL;
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_GPIO_AF, tmp);

  /* Select 2 nF filter capacitor */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_TSC_CFG, 0x9A);

  /* Select single point reading  */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_FIFO_TH, 0x01);

  /* Write 0x01 to clear the FIFO memory content. */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_FIFO_STA, 0x01);

  /* Write 0x00 to put the FIFO back into operation mode  */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_FIFO_STA, 0x00);

  /* set the data format for Z value: 7 fractional part and 1 whole part */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_TSC_FRACT_XYZ, 0x01);

  /* set the driving capability of the device for TSC pins: 50mA */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_TSC_I_DRIVE, 0x01);

  /* Use no tracking index, touchscreen controller operation mode (XYZ) and
     enable the TSC */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_TSC_CTRL, 0x01);

  /*  Clear all the status pending bits */
  I2C_WriteDeviceRegister(IOE_1_ADDR, IOE_REG_INT_STA, 0xFF);

  /* Initialize the TS structure to their default values */
  TS_State.TouchDetected = TS_State.X = TS_State.Y = TS_State.Z = 0;

  /* All configuration done */
  return IOE_OK;
}

void Sensors_Init(uint8_t SensorType)
{
	switch (SensorType)
	{
	/*ITG-3200 GYRO*/
	case GYRO:

		MARG_SENSORS[GYRO]->ReadLength = 6;
		MARG_SENSORS[GYRO]->SensorType = GYRO;
		MARG_SENSORS[GYRO]->SensorValueUpdated = 0;

		/*ITG3200*/

		I2C_WriteDeviceRegister(I2C_COM1, ITG3200_ADDRESS, PWR_MGM_ITG, 0x80, SETTING_DATA_LENGTH);
		I2C_WriteDeviceRegister(I2C_COM1, ITG3200_ADDRESS, SMPLRT_DIV_ITG, 0x00, SETTING_DATA_LENGTH);
		I2C_WriteDeviceRegister(I2C_COM1, ITG3200_ADDRESS, DLPF_FS_ITG, 0x1C, SETTING_DATA_LENGTH); /* 5 hz low pass filter*/
		I2C_WriteDeviceRegister(I2C_COM1, ITG3200_ADDRESS, INT_CFG_ITG, 0x05, SETTING_DATA_LENGTH);
		/*
		 I2C_WriteDeviceRegister(I2C_COM1,L3G3200_ADDRESS,CTRL_REG4_G,0x10,SETTING_DATA_LENGTH);
		 I2C_WriteDeviceRegister(I2C_COM1,L3G3200_ADDRESS,CTRL_REG1_G,0xFF,SETTING_DATA_LENGTH);
		 */
		break;

	case ACC:

		MARG_SENSORS[ACC]->ReadLength = 6;
		MARG_SENSORS[ACC]->SensorType = ACC;
		MARG_SENSORS[ACC]->SensorValueUpdated = 0;

		/*ADXL345*/
		I2C_WriteDeviceRegister(I2C_COM1, ADXL345_ADDRESS, Register_DataFormat, 0x0B, SETTING_DATA_LENGTH);/*16 G*/
		I2C_WriteDeviceRegister(I2C_COM1, ADXL345_ADDRESS, Register_DataBW, 0xD, SETTING_DATA_LENGTH);
		I2C_WriteDeviceRegister(I2C_COM1, ADXL345_ADDRESS, Register_PowerControl, 0x00, SETTING_DATA_LENGTH);
		I2C_WriteDeviceRegister(I2C_COM1, ADXL345_ADDRESS, Register_PowerControl, 0x10, SETTING_DATA_LENGTH);
		I2C_WriteDeviceRegister(I2C_COM1, ADXL345_ADDRESS, Register_PowerControl, 0x08, SETTING_DATA_LENGTH);

		/*LSM303 Acclemeter*/
		//I2C_WriteDeviceRegister(I2C_COM1,LSM303_ACC_ADDRESS,CTRL_REG4_A,0x0,SETTING_DATA_LENGTH);
		//I2C_WriteDeviceRegister(I2C_COM1,LSM303_ACC_ADDRESS,CTRL_REG1_A,0x3F,SETTING_DATA_LENGTH);
		break;

		/*LSM303 MAG*/
	case MAG:

		MARG_SENSORS[MAG]->ReadLength = 6;
		MARG_SENSORS[MAG]->SensorType = MAG;
		MARG_SENSORS[MAG]->SensorValueUpdated = 0;

		//I2C_WriteDeviceRegister(I2C_COM1,LSM303_MAG_ADDRESS,CRA_REG_M,0x18,SETTING_DATA_LENGTH);
		//I2C_WriteDeviceRegister(I2C_COM1,LSM303_MAG_ADDRESS,CRB_REG_M,0x20,SETTING_DATA_LENGTH);
		//I2C_WriteDeviceRegister(I2C_COM1,LSM303_MAG_ADDRESS,MR_REG_M,0x0,SETTING_DATA_LENGTH);

		I2C_WriteDeviceRegister(I2C_COM1, HMC5883L_ADDRESS, MODE_REG_M, 0x00, SETTING_DATA_LENGTH); //cont measurement
		I2C_WriteDeviceRegister(I2C_COM1, HMC5883L_ADDRESS, CTRL_REG1_M, 0x18, SETTING_DATA_LENGTH); // max hertz
		I2C_WriteDeviceRegister(I2C_COM1, HMC5883L_ADDRESS, CTRL_REG2_M, 0x00, SETTING_DATA_LENGTH); // max gain

		break;
	}
}