power-play/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java

package org.firstinspires.ftc.teamcode.kinematics;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.Hardware;

public class EncoderIntegrator {
    Hardware hardware;

    FieldVector pos;
    // `yaw` is the robot angle. `imuYaw` is the last measured value from the imu. usually just
    // offset by a constant but that could be different in the future, i.e. radians or smth.
    double yaw;

    int fl;
    int fr;
    int bl;
    int br;
    double imuYaw;

    Telemetry telemetry;

    public final double MM_PER_ENCODER_TICK = (96 * Math.PI) / 537.7;
    public final double WHEELBASE_LENGTH = 337;
    public final double WHEELBASE_WIDTH = 355;
    public final double WHEELBASE_DIAGONAL = Math.hypot(WHEELBASE_LENGTH, WHEELBASE_WIDTH );
    public final double DEGREES_PER_MM = 360 / (WHEELBASE_DIAGONAL * Math.PI);

    public EncoderIntegrator(Hardware hardware, FieldVector initialPos, double initialYaw, Telemetry telemetry) {
        this.hardware = hardware;
        this.pos = initialPos;
        this.yaw = initialYaw;
        this.telemetry = telemetry;

        this.fl = hardware.fl.getCurrentPosition();
        this.fr = hardware.fr.getCurrentPosition();
        this.bl = hardware.bl.getCurrentPosition();
        this.br = hardware.br.getCurrentPosition();
        this.imuYaw = hardware.getImuYaw();
    }

    private static double sinc(double x) {
        if (x == 0) {
            return 1;
        } else {
            return Math.sin(x) / x;
        }
    }

    // this function has no generally well-known name, but its similar to `sinc` so im calling it `cosc`
    private static double cosc(double x) {
        if (x == 0) {
            return 0;
        } else {
            return (1 - Math.cos(x)) / x;
        }
    }

    public void update() {
        int newFl = hardware.fl.getCurrentPosition();
        int newFr = hardware.fr.getCurrentPosition();
        int newBl = hardware.bl.getCurrentPosition();
        int newBr = hardware.br.getCurrentPosition();
        double newImuYaw = hardware.getImuYaw();

        double flDiff = newFl - this.fl;
        double frDiff = newFr - this.fr;
        double blDiff = newBl - this.bl;
        double brDiff = newBr - this.br;
        double imuYawDiff = newImuYaw - this.imuYaw;

//        telemetry.addData("flDiff", flDiff);
//        telemetry.addData("frDiff", frDiff);
//        telemetry.addData("blDiff", blDiff);
//        telemetry.addData("brDiff", brDiff);
//        telemetry.addData("imuYawDiff", imuYawDiff);

//        fl = powerY + powerX + turn + noop
//        fr = powerY - powerX - turn + noop
//        bl = powerY - powerX + turn - noop
//        br = powerY + powerX - turn - noop

        double relativeYDiff = ((flDiff + frDiff + blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
        double relativeXDiff = ((flDiff - frDiff - blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
        double turnDiff = ((flDiff - frDiff + blDiff - brDiff) / 4.) * MM_PER_ENCODER_TICK * DEGREES_PER_MM;
        double noopDiff = (flDiff + frDiff - blDiff - brDiff) / 4.;

        // aaaaand then we are just going to ignore `turnDiff` and `noopDiff`. there is nothing
        // meaningful we can do with these values to help correct the other values. the noop value
        // is ignored for obvious reasons. the turn value is ignored since we get the turn from the
        // imu and using encoders for turn has proven to work poorly.

//        telemetry.addData("relativeYDiff", relativeYDiff);
//        telemetry.addData("relativeXDiff", relativeXDiff);

        double imuYawDiffRadians = Math.toRadians(imuYawDiff);
        double poseExponentiationX = cosc(imuYawDiffRadians);
        double poseExponentiationY = sinc(imuYawDiffRadians);

        FieldVector relativePosDiff = FieldVector.inMM(
                relativeYDiff * poseExponentiationX + relativeXDiff * poseExponentiationY,
                relativeYDiff * poseExponentiationY - relativeXDiff * poseExponentiationX
        );

        FieldVector posDiff = relativePosDiff.rot(this.imuYaw);
        double yawDiff = imuYawDiff;

        this.pos = this.pos.add(posDiff);
        this.yaw += yawDiff;

        this.fl = newFl;
        this.fr = newFr;
        this.bl = newBl;
        this.br = newBr;
        this.imuYaw = newImuYaw;
        this.imuYaw %= 360;
    }
}