#include "stm32f4xx.h"
#include "LCD2x16.c"

void I2C2_Init (void)   {
  RCC->APB1ENR  |= 0x00400000;       // Enable clock for I2C2
  I2C2->CR2     |= 0x0008;           // clock == 8MHz!   
  I2C2->CCR     |= 0x0040;           // clock control register (270kHz)
  I2C2->TRISE   |= 0x0009;           // rise time register
  I2C2->CR1     |= 0x0001;           // I2C2 enable
}

void I2C2_WriteChar (char Adr, char Dat)    {
  I2C2->CR1         |= 0x0100;       // send START bit
  while (!(I2C2->SR1 & 0x0001)) {};  // wait for START condition (SB=1) 
  I2C2->DR           = 0xd0;         // slave address   -> DR & write
  while (!(I2C2->SR1 & 0x0002)) {};  // wait for ADDRESS sent (ADDR=1) 
  int Status2        = I2C2->SR2;    // read status to clear flag
  I2C2->DR           = Adr;          // Address in chip -> DR & write
  while (!(I2C2->SR1 & 0x0080)) {};  // wait for DR empty (TxE)
  I2C2->DR           = Dat;          // Dat -> DR & write
  while (!(I2C2->SR1 & 0x0080)) {};  // wait for DR empty (TxE) 
  while (!(I2C2->SR1 & 0x0004)) {};  // wait for Byte sent (BTF)
  I2C2->CR1         |= 0x0200;       // send STOP bit
}

char I2C2_ReadChar (char Adr)    {   // procedure: RM0090, pg. 584!
  
  I2C2->CR1         |= 0x0100;       // send START bit
  while (!(I2C2->SR1 & 0x0001)) {};  // wait for START condition (SB=1) 
  I2C2->DR           = 0xd0;         // slave address -> DR      (LSB=1)
  while (!(I2C2->SR1 & 0x0002)) {};  // wait for ADDRESS sent    (ADDR=1) 
  int Status2        = I2C2->SR2;    // read SR2 to clear flag
  I2C2->DR           = Adr;          // register in chip -> DR 
  while (!(I2C2->SR1 & 0x0080)) {};  // wait for DR empty        (TxE=1)
  while (!(I2C2->SR1 & 0x0004)) {};  // wait for ByteTransferred (BTF=1) 
  
  I2C2->CR1         |= 0x0100;       // send START bit
  while (!(I2C2->SR1 & 0x0001)) {};  // wait for START condition (SB=1)
  I2C2->DR           = 0xd1;         // slave address -> DR      (LSB=0)
  while (!(I2C2->SR1 & 0x0002)) {};  // wait for ADDRESS sent    (ADDR=1)
  int Status4        = I2C2->SR2;    // read status to clear flag
  while (!(I2C2->SR1 & 0x0040)) {};  // wait for ByteReceived    (RxNE=1)
  I2C2->CR1         |= 0x0200;       // send STOP bit

  return ((char)I2C2->DR);           // return byte
}

short I2C2_ReadShort (char Adr)    {
  
  I2C2->CR1         |= 0x0100;       // send START bit
  while (!(I2C2->SR1 & 0x0001)) {};  // wait for START condition (SB=1) 
  I2C2->DR           = 0xd0;         // slave address -> DR      (LSB=1)
  while (!(I2C2->SR1 & 0x0002)) {};  // wait for ADDRESS sent    (ADDR=1) 
  int Status2        = I2C2->SR2;    // read SR2 to clear flag
  I2C2->DR           = Adr;          // register in chip -> DR 
  while (!(I2C2->SR1 & 0x0080)) {};  // wait for DR empty        (TxE=1)
  while (!(I2C2->SR1 & 0x0004)) {};  // wait for ByteTransferred (BTF=1) 
  
  I2C2->CR1         |= 0x0100;       // send START bit
  while (!(I2C2->SR1 & 0x0001)) {};  // wait for START condition (SB=1)
  I2C2->DR           = 0xd1;         // slave address -> DR      (LSB=0)
  while (!(I2C2->SR1 & 0x0002)) {};  // wait for ADDRESS sent    (ADDR=1)
  I2C2->CR1         |= 0x0800;       // POS enable
  int Status4        = I2C2->SR2;    // read status to clear flag
  while (!(I2C2->SR1 & 0x0004)) {};  // wait for ByteReceived    (BTF=1)
  I2C2->CR1         |= 0x0200;       // send STOP bit
  short x1 = I2C2->DR << 8;          // safe place
  short x2 = I2C2->DR;               // safe place

  return ((short)(x1 + x2));  // return combined bytes
}

void main ()  {

  RCC->AHB1ENR  |=  0x00000002; // Enable clock for GPIOB
  GPIOB->AFR[1] |=  0x00004400; // select AF4 (I2C) for PB10,11 -> I2C2
  GPIOB->MODER  |=  0x00a00000; // PB10,11 => alternate functions
  GPIOB->OTYPER |=  0x0c00;     // use open-drain output on these pins!

  // Init accelerometer chip
  I2C2_Init();                  // initialize I2C2 block
  I2C2_WriteChar(0x6b,0x00);    // wake-up!
  I2C2_WriteChar(0x1b,0x10);    // Gyro full scale = +/-1000 deg/s 

  // Init LCD & prepare display
  LCD_init();                           // Init LCD
  LCD_string("A(z)=       mg", 0x00);   // prepare 1st row
  LCD_string("R(z)=       d/s", 0x40);  // prepare 2nd row

  // endless loop
  while (1) {
    short AccZ = I2C2_ReadShort(0x3f);  // read acc in z axis
    LCD_sInt16(AccZ / 16, 0x05, 1);     // write to LCD
    short RotZ = I2C2_ReadShort(0x47);  // read gyro in z axis
    LCD_sInt16(RotZ / 33, 0x45, 1);     // write to LCD
    for (int i=0; i<3000000; i++) {};   // waste time, ~100ms
  };
}