#include "globals.h"

// Manipulator arm

// Stepper motors for lift
// Servo to open and close claw
// Servo to tilt claw

typedef struct {
    // Claw servo variables
    int claw_command = CLAW_COMMAND_UNSET;          // COMMAND for claw servo
    int claw_set_motor_int = 0;                     // COMMAND for claw servo, formatted to integer used in set_motor()
    int claw_status = CLAW_UNKNOWN;                 // CURRENT claw position, initially unknown
    float claw_position = CLAW_DEFAULT_ANGLE;       // CURRENT angle of claw servo
    float claw_target_angle = CLAW_DEFAULT_ANGLE;   // TARGET position of claw servo, -90.0 to 90.0 degrees
    float claw_power_limit = 0.0f;                  // LIMIT power for the claw servo

    // Tilt servo variables
    int tilt_command = TILT_COMMAND_UNSET;          // COMMAND for the tilt servo
    float tilt_target_angle = TILT_FLAT_ANGLE;      // CURRENT angle in degrees for the claw tilt servo, can be between -90.0 and 90.0 degrees
    int tilt_set_motor_int = 0;                     // Tilt motor target angle value as an integer used for set_motor()
    int tilt_angle_loops_since_update = 0;          // Number of control loops since the tilt angle was modified, don't modify the tilt angle if this is less than TILT_ANGLE_MIN_UPDATE_LOOPS

    // Arm motor variables (stepper motors)
    float arm_speed = 0.0f;                         // Arm command, initialize as stopped, between -1.0 and 1.0
    float arm_speed_limit = 1.0f;                   // Limit applied to the speed of the arm (before the coefficient), must be between 0.0 and 1.0
    float arm_speed_coefficient = 1.0f;             // Coefficient applied to the arm speed, has no constraints, default to 1.0 (unmodified)
    int arm_set_motor_int = 0;                      // Arm speed used for the set_motor() function

} manipulator_arm;


// Utilities

manipulator_arm initializeManipulatorArm(manipulator_arm input); // Initialize a manipulator_arm struct

int servoDegreesToInt(float servo_degrees); // Convert a servo position in degrees to an integer between -127 and 127 to be used by the set_motor() function

float clampServoAngle(float servo_angle); // Clamp an angle for a servo between -90.0 and 90.0

// Tilt servo functions
manipulator_arm setTiltAngle(manipulator_arm input, float new_claw_angle); // Internal function which sets the target angle of the tilt servo

manipulator_arm updateTiltCommand(manipulator_arm input, int tilt_command); // Update the command for the tilt motor

// Claw servo functions

manipulator_arm setClawTargetAngle(manipulator_arm input, float new_claw_angle); // Internal function which sets the target angle of the claw servo (NOT THE ACTUAL COMMAND ANGLE)

manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command); // Update the command and position for the claw servo

// Arm functions (stepper motors)

manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed); // Set the arm's speed, must be between -1.0 and 1.0

manipulator_arm setArmSpeedLimit(manipulator_arm input, float arm_speed_limit); // Set the arm's speed limit (applied before coefficient), limit must be between 0.0 and 1.0

manipulator_arm setArmSpeedCoefficient(manipulator_arm input, float arm_speed_coefficient); // Set the arm's speed coefficient, coefficient must be between 0.0 and 1.0