#include "ev3cxx.h"
#include "app.h"
#include <time.h>
#include <ctime>
#include <math.h>
#include <iomanip>
#include <cstring>

/*
Change notes:

0.5.0 - 'HELGA'
Added diagnostics screen (opened by pressing left after boot)
Added secret quiet mode

0.4.3 - 'HELGA' (#70)
Motor problems indicator with colors:
    left - red, right - orange,
    when motor is stuck change LED color

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
*/

/* Structure to hold current version (major, minor, patch, codename, id, release_date)
    MAJOR - Major version number
    MINOR - Minor changes in code
    PATCH - Mostly just for testing
    ID - Increment every time with new version
    CODENAME - (String) Version name
    RELEASE_DATE - when was current version released
*/
struct version
{
    int id;
    int major;
    int minor;
    int patch;
    std::tm relDate;
    const char *codename;
};

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

// Structure to hold motor powers and speed modifier
struct MPWRSPlus
{
    MPWRS motor_powers;
    int SPEED_MODIFIER;
};

// Structure to hold notes
struct note
{
    uint16_t frequency;
    int32_t duration;
};

MPWRSPlus calculate_motor_pwrs(int leftMotor = 85, int rightMotor = 70)
{
    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;
}

void cleanAndTitle()
{
    ev3cxx::display.resetScreen();
    ev3cxx::display.format(0, "    DOBREMYSL    ");
    ev3cxx::display.format(1, "*****************");
}

// diacnostic functin()
void displayAllValues(version currentVersion, int lMotorPWR, int rMotorPWR, int batteryLevel, int SPEED_MODIFIER, int turningThreshold, int CORRECTION_MULTIPLIER, int shortOneCycleLimit)
{
    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 = 2;
    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.: % .% .% ") % currentVersion.major % currentVersion.minor % currentVersion.patch;
            ev3cxx::display.format(3, "Nr.: #% ") % currentVersion.id;
            ev3cxx::display.format(4, "Codename: % ") % currentVersion.codename;
            ev3cxx::display.format(5, "Rel.: % ") % std::asctime(&currentVersion.relDate);
            ev3cxx::display.format(7, "-----------------");
            break;
        case 1:
            ev3cxx::display.resetScreen();
            ev3cxx::display.format(0, "Bat.: % mV") % batteryLevel;
            ev3cxx::display.format(1, "Mod.: % ") % SPEED_MODIFIER;
            ev3cxx::display.format(2, "-------_T_-------");
            ev3cxx::display.format(3, "TT: % ") % turningThreshold;
            ev3cxx::display.format(4, "CM: % ") % CORRECTION_MULTIPLIER;
            ev3cxx::display.format(5, "-------_1_-------");
            ev3cxx::display.format(6, "SOCL: % ") % shortOneCycleLimit;
            ev3cxx::display.format(7, "");
            break;
        case 2:
            ev3cxx::display.resetScreen();
            ev3cxx::display.format(0, "% ");
            ev3cxx::display.format(1, "");
            break;

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

// TimeStamp parser
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;
}

// Function to create new program version
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;
}

// Function to open door
void open_door(ev3cxx::Motor hinge)
{
    hinge.onForDegrees(-25, 200);
}

// Function to close door
void close_door(ev3cxx::Motor hinge)
{
    hinge.onForDegrees(25, 200);
}

/*
Function for turning
Robot can turn only left
*/
void turn(ev3cxx::MotorTank motors, ev3cxx::GyroSensor gyro, int endAngle = 90, int THRESHOLD = 2)
{
    ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::GREEN);

    // MPWRSPlus calcedPWRS = calculate_motor_pwrs(-35, 40);
    motors.onForRotations(left_speed = -15,
                          right_speed = 30,
                          rotations = 2,
                          brake = true,
                          blocking = false,
                          wait_after_ms = 60);

    bool rotating = true;
    while (rotating)
    {
        int currAngle = ev3cxx::abs(gyro.angle());
        if ((ev3cxx::abs(endAngle - THRESHOLD) < currAngle) &&
            (currAngle < av3cxx::abs(endAngle + THRESHOLD)))
        {
            motors.off(brake = true);
            rotating = false;
        }
    }
    return;
}

/*
Function to pick up all cubes on shorter side
return true if ran till the end and false if stopped by middle button
*/
bool run_short_side(ev3cxx::MotorTank motors, MPWRS idealMPWRS, ev3cxx::GyroSensor gyro, int CYCLE_LIMIT = 90, int CORRECTION_MULTIPLIER = 20, int THRESHOLD = 2)
{
    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 = gyro.angle();

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

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

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

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

void main_task(intptr_t unused)
{

    const int CYCLE_LIMIT_1 = 90;
    const int THRESHOLD = 2;
    const int TURNING_THRESHOLD = 1;
    const int CORRECTION_MULTIPLIER = 20;

    int volume = 100;

    // Create version info
    // version createVersion(int versionID, const char *codename, int major, int minor, int patch, int day, int month, int year, int hour, int minute)
    const version VERSION = createVersion(72, "URSULA", 0, 5, 0);

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

    // Set up motor powers
    MPWRSPlus calcedPWRS = calculate_motor_pwrs();

    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::LARGE);  // Hinge motor
    ev3cxx::MotorTank motors(ev3cxx::MotorPort::B, ev3cxx::MotorPort::C); // Tank motors
    ev3cxx::GyroSensor gyro(ev3cxx::SensorPort::S1);                      // gyro sensor
    ev3cxx::TouchSensor touchS(ev3cxx::SensorPort::S4);                   // Touch sensor

    // 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)

    if(btnDown.isPressed()){
        volume = 10;
    }

    // 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;

    // Play starting melody
    ev3_speaker_set_volume(volume);
    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);

    // Start program
    while (1 == 1)
    {
        // ev3_speaker_play_tone(NOTE_FS6, 20);
        // tslp_tsk(20);
        if (btnLeft.isPressed())
        {
            displayAllValues(VERSION, idealMPWRS.lMotorPWR, idealMPWRS.rMotorPWR, ev3_battery_voltage_mV(), SPEED_MODIFIER, TURNING_THRESHOLD, CORRECTION_MULTIPLIER, CYCLE_LIMIT_1);
        }
        if (btnEnter.isPressed() || touchS.isPressed())
        {
            cleanAndTitle();
            break;
        }
        ev3_speaker_play_tone(NOTE_F6, 50);
        tslp_tsk(50);
    }

    ev3_speaker_play_tone(NOTE_F4, 100);
    tslp_tsk(200);

    for (int i = 0; i < 10; i++)
    {
        gyro.resetHard();
        if (gyro.angle() == 0)
        {
            ev3_speaker_play_tone(NOTE_F6, 200);
            tslp_tsk(200);
        }
        turn(motors, gyro, 90, TURNING_THRESHOLD);
        ev3_speaker_play_tone(NOTE_F4, 100);
        tslp_tsk(200);
        ev3_speaker_play_tone(NOTE_A4, 100);
        tslp_tsk(200);
        ev3_speaker_play_tone(NOTE_C4, 100);
        tslp_tsk(200);
        ev3_speaker_play_tone(NOTE_F4, 50);
        tslp_tsk(100);
        ev3_speaker_play_tone(NOTE_A4, 50);
        tslp_tsk(100);
        ev3_speaker_play_tone(NOTE_C4, 50);
        tslp_tsk(100);
        ev3_speaker_play_tone(NOTE_F4, 25);
        tslp_tsk(50);
        ev3_speaker_play_tone(NOTE_A4, 25);
        tslp_tsk(50);
        ev3_speaker_play_tone(NOTE_C4, 25);
        tslp_tsk(50);
        ev3_speaker_play_tone(NOTE_F4, 12);
        tslp_tsk(25);
        ev3_speaker_play_tone(NOTE_A4, 12);
        tslp_tsk(25);
        ev3_speaker_play_tone(NOTE_C4, 12);
        tslp_tsk(25);
    }
    bool side_1 = false; // run_short_side(motors, motor_powers, gyro, CYCLE_LIMIT_1, CORRECTION_MLUTIPLIER, THRESHOLD);

    if (!side_1)
    {
        return;
    }
    else
    {
        return;
    }
}