#include "globals.h"

typedef struct { // swerve_drive struct, used to track and manage the state of the robot's swerve drive system
    // byte spin_direction = CLOCKWISE; // Current state, 0 = clockwise, 1 = anticlockwise
    // byte drive_direction = DRIVE_FORWARDS; // Current state, Directions: 0 = forwards, 1 = right, 2 = backwards, 3 = left
    // byte target_spin_direction = CLOCKWISE; // Target state
    // byte target_drive_direction = DRIVE_FORWARDS; // Target state
    
    byte drive_mode = DRIVE_BASIC;
    float target_drive_power = 127.0; // 0.0 - 127.0 , TARGET speed: this is the speed that the robot is trying to get to
    float current_drive_power = 127.0; // 0.0 - 127.0 : CURRENT / COMMAND speed (power) being given to drive motors (before coefficient is applied)
    
    float spin_location = 0; // 0 - 360, CURRENT tracked body orientation relative to initial (or reset) orientation
    float spin_offset = 0; // 0 - 360, offset applied to target spin when setting, this allows modification of the robot's forward orientation relative to its body
    
    // CURRENT wheel orientations relative to robot body, read from encoders & convert
    float front_left_current_spin = 0;
    float front_right_current_spin = 0;
    float back_left_current_spin = 0;
    float back_right_current_spin = 0;

    // COMMAND wheel orientations relative to robot body, this is what the motors are currently being told to do
    float front_left_command_spin = 0;
    float front_right_command_spin = 0;
    float back_left_command_spin = 0;
    float back_right_command_spin = 0;

    // TARGET wheel orientations relative to robot body, this is the state that the robot is trying to get to
    float front_left_target_spin = 0;
    float front_right_target_spin = 0;
    float back_left_target_spin = 0;
    float back_right_target_spin = 0;

    // Motor power coefficients, this is used when motors must turn at different speeds. This is an input value, and is not directly affected by the current robot conditions
    // Between 0 and 1
    float front_left_coefficient = 0;
    float front_right_coefficient = 0;
    float back_left_coefficient = 0;
    float back_right_coefficient = 0;

    
    
    // Encoder tracking, used to track speed of the robot to determine when it is safe to reorient the wheels
    // The 0th entry in the buffer is the most recent, the highest entry is the oldest
    // The buffer is modified each time updateSwerveCommand() is called
    int encoder_buffer_times_ms[ENCODER_BUFFER_ENTRY_COUNT]; // Buffer that tracks the times at which encoder states were measures, uses milliseconds (either unix millis or relative to pico start)
    int encoder_buffer_front_left[ENCODER_BUFFER_ENTRY_COUNT];
    int encoder_buffer_front_right[ENCODER_BUFFER_ENTRY_COUNT];
    int encoder_buffer_back_left[ENCODER_BUFFER_ENTRY_COUNT];
    int encoder_buffer_back_right[ENCODER_BUFFER_ENTRY_COUNT];
    
} swerve_drive;

float closestAngle(float current, float target);

// TODO as of 20230923 for setDirection() : fix to work with modifications made to swerve_drive struct on 20230922
// swerve_drive setDirection(swerve_drive input, float setpoint);

swerve_drive updateSwerveCommand(swerve_drive input); // 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 tempTranslationDrive(swerve_drive input, float target_speed, float target_angle); // Temporary implementation for translation drive mode

swerve_drive tempRotationDrive(swerve_drive input, float target_speed); // Temporary implementation for rotation drive mode (rotating in place), positive speed is clockwise, negative speed is counterclockwise

swerve_drive tempBasicDrive(swerve_drive input, float target_speed, float target_angle); // Temporary implementation for basic drive mode

byte identifyBasicDriveCondition(float target_speed, float target_angle); // Identify the condition in which the basic drive mode will be operating

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

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 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 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 setDriveTargetPower(swerve_drive input, float target_drive_power); // Set a new drive power