#include <Servo.h>
#include <Streaming.h>
Print &cout {Serial};

enum class Direction : int8_t { up = 1, down = -1 };
enum AirQuality : uint8_t { good, bad };

constexpr uint8_t SERVO_PIN {6};
constexpr int servoPos[5] {0, 180};

Servo servo;

int setServo(Servo &sv, uint16_t position, Direction dir, uint16_t stepDelay_ms = 5) {
  sv.write(position);
  position += static_cast<int8_t>(dir);
  delay(stepDelay_ms);
  return position;
}

void showCO(uint16_t co2V, Servo &sv) {
  int pos = sv.read();
  int targetPos;

  switch (co2V) {
    case 0 ... 999: targetPos = servoPos[AirQuality::good]; break;
    default: targetPos = servoPos[AirQuality::bad]; break;
  }
  do { pos = setServo(sv, pos, (pos > targetPos) ? Direction::down : Direction::up); } while (pos != targetPos);
  cout << _FMT("CO2: % -> Pos: %", _WIDTH(co2V, 4), _WIDTH(pos, 3)) << "°\n";
}

void setup() {
  Serial.begin(115200);
  cout << "Start" << __FILE__ << endl;
  servo.attach(SERVO_PIN);
  servo.write(servoPos[AirQuality::good]);
}

void loop() {
  static uint8_t lastCo2Value {0};
  delay(2000);
  uint16_t co2Value = random(0, 1500);
  if (co2Value != lastCo2Value) {
    lastCo2Value = co2Value;
    showCO(co2Value, servo);
  }
}