/*
  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/>. 
*/

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 command queue
char queue[QUEUE_SIZE];
byte queue_count;

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

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

// Sets up our environment
void setup() {
  Serial.begin(115200);
  timer = millis();
}

// 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 = 20;
            break;
          case CMD_FLASH:
            flashtime = 20;
            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");
    }
  }
}

// This runs every second
void everysecond() {
  static double deg = 0;
  deg += 0.1;
  if(brewtime) {
    if(--brewtime == 0) holdtime = 20;
  }
  if(holdtime) holdtime--;  
  // Queue execution
  if(queue_count) pop();
  temperature = (sin(deg) + 1) * 50 + 25;
}

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 - timer > 1000) {
    timer = now;
    everysecond();
  }
}
