package robot.subsystems;

import robot.Pose2d;

/**
 * Drive subsystem - business logic only.
 * Hardware interface defined as inner interface.
 * Tests use inline mocks - no FTC SDK needed.
 */
public class Drive {
    private final Hardware hardware;
    private Pose2d pose;
    
    public Drive(Hardware hardware) {
        this.hardware = hardware;
        this.pose = new Pose2d();
    }
    
    /**
     * Drive using field-centric controls.
     * @param forward Forward/backward speed (-1.0 to 1.0)
     * @param strafe Left/right speed (-1.0 to 1.0)
     * @param rotate Rotation speed (-1.0 to 1.0)
     */
    public void drive(double forward, double strafe, double rotate) {
        // Your drive logic here
        double heading = hardware.getHeading();
        
        // Example: field-centric conversion
        double cos = Math.cos(heading);
        double sin = Math.sin(heading);
        
        double rotatedForward = forward * cos - strafe * sin;
        double rotatedStrafe = forward * sin + strafe * cos;
        
        hardware.setPowers(rotatedForward, rotatedStrafe, rotate);
    }
    
    /**
     * Stop all drive motors.
     */
    public void stop() {
        hardware.setPowers(0, 0, 0);
    }
    
    /**
     * Get current estimated pose.
     */
    public Pose2d getPose() {
        return pose;
    }
    
    /**
     * Hardware interface - implement this for real robot.
     */
    public interface Hardware {
        /**
         * Get robot heading in radians.
         */
        double getHeading();
        
        /**
         * Set drive motor powers.
         * @param forward Forward power
         * @param strafe Strafe power
         * @param rotate Rotation power
         */
        void setPowers(double forward, double strafe, double rotate);
    }
}