power-play/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonMovement.java

package org.firstinspires.ftc.teamcode.auto;

import org.firstinspires.ftc.robotcore.external.Telemetry;
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.MovementAPI;

public class AutonMovement {
    Hardware hardware;
    MovementAPI movement;
    Telemetry telemetry;

    double speed;

    public final double ENCODER_TICKS_PER_MM = 537.7 / (96 * Math.PI);

    public AutonMovement(Hardware hardware, double speed) {
        this.hardware = hardware;
        this.movement = new MovementAPI(hardware);
        this.speed = speed;
        this.telemetry = hardware.opMode.telemetry;
    }

    private void sleep(double seconds) {
        double startTime = System.nanoTime();
        double targetTime = startTime + seconds * 1000 * 1000 * 1000;

        while (System.nanoTime() < targetTime);
    }

    private void movementHelper(Distance distance, boolean strafe) {
        int flStart = hardware.fl.getCurrentPosition();
        int frStart = hardware.fr.getCurrentPosition();

        int targetDisplacement = (int) (distance.valInMM() * ENCODER_TICKS_PER_MM);

        if (strafe) targetDisplacement *= (60/52.);

        int loopsCorrect = 0;

        while (true) {
            int flDisplacement = hardware.fl.getCurrentPosition() - flStart;
            int frDisplacement = hardware.fr.getCurrentPosition() - frStart;

            if (strafe) frDisplacement *= -1;

            int totalDisplacement = (flDisplacement + frDisplacement) / 2;
            int displacementDiff = targetDisplacement - totalDisplacement;

            if (Math.abs(displacementDiff) < 10) {
                loopsCorrect += 1;
            } else {
                loopsCorrect = 0;
            }

            if (loopsCorrect >= 80) {
                movement.stop();
                break;
            }

            double pow;
            if (Math.abs(displacementDiff) > 300) {
                pow = 1;
            } else {
                pow = Math.sqrt(1 - Math.pow((Math.abs(displacementDiff) / 300.) - 1, 2));
            }

            telemetry.addLine("pow = " + pow);
            telemetry.addLine("loopsCorrect = " + loopsCorrect);
            telemetry.update();

            if (strafe) {
                movement.move(Math.signum(displacementDiff) * pow, 0, 0, speed);
            } else {
                movement.move(0, Math.signum(displacementDiff) * pow, 0, speed);
            }
        }
    }

    public void straight(Distance distance) {
        this.movementHelper(distance, false);
    }

    public void strafe(Distance distance) {
        this.movementHelper(distance, true);
    }

    private double modAngle(double angle) {
        double angle2 = angle % 360;
        if (angle2 >= 180) {
            return angle2 - 360;
        } else {
            return angle2;
        }
    }

    public void turn(double degrees) {
        double startYaw = hardware.getImuYaw();
        double targetYaw = startYaw + degrees;

        int loopsCorrect = 0;

        while (true) {
            double yaw = hardware.getImuYaw();

            double yawDiff = modAngle(targetYaw - yaw);

            if (Math.abs(yawDiff) < 1) {
                loopsCorrect += 1;
            } else {
                loopsCorrect = 0;
            }

            if (loopsCorrect >= 100) {
                movement.stop();
                break;
            }

            double pow;
            if (Math.abs(yawDiff) > 60) {
                pow = 1;
            } else {
                pow = Math.sqrt(1 - Math.pow((Math.abs(yawDiff) / 60.) - 1, 2));
            }

            movement.move(0, 0, Math.signum(yawDiff) * pow, speed);
        }
    }

    public void liftTo(LiftPosition height) {
        int loopsCorrect = 0;

        while (true) {
            int spoolPosition = hardware.getLiftEncoder();

            int spoolPosDiff = height.getEncoderVal() - spoolPosition;

            if (Math.abs(spoolPosDiff) < 10) {
                loopsCorrect += 1;
            } else {
                loopsCorrect = 0;
            }

            if (loopsCorrect >= 100) {
                hardware.spool.setPower(0);
                break;
            }

            double pow;
            if (Math.abs(spoolPosDiff) > 300) {
                pow = 1;
            } else {
                pow = Math.sqrt(1 - Math.pow((Math.abs(spoolPosDiff) / 400.) - 1, 2));
            }

            telemetry.addLine("spool pos = " + spoolPosition);
            telemetry.addLine("pow = " + pow);
            telemetry.addLine("loopsCorrect = " + loopsCorrect);
            telemetry.update();

            hardware.spool.setPower(Math.signum(spoolPosDiff) * pow);
        }
    }
}