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.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.15, FieldVector.ZERO, 0);

        poser.goTo();

        waitForStart();

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

        while (!isStopRequested()) {
            poser.goTo(FieldVector.inTiles(1, 0));
            poser.goTo(FieldVector.inTiles(0, 0));
        }
    }
}
