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

    // For onboard LED //

        #define ledPin 2

    // For gyroscope //

        #include <Adafruit_MPU6050.h>
        #include <Adafruit_Sensor.h>
        #include <Wire.h>
        
        Adafruit_MPU6050 mpu;
        sensors_event_t a, g, temp;
        
        bool canStDy = true, canCount;
        unsigned long prevT, dyT_mpu = 1000;

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

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

    // For user input //

        void UserInput() {
            if (Serial.available()) {
                String input = Serial.readStringUntil('\n'); // (1, 2, 3, 4, 5)
                int commaIndexArr[5] = {0, -1, -1, -1, -1};
                String strArr[6];

                // Find comma indices //

                for (int i = 1, pos = 0; i < 5; i++) {
                    pos = input.indexOf(',', pos);
                    if (pos == -1) break;
                    commaIndexArr[i] = pos++;
                }

                // Extract substrings //

                strArr[1] = input.substring(0, commaIndexArr[1]);
                for (int i = 1; i < 5; i++) {
                    if (commaIndexArr[i] != -1) {
                        int start = commaIndexArr[i] + 2; // +2 to remove space
                        int end = (i < 4 && commaIndexArr[i + 1] != -1) ? commaIndexArr[i + 1] : input.length();
                        strArr[i + 1] = input.substring(start, end);
                    }
                }

                Serial.printf("Received strings: (%s, %s, %s, %s, %s)\n", strArr[1], strArr[2], strArr[3], strArr[4], strArr[5]);

                DoTask(strArr[1], strArr[2], strArr[3], strArr[4], strArr[5]);
            }
        }

            void DoTask(String str1, String str2, String str3, String str4, String str5){
                int cmd = str1.toInt();
                switch (cmd) {
                    case (1):
                        //Serial.printf("Received strings2: (%s, %s, %s, %s, %s)\n", str1, str2, str3, str4, str5);
                        break;
                    case (2):
                        break;
                    case (3):
                        break;
                    default:
                        break;
                }
            }

    void Gyroscope(){
	    if (canStDy){
	        canStDy = false; canCount = true; prevT = millis();
	        // Body
	    }
	    else if (millis()-prevT >= dyT_mpu && canCount){
	        canStDy = true; canCount = false;
	        
            mpu.getEvent(&a, &g, &temp);
            
            Serial.printf("Acceleration(G)(x, y, z): (%.1f, %.1f, %.1f)\n", a.acceleration.x/9.8, a.acceleration.y/9.8, a.acceleration.z/9.8);
            Serial.printf("Rotation(°/s)(x, y, z): (%.f, %.f, %.f)\n", degrees(g.gyro.x), degrees(g.gyro.y), degrees(g.gyro.z));
            Serial.printf("Rotation(°/s)(x, y, z): (%.f, %.f, %.f)\n", g.gyro.x, g.gyro.y, g.gyro.z);
            Serial.printf("Temperature: %.1f°C\n", temp.temperature);
            Serial.println("_______________________________________________");
        }
    }

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

    void GyroSetup(){
        if (!mpu.begin()) {
            Serial.println("Failed to find MPU6050 chip");
            while (1) delay(10);
        }
        else Serial.println("MPU6050 Found!");
        
        mpu.setAccelerometerRange(MPU6050_RANGE_2_G); // ± 2/4/8/16 G
        Serial.print("Accelerometer range = ");
        switch (mpu.getAccelerometerRange()) {
            case MPU6050_RANGE_2_G:
                Serial.println("±2G");
                break;
            case MPU6050_RANGE_4_G:
                Serial.println("±4G");
                break;
            case MPU6050_RANGE_8_G:
                Serial.println("±8G");
                break;
            case MPU6050_RANGE_16_G:
                Serial.println("±16G");
                break;
            default:
                break;
        }

        mpu.setGyroRange(MPU6050_RANGE_250_DEG); // ± 250/500/1000/2000 °/s
        Serial.print("Gyro range (angular velocity) = ");
        switch (mpu.getGyroRange()) {
            case MPU6050_RANGE_250_DEG:
                Serial.println("±250°/s");
                break;
            case MPU6050_RANGE_500_DEG:
                Serial.println("±500°/s");
                break;
            case MPU6050_RANGE_1000_DEG:
                Serial.println("±1000°/s");
                break;
            case MPU6050_RANGE_2000_DEG:
                Serial.println("±2000°/s");
                break;
            default:
                break;
        }

        mpu.setFilterBandwidth(MPU6050_BAND_44_HZ); // ± 5/10/21/44/94/184/260 Hz
        Serial.print("Filter bandwidth = ");
        switch (mpu.getFilterBandwidth()) {
            case MPU6050_BAND_260_HZ:
                Serial.println("260Hz");
                break;
            case MPU6050_BAND_184_HZ:
                Serial.println("184Hz");
                break;
            case MPU6050_BAND_94_HZ:
                Serial.println("94Hz");
                break;
            case MPU6050_BAND_44_HZ:
                Serial.println("44Hz");
                break;
            case MPU6050_BAND_21_HZ:
                Serial.println("21Hz");
                break;
            case MPU6050_BAND_10_HZ:
                Serial.println("10Hz");
                break;
            case MPU6050_BAND_5_HZ:
                Serial.println("5Hz");
                break;
            default:
                break;
        }
    }

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

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

    void loop() {
        UserInput();
        Gyroscope();
    }