package com.edinarobotics.zeke.vision;

import com.edinarobotics.utils.log.Level;
import edu.wpi.first.wpilibj.Timer;
import java.io.IOException;
import java.io.InputStream;
import javax.microedition.io.SocketConnection;

public class VisionReadingThread extends Thread {

    private volatile boolean stop;
    private SocketConnection connection;
    private VisionServer server;
    private static final double TIMEOUT = 10.0;

    public VisionReadingThread(SocketConnection connection, VisionServer server) {
        stop = false;
        this.connection = connection;
        this.server = server;
    }

    public void requestStop() {
        this.stop = true;
    }

    public void run() {
        InputStream inputStream = null;
        try {
            inputStream = connection.openInputStream();
            byte[] readBytes = new byte[5];
            double lastHeartbeat = Timer.getFPGATimestamp();
            while (!stop) {
                try {
                    if (Timer.getFPGATimestamp() < lastHeartbeat + TIMEOUT) {
                        requestStop();
                        break;
                    }
                    int inputData = inputStream.read(readBytes);
                    for (int i = 0; i < inputData; i++) {
                        byte reading = readBytes[i];
                        boolean status = reading != 0;
                        System.out.println(">>>"+(status?"HOT HOT HOT":"COLD COLD COLD"));
                        this.server.reportHotGoal(status);
                    }
                    lastHeartbeat = Timer.getFPGATimestamp();
                    Thread.sleep(50);
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }
            }
        } catch (IOException ex) {
            ex.printStackTrace();
        } finally {
            if (inputStream != null) {
                try {
                    inputStream.close();
                } catch (IOException ex) {
                    ex.printStackTrace();
                }
            }
            try {
                this.connection.close();
            } catch (IOException ex) {
                ex.printStackTrace();
            }
        }
    }
}

@@ -1007,17 +1007,17 @@
 stamp() 
-%3C
+%3E
  lastHea
@@ -1103,13 +1103,16 @@
     
-break
+continue
 ;%0A  
@@ -1192,24 +1192,126 @@
 readBytes);%0A
+                    if(inputData == -1)%7B%0A                        requestStop();%0A                    %7D%0A
             
@@ -1409,16 +1409,16 @@
 tes%5Bi%5D;%0A
+
         
@@ -1468,99 +1468,8 @@
  0;%0A
-                        System.out.println(%22%3E%3E%3E%22+(status?%22HOT HOT HOT%22:%22COLD COLD COLD%22));%0A
     

Fix VisionReadingThread heartbeat calculation.