#include "OmniWheelMotors.hpp"

OmniWheelMotors::OmniWheelMotors(int m1_r_pwm, int m1_l_pwm, 
                               int m2_r_pwm, int m2_l_pwm, 
                               int m3_r_pwm, int m3_l_pwm) {
    motor_1 = {m1_r_pwm, m1_l_pwm};
    motor_2 = {m2_r_pwm, m2_l_pwm};
    motor_3 = {m3_r_pwm, m3_l_pwm};
    
    MAX_SPEED = 255;      // Max PWM value
    MIN_SPEED = 70;       // Default minimum speed
    FULL_ROTATION_TIME = 2000;  // Default time for 360 degrees in milliseconds
}

void OmniWheelMotors::begin() {
    // Set all motor pins as outputs
    pinMode(motor_1.R_PWM, OUTPUT);
    pinMode(motor_1.L_PWM, OUTPUT);
    pinMode(motor_2.R_PWM, OUTPUT);
    pinMode(motor_2.L_PWM, OUTPUT);
    pinMode(motor_3.R_PWM, OUTPUT);
    pinMode(motor_3.L_PWM, OUTPUT);
    
    // Initialize all motors to stopped
    stop();
}

void OmniWheelMotors::setMotorSpeed(Motor motor, int speed, int direction) {
    // direction: 0 = stop, 1 = forward only, 2 = left, 3 = right
    
    // Constrain speed value to valid PWM range
    speed = constrain(speed, 0, MAX_SPEED);
    
    switch(direction) {
        case 0: // Stop
            analogWrite(motor.R_PWM, 0);
            analogWrite(motor.L_PWM, 0);
            break;
        case 1: // Forward only (for motors 1 and 2)
            analogWrite(motor.R_PWM, speed);
            analogWrite(motor.L_PWM, 0);
            break;
        case 2: // Left (for motor 3)
            analogWrite(motor.R_PWM, speed);
            analogWrite(motor.L_PWM, 0);
            break;
        case 3: // Right (for motor 3)
            analogWrite(motor.R_PWM, 0);
            analogWrite(motor.L_PWM, speed);
            break;
    }
}

void OmniWheelMotors::stop() {
    // Stop all motors
    setMotorSpeed(motor_1, 0, 0);
    setMotorSpeed(motor_2, 0, 0);
    setMotorSpeed(motor_3, 0, 0);
}

void OmniWheelMotors::moveForward(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    speed = max(speed, MIN_SPEED);
    
    // Set motor directions for forward movement
    setMotorSpeed(motor_1, speed, 1);  // Motor 1 forward
    setMotorSpeed(motor_2, speed, 1);  // Motor 2 forward
    setMotorSpeed(motor_3, 0, 0);      // Motor 3 neutral (no steering)
}

void OmniWheelMotors::moveBackward(int speed) {
    // To move backward, we'll rotate 180 degrees and then move forward
    
    // First, rotate 180 degrees
    rotateToAngle(180);
    
    // Then move forward (which is now effectively backward relative to original position)
    moveForward(speed);
}

void OmniWheelMotors::moveLeft(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    speed = max(speed, MIN_SPEED);
    
    // Set motor directions for left movement
    setMotorSpeed(motor_1, speed, 1);  // Motor 1 forward
    setMotorSpeed(motor_2, speed, 1);  // Motor 2 forward
    setMotorSpeed(motor_3, speed, 2);  // Motor 3 steer left
}

void OmniWheelMotors::moveRight(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    speed = max(speed, MIN_SPEED);
    
    // Set motor directions for right movement
    setMotorSpeed(motor_1, speed, 1);  // Motor 1 forward
    setMotorSpeed(motor_2, speed, 1);  // Motor 2 forward
    setMotorSpeed(motor_3, speed, 3);  // Motor 3 steer right
}

void OmniWheelMotors::rotate360() {
    // Perform full 360 degree rotation
    rotateClockwise(MIN_SPEED);
    
    // Wait for approximately one full rotation
    delay(FULL_ROTATION_TIME);
    
    // Stop all motors
    stop();
}

void OmniWheelMotors::rotateClockwise(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    speed = max(speed, MIN_SPEED);
    
    // Set motor directions for clockwise rotation
    setMotorSpeed(motor_1, speed, 1);  // Motor 1 forward
    setMotorSpeed(motor_2, 0, 0);      // Motor 2 stopped
    setMotorSpeed(motor_3, speed, 3);  // Motor 3 steer right
}

void OmniWheelMotors::rotateCounterClockwise(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    speed = max(speed, MIN_SPEED);
    
    // Set motor directions for counter-clockwise rotation
    setMotorSpeed(motor_1, 0, 0);      // Motor 1 stopped
    setMotorSpeed(motor_2, speed, 1);  // Motor 2 forward
    setMotorSpeed(motor_3, speed, 2);  // Motor 3 steer left
}

void OmniWheelMotors::rotateToAngle(int targetAngle) {
    // Ensure angle is between 0 and 360
    targetAngle = targetAngle % 360;
    if (targetAngle < 0) targetAngle += 360;
    
    // Calculate rotation time based on angle
    int rotationTime = (targetAngle * FULL_ROTATION_TIME) / 360;
    
    // Start rotation (use counter-clockwise for positive angles)
    rotateCounterClockwise(MIN_SPEED);
    
    // Wait for calculated time
    delay(rotationTime);
    
    // Stop all motors
    stop();
}

void OmniWheelMotors::setSpeedLimits(int min, int max) {
    // Set minimum and maximum speed values with bounds checking
    MIN_SPEED = constrain(min, 0, 255);
    MAX_SPEED = constrain(max, MIN_SPEED, 255);
}

void OmniWheelMotors::setRotationTime(int milliseconds) {
    // Set time for full rotation (must be positive)
    FULL_ROTATION_TIME = max(1, milliseconds);
}