/*
  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 <TimerOne.h>
#include <avr/wdt.h>

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

#define BREW_TIME 240 // time to brew in seconds
#define HOLD_TIME 1800 // time to keep warm in seconds

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

const byte QUEUE_SIZE = 10;   // Max number of commands in queue
const byte COMMAND_SIZE = 20; // Max number of bytes in command

const char CMD_BREW = 'B';
const char CMD_FLASH = 'F';
const char CMD_ABORT = 'A';

// Stores incoming commands
char command[COMMAND_SIZE];
byte command_n;

// Stores command queue
char queue[QUEUE_SIZE];
byte queue_count;

// Stores current doings
word brewtime;   // brewing
word holdtime;   // holding
word flashtime;   // flashing
word temperature; // heater temp
word interpause; // pause between queue items

// For second timing
unsigned long seconds;

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

// Sets up our environment
void setup() {
  Serial.begin(115200);

  boolean initialized = false;
  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);    
  seconds = millis();

  //while(1) Serial.write("-");
  
  recall();
  wdt_enable (WDTO_1S);
}

// Places a command in the queue, unless it's full
// Writes response to serial port
void push(char cmd) {
  if(queue_count < QUEUE_SIZE) {
    //memmove(&queue[1], &queue[0], &queue[QUEUE_SIZE - 1] - &queue[0]);
    queue[queue_count++] = cmd;
    Serial.print("OK\n");
  } else {
    Serial.print("FULL\n");
  }
}

// Runs command at top of queue
// Pauses between each command
void pop() {
  if(queue_count) {
    if(interpause == 0) {
      interpause = 2;
    } else {
      if(--interpause == 0) {
        switch(queue[0]) {
          case CMD_BREW:
            if(brewtime || holdtime) return;
            brewtime = BREW_TIME;
            break;
          case CMD_FLASH:
            flashtime = 800;
            break;
          default:
            // wtf?
            break;
        }
        memmove(&queue[0], &queue[1], &queue[QUEUE_SIZE - 1] - &queue[0]);  
        queue_count--;
      }
    }
  }
}

// Aborts everything
void abort() {
  brewtime = 0; // abort brewing
  holdtime = 0; // abort holding
  queue_count = 0; // clear queue
  Serial.print("OK\n");
}

// Executes incoming command from linux board
void execute() {
  command[command_n] = 0;
  command_n = 0;
  if(!strncmp(command, "ATQ", 3)) {
    if(strlen(command) > 3) {
      // Respond with queue command
      word index = strtoul(&command[3], NULL, 10);
      if(index > 0 && index <= queue_count) {
        switch(queue[index - 1]) {
          case CMD_BREW:
            Serial.print("BREW\n"); // Brew queued
            break;
          case CMD_FLASH:
            Serial.print("FLASH\n"); // Flash queued
            break;
          default:
            Serial.print("ERROR\n"); // WTF?
            break;
        }
      } else {
        Serial.print("END\n"); // End of queue
      }
    } else {
      // Respond with current status
      if(brewtime) {
        Serial.print("BREW ");
        Serial.print(brewtime);
        Serial.print(" ");
        Serial.print(temperature);
        Serial.print("\n");
      } else if(holdtime) {
        Serial.print("HOLD ");
        Serial.print(holdtime);
        Serial.print(" ");
        Serial.print(temperature);
        Serial.print("\n");
      } else {
        Serial.print("IDLE "); 
        Serial.print(temperature);
        Serial.print("\n");
      }
    }
  } else if(!strncmp(command, "ATP", 3)) {
    // Try to put command in queue
    if(strlen(command) != 4) command[3] = 0;
    switch(command[3]) {
      case CMD_BREW:
        push(CMD_BREW);
        break;
      case CMD_FLASH:
        push(CMD_FLASH);
        break;
      case CMD_ABORT:
        abort();
        break;
      default:
        Serial.print("ERROR\n");
    }
  }
}

void brew_handler(byte target) {
  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(flashtime) {
      flashtime--;
      // Lamp flash when idle, on when brewing
      FrontLamp.digitalWrite(((flashtime & 63) > 31) ? Relay::DISENGAGED : Relay::ENGAGED);
      delay(1);
    }

    if(++filter_count == FILTER_COUNT) {
      filter_count = 0;
      celsius /= (double)FILTER_COUNT;
      temperature = celsius;
      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(target - celsius_delta * SENSOR_FTH, target);
        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
        if(!target) heat = false;

        // Set heater relay
        Heater.digitalWrite(heat ? Relay::ENGAGED : Relay::DISENGAGED);
      
        if(!flashtime) {
          // Lamp flash when idle, on when brewing
          FrontLamp.digitalWrite(target || ((++idler & 3) > 0) ? Relay::DISENGAGED : Relay::ENGAGED);
        }      
        
      }
      celsius_mem = celsius;
      celsius = 0;
    }
  }
}

void commit() {
  EEPROM.write(1, brewtime >> 8); EEPROM.write(2, brewtime & 0xFF);  
  EEPROM.write(3, holdtime >> 8); EEPROM.write(4, holdtime & 0xFF);  
  EEPROM.write(5, flashtime >> 8); EEPROM.write(6, flashtime & 0xFF);  
  EEPROM.write(7, temperature >> 8); EEPROM.write(8, temperature & 0xFF);  
  EEPROM.write(9, interpause >> 8); EEPROM.write(10, interpause & 0xFF);  
  EEPROM.write(11, queue_count);
  for(uint8_t n = 0; n < QUEUE_SIZE; n++) {
    EEPROM.write(12 + n, queue[n]);
  }
  EEPROM.write(0, 2);
}

void recall() {
  if(EEPROM.read(0) != 2) return;
  brewtime = (EEPROM.read(1) << 8) | EEPROM.read(2);
  holdtime = (EEPROM.read(3) << 8) | EEPROM.read(4);
  flashtime = (EEPROM.read(5) << 8) | EEPROM.read(6);
  temperature = (EEPROM.read(7) << 8) | EEPROM.read(8);
  interpause = (EEPROM.read(9) << 8) | EEPROM.read(10);
  queue_count = EEPROM.read(11);
  for(uint8_t n = 0; n < QUEUE_SIZE; n++) {
    queue[n] = EEPROM.read(12 + n);
  }
}



// This runs every second
void everysecond() {
  static double deg = 0;
  deg += 0.1;
  if(brewtime) {
    if(--brewtime == 0) holdtime = HOLD_TIME;
  }
  if(holdtime) holdtime--;  
  // Queue execution
  if(queue_count) pop();
  commit();
}

void loop() {
  char c;
  // Serial communication
  while(Serial.available()) {
    c = Serial.read();
    if(c == '\n' || c == '\r') {
      execute();
    } else if(command_n < COMMAND_SIZE) {
      command[command_n++] = c;
    }
  }
  // Count seconds
  unsigned long now = millis();
  if(now - seconds > 1000) {
    seconds += 1000;
    everysecond();
  }
  brew_handler(brewtime ? 115 : (holdtime ? 80 : 0));
  wdt_reset ();
}
