package robot.subsystems;

import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.*;

/**
 * Unit tests for Drive subsystem.
 * Uses inline mock - no FTC SDK required.
 */
class DriveTest {
    
    /**
     * Simple mock implementation of Drive.Hardware.
     * Captures method calls for verification in tests.
     */
    static class MockHardware implements Drive.Hardware {
        double lastForward = 0;
        double lastStrafe = 0;
        double lastRotate = 0;
        double heading = 0;
        int setPowersCallCount = 0;
        
        @Override
        public double getHeading() {
            return heading;
        }
        
        @Override
        public void setPowers(double forward, double strafe, double rotate) {
            this.lastForward = forward;
            this.lastStrafe = strafe;
            this.lastRotate = rotate;
            this.setPowersCallCount++;
        }
    }
    
    @Test
    void testDriveCallsSetPowers() {
        MockHardware mock = new MockHardware();
        Drive drive = new Drive(mock);
        
        drive.drive(0.5, 0.3, 0.1);
        
        assertEquals(1, mock.setPowersCallCount, "setPowers should be called once");
    }
    
    @Test
    void testStopSetsZeroPower() {
        MockHardware mock = new MockHardware();
        Drive drive = new Drive(mock);
        
        drive.stop();
        
        assertEquals(0.0, mock.lastForward, 0.001);
        assertEquals(0.0, mock.lastStrafe, 0.001);
        assertEquals(0.0, mock.lastRotate, 0.001);
    }
    
    @Test
    void testGetPoseReturnsNonNull() {
        MockHardware mock = new MockHardware();
        Drive drive = new Drive(mock);
        
        assertNotNull(drive.getPose(), "Pose should never be null");
    }
}