#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  DetectCycleCount	8
#define  MLX90640_ADDR 0x33
#define	 RefreshRate FPS4HZ //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
#define  WATER_DETECT 0x01
#define  HUMAN_DETECT 0x02

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
volatile uint8_t thermalDetectTag = WATER_DETECT;
// The Above  be stored in NVM

volatile uint8_t normalWaterTemp=25;   // 25 C

volatile uint16_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;
volatile uint16_t humanSpotCount=0;
extern volatile uint8_t sensor_data_ready;
extern volatile float env_temperature;
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 uint16_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 void setTempScale(void);
static void setAbcd(void);
static void findFocusArea(void);
static void readTempValues(void);
static void sortFocusAreas(void);
static void bubbleSort(uint16_t arr[], uint16_t len, uint16_t order[]);



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


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


static void findFocusArea(void)
{
	uint16_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++)
	{
		for (x=1; x<COL-1; x++)		// Start from 2, ignore edge of FOV
		{

			if (zoneMask[y*COL+x] > 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;
			}
		}
	}
	// 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);
  }
}

static void sortFocusAreas(void)
{
	float temp1=0.0;

	if ((maxTemp > humanTempThreshold)&&(thermalDetectTag==WATER_DETECT))
	{
	  blackOutTag =1;
	} else
	{
		  blackOutTag =0;
	}
	  // ignore edge of FOV
	  waterSpillCount = 0;
	  humanSpotCount  = 0;
	  for (y=1; y<ROW; y++)
	  {
			for (x=1; x<COL; x++)
			{
				temp1  = (float)tempValues[(x)+(y*COL)];


				if (thermalDetectTag == WATER_DETECT)
				{
					if ((temp1 + (float)(waterTempThreshold / 10.0)) <  (float)(min(averageTemp, env_temperature)))   // was max(averageTemp, env_temperature))
					{
						if (blackOutTag == 0)
						{
							zoneMask[y*COL+x] ++;
							upMask[(uint8_t)(y/3)][(uint8_t)(x/3)] ++;		 //translate to 11*8 matrix for upload
							waterSpillCount ++;
						}
					}
				} else if (thermalDetectTag == HUMAN_DETECT) {
					if ((temp1 > (float) humanTempThreshold)&&(temp1 <(float) (humanTempThreshold+6.0))) {

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

}


void STS_M1A_SENSOR_Read(STS_M1A_SensorDataTypeDef *m1a_data)
{
    m1a_data->thermalDetectTag = thermalDetectTag;

    m1a_data->waterSpillCount = blackOutTag==0?waterSpillCount:0;
    m1a_data->humanSpotCount = humanSpotCount;

    m1a_data->humanSpot_level = (uint8_t)((m1a_data->humanSpotCount)*99/768.0);
    m1a_data->spillage_level = (uint8_t)((m1a_data->waterSpillCount)*99/768.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;
	spot_cnt = max(v_water_cnt, h_water_cnt);
	m1a_data->spot_cnt = blackOutTag ==0? spot_cnt:0; //max(v_water_cnt, h_water_cnt);

	// TODO XXXX for fall detection logic
	m1a_data->detectedEvent = (m1a_data->spot_cnt>0)? 1:0;  //fall detection or water spillage detection result

    uint8_t i=0;
	if ((spot_cnt != 0) && (blackOutTag == 0)) {
		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;
		}
	}
	// MIRROR REVERT OF MATRIX
	for (i=0; i<spot_cnt;i++)
	{
		order[i]= 10*((order[i]+10)/10)+ (9 - ((order[i])%10));
	}

    memcpy((void *)m1a_data->order, (const void *)order, sizeof(order));
	memset(tempBuffer,0x0,sizeof(tempBuffer));
    if (thermalDetectTag == WATER_DETECT)
	{
    	sprintf(tempBuffer,(char *)"\r\n## Blackout=%u ######\n##Read Sensor Spot CNT=%4d (areas), \r\n Env_Temp =%2.2f C \r\n## V_cnt=%2d (lane) H_cnt=%2d (lane)\r\n## Spillage Level =%u %% \r\n## averageTempInner=%02.2f C averageTemp=%02.2f C, centerTemp=%2.2f C  MinTemp=%2.2f C  maxTemp=%2.2f C \r\n",
			(uint8_t)blackOutTag, (uint16_t)(m1a_data->waterSpillCount), (float)env_temperature, (int)v_water_cnt, (int)h_water_cnt, (uint8_t)(m1a_data->spillage_level), (float)averageTempInner, (float)averageTemp, (float)centerTemp, (float)minTemp, (float)maxTemp);

	} else if (thermalDetectTag == HUMAN_DETECT)
	{
		sprintf(tempBuffer,(char *)"\r\n######Read Sensor Spot CNT=%4d (areas), \r\n Env_Temp =%2.2f C \r\n## V_cnt=%2d (lane) H_cnt=%2d (lane)\r\n## Spillage Level =%u %% \r\n## averageTempInner=%02.2f C averageTemp=%02.2f C, centerTemp=%2.2f C  MinTemp=%2.2f C  maxTemp=%2.2f C \r\n",
				(uint16_t)(m1a_data->humanSpotCount), (float)env_temperature, (int)v_water_cnt, (int)h_water_cnt, (uint8_t)(m1a_data->humanSpot_level), (float)averageTempInner, (float)averageTemp, (float)centerTemp, (float)minTemp, (float)maxTemp);

	}

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

	sprintf(tempBuffer,(char *)"\r\n######## Gap_Average= %2.2f C   Gap_Inner =%2.2f C\r\n ##### Thermal Detect Tag =%s \r\n        Human Body Threshold=%2.2f C WaterThreshold=%2.2f C\r\n",(float)(averageTemp - minTemp),(float)(averageTempInner - minTemp), ((thermalDetectTag == WATER_DETECT)? "Water Detect":"Human Detect"), (float)humanTempThreshold, (float)(waterTempThreshold/10));
	APP_LOG(TS_OFF, VLEVEL_H,(char *)tempBuffer);

	if ((m1a_data->spot_cnt !=0 ))
	{
		if ((blackOutTag == 0) || (thermalDetectTag == HUMAN_DETECT))
		{
			for (uint8_t i=0; i< 4; i++)
			{
				memset(tempBuffer,0x0,sizeof(tempBuffer));
				sprintf(tempBuffer, (char *) " Top {%2u} : order =%2u  X=%2u : Y=%2u \r\n", (uint8_t)i,	(int)(order[i]),
						(int)(order[i]%(10)), (int)(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));

	 detectCycle = 0;
	 uint16_t spillcountTemp =0, spot_cntTemp=0;
	 uint16_t humanspotcountTemp =0;

	 do {
		readTempValues();
		setTempScale();
		sortFocusAreas();
		findFocusArea();

		spillcountTemp += waterSpillCount;
		humanspotcountTemp += humanSpotCount;
		spot_cntTemp += spot_cnt;

	 } while (++detectCycle < DetectCycleCount);

	 waterSpillCount = (uint16_t) (spillcountTemp/DetectCycleCount);
	 humanSpotCount = (uint16_t) (humanspotcountTemp/DetectCycleCount);

	 spot_cnt /= DetectCycleCount ; //max(v_water_cnt, h_water_cnt);


		if ((blackOutTag == 0)&& (thermalDetectTag == WATER_DETECT))
		{
			APP_LOG(TS_OFF, VLEVEL_H, "Water Spill Detected Level = %u of 768 \r\n", (uint16_t)waterSpillCount);
		}
		else if (thermalDetectTag == HUMAN_DETECT)
		{
			APP_LOG(TS_OFF, VLEVEL_H, "Human Spot Detected Level = %u of 768 \r\n", (uint16_t)humanSpotCount);
		}
}

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(uint16_t arr[], uint16_t len, uint16_t order[])
{
    uint16_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;
            }
        }
    }
}