#include "main.h"
#include "api.h"
#include <cmath>
#include <cstdio>
#include <memory>
#include <set>
#include <string>

/**
 * You should add more #includes here
 */
#include "okapi/api.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/units/QLength.hpp"
#include "okapi/impl/device/button/controllerButton.hpp"
#include "okapi/impl/device/controllerUtil.hpp"
#include "pros/adi.h"
#include "pros/adi.hpp"
#include "pros/llemu.hpp"
#include "pros/motors.h"
#include "pros/rtos.hpp"

#include <bits/stdc++.h>

int autonpick = 0;
int automod = 0;
lv_res_t sa0(lv_obj_t *btn) {
    autonpick = 0;
    printf("%i\n", autonpick);
    return LV_RES_OK;
}

lv_res_t sa1(lv_obj_t *btn) {
    autonpick = 1;
    printf("%i\n", autonpick);
    return LV_RES_OK;
}

lv_res_t sa2(lv_obj_t *btn) {
    autonpick = 2;
    printf("%i\n", autonpick);
    return LV_RES_OK;
}
lv_res_t sa3(lv_obj_t *btn) {
    autonpick = 3;
    printf("%i\n", autonpick);
    return LV_RES_OK;
}

lv_res_t sa4(lv_obj_t *btn) {
    autonpick = 4;
    printf("%i\n", autonpick);
    return LV_RES_OK;
}

using namespace okapi;
/**
 * A callback function for LLEMU's center button.
 *
 * When this callback is fired, it will toggle line 2 of the LCD text
 * between "I was pressed!" and nothing.
 */
void on_center_button() {
    static bool pressed = false;
    pressed = !pressed;
    if (pressed) {
        // pros::lcd::set_text(2, "I was pressed!");
    } else {
        // pros::lcd::clear_line(2);
    }
} // namespace okapivoid on_center_button()

void shiftarray(bool *arr, bool ap) {
    for (int i = 9; i > 0; i--) {

        arr[i] = arr[i - 1];
    }
    arr[0] = ap;
}

/*
 * When given a pointer to a log array and a boolean, it will return true if the
 * boolean was tapped on, and not just held on
 */
bool is_tapped(bool *log, bool ap) {
    shiftarray(log, ap);
    if (log[0] == true && log[9] == false && log[1] == false) {
        printf("a: %d\n", log[0]);
        return true;
    } else {
        return false;
    }
};

bool is_held(bool *log, bool ap) {
    shiftarray(log, ap);
    if (log[0] == true && log[1] == true && log[2] == true && log[3] == true &&
        log[4] == true && log[5] == true && log[6] == true && log[7] == true &&
        log[8] == true && log[9] == true) {
        return true;
    }
    return false;
}

/**
 * Runs initialization code. This occurs as soon as the program is started.
 *
 * All other competition modes are blocked by initialize; it is recommended
 * to keep execution time for this mode under a few seconds.
 */
void initialize() {
    pros::lcd::initialize();
    // pros::lcd::set_text(1, "Hello PROS User!");

    pros::lcd::register_btn1_cb(on_center_button);
}

/**
 * Runs while the robot is in the disabled state of Field Management System or
 * the VEX Competition Switch, following either autonomous or opcontrol. When
 * the robot is enabled, this task will exit.
 */
void disabled() {}

/**
 * Runs after initialize(), and before autonomous when connected to the Field
 * Management System or the VEX Competition Switch. This is intended for
 * competition-specific initialization routines, such as an autonomous selector
 * on the LCD.
 *
 * This task will exit when the robot is enabled and autonomous or opcontrol
 * starts.
 */
void competition_initialize() {

    lv_obj_t *guibtn1 = lv_btn_create(lv_scr_act(), NULL);
    lv_btn_set_action(guibtn1, LV_BTN_ACTION_CLICK, sa0);
    lv_obj_align(guibtn1, NULL, LV_ALIGN_IN_TOP_LEFT, 0, 0);
    lv_obj_t *guilbl1 = lv_label_create(guibtn1, NULL);
    lv_label_set_text(guilbl1, "To Goal");

    lv_obj_t *guibtn2 = lv_btn_create(lv_scr_act(), NULL);
    lv_btn_set_action(guibtn2, LV_BTN_ACTION_CLICK, sa1);
    lv_obj_align(guibtn2, NULL, LV_ALIGN_IN_TOP_LEFT, 0, 100);
    lv_obj_t *guilbl2 = lv_label_create(guibtn2, NULL);
    lv_label_set_text(guilbl2, "Slap");

    lv_obj_t *guibtn3 = lv_btn_create(lv_scr_act(), NULL);
    lv_btn_set_action(guibtn3, LV_BTN_ACTION_CLICK, sa3);
    lv_obj_align(guibtn3, NULL, LV_ALIGN_IN_TOP_LEFT, 100, 0);
    lv_obj_t *guilbl3 = lv_label_create(guibtn3, NULL);
    lv_label_set_text(guilbl3, "SkillIssue");

    lv_obj_t *guibtn4 = lv_btn_create(lv_scr_act(), NULL);
    lv_btn_set_action(guibtn4, LV_BTN_ACTION_CLICK, sa2);
    lv_obj_align(guibtn4, NULL, LV_ALIGN_IN_TOP_LEFT, 100, 100);
    lv_obj_t *guilbl4 = lv_label_create(guibtn4, NULL);
    lv_label_set_text(guilbl4, "SlTo");

    lv_obj_t *guibtn5 = lv_btn_create(lv_scr_act(), NULL);
    lv_btn_set_action(guibtn5, LV_BTN_ACTION_CLICK, sa4);
    lv_obj_align(guibtn5, NULL, LV_ALIGN_IN_TOP_LEFT, 200, 100);
    lv_obj_t *guilbl5 = lv_label_create(guibtn5, NULL);
    lv_label_set_text(guilbl5, "RFW:)");
}

/**
 * Runs the user autonomous code. This function will be started in its own task
 * with the default priority and stack size whenever the robot is enabled via
 * the Field Management System or the VEX Competition Switch in the autonomous
 * mode. Alternatively, this function may be called in initialize or opcontrol
 * for non-competition testing purposes.
 *
 * If the robot is disabled or communications is lost, the autonomous task
 * will be stopped. Re-enabling the robot will restart the task, not re-start it
 * from where it left off.
 */
void autonomous() {
    FILE *usd_file_read = fopen("/usd/auto.txt", "r");
    int num; // This just needs to be larger than the contents of the file

    fscanf(usd_file_read, "%d", &num);

    // passing 1 because a `char` is 1 byte,
    // and 50 b/c it's the length of buf
    printf("here\n");

    printf("%d\n", num); // print the string read from the file
    // Should print "Example text" to the terminal
    fclose(usd_file_read); // always close files when you're done with them
    autonpick = num;
    // int autonpick = 3;
    std::shared_ptr<ChassisController> drive =
        ChassisControllerBuilder()
            .withMotors(

                {-1, -2, -3},
                {
                    5,
                    6,
                    7,
                })
            .withDimensions({AbstractMotor::gearset::blue, (60.0 / 48.0)},
                            {{3.25_in, 12_in}, imev5BlueTPR})
            .build();

    pros::Imu IMU(19);
    IMU.reset(true);
    IMU.tare();
    pros::delay(200);

    bool pullback = false;
    bool ready = false;

    Motor pull(10);
    printf("%d\n", autonpick);
    // pull.setReversed(true);

    pros::ADIDigitalOut piston(2);
    if (autonpick == 0) { // Auton #1, used for adding tribal to goal
        float x = -3.2;
        drive->moveDistanceAsync(x * 1_ft);
        pros::delay(2500);
        drive->moveDistance(1_ft);
        double yaw = IMU.get_heading();
        std::printf("%f", yaw);
        double turn = -95 - yaw;
        drive->setMaxVelocity(300);
        drive->turnAngle(turn * 1_deg);
        drive->moveDistanceAsync(-4.1_ft);
        pros::delay(3000);
        piston.set_value(true);
    } else if (autonpick == 1 ||
               autonpick ==
                   2) { // Auton #2, Used for removing triball from match loader
        piston.set_value(true);
        if (autonpick == 2) {
            drive->setMaxVelocity(300);
            pros::delay(1000);
            piston.set_value(false);
            drive->moveDistance(-1.7_ft);
            drive->turnAngle(87_deg);
            drive->moveDistance(-3.5_ft);
            drive->turnAngle(95_deg);
            drive->moveDistance(-2_in);
            piston.set_value(true);
            drive->moveDistance(-4_in);
        }

        // drive->turnAngle(45_deg);

    } else if (autonpick == 3) {
        bool reqtopull = false;
        bool good[10];
        pros::Distance dist(11); // fixed
        pull.tarePosition();
        
        
          while (true) {
	  
            if (is_held(good, dist.get() < 70)) {
                reqtopull = true;
            }
            if (reqtopull) {
                // pull.moveRelative((1800 - std::fmod(pull.getPosition(),
                // 1800)),
                //                   1000);
                // reqtopull = false;
                float temp = std::fmod(pull.getPosition(), 1800);
                if ((1800 - temp) < 150) {
                    temp = -(1800 - temp);
                }
                pull.moveRelative(
                    (1800 -
                     temp), //            pull.moveRelative((1800 -
                            //            std::fmod(pull.getPosition(), 1800)),

                    10000000);
                reqtopull = false;
            }

            pros::delay(10);
        }
    }
    if (autonpick == -1) {
        drive->turnAngle(90_deg);
    }
    if (autonpick == 4) {
        float x = sqrt(8);
        drive->moveDistance(x * -1_ft);
        pros::delay(500);
        drive->moveDistance(0.5_ft);
    }

    if (autonpick == 5) {
        pull.moveRelative(1800, 1000000);
    }
}

/**
 * Runs the operator control code. This function will be started in its own task
 * with the default priority and stack size whenever the robot is enabled via
 * the Field Management System or the VEX Competition Switch in the operator
 * control mode.
 *
 * If no competition control is connected, this function will run immediately
 * following initialize().
 *
 * If the robot is disabled or communications is lost, the
 * operator control task will be stopped. Re-enabling the robot will restart the
 * task, not resume it from where it left off.
 */
void opcontrol() {
    // autonomous();
    // autonpick = 2;

    // pros::delay(100);
    std::shared_ptr<ChassisController> drive =
        ChassisControllerBuilder()
            .withMotors(
                {
                    -5,
                    -6,
                    -7,
                },
                {1, 2, 3})
            // Green gearset, 4 in wheel diam, 11.5 in wheel track
            .withDimensions({AbstractMotor::gearset::blue, (48.0 / 36.0)},
                            {{3.25_in, 12_in}, imev5BlueTPR})
            .build();

    Controller controller;

    pros::ADIDigitalIn limit(1);
    pros::ADIDigitalOut piston(2);
    pros::ADIDigitalOut upflap(3);
    pros::ADIDigitalOut funnypiston(8);

    Motor leftMotor1(-5);
    Motor leftMotor2(-6);
    Motor leftMotor3(-7);

    Motor rightMotor1(1);
    Motor rightMotor2(2);
    Motor rightMotor3(3);


    leftMotor1.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
    leftMotor2.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
    leftMotor3.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);

    rightMotor1.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
    rightMotor2.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
    rightMotor3.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);

    bool flipped = false;

    Motor flywheel(15);

    Motor pull(11);
    pull.setReversed(true);

    Motor lift(16);

    // Motor pull2(11);

    // pull1.setReversed(true);
    // pull2.setReversed(false);

    // pull1.setGearing(AbstractMotor::gearset::green);
    // pull2.setGearing(AbstractMotor::gearset::green);

    // MotorGroup pull = MotorGroup({pull1, pull2});
    flywheel.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);
    flywheel.setGearing(okapi::AbstractMotor::gearset::blue); //should be red for cata
    pull.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);


    lift.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

    ControllerButton R2(ControllerDigital::R2);

    ControllerButton R1(ControllerDigital::R1);

    ControllerButton L2(ControllerDigital::L2);

    ControllerButton L1(ControllerDigital::L1);

    ControllerButton rightarrow(ControllerDigital::right);
    ControllerButton YButton(ControllerDigital::Y);

    ControllerButton leftarrow(ControllerDigital::left);

    ControllerButton Abutton(ControllerDigital::A);

    ControllerButton uparrow(ControllerDigital::up);
    ControllerButton Xbutton(ControllerDigital::X);
    ControllerButton downarrow(ControllerDigital::down);
    ControllerButton Bbutton(ControllerDigital::B);

    bool arr[10];
    bool flaparr[10];
    for (int i = 0; i < 10; i++) {
        arr[i] = false;
    }
    for (int i = 0; i < 10; i++) {
        flaparr[i] = false;
    }

    bool pullback = false;
    bool ready = false;
    int tcount = 0;
    bool flipperstatus = false;

    bool flapsOn = false;

    bool manpullarr[10];
    for (int i = 0; i < 10; i++) {
        manpullarr[i] = 0;
    }

    bool funnyarr[10];
    for (int i = 0; i < 10; i++) {
        funnyarr[i] = false;
    }

    bool loopmodearr[10];
    for (int i = 0; i < 10; i++) {
        loopmodearr[i] = false;
    }

    bool distarr[10];
    for (int i = 0; i < 10; i++) {
        distarr[i] = false;
    }

    bool r1arr[10];
    for (int i = 0; i < 10; i++) {
        r1arr[i] = false;
    }

    bool y1arr[10];
    for (int i = 0; i < 10; i++) {
        y1arr[i] = false;
    }

    bool modeswap[10];
    for (int i = 0; i < 10; i++) {
        modeswap[i] = false;
    }

    bool funnypistonstatus = false;
    auto IMU = okapi::IMU(19, IMUAxes::z);
    bool reqtoloop = false;
    bool lmode = false;

    bool upflapstatus = false;

    bool autoselectormode = false;

    pros::Distance dist(20);

    pros::Distance dististriball(11); // fixed

    while (true) {
        // Tank drive with left and right sticks.
        drive->getModel()->tank(controller.getAnalog(ControllerAnalog::leftY),
                                controller.getAnalog(ControllerAnalog::rightY));

        // Wait and give up the time we don't need to other tasks.
        // Additionally, joystick values, motor telemetry, etc. all updates
        // every 10 ms.

        float lsum = leftMotor1.getTemperature() + leftMotor2.getTemperature() +
                     leftMotor3.getTemperature();
        float lavg = lsum / 3;

        float rsum = rightMotor1.getTemperature() +
                     rightMotor2.getTemperature() +
                     rightMotor3.getTemperature();
        float ravg = rsum / 3;

	if (lavg > 150) {
	    lavg = -9;
	}
	if (ravg >150) {
	    ravg = -9;
	}

        if (pullback) {
            if (!(dist.get() < 35)) {
                // pull.moveVoltage(-12000);
                pull.moveVoltage(12000);

            } else {
                pullback = false;
                ready = true;
                pull.moveVoltage(0);
                pull.tarePosition();
            }
        } else {
        }
        pros::lcd::set_text(2, std::to_string(pullback));
        /* CURENTLY DISABLED BODY FLIP CODE. DISABLED BECAUSE OF BASE REBUILD,
         * NEEDS MOTOR TWEEKING*/

        if (is_tapped(arr, (YButton.isPressed() && rightarrow.isPressed()))) {
            flipperstatus = !flipperstatus;
            tcount++;
        }

        if (is_tapped(flaparr, R2.isPressed())) {
            flapsOn = !flapsOn;
        }

        if (is_tapped(manpullarr,
                      (uparrow.isPressed() && Xbutton.isPressed()))) {
            pullback = true;
            tcount++;
        }

        if (is_tapped(funnyarr, leftarrow.isPressed())) {
            funnypistonstatus = !funnypistonstatus;
        }

        if (is_tapped(y1arr, Abutton.isPressed())) {
            upflapstatus = !upflapstatus;
        }

        if (is_tapped(modeswap,
                      (leftarrow.isPressed() && rightarrow.isPressed() &&
                       uparrow.isPressed() && downarrow.isPressed()))) {
            autoselectormode = !autoselectormode;
        }

        if (autoselectormode) {
            int tempaselect = 0;
            bool left[10];
            for (int i = 0; i < 10; i++) {
                left[i] = false;
            }
            bool right[10];
            for (int i = 0; i < 10; i++) {
                right[i] = false;
            }
            bool a[10];
            for (int i = 0; i < 10; i++) {
                a[i] = false;
            }

            while (autoselectormode) {
                if (is_tapped(right, rightarrow.isPressed())) {
                    tempaselect = tempaselect + 1;
                }
                if (is_tapped(left, leftarrow.isPressed())) {
                    tempaselect = tempaselect - 1;
                }
                if (is_tapped(a, Abutton.isPressed())) {
		    // write to file;
                    FILE *autofile = fopen("/usd/auto.txt", "w");
                    fprintf(autofile, "%d", tempaselect);
                    fclose(autofile);
                    autoselectormode = false;
                }

                controller.setText(0, 0, std::to_string(tempaselect));

                pros::delay(10);
            }
        }

        upflap.set_value(upflapstatus);

        piston.set_value(flapsOn);
        funnypiston.set_value(funnypistonstatus);

        pros::lcd::set_text(1, std::to_string(tcount));
        if (!flipperstatus) {
            pros::lcd::set_text(0, "flip f flippcmdreceive");
            drive = ChassisControllerBuilder()
                        .withMotors({-5, -6, -7}, {1, 2, 3})
                        .withDimensions(
                            {AbstractMotor::gearset::blue, (48.0 / 36.0)},
                            {{3.25_in, 12_in}, imev5BlueTPR})
                        .build();
            flipped = false;
        }
        if (flipperstatus) {
            pros::lcd::set_text(0, "flip b flippcmdreceive");

            drive = ChassisControllerBuilder()
                        .withMotors({-1, -2, -3}, {5, 6, 7})
                        .withDimensions(
                            {AbstractMotor::gearset::blue, (48.0 / 36.0)},
                            {{3.25_in, 12_in}, imev5BlueTPR})
                        .build();
            flipped = true;
        }


	// if (R1.isPressed()) {
	//     flywheel.moveVoltage(-12000);
	// }
	// else {
	//     flywheel.moveVoltage(0);
	// }

	if(L1.isPressed()) {
	    lift.moveVoltage(-12000);
	}
	else if (L2.isPressed()) {
	    lift.moveVoltage(12000);
	} else {
	    lift.moveVoltage(0);
	}
	
        // if (fly.isPressed()) {
        //     pullback = true;

        //     pros::lcd::set_text(5, "hit while not");
        // }

        if (is_tapped(loopmodearr, downarrow.isPressed())) {
            // lmode = !lmode;
            lmode = false;
        }

        // printf("%i",dist.get());
        if (is_held(distarr, dist.get() < 70) && lmode) {
            reqtoloop = true;
            // printf("raaaaaaaaaaaaaa");
        }

        if (is_tapped(r1arr, R1.isPressed()) || reqtoloop == true) {
            float temp = std::fmod(pull.getPosition(), 1800);
            if ((1800 - temp) < 150) {
                temp = -(1800 - temp);
            }
            pull.moveRelative(
                (1800 -
                 temp), //            pull.moveRelative((1800 -
                        //            std::fmod(pull.getPosition(), 1800)),

                10000000);
            reqtoloop = false;
            // printf("hhhhhhhhh\n");
        }

        if (std::abs(pull.getPosition() - pull.getTargetPosition()) < 10) {
            // pull.moveRelative(pull.getTargetPosition(), 0);
            pull.moveVoltage(0);
            pros::lcd::set_text(3, "hhh");
        } else {
            pros::lcd::set_text(3, "bbb");
        }

        if (dist.get() < 40 &&
            (std::abs(pull.getPosition() - pull.getTargetPosition()) < 5)) {
            pull.moveVoltage(0);
        } else {
        }

        
        pros::lcd::set_text(4, std::to_string(pull.getTargetPosition()));

        if (Bbutton.isPressed()) {
            pull.moveVoltage(12000);
        }
        if (uparrow.isPressed()) {
            pull.moveVoltage(0);
        }

        pros::lcd::set_text(5, std::to_string(IMU.get()));

        // if (L2.isPressed()) {
        //     intake.moveVoltage(-12000);
	//     testMotor.moveVoltage(-12000);
        // } else if (L1.isPressed()) {
        //     intake.moveVoltage(12000);
	//     testMotor.moveVoltage(-12000);
        // } else {
        //     intake.moveVoltage(0);
	//     testMotor.moveVoltage(0);
        // }

	


        controller.setText(
            0, 0,
            std::to_string(static_cast<int>(flywheel.getTemperature())) +
                std::to_string(static_cast<int>(lavg)) +
                std::to_string(static_cast<int>(ravg)) +
	    std::to_string(static_cast<int>(lift.getTemperature()))



			   );
        pros::delay(10);
    }
}
