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

enum class Direction : int8_t { up = 1, down = -1 };
enum AirQuality : uint8_t { good, medium, poor, veryPoor, danger };

constexpr uint8_t SERVO_PIN {6};
constexpr int servoPos[5] {0, 45, 90, 135, 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 ... 799: targetPos = servoPos[AirQuality::good]; break;
    case 800 ... 999: targetPos = servoPos[AirQuality::medium]; break;
    case 1000 ... 1399: targetPos = servoPos[AirQuality::poor]; break;
    case 1400 ... 1999: targetPos = servoPos[AirQuality::veryPoor]; break;
    default: targetPos = servoPos[AirQuality::danger]; 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, 2500);
  if (co2Value != lastCo2Value) {
    lastCo2Value = co2Value;
    showCO(co2Value, servo);
  }
}