//LCD HD44780 library
#include <LiquidCrystal.h>

/* Hardware definitions */

/* Watchdog stuff */

#define PIN_WD_FB A7
#define PIN_WD_IO 13
#define PIN_WD_ADC A0

#define WD_THRESHOLD_MIN 512
#define WD_THRESHOLD_MAX 640
int wd_count=0;

/* Motor control */
#define PIN_MOTOR_ENABLE 8
#define PIN_MOTOR_STEP 9
#define PIN_MOTOR_DIR 10

#define MOTOR_STATE_BOOTING 0x00
#define MOTOR_STATE_READY 0x01
#define MOTOR_STATE_INJECTING 0x02
#define MOTOR_STATE_FREE_MOVE 0x03

#define MOTOR_STATE_WATCHDOG 0xFF

#define CONFIG_MOTOR_FREE_MOVE_SPEED 1024


int motor_state, motor_old_state;
int motor_requested_state;

/* UI */
#define PIN_LCD_RS 11
#define PIN_LCD_EN  12
#define PIN_LCD_IO4 4
#define PIN_LCD_IO5 5
#define PIN_LCD_IO6 6
#define PIN_LCD_IO7 7
#define PIN_BTN_PRV A2
#define PIN_BTN_NXT A3
#define PIN_BTN_OK A1

#define UI_STATE_BOOTING 0x00
#define UI_STATE_READY 0x01
#define UI_STATE_CONFIRM_INJECTION 0x02
#define UI_STATE_RUNNING 0x03
#define UI_STATE_CONFIRM_STOP 0x04
#define UI_STATE_END 0x05

#define UI_STATE_MAIN_MENU 0xF0
#define UI_STATE_MENU_MANUAL 0xF1
#define UI_STATE_MENU_SPEED 0xF2
#define UI_STATE_MENU_SYRINGE 0xF3
#define UI_STATE_MENU_CALIBRATE 0xF4
#define UI_STATE_MENU_SELFTEST 0xF5

#define UI_STATE_MENU_CONFIRM_VALUE 0xFA

#define UI_STATE_WATCHDOG 0xFF

#define UI_SELECT_CHAR_LEFT 0x7E
#define UI_SELECT_CHAR_RIGHT 0x7F

int ui_state, ui_old_state, ui_state_count=0, ui_blink=0;
int ui_selection=0;
int ui_btn_prev=0, ui_btn_next=0, ui_btn_ok=0;
int current_syringe=0, current_injection_speed=100; 

#define UI_MAIN_MENU_ITEMS 5
const char ui_menu_names[UI_MAIN_MENU_ITEMS +1][21] = {"Mouvement chariot", "Posologie", "Taille seringue", "Calibration", "Autotest", "-------retour-------"};

//should be here so it can define a global lcd object
LiquidCrystal lcd(PIN_LCD_RS,PIN_LCD_EN, PIN_LCD_IO4, PIN_LCD_IO5, PIN_LCD_IO6, PIN_LCD_IO7);

/* Process Definition */
#define CONFIG_MAX_INJECTION_SPEED 500 // ml/h

#define UI_MAX_SYRINGE 4
const int ui_syringe_list[UI_MAX_SYRINGE+1] = {10,20,50,100,150};
const int config_syringe_step_per_ml[UI_MAX_SYRINGE+1] = {1115,558,223,111,74};
const int config_syringe_start_pos [UI_MAX_SYRINGE+1] = {0,0,0,0,0};

unsigned int ml_per_h_to_timer_value(int desired_ml_per_h, int syringe_step_per_ml){
  // value for 1ms : 7.8125 = (8*10^6) / (1000*1024) 
  return ((7.8125*3600000UL)/((long)desired_ml_per_h*(long)syringe_step_per_ml));  
}

void setup() {
  Serial.begin(9600);  

  pinMode(PIN_WD_IO, OUTPUT);  
  pinMode(PIN_WD_FB, INPUT);

  pinMode(PIN_MOTOR_ENABLE, OUTPUT);
  pinMode(PIN_MOTOR_STEP, OUTPUT);
  pinMode(PIN_MOTOR_DIR, OUTPUT);

  digitalWrite(PIN_MOTOR_ENABLE, LOW); 
  digitalWrite(PIN_MOTOR_STEP, LOW);
  digitalWrite(PIN_MOTOR_DIR, LOW);
  
  lcd.begin(20, 2);
  lcd.print("    Open pikouz!");
  
  motor_state = MOTOR_STATE_BOOTING;
  motor_old_state = MOTOR_STATE_BOOTING;
  
  ui_state = UI_STATE_BOOTING;
  ui_old_state = UI_STATE_BOOTING;

}

// the loop routine runs over and over again forever:
void loop() {

  //buttons management
  if(!digitalRead(PIN_BTN_PRV)){
    ui_btn_prev++;
  }
  else{
    ui_btn_prev = 0;
  }
   if(!digitalRead(PIN_BTN_NXT)){
    ui_btn_next++;
  }
  else{
    ui_btn_next = 0;
  }
  if(!digitalRead(PIN_BTN_OK)){
    ui_btn_ok++;
  }
  else{
    ui_btn_ok = 0;
  }
 
   
  //UI state machine

  switch(ui_state) {
    
    case UI_STATE_BOOTING:
      if(millis()/1000 > 2) ui_state = UI_STATE_READY;
    break;
    
    case UI_STATE_READY:
    
       if(ui_old_state != UI_STATE_READY){
        //send a stop message to the motor state machine
        motor_requested_state = MOTOR_STATE_READY;
        lcd.clear();
        lcd.print(ui_syringe_list[current_syringe]);
        lcd.setCursor(3, 0);
        lcd.print("mL =PRET= ");
        lcd.setCursor(13, 0);
        lcd.print(current_injection_speed);
        lcd.setCursor(16, 0);
        lcd.print("mL/h");
        lcd.setCursor(1, 1);
        lcd.print("menu");
        lcd.setCursor(11, 1);
        lcd.print("injecter");
        ui_selection=0;
        ui_state_count=0;
        ui_old_state = ui_state;
      }
      
      //tasteful animation in idle mode
      lcd.setCursor(6, 0);
      if(ui_blink){
        lcd.print("-");
      }
      else{
        lcd.print("|");
      }
      lcd.setCursor(11, 0);
      if(ui_blink){
        lcd.print("-");
      }
      else{
        lcd.print("|");
      }
      
      //display item selection
      if(!ui_selection){
        lcd.setCursor(0, 1);
        lcd.write(UI_SELECT_CHAR_LEFT);
        lcd.setCursor(5, 1);
        lcd.write(UI_SELECT_CHAR_RIGHT);
        lcd.setCursor(10, 1);
        lcd.print(" ");
        lcd.setCursor(19, 1);
        lcd.print(" "); 
      }
      else{
        lcd.setCursor(0, 1);
        lcd.print(" ");
        lcd.setCursor(5, 1);
        lcd.print(" ");
        lcd.setCursor(10, 1);
        lcd.write(UI_SELECT_CHAR_LEFT);
        lcd.setCursor(19, 1);
        lcd.write(UI_SELECT_CHAR_RIGHT);
      }
      
      //handle the ui button press
      if((ui_btn_prev == 2)|| (ui_btn_next == 2)){
        
        ui_selection = !ui_selection;
      }
      
      if(ui_btn_ok == 2){
        
        if(ui_selection){
          ui_state = UI_STATE_CONFIRM_INJECTION;
        }
        else{
          ui_state = UI_STATE_MAIN_MENU;
        }
      }
      
    break;

    case UI_STATE_CONFIRM_INJECTION:
    
       if(ui_old_state != UI_STATE_CONFIRM_INJECTION){
        lcd.clear();
        lcd.print("CONFIRMEZ INJECTION?");
        lcd.setCursor(1, 1);
        lcd.print("retour");
        lcd.setCursor(11, 1);
        lcd.print("injecter");
        ui_selection=0;
        ui_state_count=0;
        ui_old_state = ui_state;
      }
      
      //display item selection
      if(!ui_selection){
        lcd.setCursor(0, 1);
        lcd.write(UI_SELECT_CHAR_LEFT);
        lcd.setCursor(7, 1);
        lcd.write(UI_SELECT_CHAR_RIGHT);
        lcd.setCursor(10, 1);
        lcd.print(" ");
        lcd.setCursor(19, 1);
        lcd.print(" "); 
      }
      else{
        lcd.setCursor(0, 1);
        lcd.print(" ");
        lcd.setCursor(7, 1);
        lcd.print(" ");
        lcd.setCursor(10, 1);
        lcd.write(UI_SELECT_CHAR_LEFT);
        lcd.setCursor(19, 1);
        lcd.write(UI_SELECT_CHAR_RIGHT);
      }
      
      //handle the ui button press
      if((ui_btn_prev == 2) || (ui_btn_next == 2)){
        
        ui_selection = !ui_selection;

      }
      
      if(ui_btn_ok == 2){
        
        if(ui_selection){
          ui_state = UI_STATE_RUNNING;
        }
        else{
          ui_state = UI_STATE_READY;
        }
      }

    // 5 seconds timout
    if(ui_state_count > 50){
      ui_state = UI_STATE_READY;
    }

    break;

    case UI_STATE_RUNNING:
    
       if(ui_old_state != UI_STATE_RUNNING){
        //send a start message to the motor state machine
        motor_requested_state = MOTOR_STATE_INJECTING;
        lcd.clear();
        lcd.print("Injection a ");
        lcd.setCursor(13, 0);
        lcd.print(current_injection_speed);
        lcd.setCursor(16, 0);
        lcd.print("mL/h");
        lcd.setCursor(0, 1);
        lcd.print("Fin: 00:00:00");
        ui_selection=0;
        ui_state_count=0;
        ui_old_state = ui_state;
      }
      else{
        //if the motor state machine is not injecting go to stop mode
        if(motor_state != MOTOR_STATE_INJECTING){
          ui_state = UI_STATE_END;
        }
      }
      //progress bargraph animation
      /* 
      lcd.setCursor(6, 0);
      if(ui_blink){
        lcd.cursor();
      }
      else{
        lcd.noCursor();
      }
      */
      //handle the ui button press   
      if(ui_btn_ok > 30){      
         ui_state = UI_STATE_CONFIRM_STOP;
      }
      
      
    break;
    
    case UI_STATE_CONFIRM_STOP:
    
       if(ui_old_state != UI_STATE_CONFIRM_STOP){
        lcd.clear();
        lcd.print("CONFIRMEZ ARRET?");
        lcd.setCursor(1, 1);
        lcd.print("retour");
        lcd.setCursor(13, 1);
        lcd.print("arret");
        ui_selection=0;
        ui_state_count=0;
        ui_old_state = ui_state;
      }
      
      //display item selection
      if(!ui_selection){
        lcd.setCursor(0, 1);
        lcd.write(UI_SELECT_CHAR_LEFT);
        lcd.setCursor(7, 1);
        lcd.write(UI_SELECT_CHAR_RIGHT);
        lcd.setCursor(12, 1);
        lcd.print(" ");
        lcd.setCursor(19, 1);
        lcd.print(" "); 
      }
      else{
        lcd.setCursor(0, 1);
        lcd.print(" ");
        lcd.setCursor(7, 1);
        lcd.print(" ");
        lcd.setCursor(12, 1);
        lcd.write(UI_SELECT_CHAR_LEFT);
        lcd.setCursor(19, 1);
        lcd.write(UI_SELECT_CHAR_RIGHT);       
        
      }

      //handle the ui button press
      if((ui_btn_prev == 2) || (ui_btn_next == 2)){
        
        ui_selection = !ui_selection;
      }
      
      if(ui_btn_ok == 2){
        
        if(ui_selection){
          ui_state = UI_STATE_END;
        }
        else{
          ui_state = UI_STATE_RUNNING;
        }
      }

     // 5 seconds timout
    if(ui_state_count > 50){
      ui_state = UI_STATE_RUNNING;
    }

    break;

    case UI_STATE_END:
    
       if(ui_old_state != UI_STATE_END){
        //send a start message to the motor state machine
        motor_requested_state = MOTOR_STATE_READY;
        lcd.clear();
        lcd.print("Fin d'injection !");
        lcd.setCursor(0, 1);
        lcd.print("000 mL en 00:00:00");
        ui_selection=0;
        ui_state_count=0;
        ui_old_state = ui_state;
      }

      //handle the ui button press   
      if(ui_btn_ok == 2){      
         ui_state = UI_STATE_READY;
      }
      
    break;

    case UI_STATE_MAIN_MENU:
    
       if(ui_old_state != UI_STATE_MAIN_MENU){
        lcd.clear();
        lcd.print("   MENU PRINCIPAL");
        lcd.setCursor(0, 1);
        lcd.print(ui_menu_names[0]);
        ui_selection=0;
        ui_state_count=0;
        ui_old_state = ui_state;
      }
      
      //handle the ui button press
      if((ui_btn_prev == 2) && (ui_selection > 0)){
        ui_selection --;
      }
      
      if((ui_btn_next == 2) && (ui_selection < UI_MAIN_MENU_ITEMS)){
        ui_selection ++;
      }

      if((ui_btn_prev == 2) || (ui_btn_next == 2)){
        lcd.setCursor(0, 1);
        lcd.print("                    ");
        lcd.setCursor(0, 1);
        
        lcd.print(ui_menu_names[ui_selection]);
        ui_state_count=0;
      }

      if(ui_btn_ok == 2){
         if(ui_selection == UI_MAIN_MENU_ITEMS){
           ui_state = UI_STATE_READY;
         }
         else{
           ui_state = UI_STATE_MAIN_MENU + 1 + ui_selection;
         }
      }
      
      // 60 seconds timout
      if(ui_state_count > 600){
        ui_state = UI_STATE_READY;
      }

    break;

    case UI_STATE_MENU_MANUAL:
       if(ui_old_state != UI_STATE_MENU_MANUAL){
        //send a start message to the motor state machine
        motor_requested_state = MOTOR_STATE_FREE_MOVE;
        lcd.clear();
        lcd.print("Deplacement chariot");
        lcd.setCursor(0, 1);
        lcd.print("btn ok pour quitter");
        ui_selection=0;
        ui_state_count=0;
        ui_old_state = ui_state;
      }
      
      if(ui_btn_ok == 2){
        ui_state = UI_STATE_MAIN_MENU;
        motor_requested_state = MOTOR_STATE_READY;
      }
      
      // 60 seconds timout
      if(ui_state_count > 600){
        ui_state = UI_STATE_READY;
        motor_requested_state = MOTOR_STATE_READY;
      }
      
    break;
    
    case UI_STATE_MENU_SPEED:
       if(ui_old_state != UI_STATE_MENU_SPEED){
        lcd.clear();
        lcd.print("Vitesse injection");
        lcd.setCursor(0, 1);
        lcd.print(current_injection_speed);
        lcd.setCursor(4, 1);
        lcd.print("mL/h");
        ui_selection=0;
        ui_state_count=0;
        ui_old_state = ui_state;
      }

      
      //handle the ui button press
      if((ui_btn_prev == 2) || ((ui_btn_prev > 0) && !(ui_btn_prev % 2))){

        if((current_injection_speed > 10) && !(current_injection_speed % 10) && (ui_btn_prev > 50)){
          current_injection_speed -= 10;
        }
        else if(current_injection_speed > 0){
          current_injection_speed --;
        }
        
        lcd.setCursor(0, 1);
        lcd.print("   ");
        lcd.setCursor(0, 1);
        lcd.print(current_injection_speed);
        ui_state_count=0;
      }
      
      if((ui_btn_next == 2) || ((ui_btn_next > 0) && !(ui_btn_next % 2))){
        
        if((current_injection_speed >= 10) && !(current_injection_speed % 10) && (ui_btn_next > 50) && (current_injection_speed + 10 < CONFIG_MAX_INJECTION_SPEED)){
          current_injection_speed += 10;
        }
        else if(current_injection_speed < CONFIG_MAX_INJECTION_SPEED){
          current_injection_speed ++;
        }
        lcd.setCursor(0, 1);
        lcd.print("   ");
        lcd.setCursor(0, 1);
        lcd.print(current_injection_speed);
        ui_state_count=0;
      }

      if(ui_btn_ok == 2){
        ui_state = UI_STATE_MENU_CONFIRM_VALUE;
      }
    
    break;
    
    case UI_STATE_MENU_SYRINGE:
        if(ui_old_state != UI_STATE_MENU_SYRINGE){
        lcd.clear();
        lcd.print("Taille seringue");
        lcd.setCursor(0, 1);
        lcd.print(ui_syringe_list[0]);
        lcd.setCursor(4, 1);
        lcd.print("mL");
        ui_selection=0;
        ui_state_count=0;
        ui_old_state = ui_state;
      }
      
      //handle the ui button press
      if((ui_btn_prev == 2) && (ui_selection > 0)){
        ui_selection --;
      }
      
      if((ui_btn_next == 2) && (ui_selection < UI_MAX_SYRINGE)){
        ui_selection ++;
      }

      if((ui_btn_prev == 2) || (ui_btn_next == 2)){
        lcd.setCursor(0, 1);
        lcd.print("   ");
        lcd.setCursor(0, 1);
        lcd.print(ui_syringe_list[ui_selection]);
      }
      
       if(ui_btn_ok == 2){
        current_syringe = ui_selection;
        ui_state = UI_STATE_MENU_CONFIRM_VALUE;
      }
  
    break;
  
    case UI_STATE_MENU_CONFIRM_VALUE:
      if(ui_old_state != UI_STATE_MENU_CONFIRM_VALUE){
      lcd.setCursor(0, 0);
      lcd.print("VALEUR ENREGISTREE !");
      ui_selection=0;
      ui_state_count=0;
      ui_old_state = ui_state;
      } 
      
      // 2 seconds timout
      if(ui_state_count > 20){
        ui_state = UI_STATE_MAIN_MENU;
      }
  
    break;
    
    case UI_STATE_MENU_CALIBRATE:
    break;
    
    case UI_STATE_MENU_SELFTEST:
    break;

    case UI_STATE_WATCHDOG:
      if(ui_old_state != UI_STATE_WATCHDOG){
        lcd.clear();
        lcd.print("!! SECURITE ACTIVE !!");
        lcd.setCursor(0, 1);
        lcd.print("Appuyez sur reset");
        ui_state_count=0;
        ui_old_state = ui_state;
      }
      if(ui_blink){
        lcd.setCursor(0, 0);
        lcd.print("                    ");
      }
      else{
        lcd.setCursor(0, 0);
        lcd.print("!! SECURITE ACTIVE !!");
      }
      
      if(analogRead(PIN_WD_FB) > 512){
        ui_state = UI_STATE_READY;
      }
      
    break;
    
    default:
        lcd.clear();
        lcd.print("!! ETAT INCONU !!");
        lcd.setCursor(0, 1);
        lcd.print(ui_state);
        if(ui_btn_ok == 2){      
          ui_state = UI_STATE_READY;
        }

    break;
  }
  
  //if the watchdog is triggered we override the ui state
  if(analogRead(PIN_WD_FB) < 512){
    ui_state = UI_STATE_WATCHDOG;
  }

  
  ui_state_count++;

  //every second
  if(!(ui_state_count %10)){
      ui_blink = !ui_blink;
  }
  
  
  //Picouz state machine
  switch(motor_state){
    
    case MOTOR_STATE_BOOTING:
      // initialize timer1 
      TCCR1A = 0;
      TCCR1B = 0;
      
      TCNT1= 0;
      OCR1A = 0;
      
      TCCR1B |= (1 << CS12) | (1 << CS10);    // /1024 prescaler on timer 1 clock

      motor_state = MOTOR_STATE_READY;
    break;

    case MOTOR_STATE_READY:
      if(motor_old_state != MOTOR_STATE_READY){
        //stop the motor
        digitalWrite(PIN_MOTOR_ENABLE, LOW); 
        digitalWrite(PIN_MOTOR_DIR, LOW);

        //shut down the timer
        TCCR1A = 0;
        TCCR1B = 0;
        
        motor_old_state = motor_state;
      }

      //handle requests from the UI
      if(motor_requested_state == MOTOR_STATE_INJECTING){
        //quick sanity checks before allowing motor movement
        if(current_injection_speed==0) break;
        //then go for it !
        motor_state = MOTOR_STATE_INJECTING;
      }
      else if(motor_requested_state == MOTOR_STATE_FREE_MOVE){
        motor_state = MOTOR_STATE_FREE_MOVE;
      }
    break;

    case MOTOR_STATE_INJECTING:
      if(motor_old_state != MOTOR_STATE_INJECTING){

        //start the motor
        digitalWrite(PIN_MOTOR_ENABLE, HIGH); 
        digitalWrite(PIN_MOTOR_DIR, LOW);

        //make sure the clock is still set to a slow value
        TCCR1B |= (1 << CS12) | (1 << CS10);    // /1024 prescaler on timer 1 clock

        //start the timer using the calculated value
        // value for 1ms : 7.8125 = (8*10^6) / (1000*1024) 
        OCR1A = ml_per_h_to_timer_value(current_injection_speed, config_syringe_step_per_ml[current_syringe]);

        TCCR1A |= (1<< COM1A0); //toggle Oc1a on compare,
        TCCR1B |= (1<< WGM12); //CTC mode (clr TCNT1 on compare);

        motor_old_state = motor_state;
      }

      //handle requests from the UI
      if(motor_requested_state == MOTOR_STATE_READY){
        //then go for it !
        motor_state = MOTOR_STATE_READY;
      }
      else if(motor_requested_state == MOTOR_STATE_FREE_MOVE){
        motor_state = MOTOR_STATE_FREE_MOVE;
      }

    break;

    case MOTOR_STATE_FREE_MOVE:
      if(motor_old_state != MOTOR_STATE_FREE_MOVE){

        //stop the motor
        digitalWrite(PIN_MOTOR_ENABLE, LOW); 
        digitalWrite(PIN_MOTOR_DIR, LOW);

        //shut down the timer
        TCCR1A = 0;
        TCCR1B = 0;

        motor_old_state = motor_state;
      }

      //handle requests from the UI
      if(ui_state == UI_STATE_MENU_MANUAL){
        if(ui_btn_prev == 2){
          //move left
          digitalWrite(PIN_MOTOR_ENABLE, HIGH); 
          digitalWrite(PIN_MOTOR_DIR, HIGH);
          //start the timer
          OCR1A = CONFIG_MOTOR_FREE_MOVE_SPEED;
          TCCR1A |= (1<< COM1A0); //toggle Oc1a on compare,
          TCCR1B |= (1<< WGM12); //CTC mode (clr TCNT1 on compare);
          TCCR1B |= (1 << CS10) |(1 << CS11);    // /64 prescaler on timer 1 clock
        }
        else if((ui_btn_next == 2)){
          //move right
          digitalWrite(PIN_MOTOR_ENABLE, HIGH); 
          digitalWrite(PIN_MOTOR_DIR, LOW);
          //start the timer
          OCR1A = CONFIG_MOTOR_FREE_MOVE_SPEED;
          TCCR1A |= (1<< COM1A0); //toggle Oc1a on compare,
          TCCR1B |= (1<< WGM12); //CTC mode (clr TCNT1 on compare);
          TCCR1B |= (1 << CS10) |(1 << CS11);    // /64 prescaler on timer 1 clock
        }
        else if((ui_btn_prev == 0) && (ui_btn_next == 0)){
          //stop the motor
          digitalWrite(PIN_MOTOR_ENABLE, LOW); 
          digitalWrite(PIN_MOTOR_DIR, LOW);
          //shut down the timer
          TCCR1A = 0;
          TCCR1B = 0;
        }
      }
      else{
        //stop the motor
        digitalWrite(PIN_MOTOR_ENABLE, LOW); 
        digitalWrite(PIN_MOTOR_DIR, LOW);
        //shut down the timer
        TCCR1A = 0;
      }
      
      if(motor_requested_state == MOTOR_STATE_READY){
        //then go for it !
        motor_state = MOTOR_STATE_READY;
      }
    break;

    case MOTOR_STATE_WATCHDOG:
      if(motor_old_state != MOTOR_STATE_READY){
        //shut down the timer
        
        motor_old_state = motor_state;
      }

      //if watchdog reset, go back to ready
      if(analogRead(PIN_WD_FB) > 512){
        motor_state = UI_STATE_READY;
      }

    break;

    default:
    break;    
  }
  
  //if the watchdog is triggered we override the motor state
  if(analogRead(PIN_WD_FB) < 512){
    motor_state = MOTOR_STATE_WATCHDOG;
  }

  
  // Watchdog subroutine : should be called at least every second to prevent the hardware from going into safe mode
  wd_count=0;
  if(analogRead(PIN_WD_ADC)< WD_THRESHOLD_MIN)
  while(analogRead(PIN_WD_ADC) < WD_THRESHOLD_MAX){
      pinMode(PIN_WD_IO, OUTPUT);
      digitalWrite(PIN_WD_IO, HIGH);
      delay(1);
      wd_count ++;
  }
  pinMode(PIN_WD_IO, INPUT);

  //debug
  
  Serial.print("ui_state = ");
  Serial.print(ui_state);
  Serial.print(" motor_state = ");
  Serial.print(motor_state);
  Serial.print(" OCR1A = ");
  Serial.print(OCR1A);
  Serial.print("\n");

  //wait between loops
  if(wd_count < 100) delay(100 - wd_count);               
}
