package org.firstinspires.ftc.teamcode.auto;

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

import org.firstinspires.ftc.teamcode.kinematics.Distance;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.Strings;

@Autonomous(name = "1-Cone Left Side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class OneConeAutonLeft extends AprilTagAutonBase {
    Hardware hardware;
    AutonMovement movement;

    @Override
    void onInit() {
        hardware = new Hardware(this);
        movement = new AutonMovement(hardware, 0.6);
        hardware.setClawPosition(1);
        hardware.setWristPosition(-2/3.);
    }

    @Override
    void onPlay(int signalZone) {
        // move to the junction
        movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
        movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
        movement.straight(Distance.inTiles(1));
        movement.strafe(Distance.inTiles(0.5));

        // lift and move forward
        movement.liftTo(LiftPosition.HIGH);
        movement.straight(Distance.inInches(2));

        // wrist down, drop, wrist up
        hardware.setWristPosition(0);
        sleep(500);
        hardware.setClawPosition(0);
        sleep(500);
        hardware.setClawPosition(1);
        hardware.setWristPosition(-2/3.);

        // move back and lower
        movement.straight(Distance.inInches(-2));
        movement.liftTo(LiftPosition.FLOOR);

        switch (signalZone) {
            case 1:
                movement.strafe(Distance.inTiles(-2.5));
                break;
            case 2:
                movement.strafe(Distance.inTiles(-1.5));
                break;
            case 3:
                movement.strafe(Distance.inTiles(-0.5));
                break;
        }
    }
}