#include "main.h"
#include "nmea.h"
#include <cstring>
#include "libOLED/stm32_halDisplay.H"
#include "libOLED/fontclass.H"
#include "libOLED/displayDial.H"

#include "libBMP280/bmp280.h"

#include "libSmallPrintf/small_printf.h"
#include "usbd_cdc_if.h"

namespace
{
  int const WIDTH = 128;
  int const HEIGHT = 64;
  int const DISPLAY_RAMWIDTH = 132;

}

float speedMPH = 0.0;
float speedAVG = 0.0;
int heading = 0;
Location loc;

uint32_t lastTick = 0;
int32_t temp32 = 0;
uint32_t pres32 = 0;
int32_t rslt;

uint8_t displayBuffer[dataSize (WIDTH, HEIGHT)];

stm32_halDisplay_t display1 (WIDTH, HEIGHT, DISPLAY_RAMWIDTH, displayBuffer,
			     &hspi1,

			     SPI_CD_GPIO_Port,
			     SPI_CD_Pin,
			     SPI_RESET_GPIO_Port,
			     SPI_RESET_Pin,
			     SPI_NSS1_GPIO_Port,
			     SPI_NSS1_Pin);

displayDial_t dial (display1, 96, 32, 32, 180);

extern "C" void
cc_init ()
{
  display1.reset ();

  display1.init ();
  display1.clearDisplay ();

  dial.draw_scale (0, 360, 8, 1, 45);

  display1.display ();

  memset (loc.time, '-', 6);
}

char fontBuf[] = "01234567";
extern "C" void
cc_run (struct bmp280_dev *bmp)

{

  display1.clearDisplay ();
  dial.draw_scale (0, 360, 8, 1, 45);

  bool stat = updateLocation (&loc);
  if (loc.good)
    {
      heading = loc.heading;

      loc.good = false;
    }
  if (loc.valid == 'V')
    memset (loc.time, '-', 6);

  // print out the GMT time at the top of the screen
  display1.gotoxy (0, 0);
  display1.printString (small_font, &loc.time[0], 2, WHITE);
  display1.printString (small_font, ":", 1, WHITE);
  display1.printString (small_font, &loc.time[2], 2, WHITE);

  display1.printString (small_font, ":", 1, WHITE);
  display1.printString (small_font, &loc.time[4], 2, WHITE);
  int dial_ang = heading + 180;
  dial.draw_needle (dial_ang);

  display1.gotoxy (70, 25);
  if (loc.valid == 'A')
    {
      display1.fontDigits (large_font, 3, -1, heading);
    }
  else
    display1.printString (large_font, "GPS?", 4, WHITE);

  if (loc.valid == 'A')
    speedMPH = loc.speed / 1.815;

  display1.gotoxy (0, 8);
  display1.printString (small_font, "Speed", 5, WHITE);
  display1.gotoxy (0, 16);
  display1.fontDigits (large_font, 4, 1, speedMPH * 10);

  display1.gotoxy (0, 32);
  display1.printString (small_font, "Average", 7, WHITE);
  display1.gotoxy (0, 40);
  display1.fontDigits (large_font, 4, 1, speedAVG * 10);

  struct bmp280_uncomp_data ucomp_data;

  if (HAL_GetTick () - lastTick > 1000)
    {
      lastTick = HAL_GetTick ();
      /* Reading the raw data from sensor */
      rslt = bmp280_get_uncomp_data (&ucomp_data, bmp);

      if (rslt == BMP280_OK)
	{
	  /* Getting the 32 bit compensated temperature */
	  rslt = bmp280_get_comp_temp_32bit (&temp32, ucomp_data.uncomp_temp,
					     bmp);

	  rslt = bmp280_get_comp_pres_32bit (&pres32, ucomp_data.uncomp_press,
					     bmp);

	  // compile a logger message over USB
	   char buffer[100];
	   int cnt = small_sprintf(buffer,"$TP,%ld,%ld",temp32,pres32);
	   uint8_t sum=0;
	   for(int i=1; i<cnt; i++)
	     sum += buffer[i];
	   cnt+= small_sprintf(buffer+cnt,"*%02X\n",sum);

	   CDC_Transmit_FS(reinterpret_cast<uint8_t*>(&buffer[0]),cnt);


	}
    }
  display1.gotoxy (0, 56);
  display1.printString (small_font, "T", 2, WHITE);
  display1.fontDigits (small_font, 4, 1, temp32/10, WHITE);
  display1.printString (small_font, " P", 2, WHITE);
  display1.fontDigits (small_font, 5, 0, pres32/100, WHITE);
  display1.printString (small_font, " ", 1, WHITE);


  display1.display ();

}