#include <string.h>
#include "mlx90640_lcd_display.h"
#include "stm32_hx8347d_lcd.h"
#include "MLX90640_API.h"
#include "MLX90640_I2C_Driver.h"
#include "sys_app.h"
#include "yunhorn_sts_sensors.h"
//#include "bmp.h"
#define  FPS2HZ   0x02
#define  FPS4HZ   0x03
#define  FPS8HZ   0x04
#define  FPS16HZ  0x05
#define  FPS32HZ  0x06

#define  MLX90640_ADDR 0x33
#define	 RefreshRate FPS2HZ
#define EMMISIVITY 0.95f
#define TA_SHIFT 8 //Default shift for MLX90640 in open air

paramsMLX90640 mlx90640;
static uint16_t eeMLX90640[832];  
int status;
volatile uint8_t draw_legend_once=0, normalPeopleTemp=32, blackOutTag=0, waterTempThreshold=20, normalWaterTemp=25, detectCycle=0, v_water_cnt=0,h_water_cnt=0;
// start with some initial colors
volatile float minTemp = -20.0f;
volatile float maxTemp = 120.0f;
volatile float centerTemp=0.0f;
volatile float averageTemp=0.0f;
volatile uint16_t waterSpillCount=0;
extern volatile uint8_t sensor_data_ready;
char tempBuffer[128];
// variables for interpolated colors
uint8_t red, green, blue;

// variables for row/column interpolation
float intPoint, val, a, b, c, d, ii;
int x, y, i, j;

// array for the 32 x 24 measured tempValues
static float tempValues[32*24];
volatile uint8_t zoneMask[32*24]={0x0}, edgeMask[32*24]={0x0};
volatile STS_M1A_SensorDataTypeDef m1a_data;
void blackOutFilter(void);
static uint16_t TempToColor(float val);
static void setTempScale(void);
static void setAbcd(void);
static void drawLegend(void);
static void drawMeasurement(void);
static void drawPicture(void);
static void readTempValues(void);
extern LCD_DrawPropTypeDef DrawProp;
static uint16_t TempToColor(float val)
{
	  /*
    pass in value and figure out R G B
    several published ways to do this I basically graphed R G B and developed simple linear equations
    again a 5-6-5 color display will not need accurate temp to R G B color calculation

    equations based on
    http://web-tech.ga-usa.com/2012/05/creating-a-custom-hot-to-cold-temperature-color-gradient-for-use-with-rrdtool/index.html

  */

  red = constrain(255.0f / (c - b) * val - ((b * 255.0f) / (c - b)), 0, 255);

  if ((val > minTemp) & (val < a)) {
    green = constrain(255.0f / (a - minTemp) * val - (255.0f * minTemp) / (a - minTemp), 0, 255);
  }
  else if ((val >= a) & (val <= c)) {
    green = 255;
  }
  else if (val > c) {
    green = constrain(255.0f / (c - d) * val - (d * 255.0f) / (c - d), 0, 255);
  }
  else if ((val > d) | (val < a)) {
    green = 0;
  }

  if (val <= b) {
    blue = constrain(255.0f / (a - b) * val - (255.0f * b) / (a - b), 0, 255);
  }
  else if ((val > b) & (val <= d)) {
    blue = 0;
  }
  else if (val > d) {
    blue = constrain(240.0f / (maxTemp - d) * val - (d * 240.0f) / (maxTemp - d), 0, 240);
  }

  // use the displays color mapping function to get 5-6-5 color palette (R=5 bits, G=6 bits, B-5 bits)
  return BSP_LCD_GetColor565(red, green, blue);
}

static void setTempScale(void) {
  minTemp = 255;
  maxTemp = 0;
  averageTemp =0.0f;
  float sumtemp=0.0f;

  for (i = 0; i < 768; i++)
  {
	//averageTemp += tempValues[i];
	sumtemp += tempValues[i];
    minTemp = (float)min(minTemp, tempValues[i]);
    maxTemp = (float)max(maxTemp, tempValues[i]);
  }
  //averageTemp /= 768;
  averageTemp = sumtemp / (float)768.0;
  centerTemp = (float) ((tempValues[383 - 16] + tempValues[383 - 15] + tempValues[384 + 15] + tempValues[384 + 16]) / 4);

  if (maxTemp > normalPeopleTemp)
  {
	  blackOutTag = 1;
  } else {
	  blackOutTag = 0;
  }


  setAbcd();
#if 0
  drawLegend();
#endif
}


// Function to get the cutoff points in the temp vs RGB graph.
static void setAbcd(void) {
  a = minTemp + (maxTemp - minTemp) * 0.2121f;
  b = minTemp + (maxTemp - minTemp) * 0.3182f;
  c = minTemp + (maxTemp - minTemp) * 0.4242f;
  d = minTemp + (maxTemp - minTemp) * 0.8182f;
}


// Draw a legend.
static void drawLegend(void)
{
	if (draw_legend_once ==0)
	{
#if 0
		float inc = (maxTemp - minTemp) / 224.0f;
		j = 0;
		for (ii = minTemp; ii < maxTemp; ii += inc) {
			BSP_LCD_DrawVLine(8+ + j++, 260, 20, TempToColor(ii));
		}
#endif
		//BSP_LCD_FillRect(0,240,240, 320, LCD_COLOR_BLACK);
		BSP_LCD_Clear(LCD_COLOR_BLACK);
		draw_legend_once =1;

		//BSP_LCD_DrawHLine(0,ST7789V_LCD_PIXEL_HEIGHT-16-1,ST7789V_LCD_PIXEL_WIDTH, LCD_COLOR_LIGHTBLUE);
		BSP_LCD_DrawHLine(0,15,ST7789V_LCD_PIXEL_WIDTH, LCD_COLOR_BRRED);
		BSP_LCD_DrawHLine(0,ST7789V_LCD_PIXEL_HEIGHT-16-1,ST7789V_LCD_PIXEL_WIDTH, LCD_COLOR_BRRED);


	    BSP_LCD_SetFont(&Font16);
		memset(tempBuffer,0,sizeof(tempBuffer));
		DrawProp.BackColor = LCD_COLOR_BLACK;
		sprintf(tempBuffer,(char *)"Yunhorn Technology");
		BSP_LCD_DisplayStringAt(20,ST7789V_LCD_PIXEL_HEIGHT+2,(uint8_t *)tempBuffer,LEFT_MODE,LCD_COLOR_DARKBLUE);

		memset(tempBuffer,0,sizeof(tempBuffer));
		sprintf(tempBuffer,(char *)" F10.MWC#1.Z#1");
		BSP_LCD_DisplayStringAt(22, 15,(uint8_t *)tempBuffer,LEFT_MODE,LCD_COLOR_DARKBLUE);
	}
#if 1
	BSP_LCD_SetFont(&Font16);
	DrawProp.BackColor = LCD_COLOR_BLACK;
	memset(tempBuffer,0,sizeof(tempBuffer));
	sprintf(tempBuffer,(char *)"%2.1f",minTemp);
	BSP_LCD_DisplayStringAt(8,ST7789V_LCD_PIXEL_HEIGHT-16,(uint8_t *)tempBuffer,LEFT_MODE,LCD_COLOR_BLUE);

	memset(tempBuffer,0,sizeof(tempBuffer));
	sprintf(tempBuffer,(char *)"%2.1f",maxTemp);
	BSP_LCD_DisplayStringAt(190,ST7789V_LCD_PIXEL_HEIGHT-16,(uint8_t *)tempBuffer,LEFT_MODE,LCD_COLOR_RED);
#endif

}

// Draw a circle + measured value.
static void drawMeasurement(void ) {

  // Mark center measurement
#if 0
  BSP_LCD_DrawCircle(120, 8+84, 10, LCD_COLOR_WHITE);
#endif
  // Measure and print center temperature
  centerTemp = (tempValues[383 - 16] + tempValues[383 - 15] + tempValues[384 + 15] + tempValues[384 + 16]) / 4;
#if 0
    BSP_LCD_SetFont(&Font16);
#endif
	memset(tempBuffer,0,sizeof(tempBuffer));
	sprintf(tempBuffer,(char *)"%2d", (uint8_t)max(v_water_cnt, h_water_cnt));
#if 0
	BSP_LCD_DisplayStringAt(ST7789V_LCD_PIXEL_WIDTH-60, 15,(uint8_t *)tempBuffer,LEFT_MODE,LCD_COLOR_YELLOW);
#endif
	APP_LOG(TS_OFF, VLEVEL_M, "WaterSpillCount= %s\r\n",tempBuffer);

}

static void drawPicture(void)
{
	uint8_t h_cnt[32]={0},v_cnt[24]={0};	//for horizon and vertical _water spill count

  	// start from 2, ignore edge of FOV
	for (y=2; y<23; y++)
	{
#if 0
		BSP_LCD_FillRect(0, y*11+16, ST7789V_LCD_PIXEL_WIDTH, 4, LCD_COLOR_BLACK);
#endif
		for (x=1; x<31; x++)		// Start from 2, ignore edge of FOV
		{

			if (zoneMask[y*32+x] > 3) {
				APP_LOG(TS_OFF,VLEVEL_L,"\r\n X=%d Y=%d Count=%d\r\n",x,y,zoneMask[y*32+x]);
#if 0
				BSP_LCD_FillRect(x*8, y*11+16, 4, 4, LCD_COLOR_GREEN);
#endif
				h_cnt[x] =1;
				v_cnt[y] =1;
			} else 	if (edgeMask[y*32+x] > 3) {
#if 0
				BSP_LCD_FillRect(x*8, y*11+16, 2, 2, LCD_COLOR_GRAY);
#endif
			} else
			{
#if 0
				BSP_LCD_FillRect(x*8, y*11+16, 2, 2, LCD_COLOR_BLACK);
#endif
			}
		}
	}
	// simple count of water spill point cloud
	v_water_cnt=0;
	h_water_cnt=0;
	uint8_t v_1=0, v_2=0,h_1=0,h_2=0;

	for (y=1; y<12; y++) {
		if ((1 == v_cnt[y]) && (0 == v_cnt[y-1])) {
			v_1++;
		}
	}
	for (y=12; y<23; y++) {
		if ((1 == v_cnt[y]) && (0 == v_cnt[y-1])) {
			v_2++;
		}
	}
	v_water_cnt=  v_1 + v_2;

	for (x=1; x<16; x++) {
		if ((1 == h_cnt[x]) && (0 == h_cnt[x-1])) {
			h_1 ++;
		}
	}
	for (x=16; x<32; x++) {
		if ((1 == h_cnt[x]) && (0 == h_cnt[x-1])) {
			h_2 ++;
		}
	}
	h_water_cnt = h_1 + h_2;

}

// Read pixel data from MLX90640.
static void readTempValues(void)
{

  for (uint8_t x = 0 ; x < 2 ; x++) // Read both subpages
  {
    uint16_t mlx90640Frame[834];
    status = MLX90640_GetFrameData(MLX90640_ADDR, mlx90640Frame);
    if (status < 0)
    {
    	APP_LOG(TS_OFF, VLEVEL_L, "GetFrame Error: %d\r\n",status);
    }

    float vdd = MLX90640_GetVdd(mlx90640Frame, &mlx90640);
    float Ta = MLX90640_GetTa(mlx90640Frame, &mlx90640);

    float tr = Ta - TA_SHIFT; //Reflected temperature based on the sensor ambient temperature

    MLX90640_CalculateTo(mlx90640Frame, &mlx90640, EMMISIVITY, tr, tempValues);
  }
}
void blackOutFilter(void)
{
	float temp1=0.0, temph1=0.0, tempv1=0.0;

	if (maxTemp < normalPeopleTemp)
	{
	  blackOutTag =0;
	} else
	{
		  blackOutTag =1;
	}

	  // ignore edge of FOV
	  waterSpillCount =0;
	  for (y=2; y<22; y++)
	  {
			for (x=2; x<30; x++)
			{
				temp1  = (float)tempValues[(x)   + (y*32)];
				temph1 = (float)tempValues[(x)   + (y-1)*32];
				tempv1 = (float)tempValues[(x)   + (y+1)*32];

				//simple edge finding  --begin
				//if (((fabs(temp1 - temph1)> 0.3)) && (fabs(temp1 - tempv1)> 0.3))
				if ((fabs(temp1 - temph1)> 0.2) && (fabs(temp1 - tempv1)> 0.2))
				//if (((temp1 > temph1)) && ((temp1 > tempv1)))
				{	// vertical find and horizontal find
					edgeMask[x+y*32]++;
				} else {
					//edgeMask[x+y*32] = 0;
				}

				//simple edge finding  --end


				if ((temp1 + (float)(waterTempThreshold / 10.0)) < max(averageTemp, normalWaterTemp))
				{
					if (blackOutTag == 0) {

						zoneMask[y*32+x] ++;
						waterSpillCount ++;
					}
				}
			}
	  }

	if (detectCycle ++ > 20)
	{
	  detectCycle = 0;

	  memset((void *)zoneMask,0,sizeof(zoneMask));
	  memset((void *)edgeMask, 0, sizeof(edgeMask));
	}


}


void STS_M1A_SENSOR_Read(STS_M1A_SensorDataTypeDef *m1a_data)
{
	memset(tempBuffer,0,sizeof(tempBuffer));
	sprintf(tempBuffer,(char *)"Read Sensor M1A data %4d  average=%2.2fC center=%2.2fC min=%2.2fC max=%2.2fC V_cnt=%2u H_cnt=%2u \r\n",
    		(int)waterSpillCount, (float)averageTemp, (float)centerTemp, (float)minTemp, (float)maxTemp, (uint8_t)v_water_cnt, (uint8_t)h_water_cnt);

	APP_LOG(TS_OFF, VLEVEL_M,(char *)tempBuffer);


    m1a_data->waterSpillCount = waterSpillCount;
	m1a_data->averageTemp = averageTemp;
	m1a_data->centerTemp = centerTemp;
	m1a_data->minTemp = minTemp;
	m1a_data->maxTemp = maxTemp;

	m1a_data->v_water_cnt = v_water_cnt;
	m1a_data->h_water_cnt = h_water_cnt;

	sensor_data_ready = 1;
}

void mlx90640_display_process(void)
{
		readTempValues();
		setTempScale();
		blackOutFilter();

#if 1
		if (blackOutTag == 0)
		{
			//BSP_LCD_DisplayOn();
			//			LCD_BL_ON();

				if (detectCycle !=0)
				{
					drawPicture();
					drawMeasurement();
				}
		} else {

			//LCD_BL_OFF();
			//BSP_LCD_DisplayOff();
		}
#endif
		APP_LOG(TS_OFF, VLEVEL_M, "Water Spill Detected Level = %d of 600 \r\n", waterSpillCount);
}

void mlx90640_display_init(void){
	MLX90640_SetRefreshRate(MLX90640_ADDR, RefreshRate);
	MLX90640_SetChessMode(MLX90640_ADDR);
	
  status = MLX90640_DumpEE(MLX90640_ADDR, eeMLX90640);
  if (status != 0) APP_LOG(TS_OFF, VLEVEL_L, "\r\nload system parameters error with code:%d\r\n",status);
  status = MLX90640_ExtractParameters(eeMLX90640, &mlx90640);
  if (status != 0) APP_LOG(TS_OFF, VLEVEL_L, "\r\nParameter extraction failed with error code:%d\r\n",status);
}