// Filter temperature sensor readings using the Kalman process

const int sensorPin = A0;  // named constant for the pin the sensor is connected to

// kalman variables
float varVolt = 1.12184278324081E-05; //1.12184278324081E-05; // variance determined using excel and reading samples of raw sensor data
float varProcess = 1E-8; // 1e-8;
float Pc = 0.0;
float G = 0.0;
float P = 1.0;
float Xp = 0.0;
float Zp = 0.0;
float Xe = 0.0;

void setup() {
  // open a serial connection to display values
  Serial.begin(9600);
}

void loop() {
  int sensorVal = analogRead(sensorPin);     // read the value on AnalogIn pin 0 and store it in a variable
  float voltage = sensorVal * 5.0 / 1023.0;  // convert the ADC reading to voltage
  Serial.println(voltage, 6);
  // kalman process
  Pc = P + varProcess;
  G = Pc/(Pc + varVolt);    // kalman gain
  P = (1-G)*Pc;
  Xp = Xe;
  Zp = Xp;
  Xe = G*(voltage-Zp)+Xp;   // the kalman estimate of the sensor voltage
    
  serialFloatPrint(voltage);
  serialFloatPrint(Xe);
  Serial.println();
  delay(2000);
}

// function to print out floats in HEX
void serialFloatPrint(float f) {
  byte * b = (byte *) & f;  ///**********
  Serial.print("#"); /////#*******************
  for(int i=0; i<4; i++) {
    byte b1 = (b[i] >> 4) & 0x0f;
    byte b2 = (b[i] & 0x0f);
    char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
    char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
    Serial.print(c1);
    Serial.print(c2);
  }
}








/*
//*** Sensor Kalman Filter
https://www.youtube.com/watch?v=biY7F-tLwE8

https://gist.github.com/Zymotico/836c5d82d5b52a2a3695
https://gist.github.com/Zymotico/ddcb891446060ddde6a4

*/

///////////////////////////////////////////////////
/*  Processing 3.0 Code //************************
GraphSensorData.pde
...............................
import processing.serial.*;
import java.nio.ByteBuffer;

Serial myPort;  // Create object from Serial class
PFont font;
float[] xvals;
float[] yvals;
int arrayindex = 0;
float[] sensorData = {0.0, 0.0};
float tC = 0.0;
float tF = 0.0;
float tCK = 0.0;
float tFK = 0.0;

void setup() 
{
  size(640, 640);
  noSmooth();
  myPort = new Serial(this, "COM3", 9600);
  font = loadFont("CourierNewPS-BoldMT-32.vlw");
  textFont(font);
  
  xvals = new float[width];
  yvals = new float[width];
}

void draw() {
  background(0);
      
  if(myPort.available() > 0) {
    String inString = myPort.readStringUntil((int) '\n');
    
    if(inString != null && inString.length() == 20) {
      //println("   data len = " + inString.length());
      sensorData = decodeFloat(inString.substring(0, 18)); // discard the '\r''\n'
      
      tC = 100.0*sensorData[0] - 50;
      tF = tC * 1.8 + 32.0;
      tCK = 100.0*sensorData[1] - 50;
      tFK = tCK * 1.8 + 32.0;
  
      println(sensorData[0] + ", " + sensorData[1] + " --> " + tF + "°, " + tFK + "°");
    }
  }
  
  for(int i = 1; i < width; i++) { 
    xvals[i-1] = xvals[i]; 
    yvals[i-1] = yvals[i];
  } 
  
  // Add the new values to the end of the array 
  xvals[width-1] = map(sensorData[0], 0.7, 0.75, height, 0); 
  yvals[width-1] = map(sensorData[1], 0.7, 0.75, height, 0);

  for(int i=1; i<width; i++) {
    stroke(255, 0, 0);
    line(i-1, xvals[i-1], i, xvals[i]);
    stroke(0, 0, 255);
    line(i-1, yvals[i-1], i, yvals[i]);
  }  
  
  fill(255, 0, 0);
  text("   Raw: " + nf(tF,2,3) + "°", 30, 40);
  fill(0, 0, 255);
  text("Kalman: " + nf(tFK,2,3) + "°", 30, 70);
}

float[] decodeFloat(String inString) {
  float [] outData = new float[2];
  byte [] inData = new byte[4];
  
  String tempString = inString.substring(1, 9); // discard the leading "#"
  inData[0] = (byte) unhex(tempString.substring(0, 2));
  inData[1] = (byte) unhex(tempString.substring(2, 4));
  inData[2] = (byte) unhex(tempString.substring(4, 6));
  inData[3] = (byte) unhex(tempString.substring(6, 8));      
  int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
  outData[0] = Float.intBitsToFloat(intbits);
  
  tempString = inString.substring(10, 18); // discard the leading "#"
  inData[0] = (byte) unhex(tempString.substring(0, 2));
  inData[1] = (byte) unhex(tempString.substring(2, 4));
  inData[2] = (byte) unhex(tempString.substring(4, 6));
  inData[3] = (byte) unhex(tempString.substring(6, 8));      
  intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
  outData[1] = Float.intBitsToFloat(intbits);
  
  return outData;
}

*/
///////////////////////////////////////////



//////////////////////////////////////////////
/*   Conversion Process hex to float //****************

void setup() {
  Serial.begin(9600);
  
  // Hexadecimal representation of the float value 1.2345 = 0x3F9DF7B6
  uint32_t hexValue = 0x3F9DF7B6;
  
  // Convert the hex to float using a union
  union {
    uint32_t hex;
    float value;
  } converter;
  
  converter.hex = hexValue;
  
  // Print the converted float value
  Serial.print("Hex: 0x");
  Serial.print(hexValue, HEX);
  Serial.print(" -> Float: ");
  Serial.println(converter.value, 6); // Print with 6 decimal places for precision
}

void loop() {
  // Empty loop
}

*/