 #include <TimerOne.h>
 
-#define LED_R         6
-#define LED_G         9
-#define LED_B        10
-#define BUTTON        2
-#define PUSHLOCK_NO   7
-#define PUSHLOCK_NC   3
-
-#define LOCKED      LOW
-#define UNLOCKED   HIGH
-#define UNDEFINED     2
+#define LED_R                       6
+#define LED_G                       9
+#define LED_B                      10
+
+#define BUTTON                      2
+#define PUSHLOCK_NO                 7
+#define PUSHLOCK_NC                 3
+
+#define LOCKED                    LOW
+#define UNLOCKED                 HIGH
+#define UNDEFINED                   2
+
 #define HEARTBEAT_TIMEOUT       10000
 #define HEARTBEAT_SMALL_TIMEOUT  3333
 
-#define RUN           8
+#define RUN                         8
+
+#define BAUDRATE               115200
+
+//isr
+void isr_button_push();       //obsolete?
+void isr_pushlock_change();
+
+//physical routines
+void serial_flush();
+void touch_run_pin();
+
+//heartbeat detection
+long detect_heartbeat_any();
+long detect_heartbeat();
+long detect_heartbeat_ack();
+long detect_heartbeat_abort();
+long get_last_heartbeat();
+
+//heartbeat animations
+void led_heartbeat();
+void led_heartbeat_missing();
+void led_heartbeat_ack();
+void led_heartbeat_abort();
+
+//generic led routines
+void led_color(byte r, byte g, byte b);
+void led_off();
+void led_red();
+
+//States                          //state matching led animations
+void *server_bootable();          void animate_bootable();
+void *server_booting();           void animate_booting();
+void *server_running();           void animate_running();
+void *server_shutdown();          void animate_shutdown();
+void *server_shutdown_active();   void animate_shutdown_active();
+void *server_hungup();            void animate_hungup();
+void *server_reset();             void animate_reset();
+void *server_locked();            void animate_locked();
+
+//FSM
+typedef void *(*StateFunc)();
+StateFunc statefunc = server_bootable();
+
+//++++ cleanup due!
 
 byte button = 0;
 byte pushlock = UNDEFINED;

int cmode = 0;

int incomingByte = 0;
unsigned long tracker, tracker2, beat;

typedef void *(*StateFunc)();

//States
void *server_bootable();
void *server_start();
void *server_booting();
void *server_running();
void *server_shutdown();
void *server_hungup();
void *server_reset();
void *server_locked();

StateFunc statefunc;

void setup() {
  pinMode(BUTTON, INPUT_PULLUP);
  pinMode(PUSHLOCK_NO, INPUT_PULLUP);
  pinMode(PUSHLOCK_NC, INPUT_PULLUP);

  pinMode(LED_R, OUTPUT);
  pinMode(LED_G, OUTPUT);
  pinMode(LED_B, OUTPUT);

  pinMode(RUN, OUTPUT);
  
  attachInterrupt(digitalPinToInterrupt(BUTTON), button_push, FALLING);
  attachInterrupt(digitalPinToInterrupt(PUSHLOCK_NO), pushlock_change, CHANGE);
  //attachInterrupt(digitalPinToInterrupt(PUSHLOCK_NC), pushlock_open, CHANGE);

  pushlock = digitalRead(PUSHLOCK_NO);
  
//  if (pushlock == LOCKED) {
//    statefunc = server_locked;
//  } else {
    statefunc = server_booting;
//  }
  tracker = millis();
  tracker2 = tracker;

  Serial.begin(115200);
}

void loop() {
  statefunc = (StateFunc)(*statefunc)();
}

void touch_run_pins() {
  digitalWrite(RUN, HIGH);
  delay(50);
  digitalWrite(RUN, LOW);
}

void fade_bootable(byte del) {
  analogWrite(LED_R, 255);
  analogWrite(LED_G, 255-c_g);
  analogWrite(LED_B, 255);

  c_g = c_g + fade_g;
  
  if ( c_g <= 0 || c_g >= 87 ) {
    fade_g = -fade_g;
  }

  if (fade_g > 0) {
   delay(del/2); 
  } else {
   delay(del); 
  }
}

void fade_shutdown(byte del) {
  analogWrite(LED_R, 255-c_r);
  analogWrite(LED_G, 255-c_r);
  analogWrite(LED_B, 255);

  c_r = c_r + fade_b;
  
  if ( c_r <= 0 || c_r >= 140 ) {
    fade_b = -fade_b;
  }

  if (fade_b > 0) {
   delay(del); 
  } else {
   delay(del/3); 
  }
}

void fade_hungup(byte del) {
  analogWrite(LED_R, 255-c_r);
  analogWrite(LED_G, 255);
  analogWrite(LED_B, 255);

  c_r = c_r + fade_r;
  
  if ( c_r <= 0 || c_r >= 240 ) {
    fade_r = -fade_r;
  }
  
  delay(del);
}

void show_red() {
  analogWrite(LED_R, 0);
  analogWrite(LED_G, 255);
  analogWrite(LED_B, 255);
}

void show_color(byte r, byte g, byte b) {
  analogWrite(LED_R, 255-r);
  analogWrite(LED_G, 255-g);
  analogWrite(LED_B, 255-b);
}

void show_none() {
  show_color(0,0,0);
}

void show_heartbeat() {
  show_color(0,255,0);
  delay(10);
  show_color(255,0,0);
  delay(10);
  show_color(0,0,0);
  delay(15);
}

void show_missing_heartbeat() {
  show_color(255,0,0);
  delay(10);
  show_color(255,0,0);
  delay(10);
  show_color(0,0,0);
  delay(15);
}

void fade_boot(byte del) {
  analogWrite(LED_R, 255-c_r);
  analogWrite(LED_G, 255-c_g);
  analogWrite(LED_B, 255-c_b);

  //analogWrite(LED_G, 255);
  //analogWrite(LED_B, 255);

  c_r = c_r + fade_r;
  c_g = c_g + fade_g;
  c_b = c_b + fade_b;

  if ( c_r <= 0 || c_r >= 200 ) {
    fade_r = -fade_r;
  }
  if ( c_g <= 0 || c_g >= 255 ) {
    fade_g = -fade_g;
  }
  if ( c_b <= 0 || c_b >= 155 ) {
    fade_b = -fade_b;
  }

  fade_delay = fade_delay + fdel;
  if (fade_delay <= 1 || fade_delay >=del) {
    fdel = -fdel;
  }
  delay(fade_delay);
}

void button_push() {
  button = 1;
  //delay(50);
}

void pushlock_change() {
  pushlock = digitalRead(PUSHLOCK_NO);
}

void serial_flush() {
  int buf;
  while (Serial.available() > 0) {
      buf = Serial.read();
  }
}

long heartbeat() {
  int last_heartbeat = millis()-tracker;

//  if (!Serial) {
//    return -1;
//  }

  if (((millis()-tracker2) > 3000) && last_heartbeat > 3000) {
    show_missing_heartbeat();
    tracker2 = millis();
  }
  
  if (Serial.available() > 0) {
    incomingByte = Serial.read();
    
    serial_flush();
    
    if (incomingByte == 0x42) {
      tracker = millis();
      tracker2 = tracker;
      show_heartbeat();
      return last_heartbeat;
    } else {
      return -1;
    }
  } else {
    return 0;
  }
}

long last_heartbeat() {
  return millis()-tracker;
}

void *server_bootable() {
  if (pushlock == LOCKED) {
    show_red();
    delay(80);
    return server_locked;
  } else {
    fade_bootable(45);
    
    if ((last_heartbeat() < HEARTBEAT_TIMEOUT || heartbeat() > 0 )) { // && Serial
      serial_flush();
      return server_booting;
    }
    
    if (digitalRead(BUTTON)) {
      delay(100);
      tracker = millis();
      serial_flush();
      return server_start;
    } else {

  }
}

void *server_booting() {
  if (Serial) {
    Serial.println("?");
  }

  if (last_heartbeat() > 10000) {

    return server_bootable;
  }

  if (heartbeat() <= 0) {
    fade_boot(40);

    return server_booting;
  } else {

    return server_running;
  }
}

void *server_running() {
  led_off();

  heartbeart = detect_heartbeat();

  if (Serial) {
    Serial.println("running");
    Serial.println(last_heartbeat());
  }

  if (pushlock == LOCKED) {
    if (last_heartbeat() > 30000 || detect_heartbeat() < 0 ) {

      return server_locked;
    } else {
      track_uart = millis();
      return server_shutdown;
    }
  } else {
    if (last_heartbeat() > HEARTBEAT_TIMEOUT) {

      return server_hungup;
    } else {

      return server_running;
    }
  }

}

void *server_shutdown() {

  if ((millis()-track_uart) > 5000) {
    if (Serial) {
      Serial.println("shutdown");
      delay(100);
    }
    track_uart = millis();
  }

  //fade_shutdown(35);
  show_red();

  if (detect_heartbeat_ack() == 1) {
    return server_shutdown_active;
  } else {
    return server_shutdown;
  }
}

void *server_shutdown_active() {
  heartbeart = detect_heartbeat();
  show_missing_heartbeat();
  delay(100);
  show_red();

  if (digitalRead(BUTTON)) {
    delay(100);
    track_last_heartbeat = millis();
    serial_flush();
    if (detect_heartbeat_abort() == 1) {
      if (Serial) {
        Serial.println("abort shutdown");
      }
      return server_running;
    }
  }

  if (last_heartbeat() < HEARTBEAT_TIMEOUT) {
    serial_flush();
    return server_shutdown_active;
  } else {
    serial_flush();
    return server_locked;
  }
}

void *server_hungup() {
  if (Serial) {
    Serial.println("hung up");
    Serial.println(last_heartbeat());
  }

  heartbeart = detect_heartbeat();
  heartbeart = last_heartbeat();

  if (heartbeart < HEARTBEAT_TIMEOUT || detect_heartbeat() > 0) {

    return server_running;
  } else {

    if (heartbeart > HEARTBEAT_TIMEOUT && heartbeart < 20000UL) {
      fade_hungup(25);
    } else if (heartbeart > 20000UL && heartbeart < 30000UL) {
      fade_hungup(15);
    } else if (heartbeart > 30000UL && heartbeart < 450000UL) {
      fade_hungup(5);
    }

    if (heartbeart > 45000UL) {
      show_red();
    }

    if (last_heartbeat() > 60000UL) {

      return server_reset;
    } else {

      return server_hungup;
    }

  }
}

void *server_reset() {
  if (Serial) {
    Serial.println("touching run pins on pi");
  }
  touch_run_pin();
  serial_flush();
  return server_booting;
}

void *server_locked() {
  if (Serial) {
    Serial.println("LOCKED");
  }
  if (pushlock == LOCKED) {
    led_off();
    serial_flush();
    return server_locked;
  } else {
    serial_flush();
    return server_bootable;
  }
}
