package org.firstinspires.ftc.teamcode.auto;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.reflection.FieldProvider;
import com.acmerobotics.dashboard.config.variable.BasicVariable;
import com.acmerobotics.dashboard.config.variable.CustomVariable;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

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

@TeleOp(name = "Dashboard Poser")
public class DashboardPoser extends LinearOpMode {
    private double targetXPos = 0;
    private double targetYPos = 0;
    private double targetYaw = 0;
    private LiftPosition targetLiftPos = LiftPosition.FLOOR;
    private double speed = 0.2;

    @Override
    public void runOpMode() throws InterruptedException {
        Hardware hardware = new Hardware(this);
        Poser poser = new Poser(hardware, speed, FieldVector.inTiles(targetXPos, targetYPos), targetYaw);

        FtcDashboard.getInstance().withConfigRoot(root -> {
            CustomVariable subVar = new CustomVariable();
            this.addFieldVariable(subVar, "Target X Position", "targetXPos");
            this.addFieldVariable(subVar, "Target Y Position", "targetYPos");
            this.addFieldVariable(subVar, "Target Yaw", "targetYaw");
            this.addFieldVariable(subVar, "Target Lift Height", "targetLiftPos");
            this.addFieldVariable(subVar, "Speed", "speed");

            root.putVariable(this.getClass().getSimpleName(), subVar);
        });
        FtcDashboard.getInstance().updateConfig();

        waitForStart();

        while (!isStopRequested()) {
            poser.setSpeed(speed);
            poser.goTo(FieldVector.inTiles(targetXPos, targetYPos), targetYaw, targetLiftPos);
        }
    }

    private <T> void addFieldVariable(CustomVariable root, String name, String field) {
        try {
            root.putVariable(name, new BasicVariable<T>(
                    new FieldProvider<>(this.getClass().getField(field), this)
            ));
        } catch (NoSuchFieldException e) {
            telemetry.log().add("warning: field `" + field + "` not found on " + this.getClass().getSimpleName());
        }
    }
}