File content as of revision 7:ed473cf0afaf:

#include "mbed.h"
#include "USBSerial.h"
#include "IAP.h"
#include "Crypto.h"

//#define USE_CIPHER
#define FAKE_HW

#define SERIAL //comment for USB operation, uncomment for serial
#define CALIBRATE 0
#define FUNCTION_CHECK 0

// debounce duration for ON/OFF switch
#define ON_OFF_DEBOUNCE 3000

// longest user command we will accept
#define MAX_COMMAND_LEN 64

// where to start reading stored values from the EEPROM
#define BASE_ADDRESS 1

// temperature control stuff //
// fixed values
#define CONTROL_INTERRUPT 1000 // every 1ms
#define MAX_DUTY 100 // This is both the number of times the heater_control interrupt will run per control period and the max amount of interrupts the heater triac can be on.

// adjustable values
#define DEFAULT_FILTER 0.05 // first order digital filter parameter, should be [0.0 - 1]. less means filter more, a value of 1 should turn it off
//#define INITIAL_DUTY 25 // 25% duty generally results in a few degrees under 57C output for the ambient temperatures I've been testing at
#define DEFAULT_INITIAL_DUTY 45 
#define DEFAULT_NOMINAL_DUTY 28 // 11% duty is a good set point for Beta init (testing at 20C ambient)
//#define SETPOINT 435 // this results in roughly 57C at the tines on the Alphas (really 53)
#define DEFAULT_SETPOINT 58 //seems like there is a ~4C temperature drop between the sensor and the outside of the tips 
#define DEFAULT_TOLERANCE 2 // if we are within this many ADC counts of the setpoint don't make any changes to the duty cycle
//#define DUTY_TOO_LARGE 40 // If the duty gets larger than this we have problems (Alphas)
#define DEFAULT_DUTY_TOO_LARGE 70 // If the duty gets larger than this we have problems (Betas) -> 65 duty should result in roughly 62C tip temps
//#define DUTY_TOO_LARGE 85 // For testing
#define DEFAULT_TEMP_LIMIT_LOWER 12 // If the temperature measures too low during operation we are either doing a bad job or have a faulty temperature sensor
//#define DEFAULT_TEMP_LIMIT_LOWER 0 // For testing
//#define TEMP_LIMIT_UPPER 485 // No burning allowed (Alphas)
#define DEFAULT_TEMP_LIMIT_UPPER 65 // No burning allowed (Betas)

#define CRYPT_BUFF_SZ 16
#define BYTES_PER_100_MS 8 // the number of bytes to send per 100 ms
#define TIP_UPDATE_INTERVAL_S 3 // Update the tip remaining time every x seconds
#define TIP_READ_BLANK_TIME_MS 70
#define TIP_TIME_BETWEEN_MESSAGES_MS 500 // Wait at least xms between sending messages

float filter = DEFAULT_FILTER;
uint8_t initial_duty = DEFAULT_INITIAL_DUTY;
uint8_t nominal_duty = DEFAULT_NOMINAL_DUTY;
uint16_t setpoint = DEFAULT_SETPOINT;
uint8_t tolerance = DEFAULT_TOLERANCE;
uint8_t duty_too_large = DEFAULT_DUTY_TOO_LARGE;
uint16_t temp_limit_lower = DEFAULT_TEMP_LIMIT_LOWER;
uint16_t temp_limit_upper = DEFAULT_TEMP_LIMIT_UPPER;

unsigned char myIV[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
AES *myAES = NULL;

enum State {
    IDLE,
    WAIT_FOR_TIP,
    GET_TIP_CONFIG,
    INITIAL_RAMP,
    ACTIVE,
    DONE,
    PAUSED,
    ERROR,
};

enum Heater_State {
    ON,
    OFF,
};

// not used yet
enum Error {
    NONE,
    TEMP_TOO_LOW,
    TEMP_TOO_HIGH,
    DUTY_TOO_LARGE,
};

enum 
{
    READ_TIME_REMAINING=1,
    WRITE_TIME_REMAINING,
    RESET_BUFFER // This is a local command, resets the rx buffer
}tip_command;

struct tip_message
{
    int command __attribute__((packed));
    long value  __attribute__((packed)); 
};

// Heater control pins
DigitalOut fan(P0_22);
DigitalOut heater_pin(P1_27);
AnalogIn temp_sense(P0_16);

// LED pins
DigitalInOut empty_led(P0_10);
DigitalInOut fuel_gage_1(P0_9);
DigitalInOut fuel_gage_2(P0_8);
DigitalInOut fuel_gage_3(P1_21);
DigitalInOut fuel_gage_4(P1_31);
DigitalOut tip_light(P1_14);

// Other pins
#ifdef FAKE_HW
bool tip_sensor = false;
bool on_off = false;
#else
DigitalIn tip_sensor(P0_14);
DigitalIn on_off(P1_28);
#endif

#define ON_TIME_S 2700L // The device is active for 45 minutes
//#define ON_TIME_S 150L
#define RED_LED_ON_TIME_S 300L // The red LED is active for the last 5 minutes
//#define RED_LED_ON_TIME_S 30L
#define TIP_FLASH_INTERVAL_S 25L // How often the tip will flash
//Serial pc(0x1f00, 0x2012, 0x0001, false);

#ifdef SERIAL
Serial pc(P0_19, P0_18); // tx, rx
#else
USBSerial pc(0x1f00, 0x2012, 0x0001);
#endif

Ticker control_interrupt;
Ticker check_limits_interrupt;
Ticker temperature_interrupt;
Ticker tick_interrupt;

volatile Heater_State heater = OFF;
volatile State state = IDLE;
volatile Error error_state = NONE;

volatile uint8_t duty = DEFAULT_INITIAL_DUTY;
volatile float temperature = 0.0;
unsigned long start_time = 0L;
unsigned long current_time = 0L;
uint32_t tip_start_value_s = 0L; // The number of seconds remaing on the tip at the start of the cycle
uint32_t current_cycle_on_time_s = 0; // The number of seconds this cycle has been operational
uint32_t last_serial_send = 0; // Last time something was sent on the tip serial connection
uint32_t last_message_send = 0; // Last time a message was sent on te tip serial connection
bool connected = false;
volatile uint32_t millis = 0;

void loop(void);
void calibrate(bool first);
void interpret(char parameter, int value);
void spin_lights(bool first);

void do_get_tip_config(bool first);
bool get_message_from_tip(struct tip_message *tm);


//INTERRUPT - increment this every ms since the normal mbed timer returns an int and we need an unsigned type
void tick(void) {millis++;}

uint32_t get_time(void){
    uint32_t copy = 0;
    __disable_irq();
    copy =  millis;
    __enable_irq();
    return copy;
}

uint8_t get_duty(void){
  __disable_irq();
  uint8_t duty_copy = duty;
  __enable_irq();
  return duty_copy;
}

void set_duty(uint8_t new_duty){
  __disable_irq();
  duty = new_duty;
  __enable_irq();
}

float get_temperature(void){
  __disable_irq();
  float temperature_copy = temperature;
  __enable_irq();
  
  #ifdef FAKE_HW
  return setpoint;
  #else
  return temperature_copy;
  #endif
}

/**
 * Packs the message into a string whic can be sent.
 *
 * @param *buff - buffer to populate
 * @param buff_sz - the size of the output buffer
 * @param *tm - the message to convert into a string
 */
bool pack_message(char *buff, int buff_sz, struct tip_message *tm)
{
    memset(buff, 0x00, buff_sz);
    
    if (buff_sz != CRYPT_BUFF_SZ)
    {
        return false; 
    } 
    
    snprintf(buff, buff_sz, "%d:%ld", tm->command, tm->value);
    
    return true;
}

/**
 * Waits 100ms from the last send.
 *
 * @param last_send - time of last 8 byte send
 */
void wait_until_can_send_tip_message(uint32_t last_send)
{
    uint32_t counter = 0;
    
    while (get_time() - last_send < 100)
    {
        wait_ms(1);
        ++counter;
    }
}

/**
 * Sends the buffer one byte at a time to the tip
 * 
 * @param *buff - the buffer to send
 * @param sz - size of the buffer
 */
void tip_send_buff(char *buff, int sz)
{
    for (int i = 0; i < sz; ++i)
    {
        pc.putc(buff[i]);
    }
}

/**
 * Sends a message to the tip. This accounts for the
 * max number of bytes which can be sent per 100ms.
 *
 * @param *tm  - the message to send
 * @param block - when true wait for TIP_TIME_BETWEEN_MESSAGES_MS to elapse, else return
 * @returns true if sent successfully
 */
bool send_message_to_tip(struct tip_message *tm, bool block=false)
{
    char buff[CRYPT_BUFF_SZ] = {0};
    char send_buff[BYTES_PER_100_MS] = {0};
    
    static uint32_t last_send = 0;
    
    int bytes_sent = 0;
    
    if (!block && (get_time() - last_message_send) < TIP_TIME_BETWEEN_MESSAGES_MS)
    {
        return false;
    }
    else
    {
        while (get_time() - last_message_send < TIP_TIME_BETWEEN_MESSAGES_MS)
        {
            
        }    
    }
    
    pack_message(buff, sizeof(buff), tm);
    
    #ifdef USE_CIPHER
    myAES->encrypt((uint8_t*)buff, (uint8_t*)buff, sizeof(buff));
    #endif
    
    bytes_sent = 0;
    
    wait_until_can_send_tip_message(last_send);
    
    if (tm->command == RESET_BUFFER)
    {
        pc.printf("\r\n"); 
        return true;   
    }
    
    /* The start & end of the message aren't encrypted to
     * that we can recover from a failure in the txing 
     * of a message.
     */
    pc.printf("!");
    last_send = get_time();
    
    wait_until_can_send_tip_message(last_send);
    
    memcpy(send_buff, buff, BYTES_PER_100_MS);
        
    tip_send_buff(send_buff, sizeof(send_buff)); 
    bytes_sent = BYTES_PER_100_MS;
    last_send = get_time();
    
    wait_until_can_send_tip_message(last_send);
            
    memcpy(send_buff, buff+bytes_sent, BYTES_PER_100_MS);
    tip_send_buff(send_buff, sizeof(send_buff)); 
    bytes_sent = BYTES_PER_100_MS;
    last_send = get_time(); 
    
    // Send \r\n to terminate message
    wait_until_can_send_tip_message(last_send);
    pc.printf("\r\n");
    last_serial_send = get_time();
    last_message_send = last_serial_send;
    
    return true; 
}

/**
 * Reads a message from the tip and populates the 
 * struct given.
 *
 * @param *tm - storage for message
 * @returns true when message read, else false
 */
bool get_message_from_tip(struct tip_message *tm)
{
    static char buff[40] = {0};
    static int pos = 0;
    bool rval = false;
    
    if (tm->command == RESET_BUFFER)
    { 
        memset(buff, 0x00, sizeof(buff));
        pos = 0;
        
        return false;    
    }
    
    while (pc.readable())
    {
        char t = pc.getc();
        
        if (get_time() - last_serial_send < TIP_READ_BLANK_TIME_MS)
        {
            continue; // Ignore for 100ms
        }
        
        if (t == '\r' || t == '\n')
        {
            if (pos >= 4 && buff[0] == '!')
            {
                #ifdef USE_CIPHER
                myAES->decrypt((uint8_t*)buff+1, (uint8_t*)buff+1, CRYPT_BUFF_SZ);
                #endif
                
                tm->command = (int)(buff[1]-'0');
                tm->value   = atol(buff+3);  
                rval = true;
                
                break;
            }
            
            pos = 0;
            memset(buff, 0x00, sizeof(buff));
        }
        else
        {
            if (pos > sizeof(buff))
            {
                pos = 0;    
            }
            
            buff[pos++] = t;    
        } 
    }
    
    if (rval)
    {   
        pos = 0;
        memset(buff, 0x00, sizeof(buff));
    }
    
    return rval;
}

//pull the settings saved to flash into memory
void read_settings(void){
    uint8_t address = BASE_ADDRESS;
    uint16_t filter_temp = 0;
    
    IAP iap;  

    iap.read_eeprom((char*)address, (char*)&filter_temp, sizeof(filter_temp));
    address += sizeof(filter_temp);
    filter = (float)filter_temp/100;
    iap.read_eeprom((char*)address, (char*)&initial_duty, sizeof(initial_duty));
    address += sizeof(initial_duty);
    iap.read_eeprom((char*)address, (char*)&setpoint, sizeof(setpoint));
    address += sizeof(setpoint);
    iap.read_eeprom((char*)address, (char*)&tolerance, sizeof(tolerance));
    address += sizeof(tolerance);
    iap.read_eeprom((char*)address, (char*)&duty_too_large, sizeof(duty_too_large));
    address += sizeof(duty_too_large);
    iap.read_eeprom((char*)address, (char*)&temp_limit_lower, sizeof(temp_limit_lower));
    address += sizeof(temp_limit_lower);
    iap.read_eeprom((char*)address, (char*)&temp_limit_upper, sizeof(temp_limit_upper));
    address += sizeof(temp_limit_upper);
    iap.read_eeprom((char*)address, (char*)&nominal_duty, sizeof(nominal_duty));
    address += sizeof(nominal_duty);
}

//write settings to persistent memory
void write_settings(void){
 IAP iap;
 uint8_t address = 1; //BASE_ADDRESS;
 uint16_t filter_temp = 0;
    
 filter_temp = (int)(filter*100);
 
 iap.write_eeprom((char*)&filter_temp, (char*)address, sizeof(filter_temp));
 address += sizeof(filter_temp);
 iap.write_eeprom((char*)&initial_duty, (char*)address, sizeof(initial_duty));
 address += sizeof(initial_duty);
 iap.write_eeprom((char*)&setpoint, (char*)address, sizeof(setpoint));
 address += sizeof(setpoint);
 iap.write_eeprom((char*)&tolerance, (char*)address, sizeof(tolerance));
 address += sizeof(tolerance);
 iap.write_eeprom((char*)&duty_too_large, (char*)address, sizeof(duty_too_large));
 address += sizeof(duty_too_large);
 iap.write_eeprom((char*)&temp_limit_lower, (char*)address, sizeof(temp_limit_lower));
 address += sizeof(temp_limit_lower);
 iap.write_eeprom((char*)&temp_limit_upper, (char*)address, sizeof(temp_limit_upper));
 address += sizeof(temp_limit_upper);
 iap.write_eeprom((char*)&nominal_duty, (char*)address, sizeof(nominal_duty));
 address += sizeof(nominal_duty);
}

//parse commands. commands take the form of a character followed by a number, delimited by "\r|\n|;"
void getInput(void){
  static int i = 0;
  static char parameter = '_';
  static char buffer[MAX_COMMAND_LEN + 1];
  int value = 0;
  //char *endp = NULL;
  
  if(!connected) return;
  
  // listen for commands coming in
#ifdef SERIAL
  while (pc.readable()){
      char ch = pc.getc();
#else
  while (pc.available()){
      char ch = pc._getc();
#endif
    if((ch == '\r' || ch == ';' || ch == '\n') && parameter != '_'){
      if(i > 1){
        buffer[i-1] = 0;
        value = atoi(buffer);
      }
      
      //Serial.println("not _");
      interpret(parameter, value);
      parameter = '_';
      buffer[0] = 0;
      i=0;
      break;
    }
    else{
      if(i==0) parameter = ch;
      else buffer[i-1] = ch;
      i++;
    }
    
    if(ch == '_' || ch == '\r' || ch == ';' || ch == '\n'){
      parameter = '_';
      buffer[0] = 0;
      i=0;
    }
  }
}

//print usage info
void usage(void){
    if(!connected)return;
    pc.printf("\r\nCommands are a character followed by a number.\r\n");
    wait_ms(1);
    pc.printf("Available commands are:\r\n");
    wait_ms(1);
    pc.printf("'c': duty cap [1-100]\r\n");
    wait_ms(1);
    pc.printf("'d': set the initial duty used on startup [1-100]\r\n");
    wait_ms(1);
    pc.printf("'f': set the smoothing filter for the temperature sensor.");
    wait_ms(1);
    pc.printf(" smaller is more smoothing. [1-100]\r\n");
    wait_ms(1);
    pc.printf("'l': lower temperature limit. arbitrary units. [1-1000]\r\n");
    wait_ms(1);
    pc.printf("'r': reset all parameters to default values.\r\n");
    wait_ms(1);
    pc.printf("'n': set the nominal duty used on control cycle start [1-100]\r\n");
    wait_ms(1);
    pc.printf("'s': setpoint. same units as upper and lower temperature");
    wait_ms(1);
    pc.printf(" limit [1-1000]\r\n");
    wait_ms(1);
    pc.printf("'t': tolerance. no control action within this band. same ");
    wait_ms(1);
    pc.printf("units as upper and lower temperature limit [1-1000]\r\n");
    wait_ms(1);
    pc.printf("'u': upper temperature limit. arbitrary units. [1-1000]\r\n");
    wait_ms(1);
    pc.printf("'w': write the current control parameters to permanent memory\r\n");
    wait_ms(1);
    pc.printf("\r\n\r\n");
    wait_ms(1);
    
    pc.printf("Values can be modified by entering a ");
    pc.printf("command followed by a number.");
    wait_ms(1);
    pc.printf(" For example, entering 'c50' <enter> would limit the duty");
    wait_ms(1);
    pc.printf(" to 50%\r\n\r\n");
    wait_ms(1);

    pc.printf("Status of individual variables can be queried by entering ");
    wait_ms(1);
    pc.printf("that command name (no number afterwards), or typing any ");
    wait_ms(1);
    pc.printf("unrecognised command (which prints this message).\r\n\r\n");
    wait_ms(1);
}

void print_active_settings(void){
    if(!connected)return;
    pc.printf("Duty capped at: %u%\r\n", duty_too_large);
    wait_ms(1);
    pc.printf("Initial duty is: %u\r\n", initial_duty);
    wait_ms(1);
    pc.printf("Nominal duty is: %u\r\n", nominal_duty);
    wait_ms(1);
    pc.printf("Temperature filter is: %d\r\n", (int)(filter * 100));
    wait_ms(1);
    pc.printf("Lower temperature limit is: %u\r\n", temp_limit_lower);
    wait_ms(1);
    pc.printf("Upper temperature limit is: %u\r\n", temp_limit_upper);
    wait_ms(1);
    pc.printf("Setpoint is: %u\r\n", setpoint);
    wait_ms(1);
    pc.printf("Tolerance is %u\r\n\r\n", tolerance);
    wait_ms(1);
    //pc.printf("Spot treatment time is %d\r\n\r\n", TIP_FLASH_INTERVAL_S);
    //wait_ms(1);
}

//interpret a user command
void interpret(char parameter, int value){
  switch(parameter){
  case 'c':
    if(value != 0) duty_too_large = value;
    pc.printf("Duty cap is %u\r\n", duty_too_large);
    break;
  case 'd':
    if(value != 0) initial_duty = value;
    pc.printf("Initial duty is %u\r\n", initial_duty);
    break;
  case 'f':
    if(value != 0) filter = ((float)value) / 100.0;
    pc.printf("Filter is %d\r\n", (int)(filter * 100));
    break;
  case 'r':
    filter = DEFAULT_FILTER;
    initial_duty = DEFAULT_INITIAL_DUTY;
    nominal_duty = DEFAULT_NOMINAL_DUTY;
    setpoint = DEFAULT_SETPOINT;
    tolerance = DEFAULT_TOLERANCE;
    duty_too_large = DEFAULT_DUTY_TOO_LARGE;
    temp_limit_lower = DEFAULT_TEMP_LIMIT_LOWER;
    temp_limit_upper = DEFAULT_TEMP_LIMIT_UPPER;
    write_settings();
    pc.printf("All parameters reset to default values:\r\n");
    print_active_settings();
    break; 
  case 'l':
    if(value != 0) temp_limit_lower = value;
    pc.printf("Lower temperature limit is %u\r\n", temp_limit_lower);
    break;
  case 'n':
    if(value != 0) nominal_duty = value;
    pc.printf("Nominal duty is %u\r\n", nominal_duty);
    break;
  case 's':
    if(value != 0) setpoint = value;
    pc.printf("Setpoint is %u\r\n", setpoint);
    break;
  case 't':
    if(value != 0) tolerance = value;
    pc.printf("Tolerance is %u\r\n", tolerance);
    break;
  case 'u':
    if(value != 0) temp_limit_upper = value;
    pc.printf("Upper temperature limit is %u\r\n", temp_limit_upper);
    break;
  case 'w':
    write_settings();
    pc.printf("Wrote the current control parameters to memory.\r\n");
    break;

  default:
    usage();    
    print_active_settings();
    break;
  }
}

//put everything into a low power/off state. control loop will be unaffected by this command, so that will also need to be disabled before the heater will stay off
void all_off(){
  heater_pin = 0;
  heater = OFF; //need to treat this as volatile
  fan = 0;
  empty_led.input(); //
  fuel_gage_1.input(); // = 1;
  fuel_gage_2.input(); // = 1;
  fuel_gage_3.input(); // = 1;
  fuel_gage_4.input(); // = 1;
  tip_light = 0;
}

// run this to manually check the hardware works
// probably best to disable the heater on the first try
void functional_check(void){
  all_off();
  if(connected)pc.printf("Tip light\r\n");
  tip_light = 1;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Empty\r\n");
  empty_led.output();
  empty_led = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fuel Gage 1\r\n");
  fuel_gage_1.output();
  fuel_gage_1 = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fuel Gage 2\r\n");
  fuel_gage_2.output();
  fuel_gage_2 = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fuel Gage 3\r\n");
  fuel_gage_3.output();
  fuel_gage_3 = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fuel Gage 4\r\n");
  fuel_gage_4.output();
  fuel_gage_4 = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fan\r\n");
  fan = 1;
  wait_ms(5000);
  if(connected)pc.printf("Heater\r\n");
  heater_pin = 1;
  
  while(1){
    if(connected)pc.printf("Temp: %u\r\n", temp_sense.read_u16()>>6);
    wait_ms(50);
    tip_light = 1;
    wait_ms(50);
    tip_light = 0;
  }
}

// INTERRUPT - called every 4ms, samples the ADC and filters the value with a low pass first order digital filter
void get_temp(void){
  // not bothering with units, this means we need to keep the update constant at once every 4ms or we will need to set the filter again
  //temperature = filter * (temp_sense.read_u16()>>6) + (1 - filter) * temperature;
  // temperature is now in degrees C (value is at sensor, not at tips)
  temperature = filter * (51.282 * (temp_sense.read()*3.3 - 0.4)) + (1 - filter) * temperature;
}

// INTERRUPT - called every millisecond to handle maintaining the on/off state of the triac
void heater_control(void){
  static uint8_t count = 0;
  
  // count up once per interrupt.
  // safety: check to make sure we haven't exceeded MAX_DUTY safety limit
  // after duty is reached for this control period, turn the heat off 
  count++;
  
  if(heater == ON && fan.read()){
    if(count > MAX_DUTY){
      count = 0;
      heater_pin = 1;
    }
    if(count > duty)heater_pin = 0;
  }
  else heater_pin = 0;
}

// bail, something is wrong
void abort(bool error){
  uint16_t period = 1000;
  if(error) period = 250;
  // turn everything off, leave the fan on for a few seconds and flash the red LED
  control_interrupt.detach();
  all_off();
  empty_led.input();
  fan = 1;
  wait_ms(3000);
  fan = 0;
  while(1){
    empty_led.output();
    empty_led = 0;
    heater_pin = 0;
    wait_ms(period);
    heater_pin = 0;
    empty_led.input();
    heater_pin = 0;
    wait_ms(period);
  }
}

// INTERRUPT - monitor stuff that might indicate an error condition
void check_limits(void){
// need to move printing to the main loop
  if(duty >= duty_too_large){
    if(connected){
        pc.printf("Error!!! Duty cycle has become unreasonably ");
        pc.printf("large, Aborting.\r\n");
    }
    abort(true);
  }
   
  if(get_temperature() > temp_limit_upper){
    if(connected)pc.printf("Error!!! Temperature is too high, Aborting.\r\n");
    abort(true);
  }
  
  if((state == ACTIVE || state == INITIAL_RAMP) && (get_temperature() < temp_limit_lower)){
    if(connected){
        pc.printf("%f\r\n", get_temperature());
        pc.printf("Error!!! Abnormally low temperature detected. ");
        pc.printf("Please check the sensor, Aborting.\r\n");
    }
    abort(true);
  }
}

// values pulled from the MCP9701T-E/LT datasheet
// 19.5mV/deg-C, 400mV @ 0C
float adc_to_temp(float adc_value){
 //return 0.195 * adc_value - 
 return 0.196 * adc_value - 31.322;
}

void init(void){
#ifdef SERIAL
  pc.baud(9600);
  //connected = true;
#else
  pc.connect();
  connected = pc.vbusDetected();
#endif

  if(connected)pc.printf("hello\r\n");
  
  tick_interrupt.attach(&tick, 0.001);
  control_interrupt.attach(&heater_control, 0.001);
  temperature_interrupt.attach(&get_temp, 0.004);
  if(!CALIBRATE)check_limits_interrupt.attach(&check_limits, 0.5);

  //read_settings();
  //set_duty(initial_duty);
  //all_off();
}

void check_on_off(void){
static uint32_t count = 0;

    // debounce
    if(on_off){
      count++;
      if(count > ON_OFF_DEBOUNCE){
        all_off();
        state = IDLE;
      }
    }
    else count = 0; 
}

void do_idle(bool first){
    if(!on_off) state = WAIT_FOR_TIP;
}

void do_paused(bool first)
{
    all_off(); 
    state = WAIT_FOR_TIP;
}

// tip neeeds to be present before we can start the cycle
// later we should also abort the cycle if the tip is removed
void do_wait_for_tip(bool first){
  unsigned long time = get_time();
  unsigned long time_increment = time % 1800;
  
  if(first){
    if(connected)pc.printf("Looking for tip\r\n");  
  }  
    if(!tip_sensor){ //tip present is low == present
      tip_light = 1;
      fuel_gage_1.output();
      fuel_gage_1 = 0;
      fuel_gage_2.output();
      fuel_gage_2 = 0;
      fuel_gage_3.output();
      fuel_gage_3 = 0;
      fuel_gage_4.output();
      fuel_gage_4 = 0;
      empty_led.input();
      state = GET_TIP_CONFIG;
      if(connected)pc.printf("Found the tip\r\n");
      return;
    }
        
    if(time_increment < 300){
        fuel_gage_1.input(); // = 1;
        fuel_gage_2.input(); // = 1;
        fuel_gage_3.input(); // = 1;
        fuel_gage_4.input(); // = 1;
    }
    if(time_increment > 600){
      fuel_gage_1.output();
      fuel_gage_1 = 0;
    }
    if(time_increment > 900){
      fuel_gage_2.output();
      fuel_gage_2 = 0;
    }
    if(time_increment > 1200){
      fuel_gage_3.output();
      fuel_gage_3 = 0;
    }
    if(time_increment > 1500){
      fuel_gage_4.output();
      fuel_gage_4 = 0;
    }
}

/**
 * Get the configuration data from the tip. Currently just 
 * reads the time remaining parameter.
 *
 * @param first - true when state just changed, else false
 */
void do_get_tip_config(bool first)
{
    static int _state = 1;
    static uint32_t last_print = 0;
    static uint32_t sent_time = 0;

    if (first)
    {
        _state = 1;    
        sent_time = 0;
        last_print = get_time();
    }
    
    if (connected && get_time() - last_print > 5000)
    {
        last_print = get_time();  
    }
    
    switch (_state)
    {
        case 1:
        {
            tip_message tm;
        
            tm.command = READ_TIME_REMAINING;
            tm.value   = 10L;
            
            if (send_message_to_tip(&tm))
            {
                _state = 2;
                sent_time = get_time();
            }
            break;
        }
        case 2:
        {
            tip_message tm;
            tm.command = 0;
            
            if (get_message_from_tip(&tm))
            {
                tip_start_value_s = tm.value;
                
                if (connected)
                {
                    pc.printf("TIP_START_VAL: %ld\r\n", tip_start_value_s);
                }
                
                if (tip_start_value_s <= 0)
                {
                    if (connected)
                    {
                        pc.printf("TIP HAS ZERO VAL\r\n");    
                    }
                    
                    state = PAUSED;
                }
                else
                {
                    state = INITIAL_RAMP;
                }
            }
            else if (get_time() - sent_time > 3000) // Wait 3s for reply
            {
                // Try again
                tm.command = RESET_BUFFER;
                get_message_from_tip(&tm);
                
                if (connected)
                {
                    pc.printf("TIP TIMEOUT\r\n");
                }
                
                _state = 1;
            }
            break;
        }
    }
}

//This should quickly take us up from ambient to the setpoint.
void do_initial_ramp(bool first){
  // set duty to initial_duty and wait to reach the setpoint to break out
  static uint32_t start_time = 0;
  static uint32_t last_print = get_time();
  
  if(first){
    print_active_settings();
    start_time = get_time();
    fan = 1;
    set_duty(initial_duty);
    heater = ON;
    if(connected)pc.printf("Initial ramp up. Duty will be held constant until setpoint is reached.\r\n");
  }
  
  if(get_time() - start_time > 60000){
     if(connected)pc.printf("Took too long to reach setpoint, aborting.\r\n");
     abort(true);
  }
  
  if(get_time() - last_print > 5000){
    if(connected)pc.printf("Duty: %u, Temp: %.2f, Time: %.2fs\r\n", get_duty(), get_temperature(), (float)get_time()/1000);
    last_print = get_time();
  } 
  
  if(get_temperature() > setpoint - tolerance){
    //now we are roughly up to temperature
    set_duty(nominal_duty);
    state = ACTIVE; 
    return;
  }
}

void do_treatment_cycle(bool first){
  static uint32_t start_time = 0;
  static uint32_t control_timer = 0;
  static uint32_t last_tip_update = 0;
  uint8_t duty_copy;
  float temperature_copy;
  
  if(first){
    start_time = get_time();
    control_timer = start_time;
    last_tip_update = 0;
  }
  
  uint32_t current_time = get_time() - start_time;
  
  // Check if we should update the tip time remaining
  if (current_time - last_tip_update > TIP_UPDATE_INTERVAL_S * 1000L)
  {
    struct tip_message tm;
    tm.command = WRITE_TIME_REMAINING;
    tm.value   = tip_start_value_s - (current_time / 1000);
    
    if (send_message_to_tip(&tm))
    {
        last_tip_update = current_time;
    }
  }
  
  // check if we're done
  if((current_time / 1000L) + current_cycle_on_time_s >= ON_TIME_S){
    if(connected)pc.printf("Done!\r\n");
    //abort(false);
    set_duty(0);
    fan = 0;
    state = DONE;
  }
  
  if (tip_sensor || current_time > tip_start_value_s * 1000L) 
  {
      struct tip_message tm;
      tm.command = WRITE_TIME_REMAINING;
      tm.value   = tip_start_value_s - (current_time / 1000);
    
      send_message_to_tip(&tm, true);
      
      // The tip has been removed or is out of juice
      current_cycle_on_time_s += current_time / 1000L;
      
      if (connected)
      {
        pc.printf("ACTIVE -> PAUSED: %d %ld %ld %ld\r\n", tip_sensor, current_time, current_cycle_on_time_s, tip_start_value_s);
      }
      
      state = PAUSED;
      return;
  }
  
  if(current_time - control_timer > 5000){ // run the control loop every 5 seconds
    control_timer = current_time;
    duty_copy = get_duty();
    temperature_copy = get_temperature();
    if(temperature_copy > setpoint + tolerance) duty_copy--;
    if(temperature_copy < setpoint - tolerance) duty_copy++;
    set_duty(duty_copy);
    
    if(connected)pc.printf("Duty: %u, Temp: %.2f, Time: %.2fs\r\n", get_duty(), get_temperature(), ((float)get_time() - (float)start_time)/1000);
  }
}

void do_done(bool first){
  static uint32_t start_time = 0;
  
  if(first){
    start_time = get_time();
    //control_interrupt.detach();
    all_off();
    if(connected)pc.printf("Done!\r\n");
  }
  
  heater_pin = 0;
  uint32_t current_time = get_time() - start_time;

  if(current_time % 2000 > 1000){
    empty_led.input();
  }
  else{
    empty_led.output();
    empty_led = 0;  
  }
  
  current_cycle_on_time_s = 0;
}

void print_state(void){
    if(!connected)return;
    printf("State:\t");
    switch(state){
        case IDLE:
          printf("IDLE\r\n");
          break;
        case WAIT_FOR_TIP:
          printf("WAIT_FOR_TIP\r\n");
          break;
        case INITIAL_RAMP:
          printf("INITIAL_RAMP\r\n");
          break;
        case ACTIVE:
          printf("ACTIVE\r\n");
          break;
        case DONE:
          printf("DONE\r\n");
          break;
        case ERROR:
          printf("ERROR\r\n");
          break;
        case PAUSED:
          printf("PAUSED\r\n");
          break;
        case GET_TIP_CONFIG:
            printf("GET_TIP_CONFIG\r\n");
            break;
        default: break;
    }
}

int main(){
  wait_ms(8000);
  
  static State last_state = IDLE;
  
  init();
  
  unsigned char myKEY[] ={0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f};
  myAES = new AES(AES_128, myKEY, myIV, ECB_MODE); // will default to CBC_MODE
  
  if(FUNCTION_CHECK) functional_check();
  if(CALIBRATE){
      calibrate(true);
      while(1)calibrate(false);
  }
  
  while(1){
    
    //getInput();
    
    //check_on_off();
    
    bool state_change = false;
    if(state != last_state){
        state_change = true;
        last_state = state;
        print_state();
    }
    
   switch(state){
      case IDLE:
        do_idle(state_change);
        break;
      case WAIT_FOR_TIP:
        do_wait_for_tip(state_change);
        break;
      case GET_TIP_CONFIG:
        do_get_tip_config(state_change);
        break;
      case INITIAL_RAMP:
        do_initial_ramp(state_change);
        spin_lights(state_change);
        break;
      case ACTIVE:
        do_treatment_cycle(state_change);
        spin_lights(false);
        break;
      case DONE:
        do_done(state_change);
        break;
      case ERROR:
        abort(true);
        break;
      case PAUSED:
        do_paused(state_change);
      default: break;
    }  
    
  }
  
}

void spin_lights(bool first){
  static uint32_t start_time = 0;
  
  if(first) start_time = get_time();
  uint32_t current_time = get_time() - start_time;
  
  // tip should be solid for TIP_FLASH_INTERVAL_S seconds, then flash for 3 seconds
  uint32_t tip_time = current_time % (TIP_FLASH_INTERVAL_S * 1000 + 3000);
  if(tip_time < (TIP_FLASH_INTERVAL_S * 1000)) tip_light = 1;
  else tip_light = (tip_time % 100 < 50)?1:0;
  
  // handle fuel gage LEDs
  // this should be more directly linked to the treatment stop condition
  uint32_t step = (ON_TIME_S - RED_LED_ON_TIME_S)/5;
  step *= 1000;
  
  if(current_time > step) fuel_gage_4.input();
  else{
    fuel_gage_4.output();
    fuel_gage_4 = 0;
  }
  if(current_time > 2*step) fuel_gage_3.input();
  else{
    fuel_gage_3.output();
    fuel_gage_3 = 0;
  }
  if(current_time > 3*step) fuel_gage_2.input();
  else{
    fuel_gage_2.output();
    fuel_gage_2 = 0;
  }
  if(current_time > 4*step) fuel_gage_1.input();
  else{
    fuel_gage_1.output();
    fuel_gage_1 = 0;
  }
  if(current_time > 5*step){
      empty_led.output();
      empty_led = 0;
  }
  else empty_led.input();
}

void calibrate(bool first){
  static uint32_t start_time = 0;
  static uint32_t control_timer = 0;
  uint8_t duty_copy;
  
  if(first){
    start_time = get_time();
    control_timer = start_time;
    fan = 1;
    set_duty(5);
    heater = ON;
  }
  
  uint32_t current_time = get_time() - start_time;
  
  if(current_time - control_timer > 15000){ // increase the duty by 5% every 15 seconds
    if(connected)pc.printf("Duty: %u, Temp: %f, Time: %.2fs\r\n", get_duty(), get_temperature(), ((float)get_time() - (float)start_time)/1000);
    control_timer = current_time;
    duty_copy = get_duty();
    duty_copy += 5;
    // check if we're done
    if(duty > 75 && connected){
      set_duty(1);
      pc.printf("Done!\r\n");
      abort(false);
    }
    set_duty(duty_copy);
  }    
}

/*
void calibrate(void){
  // this is really not necessary because all we care about is the temperature we need to hold the sensor at,
  // which we are assuming directly correlates to output air temp.
  // in reality it will probably be affected by ambient temperature,
  // humidity, air flow rate, elevation (air density), and other factors 
  static uint16_t count = 0;
  static uint8_t duty_copy = 5; //inital duty
  float temperature_copy;
  static int tester_count = 0;

  current_time = get_time();
  count++;

  check_limits();

  if(count > 200){ // run the control loop every second
    count = 0;
    tester_count++;
    if(tester_count % 30 == 0){ //30 seconds each duty cycle
      duty_copy += 5;
      if(duty > 85)abort(false);
      if(connected)pc.printf("Duty: %u\r\nTemps:\r\n", duty_copy);
    }
    __disable_irq();
    duty = duty_copy;
    temperature_copy = temperature;
    __enable_irq();
  
    if(connected && (tester_count % 30) > 24)pc.printf("\t%f\t%u\t%f\r\n", temperature_copy, temp_sense.read_u16(), temp_sense.read());
  }
}*/