#include <Arduino.h>
#include "globals.h"
#include "manipulator.h"
//#include <math.h>

// Utilities

manipulator_arm initializeManipulatorArm(manipulator_arm input) // Initialize an existing manipulator_arm struct
{
    manipulator_arm out = input;

    // Do initialization of values here
    
    return out;
}

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
{
    return (int)(servo_degrees * (90.0 / 127.0));
}

float clampServoAngle(float servo_angle) // Clamp an angle for a servo between -90.0 and 90.0
{
    return max(-90.0f, min(90.0f, servo_angle));
}


// Tilt servo functions

manipulator_arm setTiltAngle(manipulator_arm input, float new_tilt_angle) // Internal function which sets the target angle of the tilt servo
{
    manipulator_arm out = input;
    new_tilt_angle = clampServoAngle(new_tilt_angle); // Clamp the new tilt angle
    out.tilt_target_angle = new_tilt_angle;
    out.tilt_set_motor_int = max(TILT_MIN_ANGLE, min(TILT_MAX_ANGLE, new_tilt_angle));
    return out;
}

manipulator_arm updateTiltCommand(manipulator_arm input, int new_tilt_command) // Update the command for the tilt motor
{
    manipulator_arm out = input;
    if(new_tilt_command != out.tilt_command || out.tilt_angle_loops_since_update >= TILT_ANGLE_MIN_UPDATE_LOOPS) { // Make sure that it's been long enough since the last update to not spam the servo command
        float new_angle = out.tilt_target_angle; // Maintain the existing angle by default
        switch(new_tilt_command) {
            case TILT_COMMAND_UNSET:
                new_angle = out.tilt_target_angle; // Maintain the existing angle
            case TILT_COMMAND_RESET:
                new_angle = TILT_FLAT_ANGLE;
            case TILT_COMMAND_UP:
                new_angle = out.tilt_target_angle + TILT_ANGLE_UPDATE_DISTANCE;
                break;
            case TILT_COMMAND_DOWN:
                new_angle = out.tilt_target_angle - TILT_ANGLE_UPDATE_DISTANCE;
                break;
        }
        out = setTiltAngle(out, new_angle);
        out.tilt_angle_loops_since_update = 0; // Reset the counter tracking loops since the last update, since an update was just performed
    } else { // Increment the number of loops since the previous update, since an update was not performed during this loop
        out.tilt_angle_loops_since_update++;
    }

    out.tilt_command = new_tilt_command; // Save the new command to check whether it has changed in the next loop

    return out;
}

// 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 out = input;
    out.claw_target_angle = clampServoAngle(new_claw_angle);
    return out;
}

manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command) // Update the command and position for the claw servo
{
    manipulator_arm out = input;
    float new_claw_angle;
    switch(new_claw_command) {
        case CLAW_COMMAND_UNSET:
            new_claw_angle = CLAW_DEFAULT_ANGLE;
            break;
        case CLAW_COMMAND_OPEN:
            new_claw_angle = CLAW_OPEN_ANGLE;
            break;
        case CLAW_COMMAND_CLOSE:
            new_claw_angle = CLAW_CLOSED_ANGLE;
            break;
        case CLAW_COMMAND_STAY:
            new_claw_angle = out.claw_position;
            break;
        default:
            new_claw_angle = CLAW_DEFAULT_ANGLE;
    }
    out.claw_position = new_claw_angle;
    out.claw_set_motor_int = servoDegreesToInt(new_claw_angle);
    return out;
}


// 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 out = input;
    arm_speed = out.arm_speed_coefficient * min(out.arm_speed_limit, max(-out.arm_speed_limit, arm_speed));
    out.arm_speed = arm_speed;
    out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed)));
    return out;
}
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 out = input;
    out.arm_speed_limit = min(1.0f, max(0.0f, arm_speed_limit));
    return out;
}
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
{
    manipulator_arm out = input;
    out.arm_speed_coefficient = min(1.0f, max(0.0f, arm_speed_coefficient));;
    return out;
}

// Template function

manipulator_arm manipulatorTemplate(manipulator_arm input) // manipulator_arm template function
{
    manipulator_arm out = input;
    
    // Do stuff
    
    return out;
}