/*
  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>. 
*/

#include <Wire.h>
#include <EEPROM.h>
#include <SandS.h>

// Resistance vs temperature table for the sensor
double restab[] = {
  121.300, 83.3200, 57.9100, 40.7100, 28.9500, 20.8000, 15.1000, // -55 ... -25
  11.0700, 8.19600, 6.12300, 4.61500, 3.50700, 2.68800, 2.07800, // -20 ...  10
  1.61600, 1.26700, 1.00000, 0.79490, 0.63600, 0.51200, 0.41480, //  15 ...  45
  0.33800, 0.27690, 0.22820, 0.18900, 0.15730, 0.13160, 0.11060, //  50 ...  80
  0.09338, 0.07919, 0.06744, 0.05767, 0.04951, 0.04267, 0.03691, //  85 ... 115
  0.03204, 0.02791, 0.02440, 0.02139, 0.01882, 0.01660, 0.01469  // 120 ... 150
};

Relay FrontLamp(4);
Relay Heater(3);
Consumption Mains(1);
Input Temp(2);

// Convert resistance to temperature
double resToTemp(double res) {
  uint8_t n;
  res /= 22000.0; // 22K sensor
  for(n = 1; n < 42; n++) {
    if(res > restab[n]) break;
  }
  if(n == 42) return 150.0; // max
  double p = (res - restab[n]) / (restab[n - 1] - restab[n]);
  return -55 + (5 * n) - (5.0 * p);
}

void setup() {
  boolean initialized = false;
  Serial.begin(115200);
  Wire.begin();
  while(!initialized) {
    initialized = true;
    if(!Mains.initialize()) {
      initialized = false;
      Serial.println("Init failed: #1 mains consumption");
    }
    if(!Temp.initialize()) {
      initialized = false;
      Serial.println("Init failed: #2 temperature input");
    }
    if(!Heater.initialize()) {
      initialized = false;
      Serial.println("Init failed: #3 heater relay");
    }
    if(!FrontLamp.initialize()) {
      initialized = false;
      Serial.println("Init failed: #4 lamp relay");
    }
  };
  Heater.digitalWrite(Relay::DISENGAGED);
  FrontLamp.digitalWrite(Relay::ENGAGED);  
}

boolean heat = false;

void brew_handler(boolean brew) {
  static const byte FILTER_COUNT = 100;
  static const double SENSOR_FUP = 100;
  static const double SENSOR_FTH = 20;
  static const double SENSOR_FDN = 10;
  
  static byte filter_count = 0;
  static double celsius = 0;
  static double celsius_mem = 0;

  static unsigned long timer = 0;

  static boolean heat = false;
  static byte idler;

  byte n;
  
  if(timer == 0) timer = millis();
  unsigned long now = millis();
  
  if(now - timer > 5) {
    timer += 5;

    celsius += resToTemp(Temp.analogRead());

    if(++filter_count == FILTER_COUNT) {
      filter_count = 0;
      celsius /= (double)FILTER_COUNT;
      if(celsius_mem != 0) {
        // Calculate temperature delta across 0.5 seconds
        double celsius_delta = celsius - celsius_mem;
        // Extrapolate "real" temperature
        double celsius_real = celsius + (celsius_delta) * (celsius_delta > 0 ? SENSOR_FUP : SENSOR_FDN); 
        // Calculate "soft" threshold, lowering the threshold at high delta
        double thres_hi = min(100 - celsius_delta * SENSOR_FTH, 100);
        double thres_lo = thres_hi - 5;

        Serial.print(thres_lo);
        Serial.print(" < ");
        Serial.print(celsius_real);
        Serial.print(" < ");
        Serial.print(thres_hi);
        Serial.print(" : ");
        
        // Hysteresis control
        if(celsius_real > thres_hi) heat = false;
        else if(celsius_real < thres_lo) heat = true;
        
        // Respect brew request
        heat &= brew;

        // Set heater relay
        Heater.digitalWrite(heat ? Relay::ENGAGED : Relay::DISENGAGED);
        
        // Lamp flash when idle, on when brewing
        FrontLamp.digitalWrite(brew | ((++idler & 3) > 0) ? Relay::DISENGAGED : Relay::ENGAGED);

        Serial.println(heat ? "ON" : "OFF");
        
      }
      celsius_mem = celsius;
      celsius = 0;
    }
  }
}

void loop() {
  static boolean brew = false;
  if(Serial.available()) {
    Serial.read();
    brew = !brew;
  }
  brew_handler(brew);
}

/*
#define PIN_IDAL 4

byte ser_in;
const char hex[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};

bool SerialReadHexByte(byte *out) {
  do {
    while(!Serial.available());
    ser_in = Serial.read();
  } while(ser_in == ' ');
  if(ser_in >= '0' && ser_in <= '9') *out = ser_in - '0';
  else if(ser_in >= 'a' && ser_in <= 'f') *out = ser_in - 'a' + 10;
  else if(ser_in >= 'A' && ser_in <= 'F') *out = ser_in - 'A' + 10;
  else return false;
  *out <<= 4;
  while(!Serial.available());
  ser_in = Serial.read();
  if(ser_in >= '0' && ser_in <= '9') *out |= ser_in - '0';
  else if(ser_in >= 'a' && ser_in <= 'f') *out |= ser_in - 'a' + 10;
  else if(ser_in >= 'A' && ser_in <= 'F') *out |= ser_in - 'A' + 10;
  else return false;
  return true;
}

void SerialWriteHexByte(byte data) {
  Serial.write(hex[data >> 4]);
  Serial.write(hex[data & 0xF]);
}

void loop() {
  byte address, data;
  int ok;
  while(Serial.available()) {
    ser_in = Serial.read();
    if(ser_in != ' ' && ser_in != '\n') {
      if(ser_in == 'W') {
        ok = 0;
        if(SerialReadHexByte(&address)) {
          if(address & 0x80) {
            Serial.write("Address must be in range 00-7F HEX\n");
          } else {
            Wire.beginTransmission(address);
            SerialWriteHexByte(address);
            Serial.write(" <-");
            while(SerialReadHexByte(&data)) {
              Serial.write(' ');
              SerialWriteHexByte(data);
              Wire.write(data);
              ok++;
            }
            Serial.write('\n');
            if(ser_in != '\n') Serial.write("Encountered bad character\n");
            Wire.endTransmission();
            Serial.write("Sent ");
            Serial.print(ok);
            Serial.write(" bytes\n");
            ok++;
          }
        }
        if(!ok) Serial.write("W [Address HEX] ([Byte-0 HEX]( ... ([Byte-n HEX])))\n");
      } else if(ser_in == 'R') {
        ok = 0;
        if(SerialReadHexByte(&address)) {
          ok = 1;
          if(address & 0x80) {
            Serial.write("Address must be in range 00-7F\n");
          } else {
            if(SerialReadHexByte(&data)) {
              if((data - 1) & 0xE0) {
                Serial.write("Byte count must be in range 01-20 HEX\n");
              } else {
                Wire.requestFrom(address, data);
                ok = 0;
                SerialWriteHexByte(address);
                Serial.write(" ->");
                while(Wire.available()) {
                  data = Wire.read();
                  Serial.write(' ');
                  SerialWriteHexByte(data);
                  ok++;
                }
                Serial.write("\nReceived ");
                Serial.print(ok);
                Serial.write(" bytes\n");
                ok++;
              }
            } else {
              ok = 0;
            }
          }
        }
        if(!ok) Serial.write("R [Address HEX] [Byte count HEX]\n");
      } else if(ser_in == 'O') {
        if(SerialReadHexByte(&data)) {
          if(data & 0x01) {
            pinMode(PIN_IDAL, OUTPUT);
            digitalWrite(PIN_IDAL, HIGH);
            Serial.write("Ident enabled\n");
          } else {
            digitalWrite(PIN_IDAL, LOW);
            pinMode(PIN_IDAL, INPUT);
            Serial.write("Ident disabled\n");
          }
        } else {
          Serial.write("O [Flags]\n");
        }
      } else if(ser_in == 'I') {
        data = 0x00;
        if(digitalRead(PIN_IDAL)) data |= 0x01;
        Serial.write("Flags: ");
        SerialWriteHexByte(data);
        Serial.write('\n');
      } else {
        Serial.write("Bad command\n");
      }
      if(ser_in != '\n') while(Serial.read() != '\n');
    }
  }
}
*/
