#include "ev3cxx.h"
#include "app.h"
#include <ctime>
#include <math.h>
#include <iomanip>
#include <stdio.h>
#include <string.h>

/*
CHANGELOG:

0.9.0 - 'KATRIN' (#90)
now I have no idea what am I doing... but it does something

0.8.1 - 'MONIKA' (#86)
Fixed error correction when brick stuck (changed motor speeds to fix the angle)

0.8.0 - 'MONIKA' (#85)
Error correction on every straight drive
Multiple turning step
Complete code for building one tower

0.7.0 - 'URSULA' (#80)
Done first short and long side
Modified turn function
Changed drive plan (long - long - middle - short - short - middle)

0.6.0 - 'URSULA' (#75)
Added Doxygen comments
Added volume to display_all_values (diagnostics screen)

0.5.0 - 'HELGA' (#72)
Added diagnostics screen (opened by pressing left after boot)
Added secret quiet mode
Added turning_corection_factor for correcing gyro sensor measurements due to its bad placement

0.4.3 - 'HELGA' (#70)
Motor problems indicator with colors:
    left - red, right - orange,
    when motor is stuck change LED color
Added calculating motor powers based on battery level

0.4.2 - 'HELGA' (#67)
Updated run_short() to run_short_side()
Added gyro
Done calculations with gyro to EC driving straight
TODO: problems with battery

0.3.0 - 'INGRID' (#42)
Some weird magic in calculations of motor speeds

0.2.7 - 'ERIKA' (#36)
Created function run_short for picking up all cubes on short sides

0.2.5 - 'ERIKA' (#27)
Added "boot-up melody"
Added btnEnter.waitForClick() for program start
Fixed display printouts
HW change: gearbox on the left gear changed to 1:1

0.2.0 - 'ERIKA' (#15)
Tested functions for opening and closing doors, updated speeds
Added idealMPWRS - base MPWRS to update back to, testing with different speeds for left and right wheels

0.1.1 - 'ERIKA' (#2)
Re-created structs for version, MPWRS, note
Re-created function for opening and closing door and function for generating Version ID
*/

/// @struct version
/// @brief Struct for holding all informations about current version
struct version
{
    /// @brief int: version id
    int id;
    /// @brief int: major
    int major;
    /// @brief int: minor
    int minor;
    /// @brief int: patch
    int patch;
    /// @brief std::tm: release date (date & time of compilation)
    std::tm relDate;
    /// @brief const char*: string codename of the current version
    const char *codename;
};

/// @struct MPWRS
/// @brief  Structure to hold motor powers (left and right)
struct MPWRS
{
    /// @brief int: left motor power
    int lMotorPWR;
    /// @brief int: right motor power
    int rMotorPWR;
};

/// @struct MPWRSPlus
/// @brief Structure to hold motor powers and speed modifier (extends MPWRS)
struct MPWRSPlus
{

    /// @brief MPWRS: motor powers
    MPWRS motor_powers;
    /// @brief int: speed modifier
    int SPEED_MODIFIER;
};

/// @struct note
/// @brief Structure for holding note (frequency and duration)
struct note
{
    /// @brief uint16_t: Frequency of the note
    double frequency;
    /// @brief int32_t: Duration of the note
    int32_t duration;
};

/// @struct all Values for diagnostic function
struct allValues
{
    version currentVersion;
    int volume;
    int lMotorPWR;
    int rMotorPWR;
    int SPEED_MODIFIER;
    int turningThreshold;
    int TURNING_FACTOR_CORRECTION;
    int CORRECTION_MULTIPLIER;
    int shortOneCycleLimit;
    int longOneCycleLimit;
    int longTwoCycleLimit;
    double RATIO;
};

/// @brief Function to calculate motor powers using quadratic equation, depending on battery powerMPWRSPlus
/// @param leftMotor int: wanted power of left motor (default 85)
/// @param rightMotor int: wante power or right motor (default 70)
/// @return MPWRSPlus: motor powers and SPEED_MODIFIER
MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 60)
{
    int batteryLevel = ev3_battery_voltage_mV();

    double squared = (double)0.00003 * (double)(batteryLevel * batteryLevel);
    double linear = -0.42 * (double)batteryLevel;
    double constant = 1470;
    double modifier = squared + linear + constant;
    int SPEED_MODIFIER = 30 - ((int)round(modifier));
    // ev3cxx::display.format(4, "Q: % \nL: % \nC: % \nSM: % ") % squared % linear % constant % modifier;

    MPWRS idealMPWRS;
    idealMPWRS.lMotorPWR = leftMotor + SPEED_MODIFIER;
    idealMPWRS.rMotorPWR = rightMotor + SPEED_MODIFIER;

    MPWRSPlus retVal;
    retVal.motor_powers = idealMPWRS;
    retVal.SPEED_MODIFIER = SPEED_MODIFIER;
    return retVal;
}

/// @brief Function to play starting melody
void play_starting_melody()
{
    // Play starting melody
    ev3_speaker_play_tone(NOTE_C5, 400);
    tslp_tsk(500);
    ev3_speaker_play_tone(NOTE_F5, 400);
    tslp_tsk(500);
    ev3_speaker_play_tone(NOTE_G5, 400);
    tslp_tsk(500);
    ev3_speaker_play_tone(NOTE_A5, 100);
    tslp_tsk(200);
    ev3_speaker_play_tone(NOTE_F5, 650);
    tslp_tsk(950);
    ev3_speaker_play_tone(NOTE_F4, 75);
    tslp_tsk(110);
    ev3_speaker_play_tone(NOTE_F4, 75);
    tslp_tsk(110);
    ev3_speaker_play_tone(NOTE_F4, 75);
    tslp_tsk(200);
}

/// @brief Function to clean display and write first two line
void cleanAndTitle()
{
    ev3cxx::display.resetScreen();
    ev3cxx::display.format(0, "    DOBREMYSL    ");
    ev3cxx::display.format(1, "*****************");
}

/// @brief Diagnostic function to display all value on display with ability to show multiple pages
/// @param allValues values: all values in allValues structure
void displayAllValues(allValues values)
{
    ev3cxx::display.resetScreen();
    ev3cxx::display.setFont(EV3_FONT_MEDIUM);

    ev3cxx::BrickButton btnLeft(ev3cxx::BrickButtons::RIGHT); // Right button
    ev3cxx::BrickButton btnUp(ev3cxx::BrickButtons::UP);      // Up button
    ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN);  // Down button

    int pages = 3;
    int page = 0;
    while (true)
    {
        switch (page)
        {
        case 0:
            ev3cxx::display.resetScreen();
            ev3cxx::display.format(0, "    DOBREMYSL    ");
            ev3cxx::display.format(1, "*****************");
            ev3cxx::display.format(2, "Ver.: % .% .% ") % values.currentVersion.major % values.currentVersion.minor % values.currentVersion.patch;
            ev3cxx::display.format(3, "Nr.: #% ") % values.currentVersion.id;
            ev3cxx::display.format(4, "Codename: % ") % values.currentVersion.codename;
            ev3cxx::display.format(5, "Volume: % %%") % values.volume;
            ev3cxx::display.format(6, "Rel.: % ") % std::asctime(&values.currentVersion.relDate);
            break;
        case 1:
            ev3cxx::display.resetScreen();
            ev3cxx::display.format(0, "Bat.: % mV") % ev3_battery_voltage_mV();
            ev3cxx::display.format(1, "Mod.: % ") % values.SPEED_MODIFIER;
            ev3cxx::display.format(2, "-------_T_-------");
            ev3cxx::display.format(3, "TT: % ") % values.turningThreshold;
            ev3cxx::display.format(4, "CM: % ") % values.CORRECTION_MULTIPLIER;
            ev3cxx::display.format(5, "TFC: % ") % values.TURNING_FACTOR_CORRECTION;
            ev3cxx::display.format(6, "-------_G_-------");
            ev3cxx::display.format(7, "RATIO: % ") % values.RATIO;
            break;
        case 2:
            ev3cxx::display.resetScreen();
            ev3cxx::display.format(0, "-------_1_-------");
            ev3cxx::display.format(1, "LOCL: % ") % values.longOneCycleLimit;
            ev3cxx::display.format(2, "-------_2_-------");
            ev3cxx::display.format(3, "LOCL: % ") % values.longTwoCycleLimit;
            break;
        case 3:
            ev3cxx::display.resetScreen();

        default:
            break;
        }

        while (true)
        {
            if (btnLeft.isPressed())
            {
                ev3cxx::display.resetScreen();
                cleanAndTitle();
                ev3cxx::display.format(2, "Press ENTR to run");
                return;
            }

            if (btnUp.isPressed())
            {
                page -= 1;
                if (page < 0)
                {
                    page = 0;
                }
                break;
            }
            else if (btnDown.isPressed())
            {
                page += 1;
                if (page > pages)
                {
                    page = pages;
                }
                break;
            }
            tslp_tsk(500);
        }
        tslp_tsk(200);
    }
}

/// @brief Function to run starting phase of the program (selection of diagnostics screen)
/// @param btnLeft ev3cxx::BrickButton: left brick button
/// @param btnRight ev3cxx::BrickButton: right brick button
/// @param touchS ev3cxx::TouchSensor: touch sensor
void start_program_exe(ev3cxx::BrickButton btnLeft, ev3cxx::BrickButton btnEnter, ev3cxx::TouchSensor touchS, allValues values)
{
    while (true)
    {
        // ev3_speaker_play_tone(NOTE_FS6, 20);
        // tslp_tsk(20);
        if (btnLeft.isPressed())
        {
            displayAllValues(values);
        }
        if (btnEnter.isPressed() || touchS.isPressed())
        {
            cleanAndTitle();
            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
            ev3_speaker_play_tone(NOTE_F4, 100);
            tslp_tsk(750);
            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
            ev3_speaker_play_tone(NOTE_F4, 100);
            break;
        }
        // ev3_speaker_play_tone(NOTE_F6, 50);
        tslp_tsk(50);
    }
}

/// @brief Function to parse timestamp to std::tm
/// @param timestampStr const char*: string form of the timestamp
/// @return std::tm: parsed timestamp
std::tm parseTimestamp(const char *timestampStr)
{
    std::tm tmStruct = {};
    std::istringstream iss(timestampStr);

    // The format of __TIMESTAMP__ is implementation-dependent
    // The example below assumes the format "Www Mmm dd hh:mm:ss yyyy"
    // Adjust the format string based on your compiler's __TIMESTAMP__ format
    iss >> std::get_time(&tmStruct, "%a %b %d %H:%M:%S %Y");

    return tmStruct;
}

/// @brief Function to generate version struct of the current version
///        Includes getting and parsing time of compilation using parseTimestamp function
/// @param versionID int: versionID, increment with every new change
/// @param codename const char*: string codename of the current version
/// @param major int: major
/// @param minor int: minor
/// @param patch int: patch
/// @return version: current version of the code, with date and time of compilation included
version createVersion(int versionID, const char *codename, int major, int minor, int patch)
{
    version retVersion;
    retVersion.id = versionID;
    retVersion.major = major;
    retVersion.minor = minor;
    retVersion.patch = patch;
    retVersion.codename = codename;
    /*retVersion.relDate.tm_sec = timestamp_tm.tm_sec;
    retVersion.relDate.tm_min = timestamp_tm.tm_min;
    retVersion.relDate.tm_hour = timestamp_tm.tm_hour;
    retVersion.relDate.tm_mday = timestamp_tm.tm_mday;
    retVersion.relDate.tm_mon = timestamp_tm.tm_mon;*/
    retVersion.relDate = parseTimestamp(__TIMESTAMP__);
    retVersion.relDate.tm_year = 2023 - 1900;
    retVersion.relDate.tm_isdst = 0;
    std::mktime(&retVersion.relDate);

    return retVersion;
}

/// @brief Function to calculate faster motor speed by using ratio and slower motor speed
/// @param slower_motor_speed int: slower motor speed
/// @param RATIO  double: ration betwenn faster and slower wheel
/// @return int[2]: [slower motor speed, faster motor speed]
int *calculate_ratio(int slower_motor_speed, double RATIO)
{
    int *retVal = new int[2];
    retVal[0] = slower_motor_speed;
    retVal[1] = (int)round(slower_motor_speed * RATIO);
    return retVal;
}

/// @brief Function to open door
/// @param hinge ev3cxx::Motor: motor to use when opening or closing the door
void open_door(ev3cxx::Motor hinge)
{
    hinge.off();
    hinge.onForDegrees(-25, 180);
}

/// @brief Function to close door
/// @param hinge ev3cxx::Motor: motor to use when opening or closing the door
void close_door(ev3cxx::Motor hinge)
{
    hinge.off();
    ev3_motor_set_power(HINGE_MOTOR, 25);
    tslp_tsk(1000);
}

/// @brief Function for turning
///        !! ROBOT CAN TURN ONLY LEFT !!
/// @param motors ev3cxx::MotorTank: MotorTank with motors of the DriveBase to use
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
/// @param endAngle int: angle to turn to (wanted angle, will be modified internally)
/// @param THRESHOLD int: turning accuracy in degrees (default 2)
/// @param TFC int: Turning Factor Correction, modifies endAngle for more accurate results (default 5)
void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5)
{
    cleanAndTitle();
    endAngle = endAngle - TFC;
    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);

    // MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
    // left, right, rotations (faster), brake, blocking, wait_after
    motors.onForRotations(-10, 25, 1.5, true, false, 60);

    bool rotating = true;
    int counter = 0;
    while (rotating && counter < 250)
    {
        int currAngle = ev3cxx::abs(gyro.angle());
        ev3cxx::display.format(3, "Angle: % ") % currAngle;
        ev3cxx::display.format(4, "Counter: % ") % counter;
        if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle))
        {

            int error = endAngle - currAngle;
            ev3cxx::display.format(5, "Error: % deg.") % error;

            motors.off(true);
            rotating = false;
            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
            ev3_speaker_play_tone(NOTE_F5, 100);
            return;
        }
        tslp_tsk(20);
        counter++;
    }

    if ((endAngle - THRESHOLD < ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT))) &&
        (ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT)) < endAngle + THRESHOLD))
    {
        turn(motors, gyro, endAngle, THRESHOLD, TFC);
    }
    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
    return;
}

/// @brief Function for turning (alternative way XD)
///        !! ROBOT CAN TURN ONLY LEFT !!
/// @param motors ev3cxx::MotorTank: MotorTank with motors of the DriveBase to use
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
/// @param endAngle int: angle to turn to (wanted angle, will be modified internally)
/// @param THRESHOLD int: turning accuracy in degrees (default 2)
/// @param TFC int: Turning Factor Correction, modifies endAngle for more accurate results (default 5)
void turn_forever(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2, int TFC = 5)
{
    cleanAndTitle();
    endAngle = endAngle + (TFC / 2);
    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);

    // MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
    // left, right, rotations (faster), brake, blocking, wait_after
    motors.on(-15, 35);

    int min = endAngle - 10;
    int max = endAngle + 10;

    int counter = 0;
    while (counter < 4000)
    {
        int angle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT));
        ev3cxx::display.format(3, "angle: % ") % angle;

        if ((min < angle) && (angle < max))
        {
            counter = 40000;
            motors.off(true);
            tslp_tsk(400);

            if (angle > (endAngle + THRESHOLD))
            {
                bool innerRun = true;
                motors.on(15, 5);
                while (innerRun)
                {
                    int inner_angle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT));
                    ev3cxx::display.format(3, "angle: % ") % inner_angle;
                    if (inner_angle < (endAngle + THRESHOLD))
                    {
                        innerRun = false;
                        motors.off(true);
                    }
                    tslp_tsk(50);
                }
            }
            else if (angle < (endAngle - THRESHOLD))
            {
                motors.on(-5, 15);
                bool innerRun = true;
                while (innerRun)
                {
                    int inner_angle = ev3cxx::abs(ev3_gyro_sensor_get_angle(GYRO_PORT));
                    ev3cxx::display.format(3, "angle: % ") % inner_angle;
                    if (inner_angle > (endAngle - THRESHOLD))
                    {
                        innerRun = false;
                        motors.off(true);
                    }
                    tslp_tsk(50);
                }
            }
        }
        ev3_speaker_play_tone(NOTE_F6, 50);
        tslp_tsk(50);
        counter++;
    }
    motors.off(true);
    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
    return;
}

/// @brief Function to pick up all cubes on shorter side.
///        Returns true if ran till the end and false if stopped by middle button
/// @param motors ev3cxx::MotorTank: MotorTank with motors of DriveBase
/// @param idealMPWRS MPWRS: motor powers to use when nothing bad happens
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
/// @param bumper ev3cxx::TouchSensor: front touch, exit prematurely when pressed
/// @param CYCLE_LIMIT int: how many cycle to run (default 90)
/// @param CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20)
/// @param THRESHOLD int: when to start correcting drive (in degrees) (default 2)
/// @return bool: false if stopped by the middle button
bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
{
    cleanAndTitle();
    const int LEFT_THRESHOLD = -THRESHOLD;
    const int RIGHT_THRESHOLD = THRESHOLD;

    gyro.resetHard();
    ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
    MPWRS motor_powers;
    // Reset both motor's powers
    motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
    motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;

    bool run = true;
    bool error = false;
    int cycleCounter = 0;

    while (run)
    {
        motors.on(motor_powers.lMotorPWR, motor_powers.rMotorPWR);
        tslp_tsk(50);

        // Reset both motor's powers
        motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
        motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;

        // Get power of both motors
        int lPower = motors.leftMotor().currentPower();
        int rPower = motors.rightMotor().currentPower();
        int angle = ev3_gyro_sensor_get_angle(GYRO_PORT);

        ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nCycles: % \nAngle: % ") % lPower % rPower % cycleCounter % angle;

        // Emergency break using middle button (BTN_ENTER)
        if (btnEnter.isPressed())
        {
            run = false;
            error = true;
            motors.off(true);
        }

        // Exit when bumper hit
        if (bumper.isPressed())
        {
            run = false;
            motors.off(true);
        }

        // Check gyro angle and change driving speed to fix the angle
        // To the left
        if (angle < LEFT_THRESHOLD)
        {
            // ev3_speaker_play_tone(NOTE_A5, 250);
            int correction = ev3cxx::abs(angle - LEFT_THRESHOLD);
            motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);
            ev3_speaker_play_tone(correction * 1000, 30);
            // Check if the motor is stuck
            if (lPower == 0)
            {
                ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
                ev3_speaker_play_tone(NOTE_A5, 250);
                run = false;
                error = true;
                motors.off(true);
            }

            // To the right
        }
        else if (angle > RIGHT_THRESHOLD)
        {
            // ev3_speaker_play_tone(NOTE_A4, 250);
            int correction = ev3cxx::abs(angle - RIGHT_THRESHOLD);
            motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
            ev3_speaker_play_tone(correction * 1000, 30);
            // Check if the motor is stuck
            if (rPower == 0)
            {
                ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
                ev3_speaker_play_tone(NOTE_A4, 250);
                run = false;
                error = true;
                motors.off(true);
            }
        }

        cycleCounter++;

        if (cycleCounter == CYCLE_LIMIT)
        {
            run = false;
        }
    }
    motors.off(true);
    ev3_speaker_play_tone(NOTE_C4, 250);
    tslp_tsk(250);
    ev3_speaker_play_tone(NOTE_C4, 125);
    tslp_tsk(125);
    ev3_speaker_play_tone(NOTE_D4, 250);
    tslp_tsk(250);
    ev3_speaker_play_tone(NOTE_C4, 250);
    tslp_tsk(250);
    ev3_speaker_play_tone(NOTE_B4, 750);
    tslp_tsk(750);
    ev3_speaker_play_tone(NOTE_G4, 750);
    tslp_tsk(750);
    return !error;
}

/// @brief Function to pick up all cubes on longer side.
///        Returns true if ran till the end and false if stopped by middle button
/// @param motors ev3cxx::MotorTank: MotorTank with motors of DriveBase
/// @param idealMPWRS MPWRS: motor powers to use when nothing bad happens
/// @param gyro ev3cxx::GyroSensor: gyro sensor to use
/// @param bumper ev3cxx::TouchSensor: front touch, exit prematurely when pressed
/// @param CYCLE_LIMIT int: how many cycle to run (default 180)
/// @param CORRECTION_MULTIPLIER int: base value for modifying drive direction when of course (default 20)
/// @return bool: false if stopped by the middle button
bool run_long_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CYCLE_LIMIT = 180, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
{
    cleanAndTitle();

    gyro.resetHard();
    ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
    MPWRS motor_powers;
    // Reset both motor's powers
    motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
    motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;

    bool run = true;
    bool error = false;
    int cycleCounter = 0;
    int lastError = -1;
    int errorStrike = 0;

    while (run)
    {
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
        // motors.on(motor_powers.lMotorPWR + 10, motor_powers.rMotorPWR);
        ev3_motor_set_power(LEFT_MOTOR, motor_powers.lMotorPWR);
        ev3_motor_set_power(RIGHT_MOTOR, motor_powers.rMotorPWR);
        tslp_tsk(50);

        // Reset both motor's powers
        motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
        motor_powers.rMotorPWR = idealMPWRS.rMotorPWR + 5;

        // Get power of both motors
        int lPower = ev3_motor_get_power(LEFT_MOTOR);
        int rPower = ev3_motor_get_power(RIGHT_MOTOR);
        int angle = ev3_gyro_sensor_get_angle(GYRO_PORT);

        ev3cxx::display.format(4, "Cycles: % \nAngle: % ") % cycleCounter % angle;

        if (lPower == 0 || rPower == 0)
        {
            // If error in previous cycle
            if (lastError == (cycleCounter - 1))
            {
                errorStrike++;
            }
            // Otherwise clear the error strike
            else
            {
                errorStrike = 0;
            }
            // Set lastError to this cycle
            lastError = cycleCounter;

            // If this is third error cycle in row
            if (errorStrike == 5)
            {
                ev3_speaker_play_tone(NOTE_G5, 50);
                ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
                // Reset counters etc.
                lastError = 0;
                errorStrike = 0;
                // Back up a bit
                motors.off(true);
                motors.on(-80, -40);
                tslp_tsk(450);
                continue;
            }
        }

        // Turning left
        if (angle < -3)
        {
            // Stop the right wheel and boost left wheel
            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
            ev3_speaker_play_tone(NOTE_F4, 100);
            motor_powers.lMotorPWR += 30;
            motor_powers.rMotorPWR -= 20;
        }

        // Turning right
        if (angle > 5)
        {
            // Stop the left wheel and boost right wheel
            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
            ev3_speaker_play_tone(NOTE_F6, 100);
            motor_powers.lMotorPWR -= 10;
            motor_powers.rMotorPWR += 30;
        }

        // Emergency break using middle button (BTN_ENTER)
        if (btnEnter.isPressed())
        {
            run = false;
            error = true;
            motors.off(true);
        }

        // Exit when front bumper is hit
        if (bumper.isPressed())
        {
            ev3_speaker_play_tone(NOTE_C4, 100);
            motors.off(true);

            // Drive to the wall annd try to pickup all the bricks or get them to the container
            int s_cycleCounter = 0;
            int s_lastError = -1;
            int s_errorStrike = 0;
            int s_maxCycles = 70;

            while (s_cycleCounter <= s_maxCycles)
            {
                s_cycleCounter++;
                motors.on(100, 100);
                int lPower = motors.leftMotor().currentPower();
                int rPower = motors.rightMotor().currentPower();
                if (lPower == 0 || rPower == 0)
                {
                    // If error in previous cycle
                    if (s_lastError == (s_cycleCounter - 1))
                    {
                        s_errorStrike++;
                    }
                    // Otherwise clear the error strike
                    else
                    {
                        s_errorStrike = 0;
                    }
                    // Set lastError to this cycle
                    s_lastError = s_cycleCounter;

                    // If this is third error cycle in row
                    if (s_errorStrike == 5)
                    {
                        ev3_speaker_play_tone(NOTE_F6, 50);
                        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
                        // Reset counters etc.
                        lastError = 0;
                        errorStrike = 0;
                        // Back up a bit
                        motors.off(true);
                        motors.on(-80, -40);
                        tslp_tsk(450);
                        continue;
                    }
                }
                tslp_tsk(50);
            }

            run = false;
            motors.off(true);
        }

        cycleCounter++;

        if (cycleCounter == CYCLE_LIMIT)
        {
            run = false;
        }
    }
    motors.off(true);
    ev3_speaker_play_tone(NOTE_C4, 250);
    tslp_tsk(250);
    ev3_speaker_play_tone(NOTE_C4, 125);
    tslp_tsk(125);
    ev3_speaker_play_tone(NOTE_D4, 250);
    tslp_tsk(250);
    ev3_speaker_play_tone(NOTE_C4, 250);
    tslp_tsk(250);
    ev3_speaker_play_tone(NOTE_B4, 750);
    tslp_tsk(750);
    ev3_speaker_play_tone(NOTE_G4, 750);
    tslp_tsk(750);
    return !error;
}

/// @brief Function to drive forward till bumper button is pressed
/// @param motors ev3cxx::MotorTank motors to use for driving
/// @param idealMPWRS MPWRS ideal motor powers for the motors
/// @param gyro ev3cxx::GyroSensor gyro sensor to use for error correction
/// @param bumper ev3cxx::TouchSensor touch sensor to use a bumper for stoping when wall is hit
/// @param CORRECTION_MULTIPLIER int for correcting errors in direction
/// @return bool: false if stopped by the middle button
bool unlimited_drive(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, ev3cxx::TouchSensor bumper, int CORRECTION_MULTIPLIER = 2, int THRESHOLD = 2)
{
    cleanAndTitle();
    const int LEFT_THRESHOLD = -THRESHOLD;
    const int RIGHT_THRESHOLD = THRESHOLD;

    gyro.resetHard();
    ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
    MPWRS motor_powers;
    // Reset both motor's powers
    motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
    motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;

    bool run = true;
    bool error = false;

    int cycleCounter = 0;
    int lastError = -1;
    int errorStrike = 0;

    while (run)
    {
        ev3_motor_set_power(LEFT_MOTOR, motor_powers.lMotorPWR);
        ev3_motor_set_power(RIGHT_MOTOR, motor_powers.rMotorPWR);
        tslp_tsk(50);
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);

        // Reset both motor's powers
        motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
        motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;

        // Get power of both motors
        int lPower = ev3_motor_get_power(LEFT_MOTOR);
        int rPower = ev3_motor_get_power(RIGHT_MOTOR);
        int angle = ev3_gyro_sensor_get_angle(GYRO_PORT);
        ev3cxx::display.format(4, "Left motor: % \nRight motor: % \nAngle: % ") % lPower % rPower % angle;

        // Emergency break using middle button (BTN_ENTER)
        if (btnEnter.isPressed())
        {
            run = false;
            error = true;
            motors.off(true);
        }

        // Exit when bumper hit
        if (bumper.isPressed())
        {
            run = false;
            motors.off(true);
        }

        // Check gyro angle and change driving speed to fix the angle
        // To the left
        if (angle < LEFT_THRESHOLD)
        {
            // ev3_speaker_play_tone(NOTE_A5, 250);
            int correction = ev3cxx::abs(angle - LEFT_THRESHOLD);
            motor_powers.lMotorPWR = motor_powers.rMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);
            ev3_speaker_play_tone(correction * 1000, 30);
            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
        }
        // To the right
        else if (angle > RIGHT_THRESHOLD)
        {
            // ev3_speaker_play_tone(NOTE_A4, 250);
            int correction = ev3cxx::abs(angle - RIGHT_THRESHOLD);
            motor_powers.rMotorPWR = motor_powers.lMotorPWR + (correction * CORRECTION_MULTIPLIER); //(int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
            ev3_speaker_play_tone(correction * 1000, 30);
            ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
        }

        if (lPower == 0 || rPower == 0)
        {
            // If error in previous cycle
            if (lastError == (cycleCounter - 1))
            {
                errorStrike++;
            }
            // Otherwise clear the error strike
            else
            {
                errorStrike = 0;
            }
            // Set lastError to this cycle
            lastError = cycleCounter;

            // If this is third error cycle in row
            if (errorStrike == 3)
            {
                ev3_speaker_play_tone(NOTE_F6, 50);
                ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
                // Reset counters etc.
                lastError = 0;
                errorStrike = 0;
                // Back up a bit
                motors.off(true);
                motors.on(-ev3cxx::abs(motor_powers.rMotorPWR), -ev3cxx::abs(motor_powers.lMotorPWR));
                tslp_tsk(350);
                continue;
            }
        }
    }

    motors.off(true);
    ev3_speaker_play_tone(NOTE_C4, 250);
    tslp_tsk(250);
    ev3_speaker_play_tone(NOTE_C4, 125);
    tslp_tsk(125);
    ev3_speaker_play_tone(NOTE_D4, 250);
    tslp_tsk(250);
    ev3_speaker_play_tone(NOTE_C4, 250);
    tslp_tsk(250);
    ev3_speaker_play_tone(NOTE_B4, 750);
    tslp_tsk(750);
    ev3_speaker_play_tone(NOTE_G4, 750);
    tslp_tsk(750);
    return !error;
}

/**
 *
 * Driver functions
 *
 * music_task - play erika
 * sub_task - watchdog; will kill main_task after 85 seconds, beep and start music_task
 * main_task - main program lives here
 *
 */

void music_task(intptr_t unused)
{
    int PLAY_SPEED = 250;

    note erika[95];
    //
    erika[0] = {NOTE_E4, 3};
    erika[1] = {NOTE_F4, 1};
    erika[2] = {NOTE_G4, 2};
    erika[3] = {NOTE_G4, 2};
    erika[4] = {NOTE_G4, 2};
    erika[5] = {NOTE_C5, 2};
    erika[6] = {NOTE_C5, 2};
    erika[7] = {NOTE_E5, 2};
    erika[8] = {NOTE_E5, 3};
    erika[9] = {NOTE_D5, 1};
    erika[10] = {NOTE_C5, 1};
    //
    erika[11] = {0, 1};
    erika[12] = {0, 2};
    erika[13] = {0, 4};
    //
    erika[14] = {NOTE_B4, 2};
    erika[15] = {NOTE_C5, 2};
    erika[16] = {NOTE_D5, 1};
    //
    erika[17] = {0, 1};
    erika[18] = {0, 2};
    erika[19] = {0, 4};
    //
    erika[20] = {NOTE_E5, 3};
    erika[21] = {NOTE_D5, 1};
    erika[22] = {NOTE_C5, 1};
    //
    erika[23] = {0, 1};
    erika[24] = {0, 2};
    erika[25] = {0, 4};
    //
    //
    erika[26] = {NOTE_E4, 3};
    erika[27] = {NOTE_F4, 1};
    erika[28] = {NOTE_G4, 2};
    erika[29] = {NOTE_G4, 2};
    erika[30] = {NOTE_G4, 2};
    erika[31] = {NOTE_C5, 2};
    erika[32] = {NOTE_C5, 2};
    erika[33] = {NOTE_E5, 2};
    erika[34] = {NOTE_E5, 3};
    erika[35] = {NOTE_D5, 1};
    erika[36] = {NOTE_C5, 1};
    //
    erika[37] = {0, 1};
    erika[38] = {0, 2};
    erika[39] = {0, 4};
    //
    erika[40] = {NOTE_B4, 2};
    erika[41] = {NOTE_C5, 2};
    erika[42] = {NOTE_D5, 1};
    //
    erika[43] = {0, 1};
    erika[44] = {0, 2};
    erika[45] = {0, 4};
    //
    erika[46] = {NOTE_E5, 3};
    erika[47] = {NOTE_D5, 1};
    erika[48] = {NOTE_C5, 1};
    //
    erika[49] = {0, 1};
    erika[50] = {0, 2};
    erika[51] = {0, 4};
    //
    erika[52] = {NOTE_G4, 3};
    erika[53] = {NOTE_C5, 1};
    erika[54] = {NOTE_B4, 2};
    erika[55] = {NOTE_B4, 2};
    erika[56] = {NOTE_B4, 2};
    erika[57] = {NOTE_B4, 2};
    erika[58] = {NOTE_A4, 2};
    erika[59] = {NOTE_B4, 2};
    erika[60] = {NOTE_C5, 5};
    //
    erika[61] = {0, 3};
    //
    erika[62] = {NOTE_B4, 3};
    erika[63] = {NOTE_C5, 1};
    erika[64] = {NOTE_D5, 2};
    erika[65] = {NOTE_D5, 2};
    erika[66] = {NOTE_D5, 2};
    erika[67] = {NOTE_D5, 2};
    erika[68] = {NOTE_G5, 2};
    erika[69] = {NOTE_F5, 2};
    erika[70] = {NOTE_E5, 5};
    //
    erika[71] = {NOTE_E4, 3};
    erika[72] = {NOTE_F4, 1};
    erika[73] = {NOTE_G4, 2};
    erika[74] = {NOTE_G4, 2};
    erika[75] = {NOTE_G4, 2};
    erika[76] = {NOTE_C5, 2};
    erika[77] = {NOTE_C5, 2};
    erika[78] = {NOTE_E5, 2};
    erika[79] = {NOTE_E5, 3};
    erika[80] = {NOTE_D5, 1};
    erika[81] = {NOTE_C5, 1};
    //
    erika[82] = {0, 1};
    erika[83] = {0, 2};
    erika[84] = {0, 4};
    //
    erika[85] = {NOTE_B4, 2};
    erika[86] = {NOTE_C5, 2};
    erika[87] = {NOTE_D5, 1};
    //
    erika[88] = {0, 1};
    erika[89] = {0, 2};
    erika[90] = {0, 4};
    //
    erika[91] = {NOTE_E5, 3};
    erika[92] = {NOTE_D5, 1};
    erika[93] = {NOTE_C5, 1};
    erika[94] = {0, 3};

    int i = 0;
    for (i = 0; i < 1000; i++)
    {
        ev3_speaker_play_tone((250 + 2 * i), 2);
        tslp_tsk(2);
    }

    for (int ito = (250 + 2 * i); ito > (int)NOTE_E4; ito -= 2)
    {
        ev3_speaker_play_tone((250 + ito), 2);
        tslp_tsk(2);
    }

    for (const auto &note : erika)
    {
        if (note.frequency != 0)
        {
            ev3_speaker_play_tone(note.frequency, note.duration * PLAY_SPEED);
        }
        tslp_tsk(note.duration * PLAY_SPEED + (PLAY_SPEED / 5));
    }
}

void csv_task(intptr_t unused)
{

    FILE *file;
    const char *filename = "/ev3rt/data.csv";

    // Open the file for writing
    file = fopen(filename, "w");

    if (file == NULL)
    {
        ev3_speaker_play_tone(NOTE_E6, 10000);
        tslp_tsk(10000);
    }

    const char *text = "sep=;\ntime;battery;current;gyro_angle;gyro_rate;left_motor;left_motor_counts;right_motor;right_motor_counts;hinge_motor;top;bumper\n";
    fwrite(text, sizeof(char), strlen(text), file);

    // Close the file
    fclose(file);

    // Write to the file
    while (true)
    {
        FILE *file;

        // Open the file for writing
        file = fopen(filename, "a");

        if (file == NULL)
        {
            ev3_speaker_play_tone(NOTE_E6, 10000);
            tslp_tsk(10000);
        }

        SYSTIM currentTime;
        get_tim(&currentTime);

        int gyro_angle = ev3_gyro_sensor_get_angle(GYRO_PORT);
        int gyro_rate = ev3_gyro_sensor_get_rate(GYRO_PORT);
        int left_motor = ev3_motor_get_power(LEFT_MOTOR);
        int left_motor_counts = ev3_motor_get_counts(LEFT_MOTOR);
        int right_motor = ev3_motor_get_power(RIGHT_MOTOR);
        int right_motor_counts = ev3_motor_get_counts(RIGHT_MOTOR);
        int hinge_motor = ev3_motor_get_power(HINGE_MOTOR);
        bool top = ev3_touch_sensor_is_pressed(BUTTON_UP);
        bool bumper = ev3_touch_sensor_is_pressed(BUTTON_BUMPER);

        char dataString[250];
        sprintf(dataString, "%lu;%d;%d%d;%d;%d;%d;%d;%d;%d;%d;%d\n", currentTime, ev3_battery_voltage_mV(), ev3_battery_current_mA(), gyro_angle, gyro_rate, left_motor, left_motor_counts, right_motor, right_motor_counts, hinge_motor, top, bumper);
        fwrite(dataString, sizeof(char), strlen(dataString), file);

        // Close the file
        fclose(file);
        tslp_tsk(1000);
    }
}

void sub_task(intptr_t unused)
{
    // Sleep 85 seconds
    SYSTIM startTime;
    get_tim(&startTime);
    bool run = true;
    while (run)
    {
        SYSTIM currentTime;
        get_tim(&currentTime);
        if((currentTime - startTime) >= 85000){
            run = false;
        }
        ev3cxx::display.format(2, "Time: % s") % (int)round((currentTime-startTime)/1000);
        tslp_tsk(1000);
    }
    // Make sure the volume is up and play the final beep
    ev3_speaker_set_volume(100);
    ev3_speaker_play_tone(NOTE_E5, 500);
    // Terminate main task
    ter_tsk(MAIN_TASK);
    // Set up motors
    ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::MEDIUM); // Hinge motor
    ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors (Left - B; Right - C)
    hinge.off(true);
    motors.off(true);
    // Activate music task
    act_tsk(MUSIC_TASK);
    // Stop self
    ter_tsk(SUB_TASK);
}

void main_task(intptr_t unused)
{
    const int CYCLE_LIMIT_1 = 350;
    const int CYCLE_LIMIT_2 = 350;
    const double RATIO = 1.42;
    const int THRESHOLD = 2;
    const int TURNING_THRESHOLD = 1;
    const int TURNING_FACTOR_CORRECTION = 5;
    const int CORRECTION_MULTIPLIER = 20;

    int volume = 100;

    // Create version info
    const version VERSION = createVersion(90, "KATRIN", 0, 9, 0);

    // Set-up screen
    ev3cxx::display.resetScreen();
    ev3cxx::display.setFont(EV3_FONT_MEDIUM);

    // Set up motor powers
    int *rationalized_speeds = calculate_ratio(45, RATIO);
    MPWRSPlus calcedPWRS = calculate_motor_pwrs(rationalized_speeds[1], rationalized_speeds[0]);

    MPWRS idealMPWRS = calcedPWRS.motor_powers;
    const int SPEED_MODIFIER = calcedPWRS.SPEED_MODIFIER;

    MPWRS motor_powers;
    motor_powers.lMotorPWR = idealMPWRS.lMotorPWR;
    motor_powers.rMotorPWR = idealMPWRS.rMotorPWR;

    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);

    // Set up motors
    ev3cxx::Motor hinge(ev3cxx::MotorPort::A, ev3cxx::MotorType::MEDIUM); // Hinge motor
    ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors (Left - B; Right - C)

    ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1); // gyro sensor

    ev3cxx::TouchSensor touchS(ev3cxx::SensorPort::S4);     // Touch sensor
    ev3cxx::TouchSensor frontTouch(ev3cxx::SensorPort::S3); // Touch sensor (bumper)

    // Set up buttons
    ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button
    ev3cxx::BrickButton btnLeft(ev3cxx::BrickButtons::LEFT);   // Left button (for entering diagnostics)
    ev3cxx::BrickButton btnDown(ev3cxx::BrickButtons::DOWN);   // Down button (quiet mode)

    // quiet mode activation
    if (btnDown.isPressed())
    {
        volume = 0;
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
        tslp_tsk(200);
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
        tslp_tsk(200);
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
    }

    ev3_speaker_set_volume(volume);
    play_starting_melody();

    // Print version information on screen
    cleanAndTitle();
    ev3cxx::display.format(2, "% .% .%  #% \nNAME: % \nBattery: % mV % \nPress ENTR to run") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename % ev3_battery_voltage_mV() % SPEED_MODIFIER;

    allValues values;
    values.currentVersion = VERSION;
    values.volume = volume;
    values.lMotorPWR = idealMPWRS.lMotorPWR;
    values.rMotorPWR = idealMPWRS.rMotorPWR;
    values.SPEED_MODIFIER = SPEED_MODIFIER;
    values.turningThreshold = TURNING_THRESHOLD;
    values.TURNING_FACTOR_CORRECTION = TURNING_FACTOR_CORRECTION;
    values.CORRECTION_MULTIPLIER = CORRECTION_MULTIPLIER;
    values.shortOneCycleLimit = 0;
    values.longOneCycleLimit = CYCLE_LIMIT_1;
    values.longTwoCycleLimit = CYCLE_LIMIT_2;
    values.RATIO = RATIO;

    // Start program
    start_program_exe(btnLeft, btnEnter, touchS, values);

    act_tsk(SUB_TASK);
    act_tsk(CSV_TASK);
    ev3_speaker_play_tone(NOTE_F4, 100);
    tslp_tsk(200);

    // Close the door before begining the drive
    close_door(hinge);

    // Run first long side
    bool side_1 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_1, CORRECTION_MULTIPLIER, THRESHOLD);
    // If something happened
    if (!side_1)
    {
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
        ev3_speaker_play_tone(NOTE_F4, 100);
        tslp_tsk(200);
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
        ev3_speaker_play_tone(NOTE_FS4, 100);
    }

    // till here it works:: 20.11 14:34

    //gyro.resetHard();
    // turn 90 degress left
    motors.onForRotations(-50,-70, 3.25);
    tslp_tsk(350);
    turn(motors, gyro, 20, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
    motors.onForRotations(motor_powers.lMotorPWR, motor_powers.rMotorPWR, 1);
    turn(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);
    motors.onForSeconds(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR, 1000);

    // this also works, code above doesnt

    // Cross to the other side
    gyro.resetHard();
    bool crossing = unlimited_drive(motors, motor_powers, gyro, frontTouch, CORRECTION_MULTIPLIER, THRESHOLD);

    if (!crossing)
    {
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
        ev3_speaker_play_tone(NOTE_F4, 100);
        tslp_tsk(200);
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
        ev3_speaker_play_tone(NOTE_FS4, 100);
    }

    // turn 90 degress left
    gyro.resetHard();
    turn_forever(motors, gyro, 90, TURNING_THRESHOLD, TURNING_FACTOR_CORRECTION);

    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
    tslp_tsk(200);
    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
    tslp_tsk(200);
    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);

    // reset gyro
    gyro.resetHard();
    // Run second long side
    bool side_2 = run_long_side(motors, motor_powers, gyro, frontTouch, CYCLE_LIMIT_2, CORRECTION_MULTIPLIER);
    // If something happened
    if (!side_2)
    {
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
        ev3_speaker_play_tone(NOTE_F4, 100);
        tslp_tsk(200);
        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);
        ev3_speaker_play_tone(NOTE_FS4, 100);
    }
}