// Constants set by the user
#define RELAY_PIN (3)
#define ENCODER_PIN (2)
#define DIRECTION_PIN (5)
#define LIQUID_SENSOR_PIN (4)

#define DUTY_CYCLE_CHANGE_RATE (20)
#define RPM_TIMEOUT_SECONDS (1.5)
#define RPM_TOLERANCE (10)
#define MIN_RPM (120)
#define MAX_RPM (2800)
#define DEFAULT_RPM (350)

#define RPM_TIMEOUT_MICROS (RPM_TIMEOUT_SECONDS * 1000000)

// For calculating the RPMs
unsigned int total_turns = 0;
unsigned long last_turn_period_micros = 0;
unsigned long last_turn_micros = 0;
unsigned int desired_rpm;

bool desired_direction_forward;
bool desired_mode_serve;

int duty_cycle = 0;

 byte clamp(int value){
  if (value >= 255){
    return 255;
  }
  if (value <= 0){
    return 0;
  }
  return ( byte) value;
}

unsigned int clamp_rpm(unsigned int rpm){
  if (rpm >= MAX_RPM){
    return MAX_RPM;
  }
  if (rpm <= MIN_RPM){
    return MIN_RPM;
  }
  return rpm;
}
// Count a single turn
void count_turn() {
  unsigned long current_turn_micros = micros();
  total_turns += 1;
  last_turn_period_micros = current_turn_micros - last_turn_micros;
  last_turn_micros = current_turn_micros;
}



void display_variables(unsigned long actual_duration_millis,
                        unsigned int instant_rpm,
                        unsigned int total_turns,
                         byte duty_cycle) {
  Serial.print("Time: ");
  Serial.print(actual_duration_millis / 1000.0);
  Serial.print("\trpms: ");
  Serial.print(instant_rpm);
  Serial.print("\tTurns: ");
  Serial.print(total_turns);
  Serial.print("\tDuty: ");
  Serial.print(duty_cycle);
  Serial.print("\tLiquid: ");
  Serial.println(there_is_liquid_in_the_line());
}

inline bool there_is_no_user_input(){
  return Serial.available() == 0;
}
unsigned long prompt_for_desired_duration_millis(){
  Serial.read();
  Serial.print("How long (seconds) do you want the motor to rotate? (ENTER for default of infinite): ");
  while (there_is_no_user_input()) {
    // Wait for user input
  }
  float input_float = Serial.parseFloat();
  if (input_float == 0.0) {
    Serial.println("Infinite");
    return 100000000L;
  }
  Serial.println(input_float);
  return (unsigned long) (input_float * 1000.0);
}

unsigned int prompt_for_desired_rpm(){
  Serial.read();
  Serial.print("What speed (in rpm) do you want the motor to rotate? (ENTER for default of 350 rpm): ");
  while (there_is_no_user_input()) {
    // Wait for user input
  }
  unsigned int desired_rpm = Serial.parseInt();
  if (desired_rpm == 0){
    desired_rpm = DEFAULT_RPM;
  }
  desired_rpm = clamp_rpm(desired_rpm);

  Serial.print(desired_rpm);
  Serial.println(" rpm");
  return desired_rpm;
}
unsigned int prompt_for_desired_turns(){
   Serial.read();
  Serial.print("Set a MAXIMUM amount of turns (ENTER for default of infinite): ");
  while (there_is_no_user_input()) {
    // Wait for user input
  }
  unsigned int input_int = Serial.parseInt();
  Serial.read();
  if (input_int == 0) {
    Serial.println("Infinite");
    return -1;
  }
    Serial.println(input_int);
  return input_int;

}
bool prompt_for_desired_direction_forward(){
  Serial.read();
  Serial.print("Choose direction (either fF (forward) or bB (backward)) (ENTER for default of forward): ");
  while (there_is_no_user_input()) {
    // Wait for user input
  }
  String directionstr = Serial.readString();
  if (directionstr[0] == 'b' || directionstr[0] == 'B'){
    Serial.println("backward");
    return false;
  }
  Serial.println("forward");
  return true;
}

bool prompt_for_mode_serve(){
  Serial.read();
  Serial.print("Choose mode (either pP (prime) or sS (serve)) (ENTER for default of serve): ");
  while (there_is_no_user_input()) {
    // Wait for user input
  }
  String modestr = Serial.readString();
  if (modestr[0] == 'p' || modestr[0] == 'P'){
    Serial.println("prime");
    return false;
  }
  Serial.println("serve");
  return true;
}

bool there_is_liquid_in_the_line(){
    return digitalRead(LIQUID_SENSOR_PIN) == HIGH;
}



void serve_loop() {
  unsigned int desired_duration_millis = prompt_for_desired_duration_millis();
  unsigned int desired_turns = prompt_for_desired_turns();

  unsigned int instant_rpm = 0;
  unsigned int actual_duration_millis = 0;
  unsigned long start_time_millis;

  last_turn_period_micros = -1;
  total_turns = 0;

  Serial.println("Starting!");
  delay(500);
  start_time_millis = millis();
  digitalWrite(RELAY_PIN, HIGH);
  duty_cycle = 0;
  while (
          (total_turns < desired_turns) && 
          (actual_duration_millis < desired_duration_millis) && 
          (there_is_no_user_input())
        )
  {
    instant_rpm = (60000000 / last_turn_period_micros);
    if (micros() - last_turn_micros > RPM_TIMEOUT_MICROS) {
      // Timeout if we havent had any encoder clicks in a h
      instant_rpm = 0;
    }
    if (instant_rpm >= desired_rpm + RPM_TOLERANCE){
      duty_cycle -= DUTY_CYCLE_CHANGE_RATE;
    }
    if (instant_rpm <= desired_rpm - RPM_TOLERANCE){
      duty_cycle += DUTY_CYCLE_CHANGE_RATE;
    }
    duty_cycle = clamp(duty_cycle);
    analogWrite(RELAY_PIN, duty_cycle);
    display_variables(actual_duration_millis, instant_rpm, total_turns, duty_cycle);
    actual_duration_millis = millis() - start_time_millis;
  }
  analogWrite(RELAY_PIN, 0);
  Serial.println("Stopped!");
  Serial.print("Overall result: ");
  display_variables(actual_duration_millis, (60000L*total_turns)/actual_duration_millis, total_turns, 0);
}


void prime_loop() {
  unsigned int instant_rpm = 0;
  unsigned int actual_duration_millis = 0;
  unsigned long start_time_millis;

  last_turn_period_micros = -1;
  total_turns = 0;

  Serial.println("Priming!");
  delay(500);
  start_time_millis = millis();
  digitalWrite(RELAY_PIN, HIGH);
  duty_cycle = 0;
  while (
          !there_is_liquid_in_the_line()
        )
  {
    instant_rpm = (60000000 / last_turn_period_micros);
    if (micros() - last_turn_micros > RPM_TIMEOUT_MICROS) {
      // Timeout if we havent had any encoder clicks in a h
      instant_rpm = 0;
    }
    if (instant_rpm >= desired_rpm + RPM_TOLERANCE){
      duty_cycle -= DUTY_CYCLE_CHANGE_RATE;
    }
    if (instant_rpm <= desired_rpm - RPM_TOLERANCE){
      duty_cycle += DUTY_CYCLE_CHANGE_RATE;
    }
    duty_cycle = clamp(duty_cycle);
    analogWrite(RELAY_PIN, duty_cycle);
    display_variables(actual_duration_millis, instant_rpm, total_turns, duty_cycle);
    actual_duration_millis = millis() - start_time_millis;
  }
  analogWrite(RELAY_PIN, 0);
  Serial.println("Primed!");
  Serial.print("Overall result: ");
  display_variables(actual_duration_millis, (60000L*total_turns)/actual_duration_millis, total_turns, 0);
}


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

  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW);

  pinMode(DIRECTION_PIN, OUTPUT);
  digitalWrite(DIRECTION_PIN, LOW);

  pinMode(LIQUID_SENSOR_PIN, INPUT);
  pinMode(ENCODER_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCODER_PIN), count_turn, RISING);
  interrupts();

  desired_mode_serve = prompt_for_mode_serve();
  desired_rpm = prompt_for_desired_rpm();
  desired_direction_forward = prompt_for_desired_direction_forward();
}

void loop(){
  if (desired_mode_serve){
    serve_loop();
  }
  else {
    prime_loop();
  }

}