#include "main.h"

using namespace ez;

Drive chassis(
    {-11, -18, 20}, // Left motor ports.
    {1,8 ,-10},     // Right motor ports.
    7,              // IMU port.
    3.25,           // Wheel size.
    600,            // Ticks per rotation of encoder.
    1.6667          // External ratio.
    // TODO: Add tracking wheel ports.
);

/*
    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::delay(500); // Stop the user from doing anything while legacy ports
                      // configure.

    chassis.opcontrol_curve_buttons_toggle(true); // Enables modifying the
                                                  // controller curve with
                                                  // buttons on the joysticks.

    chassis.opcontrol_drive_activebrake_set(0); // Sets the active brake kP. We
                                                // recommend 2.

    chassis.opcontrol_curve_default_set(0, 0); // Defaults for curve. If using
                                               // tank, only the first
                                               // parameter is used. (Comment
                                               // this line out if you have an
                                               // SD card!).

    default_constants(); // Set the drive to your own constants from autons.cpp!

#if 0
    as::auton_selector.autons_add({
        Auton("Example Drive\n\nDrive forward and come back.", drive_example),
        Auton("Example Turn\n\nTurn 3 times.", turn_example),
        Auton("Drive and Turn\n\nDrive forward, turn, come back. ", drive_and_turn),
        Auton("Drive and Turn\n\nSlow down during drive.", wait_until_change_speed),
        Auton("Swing Example\n\nSwing in an 'S' curve", swing_example),
        Auton("Combine all 3 movements", combining_movements),
        Auton("Interference\n\nAfter driving forward, robot performs differently if interfered or not.", interfered_example),
    });
#endif

    chassis.initialize();
    as::initialize();
    master.rumble(".");
}


/*
    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() { }

/*
    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() {
    chassis.pid_targets_reset(); // Resets PID targets to 0.
    chassis.drive_imu_reset(); // Reset gyro position to 0.
    chassis.drive_sensor_reset(); // Reset drive sensors to 0.
    chassis.drive_brake_set(MOTOR_BRAKE_HOLD); // Set motors to hold. This
                                               // helps autonomous consistency.

    int dist = 48;
    chassis.pid_drive_set(dist, 127, true);
    chassis.pid_wait_until(dist - 5);

    int dist2 = -(dist-6);
    chassis.pid_drive_set(dist2, 127, true);
    chassis.pid_wait_until(dist2 + 5);
}

/*
    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() {
    chassis.drive_brake_set(MOTOR_BRAKE_COAST);
    //swing_example();
    while (true) {
        if (!pros::competition::is_connected()) { 
            if (master.get_digital_new_press(DIGITAL_X)) 
                chassis.pid_tuner_toggle();
            if (master.get_digital_new_press(DIGITAL_B)) 
                autonomous();
            chassis.pid_tuner_iterate(); // Allow PID Tuner to iterate.
        } 
        chassis.opcontrol_arcade_standard(SPLIT); // Tank control.
        pros::delay(util::DELAY_TIME); // This is used for timer calculations!
                                       // Keep this util::DELAY_TIME.
    }
}