#include "main.h"
#include "libNMEA/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"

#if defined USB_DEVICE
#include "usbd_cdc_if.h"
#endif

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

}

float speedMPH = 0.0;
float speedAvg = 0.0;

double speedAvgTime = 0;    // sum of all deltaTime;
double speedAvgSum = 0.0;  // sum of deltaTime * speed for each second
double lastLocTime = 0.0; // last time there was a location

uint32_t nextPosTime = 0;

// convert knots to MPH
double const KNOTS_TO_MPH = 1.128;
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);

// debouncer for the button
char buttonState;
char buttonCount;

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);
  while (1)
    {
      bool stat = updateLocation (&loc, &uc1);
      if (!stat)
	break;
      if (stat && loc.good)
	{

	  heading = loc.heading;
	  // add in time * speed to give "distance"
	  if (loc.valid == 'A')
	    {
	      if (lastLocTime != 0)
		{
		  double delta = difftime (loc.utc, lastLocTime);
		  // believe the speed .
		  if (delta > 0 && delta < 2)
		    {
		      speedAvgSum += loc.speed * delta;
		      speedAvgTime += delta;
		      lastLocTime = loc.utc;
		    }
		  else
		     lastLocTime = loc.utc;
		}
	      else
		{
		  lastLocTime = loc.utc;
		}
	    }
	}
      else
	{
	memset (loc.time, '-', 6);
	}
    }
  // process button press
  uint8_t const buttonLimit = 3;
  uint8_t newPush = HAL_GPIO_ReadPin ( encoder_push_GPIO_Port,
  encoder_push_Pin);
  if (newPush == buttonState)
    buttonCount = 0;
  else if (buttonCount < buttonLimit)
    buttonCount++;

  if (buttonCount == buttonLimit)
    {
      buttonState = newPush;
      buttonCount = 0;

      // if the button is held down , we set the average speed
      if (buttonState == GPIO_PIN_RESET)
	{
	  speedAvgSum = 0.0;
	  speedAvgTime = 0.0;
	}
    }

  // update the display once per second
  if (HAL_GetTick() > nextPosTime )
    {
      nextPosTime += 1000;

      // slow down the output of ata
      if (speedAvgTime > 0)
	speedAvg = (speedAvgSum / speedAvgTime) * KNOTS_TO_MPH;

      // 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 * KNOTS_TO_MPH;
      else
	speedMPH = 0.0;

      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 > 100)
	{
	  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);

#if defined USB_DEVICE
/*
	  *  $--XDR,a,x.x,a,c--c, ..... *hh<CR><LF> \\

	      Field Number:
	      1) Transducer Type
	      2) Measurement Data
	      3) Units of measurement
	      4) Name of transducer
	      x) More of the same
	      n) Checksum

	      Example:
	      $IIXDR,C,19.52,C,TempAir*19
	      $IIXDR,P,1.02481,B,Barometer*29

	      Currently, OpenCPN recognizes the following transducers:

	  Measured Value | Transducer Type | Measured Data   | Unit of measure | Transducer Name
	  ------------------------------------------------------------------------------------------------------
	  barometric     | "P" pressure    | 0.8..1.1 or 800..1100           | "B" bar         | "Barometer"
	  air temperature| "C" temperature |   2 decimals                    | "C" celsius     | "TempAir" or "ENV_OUTAIR_T"
	  pitch          | "A" angle       |-180..0 nose down 0..180 nose up | "D" degrees     | "PTCH" or "PITCH"
	  rolling        | "A" angle       |-180..0 L         0..180 R       | "D" degrees     | "ROLL"
	  water temp     | "C" temperature |   2 decimals                    | "C" celsius     | "ENV_WATER_T"
*/
	  // compile a logger message over USB
	   char buffer[200];
	   int cnt = small_sprintf(buffer,"$MJXDR,C,%ld.%02ld,C,AirTemp,P,%01ld.%05ld,B,AirPres",temp32/100,temp32%100,pres32/100000,pres32%100000);
	   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);
#endif

	    }
	}
      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 ();
    }
}