package frc.robot.util;
 
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.Timer;
 
public class IMUGyro 
{
    public I2C mpu;
 
    public double pitch;
    public double roll;
    public double yaw;
 
    private double gyroOffsets[] = {
        0.312449302886265,
        0.04883417185589417,
        -0.12945104079301352
    };
    
    // for deltatime
    private double prev_time = Timer.getFPGATimestamp();
 
    private final int PWR_MGMT_1 = 0x6B;
    private final int CONFIG_ADDR = 0x1A;
    // private final int SMPLRT_DIV = 0x19;
 
    private final int GYRO_ADDR = 0x43;
    private final int GYRO_CONFIG = 0x1B;
 
    private final double SCALE_GYRO = (Math.PI/(180*16.384) * 2 /*??*/);
 
    public IMUGyro(I2C gyro) 
    {
        mpu = gyro;
        reset();

        mpu.write(CONFIG_ADDR, 0b00000101);
        // mpu.write(SMPLRT_DIV, 0b00010011);
        mpu.write(GYRO_CONFIG, 0b00011000);
        // mpu.write(ACCL_CONFIG, 0b00000000);
    }
 
    public void reset()
    {
        mpu.write(PWR_MGMT_1, 0b00000000);

        pitch = 0;
        roll = 0;
        yaw = 0;
    }
 
    public void update()
    {
        double cur_time = Timer.getFPGATimestamp();
        double dt = cur_time - prev_time;
        prev_time = cur_time;

        double[] gRates = gyroRates();

        roll += gRates[0] * dt;
        pitch += gRates[1] * dt;
        yaw += gRates[2] * dt;
    }
 
    public Rotation2d toRotation2d()
    {
        return new Rotation2d(-yaw);
    }
 
    public void calibrateGyro(int iter)
    {
        gyroOffsets = new double[3];

        double x = 0, y = 0, z = 0;
        for(int i = 0; i < iter; i++) 
        {
            double[] rates = gyroRates();
            x += rates[0];
            y += rates[1];
            z += rates[2];
        }
 
        gyroOffsets[0] = x/iter;
        gyroOffsets[1] = y/iter;
        gyroOffsets[2] = z/iter;
    }
 
    private double[] gyroRates()
    {
        return scaledRates(GYRO_ADDR, SCALE_GYRO);
    }
 
    private double[] scaledRates(int addr, double scale)
    {
        byte[] buffer = new byte[6];
        mpu.read(addr, 6, buffer);
        double dx = decode(buffer[0], buffer[1]) * scale - gyroOffsets[0];
        double dy = decode(buffer[2], buffer[3]) * scale - gyroOffsets[1];
        double dz = decode(buffer[4], buffer[5]) * scale - gyroOffsets[2];
        return new double[] {dx, dy, dz};
    }
 
    private double decode(byte first, byte second) 
    {
        return ((first << 8) | second);
    }
}