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

/**
 * You should add more #includes here
 */
#include "display/lv_core/lv_obj.h"
#include "display/lv_misc/lv_task.h"
#include "display/lv_objx/lv_img.h"
#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/distance.hpp"
#include "pros/llemu.hpp"
#include "pros/motors.h"
#include "pros/rtos.hpp"

#include <bits/stdc++.h>

// 1 2 3 5 //left
// 7 8 9 10 //right

// FILE HANDLE CODE:

typedef FILE *pc_file_t;

static lv_fs_res_t pcfs_open(void *file_p, const char *fn, lv_fs_mode_t mode) {
    errno = 0;
    const char *flags = "";
    if (mode == LV_FS_MODE_WR)
        flags = "wb";
    else if (mode == LV_FS_MODE_RD)
        flags = "rb";
    else if (mode == (LV_FS_MODE_WR | LV_FS_MODE_RD))
        flags = "a+";

    char buf[256];
    sprintf(buf, "/%s", fn);
    pc_file_t f = fopen(buf, flags);

    if (f == NULL)
        return LV_FS_RES_UNKNOWN;
    else {
        fseek(f, 0, SEEK_SET);
        pc_file_t *fp = (pc_file_t *)file_p;
        *fp = f;
    }

    return LV_FS_RES_OK;
}

static lv_fs_res_t pcfs_close(void *file_p) {
    pc_file_t *fp = (pc_file_t *)file_p;
    fclose(*fp);
    return LV_FS_RES_OK;
}

static lv_fs_res_t pcfs_read(void *file_p, void *buf, uint32_t btr,
                             uint32_t *br) {
    pc_file_t *fp = (pc_file_t *)file_p;
    *br = fread(buf, 1, btr, *fp);
    return LV_FS_RES_OK;
}

static lv_fs_res_t pcfs_seek(void *file_p, uint32_t pos) {
    pc_file_t *fp = (pc_file_t *)file_p;
    fseek(*fp, pos, SEEK_SET);
    return LV_FS_RES_OK;
}

static lv_fs_res_t pcfs_tell(void *file_p, uint32_t *pos_p) {
    pc_file_t *fp = (pc_file_t *)file_p;
    *pos_p = ftell(*fp);
    return LV_FS_RES_OK;
}

// end file drivers

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_tapped_short(bool *log, bool ap) {
    shiftarray(log, ap);
    if (log[0] == true && 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() {
    pros::Imu IMU(19);
    IMU.reset(true);
    IMU.tare();
    pros::delay(200);

    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 =
        // -5 is
        ChassisControllerBuilder()
            .withMotors({-1, -2, -3, -5}, {7, 8, 9, 10})
            .withDimensions({AbstractMotor::gearset::blue, (48.0 / 36.0)},
                            {{3.25_in, 11.2_in}, imev5BlueTPR})
            .build();

    pros::ADIDigitalOut funnypiston(3);
    funnypiston.set_value(false);

    pros::ADIDigitalOut upflap(8);
    upflap.set_value(false);

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

    bool pullback = false;
    bool ready = false;

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

    pros::ADIDigitalOut piston(2);
    if (autonpick == 0) { // Auton #1, used for adding tribal to goal
        float x = -2.7;
        drive->moveDistanceAsync(x * 1_ft);
        pros::delay(2500);
        drive->moveDistance(0.5_ft);
        double yaw = IMU.get_heading();
        std::printf("yaw: %f\n", yaw);
        // double turn = -92 - yaw;
        double turn = 45 - yaw;
    std:
        printf("turn: %f\n", turn);
        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);
        drive->moveDistance(1_ft);
        if (autonpick == 2) {
            drive->setMaxVelocity(300);
            pros::delay(1000);
            piston.set_value(false);
            // drive->moveDistance(-1.7_ft);
            drive->turnAngle(115_deg);
            drive->moveDistance(3.5_ft);
            drive->turnAngle(95_deg);
            drive->setMaxVelocity(50);
            drive->moveDistanceAsync(2_ft);
            pros::delay(2500);
            piston.set_value(true);
            drive->moveDistance(4_in);
        }

        // drive->turnAngle(45_deg);

    } else if (autonpick == 3) {
        upflap.set_value(true);

        bool reqtopull = false;
        bool good[10];
        pros::Distance dist(13); // fixed
        int counter = 0;
        pull.tarePosition();
        printf("hereasdsadfasdfsdf\n");
        bool reqtopullarr[10];
        for (int i = 0; i < 10; i++) {
            reqtopullarr[i] = false;
        }

        while (counter < 44) {

            if (is_held(good, dist.get() < 115)) {
                reqtopull = true;
            }

            if (is_tapped_short(reqtopullarr, reqtopull)) {
                counter = counter + 1;
            }
            pros::lcd::set_text(7, std::to_string(counter));

            // if (pros::millis() % 2000 <= 10){
            //	  reqtopull = true;
            // }
            if (reqtopull) {
                // pull.moveRelative((1800 - std::fmod(pull.getPosition(),
                // 1800)),
                //					 1000);
                // reqtopull = false;
                float temp = std::fmod(pull.getPosition(), 900);
                if ((900 - temp) < 150) {
                    temp = -(900 - temp);
                }
                pull.moveRelative(
                    (900 -
                     temp), //			  pull.moveRelative((1800
                            //-
                            // std::fmod(pull.getPosition(), 1800)),

                    10000000);
                reqtopull = false;
            }

            pros::delay(10);
        }
    }

    if (autonpick == 3 || autonpick == 4) {
        pros::delay(500);
        pros::Distance distcal(20);
        bool good = false;
        while (!good) {
            if (!(distcal.get() < 35)) {
                // pull.moveVoltage(-12000);
                pull.moveVoltage(12000);

            } else {
                pullback = false;
                ready = true;
                good = true;
                pull.moveVoltage(0);
                pull.tarePosition();
            }
        }
        drive->setMaxVelocity(300);
        drive->turnAngle(30_deg);
        drive->setMaxVelocity(600);

        drive->moveDistance(-7.1_ft);

        pros::delay(500);

        double yaw = IMU.get_heading();
        std::printf("%f\n", yaw);
        double turn = -60 - yaw;

        drive->setMaxVelocity(300);
        drive->turnAngle(turn * 1_deg);

        pros::delay(300);
        drive->moveDistanceAsync(-5_ft);
        pros::delay(2000);

        piston.set_value(true);

        // drive->turnAngle(90_deg);
        yaw = IMU.get_heading();
        turn = 30 - yaw;
        drive->turnAngle(turn * 1_deg);

        drive->moveDistance(-3_ft);

        drive->turnAngle(90_deg);
        drive->moveDistance(-4_ft);
    }

    if (autonpick == 5) {
        pros::Distance distforward(17);
        // drive->moveDistanceAsync(500_ft);
        drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::hold);
        // piston.set_value(true);
        // NEED TO WRTIE IMU CENTER CODE
        // update IMU each cycle to get true rotation
        while (distforward.get() > 320) {
            drive->getModel()->curvature(12000, IMU.get_rotation() * -0.04);

            // printf("%i\n", distforward.get());
            pros::delay(10);
        }
        // piston.set_value(false);

        drive->stop();
        funnypiston.set_value(true);
        pros::delay(500);

        auto starttime = Timer().millis();
        while (Timer().millis() < starttime + 1.5_s) {
            drive->getModel()->curvature(-12000, IMU.get_rotation() * -0.04);

            // printf("%i\n", distforward.get());
            pros::delay(10);
        }
        // piston.set_value(false);
        drive->stop();
        funnypiston.set_value(true);
        pros::delay(500);

        drive->moveDistance(1_in);
        // drive->turnAngle(15_deg);
        // drive->moveDistance(-3.8_ft);
        // drive->turnAngle(90_deg);
        bool imuholdarray[10];

        while (!is_held(imuholdarray, (abs(68.0 - IMU.get_rotation()) < 1))) {
            drive->getModel()->rotate((68.0 - IMU.get_rotation()) * 0.008);
            pros::delay(10);
        }

        drive->getModel()->stop();

        funnypiston.set_value(false);
        drive->moveDistance(2_ft);
        drive->moveDistance(-2_ft);
        funnypiston.set_value(true);
        drive->moveDistance(2_ft);
        drive->moveDistance(-2_ft);
        funnypiston.set_value(false);
        drive->turnAngle(45_deg);
        drive->moveDistance(-2_ft);
        drive->moveDistance(1_ft);
    }

    if (autonpick == 6) {
        float x = -2.7;
        drive->moveDistanceAsync(x * 1_ft);
        pros::delay(2500);
        drive->moveDistance(0.5_ft);
    }

    if (autonpick == 7) {
        drive->moveDistance(-1_ft);
        piston.set_value(true);
        pros::delay(44500);

        drive->moveDistance(1_ft);

        piston.set_value(false);
        drive->turnAngle(-45_deg);
        funnypiston.set_value(true);
        pros::delay(500);
        drive->moveDistance(7.5_ft);
        drive->turnAngle(-45_deg);
        drive->moveDistance(2_ft);
        drive->moveDistance(-2_ft);
        drive->moveDistance(2_ft);
        drive->moveDistance(-2_ft);
    }
    if (autonpick == 8) {
        pros::Distance distforward(17);
        // drive->moveDistanceAsync(500_ft);
        drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::hold);
        // piston.set_value(true);
        // NEED TO WRTIE IMU CENTER CODE
        // update IMU each cycle to get true rotation
        while (distforward.get() > 400) {
            drive->getModel()->curvature(12000, IMU.get_rotation() * -0.04);

            // printf("%i\n", distforward.get());
            pros::delay(10);
        }
        // piston.set_value(false);

        drive->stop();
        funnypiston.set_value(true);
        pros::delay(500);

        auto starttime = Timer().millis();
        while (Timer().millis() < starttime + 1.5_s) {
            drive->getModel()->curvature(-6000, IMU.get_rotation() * -0.04);

            // printf("%i\n", distforward.get());
            pros::delay(10);
        }
        // piston.set_value(false);
        drive->stop();
        funnypiston.set_value(true);
        pros::delay(500);

        drive->moveDistance(1_in);
        // drive->turnAngle(15_deg);
        // drive->moveDistance(-3.8_ft);
        // drive->turnAngle(90_deg);
        bool imuholdarray[10];

        while (!is_held(imuholdarray, (abs(70.0 - IMU.get_rotation()) < 1))) {
            drive->getModel()->rotate((70.0 - IMU.get_rotation()) * 0.008);
            pros::delay(10);
        }

        drive->getModel()->stop();

        funnypiston.set_value(false);
        drive->moveDistance(2_ft);
        drive->moveDistance(-2_ft);
        funnypiston.set_value(true);
        drive->moveDistance(2_ft);
        drive->moveDistance(-2_ft);
        funnypiston.set_value(false);

        // //drive->moveDistance(0_ft);
        // pros::delay(300);
        // drive->turnAngle(50_deg);
        // pros::delay(300);
        // drive->moveDistance(-1.5_ft);
    }

    if (autonpick == 9) {
        piston.set_value(true);
        pros::delay(300);
        piston.set_value(false);
        pros::Distance distforward(17);
        // drive->moveDistanceAsync(500_ft);
        drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::hold);
        // piston.set_value(true);
        // NEED TO WRTIE IMU CENTER CODE
        // update IMU each cycle to get true rotation
        while (distforward.get() > 300) {
            drive->getModel()->curvature(12000, IMU.get_rotation() * -0.04);

            // printf("%i\n", distforward.get());
            pros::delay(10);
        }
        drive->stop();
        funnypiston.set_value(true);
        pros::delay(500);

        drive->setMaxVelocity(200);
        drive->moveDistance(-1_in);

        bool imuholdarray[10];

        drive->setMaxVelocity(600);

        // while (!is_held(imuholdarray, (abs(100.0 - IMU.get_rotation()) < 1))) {
        //     drive->getModel()->rotate((100.0 - IMU.get_rotation()) * 0.008);
        //     pros::delay(10);
        // }

		drive->turnAngle(100_deg);

        drive->getModel()->stop();

        funnypiston.set_value(false);

        drive->moveDistanceAsync(2_ft);

        pros::delay(1000);

        drive->moveDistance(-1_ft);

        while (!is_held(imuholdarray, (abs(-110.0 - IMU.get_rotation()) < 3))) {
            drive->getModel()->rotate((-110.0 - IMU.get_rotation()) * 0.008);
            pros::delay(10);
        }

		drive->getModel()->stop();
		while (distforward.get() > 300) {
            drive->getModel()->curvature(12000, (IMU.get_rotation()+110) * -0.04);

            // printf("%i\n", distforward.get());
            pros::delay(10);
        }
        drive->stop();
        funnypiston.set_value(true);
        pros::delay(500);

		drive->moveDistance(-1_ft);

		while (!is_held(imuholdarray, (abs(75.0 - IMU.get_rotation()) < 3))) {
            drive->getModel()->rotate((75.0 - IMU.get_rotation()) * 0.008);
            pros::delay(10);
        }

		drive->getModel()->stop();

		funnypiston.set_value(false);
		
		drive->moveDistanceAsync(3_ft);

		pros::delay(1000);

		drive->moveDistance(-1_ft);

		while (!is_held(imuholdarray, (abs(-70.0 - IMU.get_rotation()) < 3))) {
            drive->getModel()->rotate((-70.0 - IMU.get_rotation()) * 0.008);
            pros::delay(10);
        }

		drive->getModel()->stop();

		drive->moveDistance(1_ft);
		
		funnypiston.set_value(true);
		pros::delay(300);

		drive->turnAngle(180_deg);
		funnypiston.set_value(false);

		drive->moveDistanceAsync(3_ft);
		pros::delay(1000);
		drive->moveDistanceAsync(-1_ft);

        // piston.set_value(false);
    }
}

/**
 * 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({-1, -2, -3, -5}, {7, 8, 9, 10})

            // Green gearset, 4 in wheel diam, 11.5 in wheel track
            .withDimensions({AbstractMotor::gearset::blue, (48.0 / 36.0)},
                            {{3.25_in, 11.2_in}, imev5BlueTPR})
            .build();

    Controller controller;

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

    Motor leftMotor1(-1);
    Motor leftMotor2(-2);
    Motor leftMotor3(-3);
    Motor leftMotor4(-5);

    Motor rightMotor1(7);
    Motor rightMotor2(8);
    Motor rightMotor3(9);
    Motor rightMotor4(10);
    // lift on Ltrig

    // intake on A

    drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::brake);

    bool flipped = false;

    Motor flywheel(15);

    Motor pull(12);
    // pull.setReversed(true);

    Motor lift(18);

    // 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::coast);

    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 l1toggle[10];
    for (int i = 0; i < 10; i++) {
        l1toggle[i] = false;
    }

    bool l2toggle[10];
    for (int i = 0; i < 10; i++) {
        l2toggle[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 upflaparr[10];
    for (int i = 0; i < 10; i++) {
        upflaparr[i] = false;
    }

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

    bool barr[10];
    for (int i = 0; i < 10; i++) {
        barr[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

    bool intakebool;
    intakebool = false;

    lv_fs_drv_t pcfs_drv;                      /*A driver descriptor*/
    memset(&pcfs_drv, 0, sizeof(lv_fs_drv_t)); /*Initialization*/

    pcfs_drv.file_size = sizeof(pc_file_t); /*Set up fields...*/
    pcfs_drv.letter = 'S';
    pcfs_drv.open = pcfs_open;
    pcfs_drv.close = pcfs_close;
    pcfs_drv.read = pcfs_read;
    pcfs_drv.seek = pcfs_seek;
    pcfs_drv.tell = pcfs_tell;
    lv_fs_add_drv(&pcfs_drv);

    lv_obj_clean(lv_scr_act());

    lv_obj_t *img_var = lv_img_create(lv_scr_act(), NULL);
    lv_obj_t *dog_var = lv_img_create(lv_scr_act(), NULL);
    lv_img_set_src(img_var, "S:/usd/raa.bin");

    lv_obj_set_pos(img_var, 200, 0);
    lv_obj_set_size(img_var, 200, 200);

    lv_img_set_src(dog_var, "S:/usd/dog2.bin");
    lv_obj_set_pos(dog_var, 200, 0);
    lv_obj_set_size(dog_var, 200, 200);

    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() + leftMotor4.getTemperature();
        float lavg = lsum / 4;

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

        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(l1toggle, L1.isPressed())) {
            flapsOn = !flapsOn;
        }

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

        if (is_tapped(funnyarr, L2.isPressed())) {
            funnypistonstatus = !funnypistonstatus;
            lv_obj_set_hidden(dog_var, !funnypistonstatus);
        }

        if (is_tapped(upflaparr, R2.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({-7, -8, -9, -10}, {1, 2, 3, 5})
                        .withDimensions(
                            {AbstractMotor::gearset::blue, (48.0 / 36.0)},
                            {{3.25_in, 11.2_in}, imev5BlueTPR})
                        .build();
            flipped = true;
        }
        if (!flipperstatus) {
            pros::lcd::set_text(0, "flip b flippcmdreceive");

            drive = ChassisControllerBuilder()
                        .withMotors({-1, -2, -3, -5}, {7, 8, 9, 10})
                        .withDimensions(
                            {AbstractMotor::gearset::blue, (48.0 / 36.0)},
                            {{3.25_in, 11.2_in}, imev5BlueTPR})
                        .build();
            flipped = false;
        }

        // if (R1.isPressed()) {
        //	   flywheel.moveVoltage(-12000);
        // }
        // else {
        //	   flywheel.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, dististriball.get() < 70) && lmode) {
        //	   reqtoloop = true;
        //	   // printf("raaaaaaaaaaaaaa");
        // }

        if (is_tapped(r1arr, R1.isPressed()) || reqtoloop == true) {
            float temp = std::fmod(pull.getPosition(), 900);
            pros::lcd::set_text(6, std::to_string(temp));
            if ((900 - temp) < 150) {
                temp = -(900 - temp);
                pros::lcd::set_text(7, std::to_string(temp));
            }
            pull.moveRelative(
                (900 -
                 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()));

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

        pros::lcd::set_text(5, std::to_string(pull.getPosition()));

        controller.setText(
            0, 0,
            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);
    }
}
