#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  FPS_HALF_HZ 0x00
#define  FPS1HZ   0x01
#define  FPS2HZ   0x02
#define  FPS4HZ   0x03
#define  FPS8HZ   0x04
#define  FPS16HZ  0x05
#define  FPS32HZ  0x06

#define  MLX90640_ADDR 0x33
#define	 RefreshRate FPS_HALF_HZ //FPS1HZ   //was FPS2HZ
#define EMMISIVITY 0.96f		//water Emisivity=0.96,  human body == 0.92f
#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, blackOutTag=0;

// The following be stored in NVM
volatile uint8_t averageTempThreshold;
volatile uint8_t emmisivityThreshold= (EMMISIVITY*100);		// 96/100 = 0.96f
volatile uint8_t humanTempThreshold = 32;
volatile uint8_t waterTempThreshold=15;   //15/10= 1.5 C
// The Above  be stored in NVM

volatile uint8_t normalWaterTemp=25;   // 25 C

volatile uint8_t detectCycle=0, v_water_cnt=0,h_water_cnt=0, spot_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 float averageTempInner=0.0f;
volatile uint16_t waterSpillCount=0;
extern volatile uint8_t sensor_data_ready;
char tempBuffer[256];
// 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
#define ROW 24
#define COL 32
static float tempValues[COL*ROW];
volatile uint8_t zoneMask[ROW*COL]={0x0}, edgeMask[ROW*COL]={0x0}, upMask[ROW/3][COL/3]={0x0}, order[(ROW/3)*(COL/3)]={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);
static void bubbleSort(uint8_t arr[], uint8_t len, uint8_t order[]);

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;
  averageTempInner =0.0f;
  float sumtemp=0.0f;
  float sumtempinner=0.0f;

  for (i = 0; i < 768; i++)
  {
	//averageTemp += tempValues[i];
	sumtemp += tempValues[i];
	if (((uint8_t)(i/32) > 1)&& ((uint8_t)(i/32) <22) && ((uint8_t)(i%32) >1) && ((uint8_t)(i%32) <30))
	{
		sumtempinner += (float)tempValues[i];
	}

    minTemp = (float)min(minTemp, tempValues[i]);
    maxTemp = (float)max(maxTemp, tempValues[i]);
  }
  //averageTemp /= 768;
  averageTemp = sumtemp / (float)768.0;
  averageTempInner = sumtempinner / (float)560.0; //28*20 inner round area

  centerTemp = (float) ((tempValues[383 - 16] + tempValues[383 - 15] + tempValues[384 + 15] + tempValues[384 + 16]) / 4);

  if (maxTemp > (float)humanTempThreshold)
  {
	  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));
		}

		//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);
#endif
	}
#if 0
	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);

	memset(tempBuffer,0,sizeof(tempBuffer));
	sprintf(tempBuffer,(char *)"%2d", (uint8_t)max(v_water_cnt, h_water_cnt));

	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[COL]={0},v_cnt[ROW]={0};	//for horizon and vertical _water spill count
  	// start from 2, ignore edge of FOV
	for (y=1; y<ROW-1; y++)
	{
#if 0
		BSP_LCD_FillRect(0, y*11+16, ST7789V_LCD_PIXEL_WIDTH, 4, LCD_COLOR_BLACK);
#endif
		for (x=1; x<COL-1; x++)		// Start from 2, ignore edge of FOV
		{

			if (zoneMask[y*COL+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
			    //upMask[(x/3)*(y/3)] ++;
				upMask[(uint8_t)(y/3)][(uint8_t)(x/3)] ++;		 //translate to 11*8 matrix for upload

				h_cnt[x] =1;
				v_cnt[y] =1;
			} else 	if (edgeMask[y*COL+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<ROW/2; y++) {
		if ((1 == v_cnt[y]) && (0 == v_cnt[y-1])) {
			v_1++;
		}
	}
	for (y=12; y<ROW; y++) {
		if ((1 == v_cnt[y]) && (0 == v_cnt[y-1])) {
			v_2++;
		}
	}
	v_water_cnt=  v_1 + v_2;

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

	spot_cnt = max(h_water_cnt, v_water_cnt);
//	APP_LOG(TS_OFF, VLEVEL_L, " H cnt = %2u   V cnt =%2u   Spot Cnt=%2u \r\n", h_water_cnt, v_water_cnt, spot_cnt);

}

// 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);

    MLX90640_CalculateTo(mlx90640Frame, &mlx90640, (float)(emmisivityThreshold/100.0), 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;
	  detectCycle = 0;
	 do
	 {
	  for (y=1; y<ROW; y++)
	  {
			for (x=1; x<COL; x++)
			{
				temp1  = (float)tempValues[(x)   + (y*COL)];
				temph1 = (float)tempValues[(x)   + (y-1)*COL];
				tempv1 = (float)tempValues[(x)   + (y+1)*COL];

				//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*COL]++;
				} else {
					//edgeMask[x+y*COL] = 0;
				}

				//simple edge finding  --end


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

						zoneMask[y*COL+x] ++;
						//upMask[(uint8_t)(x/3)*(uint8_t)(y/3)] ++;		 //translate to 11*8 matrix for upload
						upMask[(uint8_t)(y/3)][(uint8_t)(x/3)] ++;		 //translate to 11*8 matrix for upload
						waterSpillCount ++;
					}
				}
			}
	  }

	  detectCycle ++;

	 } while (detectCycle <30);

	 waterSpillCount /=30;

}


void STS_M1A_SENSOR_Read(STS_M1A_SensorDataTypeDef *m1a_data)
{

    m1a_data->waterSpillCount = waterSpillCount;
    m1a_data->spillage_level = (uint8_t)(waterSpillCount*99/560.0); //((ROW-2)*(COL-2)));   // (24-4) * (32 -4) minus edge dots
	m1a_data->averageTemp = averageTemp;
	m1a_data->averageTempInner = averageTempInner;
	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;
	m1a_data->spot_cnt = spot_cnt; //max(v_water_cnt, h_water_cnt);

	//memcpy((void *)m1a_data->waterSpillMatrix,(const void *) zoneMask,sizeof(zoneMask));
    uint8_t i=0;
	if (spot_cnt != 0) {
		//bubbleSort((uint8_t*)upMask, (ROW/3)*(COL/3), (uint8_t*)order);
		bubbleSort((void *)upMask, 80, (void *)order);

		for (i= spot_cnt; i< 80;i++) {
			order[i] = 0;
		}
	} else {

		for (i= 0; i< 80;i++) {
					order[i] = 0;
		}
	}

//	memset((void*)m1a_data->order, 0x0, sizeof(m1a_data->order));
    memcpy((void *)m1a_data->order, (const void *)order, sizeof(order));

    //memset(order + m1a_data->spot_cnt, 0x0, sizeof(tempBuffer)-m1a_data->spot_cnt);  // fill non-spot position to zero

	memset(tempBuffer,0,sizeof(tempBuffer));

	sprintf(tempBuffer,(char *)"\r\n## Read Sensor Spot CNT=%4d (areas) \r\n## V_cnt=%2d (lane)  H_cnt=%2d (lane) \r\n## Spillage Level =%2.2f%% \r\n## averageTempInner=%2.2f C averageTemp=%2.2f C centerTemp=%2.2f C MinTemp=%2.2f C maxTemp=%2.2f C \r\n ######## Gap_Average= %2.2f    Gap_Inner = %2.2f \r\n",
				m1a_data->waterSpillCount, v_water_cnt, h_water_cnt, (float)(m1a_data->spillage_level), (float)averageTempInner, (float)averageTemp, (float)centerTemp, (float)minTemp, (float)maxTemp, (float)(averageTemp - minTemp),(float)(averageTempInner - minTemp));
	APP_LOG(TS_OFF, VLEVEL_H,(char *)tempBuffer);


	if (m1a_data->spot_cnt !=0 )
	{
		for (uint8_t i=0; i< 4; i++) {
			memset(tempBuffer,0,sizeof(tempBuffer));
			sprintf(tempBuffer, (char *) " Top {%1d} =order =%2u  X=%2u : Y=%2u \r\n", i,	order[i],
					(uint8_t)(order[i]%(10)), (uint8_t)(order[i]/10));
			APP_LOG(TS_OFF, VLEVEL_H,(char *)tempBuffer);
		}
	}

	sensor_data_ready = 1;
}

void mlx90640_display_process(void)
{
	 memset((void *)zoneMask, 0,sizeof(zoneMask));
	 memset((void *)edgeMask, 0, sizeof(edgeMask));
	 memset((void *)upMask, 0, sizeof(upMask));

		readTempValues();
		setTempScale();
		blackOutFilter();
		drawPicture();

#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_H, "Water Spill Detected Level = %d of 600 \r\n", waterSpillCount);
}

uint8_t mlx90640_bringup_test(void)
{

	status = MLX90640_DumpEE(MLX90640_ADDR, eeMLX90640);

	return (status==0?1:0);

}

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

void STS_SENSOR_Thermal_Graph_Test_Process(float *self_test_result, uint8_t count)
{

	mlx90640_display_init();
	readTempValues();
	setTempScale();

	self_test_result[0] = averageTemp;
	self_test_result[1] = centerTemp;
	self_test_result[2] = minTemp;
	self_test_result[3] = maxTemp;
#if 0
	memset(tempBuffer,0,sizeof(tempBuffer));
	sprintf(tempBuffer,(char *)"Upload ============ Calibrated Temp Average=%2.2fC Center=%2.2fC Min=%2.2fC Max=%2.2fC \r\n",
			(float)self_test_result[0], (float)self_test_result[1], (float)self_test_result[2], (float)self_test_result[3]);
	APP_LOG(TS_OFF, VLEVEL_L,(char *)tempBuffer);
#endif

}


static void bubbleSort(uint8_t arr[], uint8_t len, uint8_t order[])
{
    uint8_t i, j, temp, t1;
    for (j=0; j < len ; j++) {
    	order[j] = j;
    }
    for (i = 0; i < len - 1; i++) {
        for (j = 0; j < len - i - 1; j++) {
            if (arr[j] < arr[j + 1]) {
                temp = arr[j];
                arr[j] = arr[j + 1];
                arr[j + 1] = temp;

                t1 = order[j];
                order[j]=order[j+1];
                order[j+1]=t1;
            }
        }
    }
}