#include "globals.h"
#include <stdint.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
    
    int drive_mode = DEFAULT_SWERVE_DRIVE_MODE;
    bool enable_steering = DEFAULT_SWERVE_ENABLE_STEERING;
    
    float target_drive_power = 0.0f; // -127.0 to 127.0 : TARGET power that the robot is trying to get to
    float current_drive_power = 0.0f; // -127.0 to 127.0 : CURRENT power being given to drive motors (before coefficient is applied)
    
    float front_left_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
    float front_right_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
    float back_left_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
    float back_right_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor

    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 in degrees [0.0, 360.0), read from encoders & convert
    float front_left_spin_angle = 0.0f;
    float front_right_spin_angle = 0.0f;
    float back_left_spin_angle = 0.0f;
    float back_right_spin_angle = 0.0f;

    // COMMAND steering motor power, -127.0 to 127.0
    float front_left_spin_power = 0.0f;
    float front_right_spin_power = 0.0f;
    float back_left_spin_power = 0.0f;
    float back_right_spin_power = 0.0f;

    // TARGET wheel orientations relative to robot body in degrees [0.0, 360.0), this is the state that the robot is trying to get to
    float front_left_target_spin = 0.0f;
    float front_right_target_spin = 0.0f;
    float back_left_target_spin = 0.0f;
    float back_right_target_spin = 0.0f;

    // MEASURED steering motor speed in degrees per second, positive values are clockwise, calculated each time updateEncoderData() is called
    float front_left_measured_spin_speed = 0.0f;
    float front_right_measured_spin_speed = 0.0f;
    float back_left_measured_spin_speed = 0.0f;
    float back_right_measured_spin_speed = 0.0f;

    // Initial encoder values of the steering motors, current orientation of the steering motors is calculated from this (assuming that they are all facing forward upon initialization)
    int front_left_initial_spin_encoder = 0;
    int front_right_initial_spin_encoder = 0;
    int back_left_initial_spin_encoder = 0;
    int back_right_initial_spin_encoder = 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.0f;
    float front_right_coefficient = 0.0f;
    float back_left_coefficient = 0.0f;
    float back_right_coefficient = 0.0f;

    
    // Encoder tracking, used to track speed of the steering motors
    // The 0th entry in the buffer is the most recent, the highest index entry is the oldest
    // The buffer is modified each time updateEncoderData() is called
    uint64_t encoder_buffer_times_ms[ENCODER_BUFFER_ENTRY_COUNT]; // Buffer tracks the times at which encoder states were measures, uses milliseconds (relative to pico start), using 64 bit value for safety
    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;

// 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

// 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 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 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

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

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

int normalizeEncoderData(int encoder_data); // Normalize encoder data to a range of [0, STEERING_ENCODER_TICKS_PER_ROTATION)

float relativeAngleFromEncoder(int encoder_data_relative ); // Calculate the relative angle from the difference between 2 encoder data points

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


// Drive mode functions

swerve_drive stopSwerve(swerve_drive input); // Stop all motors

swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle); // Implementation for translation drive mode

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 basicDrive(swerve_drive input, float target_speed, float target_angle); // 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

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