//------------------------Variable declaration & initialization----------------------------//

// For onboard LED //

    #define ledPin 2

// For user input //

    int calPos;
    bool canRun_input, canCalibrate2;

// For servo motor //

    // Define //

        #define servoPin 13
        #define servoFreq 50
        #define servoRes 11
        #define minAngDuty 47 // minAngPw ≈ 0.46ms
        #define maxAngDuty 254 // minAngPw ≈ 2.48ms

    int currAng, targAng, currDuty;
    bool canRun_servo;

    bool canStDy_servo, canCount_servo;
    unsigned long prevT_servo, dyT_servo = 30;

//---------------------------------Callback functions-------------------------------------//

//---------------------------------------Setup-------------------------------------------//

void setup() {
    Serial.begin(115200);
    Serial.printf("______________________Setup starts_________________________\n");
    pinMode(ledPin, OUTPUT);
    ServoSetup();
    Serial.printf("______________________Setup completes_________________________\n");
}

void ServoSetup(){
    ledcAttach(servoPin, servoFreq, servoRes);
    ServoAng(0);
    //Serial.printf("minAngDuty: %d\n", minAngDuty);
    //Serial.printf("maxAngDuty: %d\n", maxAngDuty);
}

//----------------------------------------Loop--------------------------------------------//

void loop() {
    UserInput();
    ServoSlowMove();
    if (canRun_input){
    }
}

// For user input //

    void UserInput(){
        if (Serial.available()) {
            String input_str = Serial.readStringUntil('\n');
            int comma1_index = input_str.indexOf(',');
            int comma2_index = input_str.indexOf(',', comma1_index + 1);
            if (comma1_index == -1 && comma2_index == -1) { // if no commas .
                return;
            }
            String str1 = input_str.substring(0, comma1_index);
            String str2 = input_str.substring(comma1_index + 2, comma2_index); // start index from +2 to remove space .
            String str3 = input_str.substring(comma2_index + 2);
            //Serial.printf("Received strings: (%s, %s, %s)\n", str1, str2, str3);
            
            int num1 = str1.toInt();
            int num2 = str2.toInt();
            int num3 = str3.toInt();
            Serial.printf("Received numbers: (%d, %d, %d)\n", num1, num2, num3);
            
            if (canCalibrate2){
                Calibrate2(num1, num2, num3);
                return;
            }
            
            switch (num1){
                case (1): // cmd(1, X, X)
                    DoTask(num2);
                    break;
                case (2): // cmd(2, X, X)
                    Calibrate(num2, num3);
                    break;
                default:
                    break;
            }
        }
    }

        void DoTask(int num2){ // cmd(1, X, 0)
            switch (num2) {
                case (0): // cmd(1, 0, 0)
                    //Serial.printf("Stop running loop .\n");
                    canRun_input = false;
                    break;
                case (1): // cmd(1, 1, 0)
                    //Serial.printf("Start running loop .\n");
                    canRun_input = true;
                    break;
                case (2): // cmd(1, 2, 0)
                    Serial.printf("Enter your multiple values for calibration:\n");
                    Serial.printf("Enter value 1, 2, 3:\n");
                    calPos = 0; canCalibrate2 = true;
                    break;
                case (3): // cmd(1, 3, 0)
                    break;
                default:
                    break;
            }
        }

        void Calibrate(int num2, int num3){ // cmd(2, X, X)
            switch (num2) {
                case (1): // cmd(2, 1, X)
                    ServoAng(num3);
                    break; 
                case (2): // cmd(2, 2, X)
                    StartServoMove(num3);
                    break; 
                default:
                    break;
            }
        }

        void Calibrate2(int num1, int num2, int num3){ // cmd(1, 2, 0)
            calPos++;
            switch (calPos) {
                case (1):
                    Serial.printf("Value of (num1, num2, num3): (%d, %d, %d)\n", num1, num2, num3);
                    Serial.printf("Enter value 4, 5, 6:\n");
                    break; 
                case (2):
                    Serial.printf("Value of (num1, num2, num3): (%d, %d, %d)\n", num1, num2, num3);
                    Serial.printf("Done\n");
                    canCalibrate2 = false;
                    break; 
                default:
                    break;
            }
        }

//--------------------------------Self-defined functions------------------------------------//

// For servo motor //

    void ServoAng(int ang){
        currAng = ang;
        currDuty = map(currAng, 0, 180, minAngDuty, maxAngDuty);
        //Serial.printf("currDuty: %d\n", currDuty);
        ledcWrite(servoPin, currDuty); 
    }

    void StartServoMove(int ang){
        targAng = ang;
        canRun_servo = true;
    }

    void ServoSlowMove(){
        if (canRun_servo == false){
            canStDy_servo = true;
            return;
        }

        if (canCount_servo == false){
            if (currAng > targAng){
                currAng -= 1;
                ServoAng(currAng);
            }
            else if (currAng < targAng){
                currAng += 1;
                ServoAng(currAng);
            }
            else if (currAng == targAng){
                canRun_servo = false;
                canCount_servo = false;
            }
        }

        if (canStDy_servo){ // count for each degree change
            canStDy_servo = false; canCount_servo = true; prevT_servo = millis();
            // Body
        }
        else if (millis()-prevT_servo >= dyT_servo && canCount_servo){
            canStDy_servo = true; canCount_servo = false;
            // Body
        }
    }