package org.firstinspires.ftc.teamcode.auto;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.kinematics.NullablePose;
import org.firstinspires.ftc.teamcode.kinematics.FieldVector;
import org.firstinspires.ftc.teamcode.kinematics.Poser;

@Autonomous(name = "test")
public class Test extends LinearOpMode {
    Hardware hardware;
    Poser poser;

    @Override
    public void runOpMode() {
        hardware = new Hardware(this);
        hardware.setWristPosition(-0.6);
        poser = new Poser(hardware, 0.3, FieldVector.ZERO, 0);

        poser.goToNullablePose(new NullablePose());

        waitForStart();

        while (!isStopRequested()) {
            poser.goToNullablePose(new NullablePose(FieldVector.inTiles(2, 0), 90., LiftPosition.FLOOR));
            sleep(1000);
            poser.goToNullablePose(new NullablePose(FieldVector.inTiles(0, 1), -135., LiftPosition.HIGH));
            sleep(1000);
            poser.goToNullablePose(new NullablePose(FieldVector.inTiles(0, 0), 0., LiftPosition.MEDIUM));
            sleep(1000);
        }
    }
}