#include <Arduino.h>
#include "globals.h"
#include "swerve.h"
#include <math.h>
#include <stdint.h>
#include <stdio.h>

// 3-mode drive control system based on https://compendium.readthedocs.io/en/latest/tasks/drivetrains/swerve.html


template <typename T> int signum(T val) {
    return (T(0) < val) - (val < T(0));
}

// Utility functions

float closestAngle(float current, float target) // Calculate closest distance between current and target, set the sign to the fastest direction to go from current to target
{
        // get direction
        float dir = normalizeAngle(target) - normalizeAngle(current);

        // convert from -360 to 360 to -180 to 180
        if (abs(dir) > 180.0)
        {
                dir = -(signum(dir) * 360.0) + dir;
        }
        return dir;
}

swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder) // Initialize a swerve_drive struct, use the current time and encoder positions to fill the arrays
{
    swerve_drive out;

    for(int i = 1; i < ENCODER_BUFFER_ENTRY_COUNT; i++) // Iterate over the elements in the arrays
    {
        out.encoder_buffer_times_ms[i] = 0; // Initialize the contents of this array to 0, since no ongoing encoder samples have been taken yet
        out.encoder_buffer_front_left[i] = front_left_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet 
        out.encoder_buffer_front_right[i] = front_right_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
        out.encoder_buffer_back_left[i] = back_left_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
        out.encoder_buffer_back_right[i] = back_right_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
    }
    out.encoder_buffer_times_ms[0] = millis(); // Record an initial encoder state sample

    // Store the initial positions of the steering motor encoders
    out.front_left_initial_spin_encoder = front_left_encoder;
    out.front_right_initial_spin_encoder = front_right_encoder;
    out.back_left_initial_spin_encoder = back_left_encoder;
    out.back_right_initial_spin_encoder = back_right_encoder;

    return out;
}

// This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
swerve_drive updateSwerveCommand(swerve_drive input)
{
    swerve_drive out = input;
    float new_drive_coefficient = out.target_drive_power;
    // Set the new speed of the steering motors
    if(/*(out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) &&*/ out.enable_steering) { // Only set the steering power if the robot is trying to move, and if steering is enabled
        // Calculate the distance and direction each motor needs to steer from where it is now
        float front_left_delta = closestAngle(out.front_left_spin_angle, out.front_left_target_spin);
        float front_right_delta = closestAngle(out.front_right_spin_angle, out.front_right_target_spin);
        float back_left_delta = closestAngle(out.back_left_spin_angle, out.back_left_target_spin);
        float back_right_delta = closestAngle(out.back_right_spin_angle, out.back_right_target_spin);
        // Use the delta and speed of each steering motor to calculate the necessary speed
        out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta, out.front_left_measured_spin_speed);
        out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta, out.front_right_measured_spin_speed);
        out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta, out.back_left_measured_spin_speed);
        out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta, out.back_right_measured_spin_speed);

        float max_abs_steering_delta = max(max(fabs(front_left_delta), fabs(front_right_delta)), max(fabs(back_left_delta), fabs(back_right_delta)));
        if (max_abs_steering_delta > STEERING_TOLERANCE_DISABLE_DRIVE) {
            new_drive_coefficient = 0;
        }
        Serial.printf("max_abs_steering_delta = %f\t\tndc = %f\r\n", max_abs_steering_delta, new_drive_coefficient);

        // TESTING DEBUG print 20230929
        Serial.printf("FL delta = %f\t\tFL steer = %f\r\n", front_left_delta, out.front_left_spin_power);
        Serial.printf("FR delta = %f\t\tFR steer = %f\r\n", front_right_delta, out.front_right_spin_power);
        Serial.printf("BL delta = %f\t\tBL steer = %f\r\n", back_left_delta, out.back_left_spin_power);
        Serial.printf("BR delta = %f\t\tBR steer = %f\r\n", back_right_delta, out.back_right_spin_power);
        

    } else { // Stop the steering motors if the robot is stopped and not trying to move, or if steering is disabled
        out.front_left_spin_power = 0.0f;
        out.front_right_spin_power = 0.0f;
        out.back_left_spin_power = 0.0f;
        out.back_right_spin_power = 0.0f;
    }

    // Set the current drive power to the target drive power, TODO: this is TEMPORARY, add in something to slow the current (set) speed until the wheels are in the correct direction
    //out.current_drive_power = out.target_drive_power;
    // Set the new drive motor power, apply coefficients, set between -127.0 and 127.0
    out.current_drive_power = new_drive_coefficient;
    out.front_left_power = new_drive_coefficient * out.front_left_coefficient * DRIVE_MOTOR_MAX_POWER;
    out.front_right_power = new_drive_coefficient * out.front_right_coefficient * DRIVE_MOTOR_MAX_POWER;
    out.back_left_power = new_drive_coefficient * out.back_left_coefficient * DRIVE_MOTOR_MAX_POWER;
    out.back_right_power = new_drive_coefficient * out.back_right_coefficient * DRIVE_MOTOR_MAX_POWER;
    
    return out;
}

float calculateSteeringMotorSpeed(float steering_delta, float current_angular_speed) // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
{
    float abs_steering_delta = fabs(steering_delta);
    if(abs_steering_delta > STEERING_SLOW_DELTA && abs_steering_delta > STEERING_TOLERANCE) { // In full speed range, still far enough away from the target angle
        return STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f);
    } else { // Slow down the speed of the steering motor since it's close to its target angle

        float calc_steering_delta = steering_delta + (STEERING_ACCEL_SLOW_DELAY * current_angular_speed); // Modify the steering delta to the estimated delta in STEERING_ACCEL_SLOW_DELAY seconds to account for motor acceleration
        float calc_steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (calc_steering_delta < 0.0f ? -1.0f : 1.0f); // Update the sign to account for the future location estimation above
        float calc_abs_steering_delta = fabs(calc_steering_delta); // Update abs_steering_delta with the new steering_delta
        float steering_speed_fraction = powf(calc_abs_steering_delta / STEERING_SLOW_DELTA, 2.0f); // Fraction of full speed being used
        //return steering_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
        if(current_angular_speed < STEERING_STALL_DETECT_ANGULAR_SPEED || steering_speed_fraction < STEERING_SLOW_APPROACH_SPEED) { // Detect motor stall during approach and increase speed to allow for approach
            steering_speed_fraction = STEERING_SLOW_APPROACH_SPEED;
            
            if(calc_abs_steering_delta < STEERING_HOVER_RANGE) { // Decrease speed further if the steering is extremely close to the target
                steering_speed_fraction *= (calc_abs_steering_delta / STEERING_HOVER_RANGE);
            } else if(abs_steering_delta < STEERING_HOVER_RANGE) {
                steering_speed_fraction *= (abs_steering_delta / STEERING_HOVER_RANGE);
            }
        }
        if(calc_abs_steering_delta < STEERING_TOLERANCE) { // Stop the steering motors if they are within the tolerance range
            return 0.0f;
        }

        return calc_steering_limit_signed * steering_speed_fraction; // Apply the direction
    }
}

swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder) // Process new encoder data, calculate the speed and angle of the steering motors
{
    swerve_drive out = in;
    // Move values over 1 slot in the encoder data buffer to make room for new data, discard data at the end of the buffer
    for(int i = 0; i < ENCODER_BUFFER_ENTRY_COUNT - 1; i++)
    {
        out.encoder_buffer_times_ms[i + 1] = out.encoder_buffer_times_ms[i];
        out.encoder_buffer_front_left[i + 1] = out.encoder_buffer_front_left[i];
        out.encoder_buffer_front_right[i + 1] = out.encoder_buffer_front_right[i];
        out.encoder_buffer_back_left[i + 1] = out.encoder_buffer_back_left[i];
        out.encoder_buffer_back_right[i + 1] = out.encoder_buffer_back_right[i];
    }

    uint64_t current_time_ms = millis(); // Milliseconds since pico startup, using 64 bit value for safety
    out.encoder_buffer_times_ms[0] = current_time_ms;
    out.encoder_buffer_front_left[0] = front_left_encoder;
    out.encoder_buffer_front_right[0] = front_right_encoder;
    out.encoder_buffer_back_left[0] = back_left_encoder;
    out.encoder_buffer_back_right[0] = back_right_encoder;

    if(current_time_ms == 0) { // Set current_time_ms to a nonzero value if it is zero to prevent a division by zero later in the function
        current_time_ms = 1;
    }
    
    // Calculate the speed in degrees per second of each of the steering motors
    out.front_left_measured_spin_speed = 0.0f;
    out.front_right_measured_spin_speed = 0.0f;
    out.back_left_measured_spin_speed = 0.0f;
    out.back_right_measured_spin_speed = 0.0f;
    for(int i = 1; i < ENCODER_BUFFER_ENTRY_COUNT; i++)
    {
        out.front_left_measured_spin_speed += ( (float) abs(out.encoder_buffer_front_left[0] - out.encoder_buffer_front_left[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
        out.front_right_measured_spin_speed += ( (float) abs(out.encoder_buffer_front_right[0] - out.encoder_buffer_front_right[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
        out.back_left_measured_spin_speed += ( (float) abs(out.encoder_buffer_back_left[0] - out.encoder_buffer_back_left[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
        out.back_right_measured_spin_speed += ( (float) abs(out.encoder_buffer_back_right[0] - out.encoder_buffer_back_right[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
    }
    
    out.front_left_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
    out.front_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
    out.back_left_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
    out.back_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);

    // Calculate the current orientation of each drive wheel in degrees
    out.front_left_spin_angle = ((float) (front_left_encoder - out.front_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
    out.front_right_spin_angle = ((float) (front_right_encoder - out.front_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
    out.back_left_spin_angle = ((float) (back_left_encoder - out.back_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
    out.back_right_spin_angle = ((float) (back_right_encoder - out.back_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;

    return out;
}

int normalizeEncoderData(int encoder_data) // Normalize encoder data to a range of [0, STEERING_ENCODER_TICKS_PER_ROTATION)
{
    encoder_data %= (int) STEERING_ENCODER_TICKS_PER_ROTATION;
    encoder_data += (STEERING_ENCODER_TICKS_PER_ROTATION * (encoder_data < 0));
    return encoder_data;
}

float relativeAngleFromEncoder(int encoder_data_relative ) // Calculate the relative angle from the difference between 2 encoder data points
{
    return ((float) normalizeEncoderData(encoder_data_relative)) / ((float) STEERING_ENCODER_TICKS_PER_DEGREE);
}

float normalizeAngle(float angle) // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees
{
    angle = fmod(angle, 360);
    angle += (360 * (angle < 0.0));
    return angle;
}

swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right) // Set the motor speed coefficients for each motor
{
    swerve_drive out = input;
    out.front_left_coefficient = front_left;
    out.front_right_coefficient = front_right;
    out.back_left_coefficient = back_left;
    out.back_right_coefficient = back_right;
    return out;
}

swerve_drive setTargetSpin(swerve_drive input, float front_left, float front_right, float back_left, float back_right) // Set the target spin for each wheel
{
    swerve_drive out = input;
    out.front_left_target_spin = front_left + input.spin_offset;
    out.front_right_target_spin = front_right + input.spin_offset;
    out.back_left_target_spin = back_left + input.spin_offset;
    out.back_right_target_spin = back_right + input.spin_offset;
    return out;
}

swerve_drive setSpinOffset(swerve_drive input, float new_spin_offset) // Set a new spin offset, and maintain the current target spin on each motor relative to the robot body as the offset is changed
{
    swerve_drive out = input;
    
    float delta_spin_offset = new_spin_offset - input.spin_offset;
    
    out.front_left_target_spin = input.front_left_target_spin - delta_spin_offset;
    out.front_right_target_spin = input.front_left_target_spin - delta_spin_offset;
    out.back_left_target_spin = input.front_left_target_spin - delta_spin_offset;
    out.back_right_target_spin = input.front_left_target_spin - delta_spin_offset;
    return out;
}

swerve_drive setDriveTargetPower(swerve_drive input, float target_drive_power) // Set a new drive power
{
    swerve_drive out = input;
    out.target_drive_power = target_drive_power;
    return out;
}

// Drive mode functions

swerve_drive stopSwerve(swerve_drive input) // Stop all motors
{
    swerve_drive out = input;

    out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
    out = setDriveTargetPower(out, 0.0); // Set the power
    return out;
}

swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for translation drive mode
{
    swerve_drive out = input;
    
    float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle

    out = setTargetSpin(out, normalized_target_angle, normalized_target_angle, normalized_target_angle, normalized_target_angle); // Set the target angle for each rotation motor
    out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
    out = setDriveTargetPower(out, target_speed); // Set the power
    return out;
}

swerve_drive rotationDrive(swerve_drive input, float target_speed) // Implementation for rotation drive mode (rotating in place), positive speed is clockwise, negative speed is counterclockwise
{
    swerve_drive out = input;

    //float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle

    out = setTargetSpin(out, 45.0, 135.0, 315.0, 225.0); // Set the target angle for each rotation motor
    out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
    out = setDriveTargetPower(out, target_speed); // Set the power

    return out;
}

swerve_drive basicDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for basic drive mode
{
    swerve_drive out = input;

    float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
    byte drive_condition = identifyBasicDriveCondition(target_speed, normalized_target_angle); // Identify the drive condition

    // Find inner_difference_angle
    // Measure from the target angle to the nearest section of the y-axis (like on a unit circle), this is for the inner wheels in the turn (wheels closer to the center of the turning circle)
    float inner_difference_angle;
    float semicircle_normalized_target_angle = fmod(normalized_target_angle, 180.0);
    if(semicircle_normalized_target_angle < 90) { // inner_difference_angle is less than 90
        inner_difference_angle = semicircle_normalized_target_angle;
    } else { // inner_difference_angle is greater than 90
        inner_difference_angle = 180 - semicircle_normalized_target_angle;
    }

    // Find the turning radius for the inner wheels, multiply by the distance between the robot's wheels to get the actual distance
    float inner_wheel_turning_radius = tan(RADIANS_PER_DEGREE * (90.0 - inner_difference_angle)) / 2.0;

    // Find the turning radius for the outer wheels, multiply by the distance between the robot's wheels to get the actual distance
    float outer_wheel_turning_radius = inner_wheel_turning_radius + 1.0;
    
    // Find the inner drive motor coefficient (to slow down the inner motors to compensate for a smaller turning radius), this is the ratio bewteen the inner and outer turning radii
    float inner_drive_coefficient = outer_wheel_turning_radius / inner_wheel_turning_radius;

    // Find outer_difference_angle
    // Measure from the target angle to the nearest y-axis, this is for the outer wheels in the turn (wheels further from the center of the turning circle)
    float outer_difference_angle = 90.0 - (DEGREES_PER_RADIAN * atan(2.0 * outer_wheel_turning_radius));

    // Calculate the target spin angle for the outer wheels in the turn from the outer_difference_angle
    float outer_target_angle;
    int normalized_target_quadrant = ((int) normalized_target_angle ) / 90;
    if (normalized_target_quadrant % 2 == 0) { // Quadrants 0 or 2
        outer_target_angle = outer_difference_angle + (float) (90 * normalized_target_quadrant);
    } else { // Quadrants 1 or 3
        outer_target_angle = -outer_difference_angle + (float) (90 * (normalized_target_quadrant + 1));
    }

    // Prepare these variables to be written to in the switch statement below
    // Set these to 0.0, so the robot will stop if none of the cases are hit for some reason
    float front_left_coefficient = 0.0;
    float front_right_coefficient = 0.0;

    // Determine the speed coefficient for each motor (how fast motors are spinning relative to each other)
    switch(drive_condition) {
        case DRIVE_BASIC_STOP: // Motors are stopping, set target_speed to 0
            front_left_coefficient = 0.0;
            front_right_coefficient = 0.0;
            target_speed = 0.0;
            break;
        case DRIVE_BASIC_FORWARD: case DRIVE_BASIC_BACKWARD: // Motors are all spinning at the same speed since the robot is going in a straight line
            front_left_coefficient = 1.0;
            front_right_coefficient = 1.0;
            break;
        case DRIVE_BASIC_FRONTLEFT: case DRIVE_BASIC_BACKLEFT: // Left wheels are the inner wheels in the turn, so they will go slower
            front_left_coefficient = inner_drive_coefficient;
            front_right_coefficient = 1.0;
            break;
        case DRIVE_BASIC_FRONTRIGHT: case DRIVE_BASIC_BACKRIGHT: // Right wheels are the inner wheels in the turn, so they will go slower
            front_left_coefficient = 1.0;
            front_right_coefficient = inner_drive_coefficient;
            break;
    }
    // Set the coefficients for the remaining 2 motors
    float back_left_coefficient = front_left_coefficient; // Front and back coefficients are the same for each side (left and right)
    float back_right_coefficient = front_right_coefficient; // Front and back coefficients are the same for each side (left and right)

    // Set target wheel spin angles
    switch(drive_condition) {
        case DRIVE_BASIC_STOP: // Do not modify wheel target angles, robot is stopping
            break;
        case DRIVE_BASIC_FORWARD: case DRIVE_BASIC_BACKWARD: // All wheels facing in the same direction since the robot is going in a straight line
            out = setTargetSpin(out, normalized_target_angle, normalized_target_angle, normalized_target_angle, normalized_target_angle); 
            break;
        case DRIVE_BASIC_FRONTLEFT: case DRIVE_BASIC_BACKLEFT: // Vary the angles of the wheels since the robot is turning, left wheels are the inner wheels in the turn
            out = setTargetSpin(out, normalized_target_angle, outer_difference_angle, normalized_target_angle, outer_difference_angle);
            break;
        case DRIVE_BASIC_FRONTRIGHT: case DRIVE_BASIC_BACKRIGHT: // Vary the angles of the wheels since the robot is turning, right wheels are the inner wheels in the turn
            out = setTargetSpin(out, outer_difference_angle, normalized_target_angle, outer_difference_angle, normalized_target_angle);
            break;
    }

    // Wrap up, set motor speed and speed coefficient values
    out = setMotorCoefficients(out, front_left_coefficient, front_right_coefficient, back_left_coefficient, back_right_coefficient); // Set motor speed coefficients
    out = setDriveTargetPower(out, target_speed); // Set the power

    return out;
}

byte identifyBasicDriveCondition(float target_speed, float target_angle) // Identify the condition in which the basic drive mode will be operating
{
    if(target_speed <= 0.0 || target_speed >= 127.0) { // Speed is stopped, negative, or too high
        return DRIVE_BASIC_STOP;
    }
    target_angle = normalizeAngle(target_angle); // Normalize the target angle
    if(target_angle == 0) {
        return DRIVE_BASIC_FORWARD;
    }
    if(target_angle == 180) {
        return DRIVE_BASIC_BACKWARD;
    }
    int target_quadrant = ((int) target_angle) / 90; // Which quadrant is the target angle in (quadrant 0 is northeast, increases clockwise)
    byte drive_condition = DRIVE_BASIC_STOP; // Keep the drive stopped if the normalized target angle somehow exceeded 360.
    switch(target_quadrant) {
        case 0:
            drive_condition = DRIVE_BASIC_FRONTRIGHT;
            break;
        case 1:
            drive_condition = DRIVE_BASIC_BACKRIGHT;
            break;
        case 2:
            drive_condition = DRIVE_BASIC_BACKLEFT;
            break;
        case 3:
            drive_condition = DRIVE_BASIC_FRONTLEFT;
            break;
        default: // Unreachable, assuming that the target angle is less than 360
            drive_condition = DRIVE_BASIC_STOP;
            break;
    }
    return drive_condition;
}

swerve_drive tankDrive(swerve_drive input, float target_speed, float turn_angle) // Implementation for tank drive mode, positive angle is left, negative angle is right
{
    swerve_drive out = input;

    float normalized_turn_angle = normalizeAngle(turn_angle); // Normalize the turn angle

    // Calculate the speed of each motor
    float angle_distance_from_y_axis = 90.0f - fabs(fmod(normalized_turn_angle, 180.0f) - 90.0f); // Distance in degrees that the joystick is from the y-axis
    float slow_side_coefficient = angle_distance_from_y_axis / 90.0f; // Coefficient applied to whichever motor is not going full speed in the tank drive
    int turn_angle_quadrant = ((int) normalized_turn_angle) / 4;
    float left_side_power, right_side_power;
    
    // Determine the motor speeds based on the quadrant that the joystick is in
    switch(turn_angle_quadrant) {
        case 0: // Going forward and turning right, Northeast quadrant
            left_side_power = 1.0f;
            right_side_power = slow_side_coefficient;
            break;
        case 1: // Going backward and turning left, Southeast quadrant 
            left_side_power = -1.0f;
            right_side_power = -slow_side_coefficient;
            break;
        case 2: // Going backward and turning right, Southwest quadrant
            left_side_power = -slow_side_coefficient;
            right_side_power = -1.0f;
            break;
        case 3: // Going forward and turning left, Northwest quadrant
            left_side_power = slow_side_coefficient;
            right_side_power = 1.0f;
            break;
    }

    out = setMotorCoefficients(out, left_side_power, right_side_power, left_side_power, right_side_power); // Set the motor speed coefficients
    out = setDriveTargetPower(out, target_speed); // Set the power

    return out;
}


// Modulus Plus: Drive control planning: v1 on 20230922
/*

315   000   045
       
270    ^    090

225   180   135

If speed AND any wheel's target-current angle are BOTH above a certain threshold, decelerate before spinning the wheel

Variables to monitor and control:
    Drive modes:
        Translation (Linear motion in any direction while preserving body orientation)
        Basic (Forwards or backwards, may turn)
        Rotation (rotation in place)
    Rotation angle (in Translation or Basic drive mode)
    Drive speed
    Drive forward orientation relative to robot body

Common Controls:
    Left and Right shoulder buttons:
        Switch between drive modes (Left for basic, right for translation)
    D-Pad:
        Reset drive orientation relative to body
        Set direction to the corresponding d-pad button direction
    Joystick B:
        Left/Right:         Rotation speed, bypass existing drive mode and start rotating

Translation Mode Controls:
    Joystick A:
        Vector Amplitude:   Drive speed
        Vector Angle:       Drive angle

Basic Mode Controls:
    Joystick A:
        Vector Amplitude:   Drive speed
        Vector Angle:       Drive angle

All Mode Controls:

Rotation drive mode wheel orientation
Turning in place
//   \\

\\   //


Translation or Basic drive mode wheel orientation, with a rotation angle of 0
Driving in a straight line
||   ||


||   ||

Translation drive mode wheel orientation, with a rotation angle of 90 or 270
Driving to the left or right
_     _
-     -

_     _
-     -

Translation drive mode wheel orientation, with a rotation angle of 45
Driving forward and to the right
//   //

//   //

Basic drive mode wheel orientation with a rotation angle of 315
Driving forwards while turning (to the left in this example)
\\   \\

//   //

*/