 void start_servo(unsigned long current_millis) {
   myservo.write(SERVO_BASE_ANGLE);
   current_angle = 0;
+  current_max_angle = min(SERVO_MAX_ANGLE, get_max_angle());
   current_velocity_positive = true;
   previous_servo_move = current_millis;
-}
-
-void switch_servo() {
-  int num_steps = 4;
-  int base_angle = 90;
-  int step = SERVO_MAX_ANGLE / num_steps;
-
-  int interval = 60000 / current_bpm;
-  int delay_ms = interval * 4 / 5 / (num_steps * 2);
-
-  for (int i = 0; i < num_steps; ++i) {
-    myservo.write(base_angle + i * step);
-    delay(delay_ms);
-  }
-  for (int i = num_steps; i > 0; --i) {
-    myservo.write(base_angle + i * step);
-    delay(delay_ms);
-  }
-  myservo.write(base_angle);
 }
 
 int translate_ir() {


  if (previous_servo_move && current_millis - previous_servo_move >= SERVO_INTERVAL) {
    // check if it should return
    if (current_angle >= 60000 / current_bpm * 4 / 5 / (2 * SERVO_INTERVAL / DEGREE_STEP)) {
      --current_angle;
      current_velocity_positive = !current_velocity_positive;
    }
    // move the servo
    current_angle += (current_velocity_positive ? 1 : -1) * DEGREE_STEP;
    myservo.write(SERVO_BASE_ANGLE + current_angle);
    if (current_angle == 0 && !current_velocity_positive) {
      previous_servo_move = 0;
    } else {
      previous_servo_move += SERVO_INTERVAL;
    }
  }
}

void flash_beat(bool is_downbeat, unsigned long current_millis) {
  digitalWrite(LED_BUILTIN, HIGH);
  digitalWrite(is_downbeat ? RED : BLUE, HIGH);
  digitalWrite(is_downbeat ? LED_1 : LED_2, HIGH);
  start_servo(current_millis);
  (is_downbeat ? previous_blink_downbeat : previous_blink_upbeat) = current_millis;
}

void stop_beat() {
  digitalWrite(LED_BUILTIN, LOW);
  digitalWrite(RED, LOW);
  digitalWrite(GREEN, LOW);
  digitalWrite(BLUE, LOW);
  digitalWrite(LED_1, LOW);
  digitalWrite(LED_2, LOW);
}

void stop_blink(bool is_downbeat) {
  digitalWrite(LED_BUILTIN, LOW);
  digitalWrite(is_downbeat ? RED : BLUE, LOW);
  digitalWrite(is_downbeat ? LED_1 : LED_2, LOW);
  (is_downbeat ? previous_blink_downbeat : previous_blink_upbeat) = 0;
}

    switch (translate_ir()) {
      case 1: flash_beat(true, current_millis); break;
      case 2: flash_beat(false, current_millis); break;
      default: break;
    } 
    irrecv.resume();
