/**
  ******************************************************************************
  * @file    main.c
  * @author  Ac6
  * @version V1.0
  * @date    01-December-2013
  * @brief   Default main function.
  ******************************************************************************
*/


#include "stm32f4xx.h"
#include "stm32f429i_discovery.h"
#include "tm_stm32f4_ili9341.h"
#include "tm_stm32f4_fonts.h"
#include "tm_stm32f4_delay.h"
#include "tm_stm32f4_spi.h"

#include "tm_stm32f4_i2c.h"

#include "tm_stm32f4_delay.h"
#include <stdio.h>


#define TM_SPIx_PRESCALER    SPI_BaudRatePrescaler_32
//Specify datasize
#define TM_SPIx_DATASIZE     SPI_DataSize_8b
//Specify which bit is first
#define TM_SPIx_FIRSTBIT     SPI_FirstBit_MSB
//Mode, master or slave
#define TM_SPIx_MASTERSLAVE SPI_Mode_Master
//Specify mode of operation, clock polarity and clock phase
//Modes 0 to 3 are possible
#define TM_SPIx_MODE        TM_SPI_Mode_0


int L3G4200D_Address = 210;

#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24

void initGYRO(int scale)
{
     //From  Jim Lindblom of Sparkfun's code

      // Enable x, y, z and turn off power down:
    TM_I2C_Write(I2C1, L3G4200D_Address, CTRL_REG1, 0b00001111);

      // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
    TM_I2C_Write(I2C1, L3G4200D_Address, CTRL_REG2, 0b00000000);

      // Configure CTRL_REG3 to generate data ready interrupt on INT2
      // No interrupts used on INT1, if you'd like to configure INT1
      // or INT2 otherwise, consult the datasheet:
    TM_I2C_Write(I2C1, L3G4200D_Address, CTRL_REG3, 0b00001000);

      // CTRL_REG4 controls the full-scale range, among other things:

      if(scale == 250){
          TM_I2C_Write(I2C1, L3G4200D_Address, CTRL_REG4, 0b00000000);
      }else if(scale == 500){
          TM_I2C_Write(I2C1, L3G4200D_Address, CTRL_REG4, 0b00010000);
      }else{
          TM_I2C_Write(I2C1, L3G4200D_Address, CTRL_REG4, 0b00110000);
      }

      // CTRL_REG5 controls high-pass filtering of outputs, use it
      // if you'd like:
      TM_I2C_Write(I2C1, L3G4200D_Address, CTRL_REG5, 0b00000000);
}

int gyroX, gyroY, gyroZ;
void readGyro(){
    char xMSB = TM_I2C_Read(I2C1, L3G4200D_Address, 0x29);
    char xLSB = TM_I2C_Read(I2C1, L3G4200D_Address, 0x28);
    gyroX = ((xMSB << 8) | xLSB);

    char yMSB = TM_I2C_Read(I2C1, L3G4200D_Address, 0x2B);
    char yLSB = TM_I2C_Read(I2C1, L3G4200D_Address, 0x2A);
    gyroY = ((yMSB << 8) | yLSB);

    char zMSB = TM_I2C_Read(I2C1, L3G4200D_Address, 0x2D);
    char zLSB = TM_I2C_Read(I2C1, L3G4200D_Address, 0x2C);
    gyroZ = ((zMSB << 8) | zLSB);

}

void clearbuffer(char * buff, int len){
     int _i =0;
            for(_i=0; _i<len; _i++){
                buff[_i] = ' ';
            }

            buff[len] = 0;
}



int main(void) {
    /* Initialize system */
    SystemInit();

    /* Initialize delay */
    TM_DELAY_Init();

    /* Initialize ILI9341 */
    TM_ILI9341_Init();

    /* Rotate LCD for 90 degrees */
    TM_ILI9341_Rotate(TM_ILI9341_Orientation_Landscape_2);

    /* Fill lcd with color */
    TM_ILI9341_Fill(ILI9341_COLOR_MAGENTA);
    /* Draw white circle */
    TM_ILI9341_DrawCircle(60, 60, 40, ILI9341_COLOR_GREEN);
    /* Draw red filled circle */
    TM_ILI9341_DrawFilledCircle(60, 60, 35, ILI9341_COLOR_RED);
    /* Draw blue rectangle */
    TM_ILI9341_DrawRectangle(120, 20, 220, 100, ILI9341_COLOR_BLUE);
    /* Draw black filled circle */
    TM_ILI9341_DrawFilledRectangle(130, 30, 210, 90, ILI9341_COLOR_BLACK);
    /* Draw line with custom color 0x0005 */
    TM_ILI9341_DrawLine(10, 120, 310, 120, 0x0005);


    /* Put string with black foreground color and blue background with 11x18px font */
    TM_ILI9341_Puts(65, 130, "STM32F4 Discovery", &TM_Font_11x18, ILI9341_COLOR_BLACK, ILI9341_COLOR_BLUE2);
    /* Put string with black foreground color and blue background with 11x18px font */
    TM_ILI9341_Puts(60, 150, "ILI9341 LCD Module", &TM_Font_11x18, ILI9341_COLOR_BLACK, ILI9341_COLOR_BLUE2);
    /* Put string with black foreground color and red background with 11x18px font */


    TM_I2C_Init(I2C1, TM_I2C_PinsPack_2, 50000);

    initGYRO(2000);

    /* While loop */
    while (1) {
        char buff[256];

        readGyro();

        sprintf(buff, "%10hd; %10hd; %10hd;", gyroX, gyroY, gyroZ);

        TM_ILI9341_Puts(45, 230, buff, &TM_Font_7x10, ILI9341_COLOR_BLACK, ILI9341_COLOR_ORANGE);

        // delay

        int i;
        for (i = 0; i < 500000; i++);
        for (i = 0; i < 500000; i++);
        for (i = 0; i < 500000; i++);
        for (i = 0; i < 500000; i++);
        for (i = 0; i < 500000; i++);
        for (i = 0; i < 500000; i++);
        for (i = 0; i < 500000; i++);
        for (i = 0; i < 500000; i++);


    }

}