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.Strings;

@Autonomous(name = "0-Cone Right Side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class ZeroConeAutonRight 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) {
        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));

        switch (signalZone) {
            case 1:
                // do nothing
                break;
            case 2:
                movement.strafe(Distance.inTiles(1));
                break;
            case 3:
                movement.strafe(Distance.inTiles(2));
                break;
        }
    }
}
