package com.team254.lib.util.gyro;

import java.util.List;
import java.util.Timer;
import java.util.TimerTask;

import com.team254.lib.util.Util;

/**
 * Thread which is responsible for reading the gyro
 */
public class GyroThread {

    private static final int K_READING_RATE = 200;
    private static final int K_ZEROING_SAMPLES = 6 * K_READING_RATE;
    private static final int K_STARTUP_SAMPLES = 2 * K_READING_RATE;

    // synchronized access object
    private final Timer mTimer = new Timer("Gyro", true);
    // owned by the background thread
    private final GyroInterface mGyroInterface = new GyroInterface();

    // thread communication variables
    private volatile boolean mVolatileHasData = false;
    private volatile double mVolatileAngle = 0;
    private volatile double mVolatileRate = 0;
    private volatile boolean mVolatileShouldReZero = true;

    // owned by the accesser of this object
    private double mZeroHeading = 0;

    public void start() {
        synchronized (mTimer) {
            mTimer.schedule(new InitTask(), 0);
        }
    }

    public boolean hasData() {
        return mVolatileHasData;
    }

    public double getAngle() {
        return mVolatileAngle - mZeroHeading;
    }
    
    public double getRate() {
        return mVolatileRate;
    }

    public void rezero() {
        mVolatileShouldReZero = true;
    }

    public void reset() {
        mZeroHeading = mVolatileAngle;
    }

    /**
     * Initializes the gyro, verifying the its self-test results
     */
    private class InitTask extends TimerTask {
        @Override
        public void run() {
            while (true) {
                try {
                    mGyroInterface.initializeGyro();
                    break;
                } catch (GyroInterface.GyroException e) {
                    System.out.println("Gyro failed to initialize: " + e.getMessage());
                    synchronized (mTimer) {
                        mTimer.schedule(new InitTask(), 500);
                    }
                }
            }
            System.out.println("gyo initialized, part ID: 0x" + Integer.toHexString(mGyroInterface.readPartId()));
            synchronized (mTimer) {
                mTimer.scheduleAtFixedRate(new UpdateTask(), 0, (int) (1000.0 / K_READING_RATE));
            }
        }
    }

    /**
     * Updates the actual gyro data (zeroing and accumulation)
     */
    private class UpdateTask extends TimerTask {
        private int mRemainingStartupCycles = K_STARTUP_SAMPLES;
        private boolean mIsZerod = false;
        private double[] mZeroRateSamples = new double[K_ZEROING_SAMPLES];
        private int mZeroRateSampleIndex = 0;
        private boolean mHasEnoughZeroingSamples;
        private double mZeroBias;
        private double mAngle = 0;
        private double mLastAngle = 0;

        @Override
        public void run() {
            int reading;
            try {
                reading = mGyroInterface.getReading();
            } catch (GyroInterface.GyroException e) {
                System.out.println("Gyro read failed: " + e.getMessage());
                return;
            }
            GyroInterface.StatusFlag status = GyroInterface.extractStatus(reading);
            List<GyroInterface.ErrorFlag> errors = GyroInterface.extractErrors(reading);
            if (GyroInterface.StatusFlag.VALID_DATA != status || !errors.isEmpty()) {
                System.out.println("Gyro read failed. Status: " + status + ". Errors: " + Util.joinStrings(", ", errors));
                return;
            }

            if (mRemainingStartupCycles > 0) {
                mRemainingStartupCycles--;
                return;
            }

            if (mVolatileShouldReZero) {
                mVolatileShouldReZero = false;
                mVolatileHasData = false;
                mIsZerod = false;
            }

            double unbiasedAngleRate = GyroInterface.extractAngleRate(reading);
            mZeroRateSamples[mZeroRateSampleIndex] = unbiasedAngleRate;
            mZeroRateSampleIndex++;
            if (mZeroRateSampleIndex >= K_ZEROING_SAMPLES) {
                mZeroRateSampleIndex = 0;
                mHasEnoughZeroingSamples = true;
            }
            if (!mIsZerod) {
                if (!mHasEnoughZeroingSamples) {
                    return;
                }
                mZeroBias = 0;
                for (Double sample : mZeroRateSamples) {
                    mZeroBias += sample / K_ZEROING_SAMPLES;
                }
                mIsZerod = true;
                return;
            }

            mVolatileRate = unbiasedAngleRate - mZeroBias;
            mAngle += mVolatileRate / K_READING_RATE;
            mVolatileAngle = mAngle;
            mVolatileHasData = true;
        }
    }
}

@@ -2839,24 +2839,62 @@
 stAngle = 0;
+%0A        private double mLastTime = 0;
 %0A%0A        @O
@@ -4632,32 +4632,225 @@
 %0A            %7D%0A%0A
+            double now = edu.wpi.first.wpilibj.Timer.getFPGATimestamp();%0A            double timeElapsed = mLastTime == 0 ? 1.0 / K_READING_RATE : now - mLastTime;%0A            mLastTime = now;%0A%0A
             mVol
@@ -4928,32 +4928,29 @@
 ileRate 
-/ K_READING_RATE
+* timeElapsed
 ;%0A      

Fix gyro reading to use real time, java timers aren't good