package org.firstinspires.ftc.teamcode;

@Autonomous
public class VuforiaTest extends LinearOpMode{

    /* Replace with your config file*/
    private Blinker expansion_Hub_2;
    private Gyroscope imu;
    private DcMotor driveL;
    private DcMotor driveR;

    VuforiaLocalizer vuforia;
    public final static String VUFORIA_LICENSE_KEY = "";

    public void runOpMode(){
        driveL = hardwareMap.get(DcMotor.class,"left_drive" );
        driveL.setDirection(DcMotor.Direction.REVERSE);
        
        driveR = hardwareMap.get(DcMotor.class,"right_drive");
        
        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
        parameters.vuforiaLicenseKey = VUFORIA_LICENSE_KEY;
        parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
        
        /**
         * Instantiate the Vuforia engine
         */
        vuforia = ClassFactory.getInstance().createVuforia(parameters);
        
        VuforiaTrackables skystoneTargets = vuforia.loadTrackablesFromAsset("Skystone");
        VuforiaTrackableDefaultListener stoneTarget = (VuforiaTrackableDefaultListener)skystoneTargets.get(0).getListener();
        
        skystoneTargets.activate();
        
        waitForStart();
        while(opModeIsActive()){
            if(stoneTarget.isVisible()){
                VectorF trans = stoneTarget.getPose().getTranslation();
                
                telemetry.addLine("Object Sighted X:" + trans.get(0) + "Y:" + trans.get(1)+"Z:" + trans.get(2));
                
                //If the robot gets close to the object stop
                if(Math.abs(trans.get(2)) < 190){
                    telemetry.addLine("Reached Target!");
                    
                    driveL.setPower(0);
                    driveR.setPower(0);
                    telemetry.update();
                    break;
                    
                }
                
                //Bang-bang controller simpler but less accurate
                if (trans.get(0) < -20){
                    driveL.setPower(0.2);
                    driveR.setPower(0);
                } else if (trans.get(0) > 20){
                    driveR.setPower(0.2);
                    driveL.setPower(0);
                } else {
                    driveL.setPower(0.2);
                    driveR.setPower(0.2);
                }
                
                //Proportional Controller far more accurate.
                //Change kp to change how rapidly the robot turns
                double kp = 0.001;
                driveL.setPower(0.2-kp*trans.get(0));
                driveR.setPower(0.2+kp*trans.get(0));
                
            }
            telemetry.update(); 
        }     
    }
}