#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 18

#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 170  
#define open02 190  
#define closed2 215
#define open2 200
#define closed3 235
#define open3 230
#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_C5 {closed01,closed02,0,closed2,closed3,closed4,closed5,1,1};
const Pitch NOTE_D5 {closed01,closed02,0,closed2,closed3,closed4,closed5,1,0};
const Pitch NOTE_E5 {closed01,closed02,0,closed2,closed3,closed4,closed5,0,0};
const Pitch NOTE_F5 {closed01,closed02,0,closed2,closed3,closed4,open5,0,0};
const Pitch NOTE_G5 {closed01,closed02,0,closed2,closed3,open4,open5,0,0};
const Pitch NOTE_A5 {closed01,closed02,0,closed2,open3,open4,open5,0,0};
const Pitch NOTE_B5 {closed01,closed02,0,open2,open3,open4,open5,0,0};
const Pitch NOTE_C6 {closed01,closed02,1,closed2,open3,open4,open5,0,0};
const Pitch NOTE_D6 {open01,open02,1,closed2,open3,open4,open5,0,0};
const Pitch NOTE_E6 {closed01,closed02,0,closed2,closed3,closed4,closed5,0,0};
const Pitch NOTE_F6 {closed01,open02,1,closed2,closed3,closed4,open5,0,0};
const Pitch NOTE_G6 {closed01,open02,1,closed2,closed3,open4,open5,0,0};
const Pitch NONE {open01,open02,0,open2,open3,open4,open5,0,0};

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

int midiNote[] = {
 64, 72, 72, 72, 64, 64, 72, 72, 72, 69, 69, 60, 60, 65, 77, 69, 69, 69, 81, 74,
 81, 71, 69, 71, 72, 71, 69, 67, 64, 77, 76, 74, 72, 74, 76, 74, 72, 71, 71, 69,
 71, 72, 71, 69, 67, 64, 77, 76, 74, 72, 76, 79, 72, 72, 71, 69, 65, 72, 72, 71,
 69, 65
};

int startTime[] = {
 70714, 71071, 76071, 77500, 77857, 78214, 78929, 80357, 81071, 81786, 83214, 83571, 83929, 84286, 84643, 85357, 85536, 85714, 86071, 86429,
 86786, 98571, 101071, 101250, 101429, 101607, 101786, 101964, 102143, 104107, 105357, 105536, 105714, 106607, 106786, 106964, 108214, 109286, 110000, 112500,
 112679, 112857, 113036, 113214, 113393, 113571, 115536, 116786, 116964, 117143, 118214, 118571, 127143, 131429, 132143, 132857, 135714, 138571, 142857, 143571,
 144286, 147143
};

int noteDuration[] = {3571, 3214, 356, 356, 356, 356, 356, 356, 356, 356, 356, 356, 356, 356, 356, 178, 178, 356, 356, 356, 356, 2499, 178, 178, 178,
 178, 178, 178, 714, 1249, 178, 178, 803, 178, 178, 1249, 1071, 714, 2499, 178, 178, 178, 178, 178, 178, 714, 1249, 178, 178, 1071, 356, 1785, 4285,
 714, 714, 2856, 2856, 4285, 714, 714, 2856, 2856
};

const int sizeofMidi = sizeof(midiNote) / sizeof(midiNote[0]);
Note song[62];

int currentNote = 0;
ServoState Servos[6];
bool playing = true;
uint32_t timebase;
unsigned long lastServoUpdate = 0;
unsigned long lastPlayTick = 0;

void setSilencePitch()
{
  moveServo(0, NONE.hole01, 100);
  moveServo(1, open02, 100);
  moveServo(2, NONE.hole2, 100);
  moveServo(3, NONE.hole3, 100);
  moveServo(4, NONE.hole4, 100);
  moveServo(5, NONE.hole5, 100);
  digitalWrite(HOLE1, LOW);
  digitalWrite(HOLE6, LOW);
  digitalWrite(HOLE7, LOW);
}

void setup() {
 delay(2000);
 Serial.begin(921600);
 Serial.println("Setup Start");

 for (int i = 0; i < sizeofMidi; i++)
 {
   Pitch p = getNoteName(midiNote[i]);
   song[i].notePitch = p;
   song[i].startTime = startTime[i] - 70000;
   song[i].duration = noteDuration[i];
 }

 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, open01);
 ledcWrite(HOLE02, open02);
 ledcWrite(HOLE2, open2);
 ledcWrite(HOLE3, open3);
 ledcWrite(HOLE4, open4);
 ledcWrite(HOLE5, open5);

 Servos[0] = {HOLE01, open01, open01, 1.0};
 Servos[1] = {HOLE02, open02, open02, 1.0};
 Servos[2] = {HOLE2, open2, open2, 1.0};
 Servos[3] = {HOLE3, open3, open3, 1.0};
 Servos[4] = {HOLE4, open4, open4, 1.0};
 Servos[5] = {HOLE5, open5, open5, 1.0};

 pinMode(HOLE1, OUTPUT);
 pinMode(HOLE6, OUTPUT);
 pinMode(HOLE7, OUTPUT);
 pinMode(MUTE1, OUTPUT);

 digitalWrite(HOLE1,LOW);
 digitalWrite(HOLE6,LOW);
 digitalWrite(HOLE7,LOW);
 digitalWrite(MUTE1,LOW);

 delay(1000);
 Serial.println("Setup Done, HOLE02 locked open02=120");
 timebase = millis();
}

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();
 }

 if(millis() - lastPlayTick >= 20)
 {
   playSong();
   lastPlayTick = millis();
 }
}

void playSong() {
 if (!playing) return;
 uint32_t songTimer = millis() - timebase;
 Note &curNote = song[currentNote];

 if (songTimer < curNote.startTime)
 {
   setSilencePitch();
   return;
 }

 if(songTimer >= curNote.startTime + curNote.duration)
 {
   setSilencePitch();
   currentNote++;
   if(currentNote >= sizeofMidi)
   {
     currentNote = 0;
     timebase = millis();
     setSilencePitch();
   }
   return;
 }

 Pitch curPitch = curNote.notePitch;
 if (isNone(curPitch))
 {
   setSilencePitch();
   return;
 }

 moveServo(0, curPitch.hole01, 150);
 moveServo(1, curPitch.hole02, 150);
 moveServo(2, curPitch.hole2, 150);
 moveServo(3, curPitch.hole3, 150);
 moveServo(4, curPitch.hole4, 150);
 moveServo(5, curPitch.hole5, 150);

 digitalWrite(HOLE1, curPitch.hole1 ? LOW : HIGH);
 digitalWrite(HOLE6, curPitch.hole6 ? LOW : HIGH);
 digitalWrite(HOLE7, curPitch.hole7 ? LOW : HIGH);
}

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;
 }
 servo.currentPos = round(servo.currentPos);
 ledcWrite(servo.pin, (int)servo.currentPos);
}

Pitch getNoteName(int midi) {
 switch (midi) {
   case 60: return NONE;
   case 64: return NONE;
   case 65: return NONE;
   case 67: return NONE;
   case 69: return NOTE_A5;
   case 71: return NONE;
   case 72: return NOTE_C5;
   case 74: return NOTE_D5;
   case 76: return NOTE_E5;
   case 77: return NOTE_F5;
   case 79: return NOTE_G5;
   case 81: return NOTE_A5;
   default: return NONE;
 }
}

bool isNone(Pitch p) {
 return (
   p.hole01 == NONE.hole01 &&
   p.hole02 == NONE.hole02 &&
   p.hole1 == NONE.hole1 &&
   p.hole2 == NONE.hole2 &&
   p.hole3 == NONE.hole3 &&
   p.hole4 == NONE.hole4 &&
   p.hole5 == NONE.hole5 &&
   p.hole6 == NONE.hole6 &&
   p.hole7 == NONE.hole7
 );
}