//package org.firstinspires.ftc.teamcode;
//
//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
//
//public class AutonMovement2 {
//    final Hardware hardware;
//    final MovementAPI movementAPI;
//    final OpMode opMode;
//    FieldVector currentPosition;
//
//    public AutonMovement2(OpMode opMode, FieldVector initialPosition) {
//        this.opMode = opMode;
//        this.hardware = new Hardware(opMode);
//        this.movementAPI = new MovementAPI(hardware);
//        this.hardware.initIMU();
//        this.currentPosition = initialPosition;
//    }
//
//    public void move(FieldVector relativePos) {
//        this.moveTo(currentPosition.add(relativePos));
//    }
//
//    public void moveTo(FieldVector targetPos) {
//        final double KP = 1;
//        final double KI = 0.3;
//        final double KD = 0.5;
//
//        final double encoderTicksPerRev = 537.7;
//        final double mmPerRev = 96 /* diameter of gobilda mecanums */ * Math.PI;
//        final double encoderTicksPerMm = encoderTicksPerRev / mmPerRev;
//
//        FieldVector posDiffNormalized = targetPos.sub(currentPosition).normalized();
//        MotorPowers powers = MotorPowers.calculateUnscaled(posDiffNormalized.x, posDiffNormalized.y, 0, 1);
//
//        final double port0EncoderScale = powers.fl;
//        final double port3EncoderScale = powers.fr;
//
//        double port0EncoderStart = hardware.fl.getCurrentPosition();
//        double port3EncoderStart = hardware.fr.getCurrentPosition();
//
//        FieldVector integral = FieldVector.ZERO;
//        FieldVector prevPos = this.currentPosition;
//
//        while (true) {
//            double port0Encoder = hardware.fl.getCurrentPosition();
//            double port3Encoder = hardware.fr.getCurrentPosition();
//
//            double port0EncoderScaled = (port0Encoder - port0EncoderStart) / port0EncoderScale;
//            double port3EncoderScaled = (port3Encoder - port3EncoderStart) / port3EncoderScale;
//
//            double encoderTicksMoved = (port0EncoderScaled + port3EncoderScaled) / 2;
//            double mmMoved = encoderTicksMoved / encoderTicksPerMm;
//
//            opMode.telemetry.addLine("the port 0 encoder is " + port0Encoder);
//            opMode.telemetry.addLine("and it started as " + port0EncoderStart);
//            opMode.telemetry.addLine("oh and the scale value is " + port0EncoderScale);
//            opMode.telemetry.addLine("the port 3 encoder is " + port3Encoder);
//            opMode.telemetry.addLine("and it started as " + port3EncoderStart);
//            opMode.telemetry.addLine("oh and the scale value is " + port3EncoderScale);
//            opMode.telemetry.addLine("i think we have moved " + encoderTicksMoved + " encoder ticks");
//            opMode.telemetry.addLine("i think we have moved " + mmMoved + "mm");
//
//            FieldVector currentPos = posDiffNormalized.mul(mmMoved).add(this.currentPosition /* actually more like initialPosition */);
//            FieldVector posDiff = targetPos.sub(currentPos);
//
//            opMode.telemetry.addLine("i think we are at (" + currentPos.x + ", " + currentPos.y + ")");
//            opMode.telemetry.update();
//
//            if (posDiff.sqMagnitude() < 5 * 5) {
//                movementAPI.stop();
//                this.currentPosition = currentPos; // finally update `this.currentPosition`
//                return;
//            }
//
//            integral = integral.add(posDiff);
//
//            FieldVector unscaled = posDiff.mul(KP)
//                    .add(currentPos.sub(prevPos).mul(KD))
//                    .add(integral.mul(KI));
//
//            FieldVector scaled = unscaled.div(Distance.MM_PER_FOOT);
//
//            double yaw = this.hardware.getImuYaw();
//
//            FieldVector rotated = scaled.rot(yaw);
//
//            this.movementAPI.move(rotated.x, rotated.y, 0, 0.4);
//
//            prevPos = currentPos;
//        }
//    }
//
//    public void rotTo(double targetAngle) {
//        final double KP = 1;
//        final double KI = 0;
//        final double KD = 0;
//
//        double integral = 0;
//        double prevYaw = this.hardware.getImuYaw();
//
//        while (true) {
//            double currentYaw = this.hardware.getImuYaw();
//            double yawDiff = targetAngle - currentYaw;
//
//            if (yawDiff < 2) return;
//
//            integral += yawDiff;
//
//            double turnSpeed = yawDiff * KP
//                    + (currentYaw - prevYaw) * KD
//                    + integral * KI;
//
//            // note: should be `-turnSpeed` but `MovementAPI.move` has the reverse mapping of +/- to cw/ccw
//            this.movementAPI.move(0, 0, turnSpeed / 15, 0.4);
//
//            prevYaw = currentYaw;
//        }
//    }
//}