package robot.hardware;

import robot.subsystems.Drive;

// Uncomment when deploying to robot:
// import com.qualcomm.robotcore.hardware.DcMotor;
// import com.qualcomm.robotcore.hardware.HardwareMap;
// import com.qualcomm.robotcore.hardware.IMU;
// import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

/**
 * Mecanum drive hardware implementation.
 * Implements Drive.Hardware interface.
 * 
 * DEPLOYMENT NOTE:
 * Uncomment FTC imports and implementation when ready to deploy.
 * Keep commented during development/testing on PC.
 */
public class MecanumDrive implements Drive.Hardware {
    
    // Uncomment when deploying:
    // private final DcMotor frontLeft;
    // private final DcMotor frontRight;
    // private final DcMotor backLeft;
    // private final DcMotor backRight;
    // private final IMU imu;
    
    public MecanumDrive(/* HardwareMap hardwareMap */) {
        // Uncomment when deploying:
        // frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
        // frontRight = hardwareMap.get(DcMotor.class, "frontRight");
        // backLeft = hardwareMap.get(DcMotor.class, "backLeft");
        // backRight = hardwareMap.get(DcMotor.class, "backRight");
        // imu = hardwareMap.get(IMU.class, "imu");
        //
        // // Configure motors
        // frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        // frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        // backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        // backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
    }
    
    @Override
    public double getHeading() {
        // Stub for testing - returns 0
        return 0.0;
        
        // Uncomment when deploying:
        // return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
    }
    
    @Override
    public void setPowers(double forward, double strafe, double rotate) {
        // Stub for testing - does nothing
        
        // Uncomment when deploying:
        // // Mecanum drive kinematics
        // double frontLeftPower = forward + strafe + rotate;
        // double frontRightPower = forward - strafe - rotate;
        // double backLeftPower = forward - strafe + rotate;
        // double backRightPower = forward + strafe - rotate;
        //
        // // Normalize powers
        // double maxPower = Math.max(1.0, Math.max(
        //     Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)),
        //     Math.max(Math.abs(backLeftPower), Math.abs(backRightPower))
        // ));
        //
        // frontLeft.setPower(frontLeftPower / maxPower);
        // frontRight.setPower(frontRightPower / maxPower);
        // backLeft.setPower(backLeftPower / maxPower);
        // backRight.setPower(backRightPower / maxPower);
    }
}