#define EXTEND_PIN 5
#define RETRACT_PIN 3
#define SENSOR_PIN A1

// Position of the piston
int Position = 0;
int prev_position = 0;
int tolerance = 1 << 4; // Position tolerance of 1/64 total (theoretical) travel.
int projected = 0;

// Filtering variables
int raw_position = 0;

// input variables
int input = 0;
int target = 0;

// Valve state variables
#define CLOSED 0
#define EXTENDING 1
#define RETRACTING 2
int valve_state = CLOSED;
//int new_state = CLOSED;
//unsigned long valve_last_changed = millis();
//#define VALVE_ACTIVATION 12

// Toggle whether or not to limit further changes in movement
bool freeze = true;

const int tick = 8;

// Timing variables
unsigned long dong;
unsigned long now;
unsigned long last_tick;
unsigned long time_delta;

// Psuedo-velocity value; assumes constant-enough loop time
int raw_velocity = 0;
int velocity = 0;
#define V_MULTIPLIER 10
// 'Velocity' threshold to consider stopped.
// TODO: Measure/santiy-check; possibly update dynamically.
int v_min = 40;


void setup() {

  pinMode(EXTEND_PIN,OUTPUT);
  pinMode(RETRACT_PIN,OUTPUT);
  pinMode(SENSOR_PIN,INPUT);

  Serial.begin(115200);
  // Set target so piston doesn't suddenly move upon startup
  target = analogRead(SENSOR_PIN);
  // Initialize microsmoothing filter
  //*input_history = target;
  Position = target;
  // Set initial timestamp
  dong = millis();
  last_tick = dong;

}


void loop() {

  //delay(1); // Make sure there's enough delay in the loop to actualy get meaningful velocity
  // Read input & 'unfreeze' if input is available.
  // TODO: Replace 'if' block with function leveraging Firmata protocol
  if (Serial.available() > 0) {
//    input = Serial.read();
//    target = 4 * input;
  input = Serial.read() - '0';
  target = 100 * input;
    Serial.print("Position: ");
    Serial.println(Position);
    Serial.print("Target: ");
    Serial.println(target);
    // Unfreeze to allow motion. Only time freeze is ever set to false.
    freeze = false;
  }

  now = millis();

  time_delta = now - dong;
  if (time_delta == 0) { // Probably unnecessary, but divide by zero can make chips explode.
    return;
  }
  // Read position
  raw_position = analogRead(SENSOR_PIN);
  ema_filter(raw_position, &Position);

  // Calculate velocity
  raw_velocity = ((V_MULTIPLIER * Position) - (V_MULTIPLIER * prev_position)) / (int)time_delta;  // Division is painfully slow
  ema_filter(raw_velocity, &velocity);
  Serial.print(raw_position); Serial.print(" ");
  Serial.print(Position); Serial.print(" ");
  Serial.print(time_delta); Serial.print(" ");
  Serial.print(raw_velocity); Serial.print(" ");
  Serial.println(velocity);

  // Update old state
  prev_position = Position;
  dong = now;

  // If it hasn't been a tick, restart loop. Otherwise update solenoid control
  if ((now - last_tick) < tick) {
    return;
  }
  // Update tick timestamp
  last_tick = now;

  //===============================================================//
  //======================== Control logic ========================//
  //===============================================================//

  // projected position = position + velocity * tick_length
  projected = Position + (velocity / V_MULTIPLIER * tick);
  //Serial.println(velocity);

  if (velocity > v_min) { // physically extending
    if (projected > target + tolerance) {  // Going too fast
      Serial.println("State 1");
      switch (valve_state) {
        case CLOSED:
          if (freeze) {
            // No change
            break;
          }
          set_valve(RETRACTING);
          break;
        case EXTENDING:
          //freeze = true;
          set_valve(RETRACTING);
          break;
        case RETRACTING:
          if (freeze) {
            set_valve(CLOSED);
          } // else: No change
          break;
      }
    // velocity > v_min
    } else if (projected < target - tolerance) { // Keep going
      Serial.println("State 2");
      switch (valve_state) {
        case CLOSED: // Identical to RETRACTING below
          if (freeze) {
            // No change
            break;
          } else {
            set_valve(EXTENDING);
          }
          break;
        case EXTENDING:
          // No change
          break;
        case RETRACTING:
          if (freeze) {
            set_valve(CLOSED);
          } else {
            set_valve(EXTENDING);
          }
          break;
      }
    } else { // velocity > 0, projected ~= target
     Serial.println("State 3");
      switch (valve_state) {
        case CLOSED:
         // freeze = true;
          // Valve is already closed
          break;
        case EXTENDING:
          // freeze is unchanged
          set_valve(CLOSED);
          break;
        case RETRACTING:
          freeze = true;
          Serial.print("Frozen ");
          set_valve(CLOSED);
          break;
      }
    }
  } else if (velocity < -v_min) {  // physically retracting
    if (projected > target + tolerance) {  // Keep going
      Serial.println("State 4");
      switch (valve_state) {
        case CLOSED:
          if (freeze) {
            // No change
            break;
          } else {
            set_valve(RETRACTING);
          }
          break;
        case EXTENDING:
          if (freeze) {
            set_valve(CLOSED);
          } else {
            set_valve(RETRACTING);
          }
          break;
        case RETRACTING:
          // No change
          break;
      }
    // velocity < -v_min, retracting
    } else if (projected < target - tolerance) {  // Going too fast
      Serial.println("State 5");
      switch (valve_state) {
        case CLOSED:
          if (freeze) {
            // No change
            break;
          } // else: Deliberately fall through
        case RETRACTING:
          //freeze = true;
          set_valve(EXTENDING);
          break;
        case EXTENDING:
          if (freeze) {
            set_valve(CLOSED);
          } // else: No change
          break;
      }
    } else { // velocity < 0, projected ~= target
      Serial.println("State 6");
      switch (valve_state) {
        case CLOSED:
          //freeze = true;
          // Valve already closed
          break;
        case EXTENDING:
          freeze = true;
          Serial.print("Frozen ");
          set_valve(CLOSED);
          break;
        case RETRACTING:
          // freeze is unchanged
          set_valve(CLOSED);
          break;
      }
    }
  } else { // Velocity ~= 0
    //Serial.println("~0 velocity");
    if (freeze) {
      //Serial.println("7");
      set_valve(CLOSED);
    } else { // freeze == false
      if (Position > target + tolerance) {
        //Serial.println("8");
        set_valve(RETRACTING);
      } else if (Position < target - tolerance) {
        //Serial.println("9");
        set_valve(EXTENDING);
      } else { // Position ~= target, velocity ~= 0  ==> no movement
       Serial.print("Frozen ");
       Serial.println(Position);
        freeze = true;
        set_valve(CLOSED);
      }
    }
  }

  //===============================================================//
  //====================== End control logic ======================//
  //===============================================================//
}

/*
      switch (valve_state) {
        case CLOSED:

          break;
        case EXTENDING:

          break;
        case RETRACTING:

          break;
      }
*/


void set_valve(int new_state) {
  //Serial.println(new_state);

  if (valve_state == new_state) {
    return;
  }
//  if ((millis() - valve_last_changed) < VALVE_ACTIVATION) {
//    return -1;
//  }
  switch (new_state) {
    case CLOSED:
      Serial.println("CLOSED");
      digitalWrite(RETRACT_PIN, LOW);
      digitalWrite(EXTEND_PIN, LOW);
      break;
    case EXTENDING:
      Serial.println("EXTENDING");
      digitalWrite(RETRACT_PIN, LOW);
      digitalWrite(EXTEND_PIN, HIGH);
      break;
    case RETRACTING:
      Serial.println("RETRACTING");
      digitalWrite(EXTEND_PIN, LOW);
      digitalWrite(RETRACT_PIN, HIGH);
      break;
  }
  valve_state = new_state;
//  valve_last_changed = millis();
//  return 1;
}

// Uses an alpha of 1/8 to enable bit-shifting.
void ema_filter(int input_value, int *average) {
   *average = ( 2 * input_value + 18 * (*average) ) / 20;
//  *average = ((input_value << 2) + // input * 4
//              (*average << 5) - (*average << 2) + // average * (32 - 4)
//              16 // Add 2^4 to cause bitshift below to round instead of truncate
//             ) >> 5; // divide by 32
}