#include "Arduino.h"
#include "SPI.h"
#include "tft.h"
#include <Wire.h>

#include "MLX90640_API.h"
#include "MLX90640_I2C_Driver.h"


#define EMMISIVITY 0.95
#define INTERPOLATE false
#define min(a,b) ((a)<(b)?(a):(b))
#define max(a,b) ((a)>(b)?(a):(b))
#define TA_SHIFT 8 //Default shift for MLX90640 in open air

// Output size
#define O_WIDTH 224
#define O_HEIGHT 168
#define O_RATIO O_WIDTH/32

paramsMLX90640 mlx90640;
const byte MLX90640_address = 0x33; //Default 7-bit unshifted address of the MLX90640

// Added for measure Temp
boolean measure = true;
float centerTemp;
unsigned long tempTime = millis();
unsigned long tempTime2 = 0;

// start with some initial colors
float minTemp = 20.0;
float maxTemp = 40.0;

// variables for interpolated colors
byte 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];

float **interpolated = NULL;
uint16_t *imageData = NULL;

TFT tft;

void setup() {
  Serial.begin(115200);
  Serial.println("Hello.");

  // Connect thermal sensor.
  Wire.begin();
  Wire.setClock(400000); // Increase I2C clock speed to 400kHz
  Wire.beginTransmission((uint8_t)MLX90640_address);
  if (Wire.endTransmission() != 0) {
    Serial.println("MLX90640 not detected at default I2C address. Please check wiring.");
  }
  else {
    Serial.println("MLX90640 online!");
  }
  // Get device parameters - We only have to do this once
  int status;
  uint16_t eeMLX90640[832];
  status = MLX90640_DumpEE(MLX90640_address, eeMLX90640);
  if (status != 0) Serial.println("Failed to load system parameters");
  status = MLX90640_ExtractParameters(eeMLX90640, &mlx90640);
  if (status != 0) Serial.println("Parameter extraction failed");
  // Set refresh rate
  MLX90640_SetRefreshRate(MLX90640_address, 0x05); // Set rate to 8Hz effective - Works at 800kHz
  // Once EEPROM has been read at 400kHz we can increase
  Wire.setClock(800000);

  SPI.begin();
  tft.begin();
  tft.setRotation(0); // Use landscape format
  tft.fillScreen(TFT_BLACK);

  tft.setCursor(55, 228);
  tft.setTextColor(TFT_WHITE);
  tft.setTextSize(4);
  tft.print("WaveShare");

  // Prepare interpolated array
  interpolated = (float **)malloc(O_HEIGHT * sizeof(float *));
  for (int i = 0; i < O_HEIGHT; i++) {
    interpolated[i] = (float *)malloc(O_WIDTH * sizeof(float));
  }

  // Prepare imageData array
  imageData = (uint16_t *)malloc(O_WIDTH * O_HEIGHT * sizeof(uint16_t));

  // get the cutoff points for the color interpolation routines
  // note this function called when the temp scale is changed
  setAbcd();
  drawLegend();
}

void loop(void) {
  tempTime = millis();

  readTempValues();
  setTempScale();
  drawPicture();
  drawMeasurement();
}

// Read pixel data from MLX90640.
void readTempValues() {
  for (byte x = 0 ; x < 2 ; x++) // Read both subpages
  {
    uint16_t mlx90640Frame[834];
    int status = MLX90640_GetFrameData(MLX90640_address, mlx90640Frame);
    if (status < 0)
    {
      Serial.print("GetFrame Error: ");
      Serial.println(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);
  }
}


int row;
float temp, temp2;

void interpolate() {
  for (row = 0; row < 24; row++) {
    for (x = 0; x < O_WIDTH; x++) {
      temp  = tempValues[(31 - (x / 7)) + (row * 32) + 1];
      temp2 = tempValues[(31 - (x / 7)) + (row * 32)];
      interpolated[row * 7][x] = lerp(temp, temp2, x % 7 / 7.0);
    }
  }
  for (x = 0; x < O_WIDTH; x++) {
    for (y = 0; y < O_HEIGHT; y++) {
      temp  = interpolated[y - y % 7][x];
      temp2 = interpolated[min((y - y % 7) + 7, O_HEIGHT - 7)][x];
      interpolated[y][x] = lerp(temp, temp2, 1);//y%7/7.0);
    }
  }
}


// Linear interpolation
float lerp(float v0, float v1, float t) {
  return v0 + t * (v1 - v0);
}


void drawPicture() {
  if (INTERPOLATE) {
    interpolate();
    for (y = 0; y < O_HEIGHT; y++) {
      for (x = 0; x < O_WIDTH; x++) {
        imageData[(y * O_WIDTH) + x] = getColor(interpolated[y][x]);
      }
    }
    //tft.pushImage(8, 8, O_WIDTH, O_HEIGHT, imageData);
  }
  else {
    for (y = 0; y < 24; y++) {
      for (x = 0; x < 32; x++) {
        tft.fillRect(8 + x * 7, 8 + y * 7, 7, 7, getColor(tempValues[(31 - x) + (y * 32)]));
      }
    }
  }
}



// Get color for temp value.
uint16_t getColor(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.0 / (c - b) * val - ((b * 255.0) / (c - b)), 0, 255);

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

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

  // use the displays color mapping function to get 5-6-5 color palet (R=5 bits, G=6 bits, B-5 bits)
  //Serial.println(String(val) + " -> " + String(red) + "|" + String(green) + "|" + String(blue));
  return tft.color565(red, green, blue);
}


void setTempScale() {
  minTemp = 255;
  maxTemp = 0;

  for (i = 0; i < 768; i++) {
    minTemp = min(minTemp, tempValues[i]);
    maxTemp = max(maxTemp, tempValues[i]);
  }

  setAbcd();
  drawLegend();
}


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


// Draw a legend.
void drawLegend() {
  float inc = (maxTemp - minTemp) / 224.0;
  j = 0;
  for (ii = minTemp; ii < maxTemp; ii += inc) {
    tft.drawFastVLine(8 + + j++, 292, 20, getColor(ii));
  }

  tft.setTextSize(2);
  tft.setCursor(8, 272);
  tft.setTextColor(TFT_WHITE);
  tft.fillRect(8, 272, 45, 20, TFT_BLACK);
  tft.print(String(minTemp).substring(0, 5));

  tft.setCursor(192, 272);
  tft.setTextColor(TFT_WHITE);
  tft.fillRect(192, 272, 45, 20, TFT_BLACK);
  tft.print(String(maxTemp).substring(0, 5));
}


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

  // Mark center measurement
  tft.drawCircle(120, 8 + 84, 3, TFT_WHITE);

  // Measure and print center temperature
  centerTemp = (tempValues[383 - 16] + tempValues[383 - 15] + tempValues[384 + 15] + tempValues[384 + 16]) / 4;
  tft.setCursor(86, 190);
  tft.setTextColor(TFT_WHITE);
  tft.setTextSize(4);
  tft.fillRect(86, 190, 80, 40, TFT_BLACK);
  tft.print(String(centerTemp).substring(0, 5) + "C");
}
