#include "ev3cxx.h"
#include "app.h"
#include <ctime>
#include <functional>
#include <sstream>
#include <math.h>

/*
Change notes:

0.4.0 - 'HELGA' (#50)
Fixed 

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 hole 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 notes
struct note
{
    uint16_t frequency;
    int32_t duration;
};

// Function to create new program version
version createVersion(int versionID, const char *codename, int major, int minor, int patch, int day, int month, int year, int hour, int minute)
{
    version retVersion;
    retVersion.id = versionID;
    retVersion.major = major;
    retVersion.minor = minor;
    retVersion.patch = patch;
    retVersion.codename = codename;
    retVersion.relDate.tm_sec = 0;
    retVersion.relDate.tm_min = minute;
    retVersion.relDate.tm_hour = hour;
    retVersion.relDate.tm_mday = day;
    retVersion.relDate.tm_mon = month;
    retVersion.relDate.tm_year = year - 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 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)
{
    const int LEFT_THRESHOLD = -3;
    const int RIGHT_THRESHOLD = 3;
    const int CORRECTION_MULTIPLIER = 2;
    const int CYCLE_LIMIT = 0;//75;

    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;

    int lCounter = 0;
    int rCounter = 0;

    bool run = true;
    int cycleCounter = 0;
    int lastRProblem = 0;
    int lastLProblem = 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;
            return false;
        }

        // 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 = angle - LEFT_THRESHOLD;
            motor_powers.lMotorPWR += (int)pow(CORRECTION_MULTIPLIER, correction);
            ev3_speaker_play_tone(correction*1000,50);
            // Check if the motor is stuck
            if(lPower == 0){
                ev3_speaker_play_tone(NOTE_A5,250);
                
                // Set lastProblem na cycleCounter because there is a problem
                lastLProblem = cycleCounter;

                // Check if lastProblem was in previous cycle
                if(cycleCounter - 1 == lastLProblem){
                    lCounter += 1;
                    // If 5 problems occured than try to fix the problem
                    if(lCounter == 6){
                        ev3_speaker_play_tone(NOTE_C4, 250);
                        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::RED);
                        motors.on(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR);
                        tslp_tsk(250);
                        motors.off();
                        lCounter = 0;
                    }
                // Reset counter
                } else {
                    lCounter = 0;
                }
                
            }
            
        // To the right
        } else if (angle < RIGHT_THRESHOLD) {
            //ev3_speaker_play_tone(NOTE_A4, 250);
            int correction = angle - RIGHT_THRESHOLD;
            motor_powers.rMotorPWR += (int)pow(CORRECTION_MULTIPLIER, correction);//correction * CORRECTION_MULTIPLIER;
            ev3_speaker_play_tone(correction*1000,50);
            // Check if the motor is stuck
            if(rPower == 0){
                ev3_speaker_play_tone(NOTE_A4,250);
                
                // Set lastProblem na cycleCounter because there is a problem
                lastRProblem = cycleCounter;

                // Check if lastProblem was in previous cycle
                if(cycleCounter - 1 == lastRProblem){
                    rCounter += 1;
                    // If 5 problems occured than try to fix the problem
                    if(rCounter == 6){
                        ev3_speaker_play_tone(NOTE_C5, 250);
                        ev3cxx::statusLight.setColor(ev3cxx::StatusLightColor::ORANGE);
                        motors.on(-motor_powers.rMotorPWR, -motor_powers.lMotorPWR);
                        tslp_tsk(250);
                        motors.off();
                        rCounter = 0;
                    }
                // Reset counter
                } else {
                    rCounter = 0;
                }
            }
        }

        cycleCounter++;

        if(cycleCounter == CYCLE_LIMIT) {
            run = false;
        }
    }
    return true;
}


void main_task(intptr_t unused)
{

    // 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(51, "HELGA", 0, 4, 0, 8, 11, 2023, 15, 40);

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

    // Print version information on screen
    ev3cxx::display.format("    DOBREMYSL\nVERSION: % .% .%  #% \nNAME: % ") % VERSION.major % VERSION.minor % VERSION.patch % VERSION.id % VERSION.codename;

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

    // Set up motor powers
    MPWRS idealMPWRS;
    idealMPWRS.lMotorPWR = 85;
    idealMPWRS.rMotorPWR = 50;

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

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

    // Set up buttons
    ev3cxx::BrickButton btnEnter(ev3cxx::BrickButtons::ENTER); // Middle button

    // Start program
    btnEnter.waitForClick();

    //turn_left(motors);
    bool side_1 = run_short_side(motors, motor_powers, gyro);
    if(!side_1) {
        return;
    } else {
        ev3_speaker_play_tone(NOTE_G5, 500);
        return;
    }
}