#define MUTE 18
#define HOLE01 1
#define HOLE02 19
#define HOLE1 10
#define HOLE2 4
#define HOLE3 6
#define HOLE4 5
#define HOLE5 7
#define HOLE6 20
#define HOLE7 21

#define freq 50
#define resolution 12
#define PWMCHANNEL0 0
#define PWMCHANNEL1 1
#define PWMCHANNEL2 2
#define PWMCHANNEL3 3
#define PWMCHANNEL4 4
#define PWMCHANNEL5 5

#define closed01 205
#define open01 220
#define closed02 280
#define open02 300
#define closed2 225
#define open2 200
#define closed3 235
#define open3 220
#define closed4 235
#define open4 200
#define closed5 265
#define open5 240

int min_counts = 102;
int max_counts = 501;

struct ServoState {
  int pin;
  float currentPos;
  float targetPos;
  float speed;
};

struct Pitch {
  int hole01;
  int hole02;
  int hole1;
  int hole2;
  int hole3;
  int hole4;
  int hole5;
  int hole6;
  int hole7;
};

const Pitch NOTE_C6 {closed01,closed02,0,closed2,closed3,closed4,closed5,0,0};//Change the function according to each note
const Pitch NONE {open01,open02,0,open2,open3,open4,open5,0,0};

struct Note {
  Pitch notePitch;
  int duration;
  int restGap;
};

const int MELODY_LEN = 1;
Note melody[MELODY_LEN] = {
  {NOTE_C6, 1000,0}
};

ServoState Servos[6];
unsigned long lastServoUpdate = 0;
unsigned long tickStart = 0;

enum State {PLAY, REST} state;

void setup() {
  delay(5000);
  Serial.begin(921600);
  Serial.println("Only play E6, MUTE HIGH only after single note");

  ledcAttachChannel(HOLE01, freq, resolution, PWMCHANNEL0);
  ledcAttachChannel(HOLE02, freq, resolution, PWMCHANNEL1);
  ledcAttachChannel(HOLE2, freq, resolution, PWMCHANNEL2);
  ledcAttachChannel(HOLE3, freq, resolution, PWMCHANNEL3);
  ledcAttachChannel(HOLE4, freq, resolution, PWMCHANNEL4);
  ledcAttachChannel(HOLE5, freq, resolution, PWMCHANNEL5);

  ledcWrite(HOLE01, closed01);
  ledcWrite(HOLE02, closed02);
  ledcWrite(HOLE2, closed2);
  ledcWrite(HOLE3, closed3);
  ledcWrite(HOLE4, closed4);
  ledcWrite(HOLE5, closed5);

  Servos[0] = {HOLE01, closed01, closed01, 1.0};
  Servos[1] = {HOLE02, closed02, closed02, 1.0};
  Servos[2] = {HOLE2, closed2, closed2, 1.0};
  Servos[3] = {HOLE3, closed3, closed3, 1.0};
  Servos[4] = {HOLE4, closed4, closed4, 1.0};
  Servos[5] = {HOLE5, closed5, closed5, 1.0};

  pinMode(HOLE1, OUTPUT);
  pinMode(HOLE6, OUTPUT);
  pinMode(HOLE7, OUTPUT);
  pinMode(MUTE, OUTPUT);
  digitalWrite(HOLE1, HIGH);
  digitalWrite(HOLE6, LOW);
  digitalWrite(HOLE7, LOW);
  digitalWrite(MUTE, LOW);

  state = PLAY;
  delay(1000);
  Serial.println("Start E6 loop");
}

void loop() {
  if (millis() - lastServoUpdate >= 10) {
    updateServo(Servos[0]);
    updateServo(Servos[1]);
    updateServo(Servos[2]);
    updateServo(Servos[3]);
    updateServo(Servos[4]);
    updateServo(Servos[5]);
    lastServoUpdate = millis();
  }

  unsigned long now = millis();
  Note &e6 = melody[0];

  if(state == PLAY) {
    if(tickStart == 0) {
      digitalWrite(MUTE, LOW);
      Serial.println("Playing E6");
      moveServo(0, e6.notePitch.hole01, 100);
      moveServo(1, e6.notePitch.hole02, 100);
      moveServo(2, e6.notePitch.hole2, 100);
      moveServo(3, e6.notePitch.hole3, 100);
      moveServo(4, e6.notePitch.hole4, 100);
      moveServo(5, e6.notePitch.hole5, 100);
      digitalWrite(HOLE1, e6.notePitch.hole1);
      digitalWrite(HOLE6, e6.notePitch.hole6);
      digitalWrite(HOLE7, e6.notePitch.hole7);
      tickStart = now;
    }

    if(now - tickStart >= (unsigned long)e6.duration) {
      digitalWrite(MUTE, HIGH);
      Serial.println("E6 finish, mute HIGH");
      tickStart = now;
      state = REST;
    }
  } else if(state == REST) {
    if(now - tickStart >= (unsigned long)e6.restGap) {
      tickStart = 0;
      state = PLAY;
    }
  }
}

void moveServo(int servoNumber, int target, int movementTime) {
  int targetClamped = constrain(target, min_counts, max_counts);
  Servos[servoNumber].targetPos = targetClamped;
  float distance = abs(targetClamped - Servos[servoNumber].currentPos);
  int updateTimes = movementTime / 10;
  Servos[servoNumber].speed = distance / updateTimes;
  if (Servos[servoNumber].speed < 0.2)
    Servos[servoNumber].speed = 0.2;
}

void updateServo(ServoState &servo) {
  if (servo.currentPos == servo.targetPos) return;
  if (servo.currentPos < servo.targetPos) {
    servo.currentPos += servo.speed;
    if (servo.currentPos > servo.targetPos) servo.currentPos = servo.targetPos;
  } else {
    servo.currentPos -= servo.speed;
    if (servo.currentPos < servo.targetPos) servo.currentPos = servo.targetPos;
  }
  ledcWrite(servo.pin, (int)servo.currentPos);
}