package org.usfirst.frc.team4828.Vision;

import edu.wpi.first.wpilibj.AnalogInput;

import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.net.Socket;
import java.util.*;

public class PixyThread extends Thread {
    private static final String HOST = "pixyco.local";
    private static final int PORT = 5800;
    private static final int WINDOW_SIZE = 30;
    private static final double SUPPLIED_VOLTAGE = 5.0;
    volatile Frame lastFrame;
    private volatile Frame currentFrame;
    private double distCm = 0;
    private volatile double distIn = 0;
    private BufferedReader in;
    private Socket soc;
    private Thread t;
    private boolean enabled;
    private String threadName = "pixy thread";
    private AnalogInput sensor;
    private Queue<Double> values;

    /**
     * loops while it's alive
     * Create object encapsulating the last frame and ultrasonic data.
     */
    public PixyThread(int port) {
        System.out.println("constructing pixythread");
        sensor = new AnalogInput(port);
        values = new LinkedList<>();
        String[] temp = {"0 1 2 3 4 5 6"};
        currentFrame = new Frame(temp, .5);
        lastFrame = currentFrame;
    }

    public double horizontalOffset() {
        if (lastFrame.numBlocks() == 2) {
            return lastFrame.getRealDistance(((lastFrame.getFrameData().get(0).getX()
                    + lastFrame.getFrameData().get(1).getX()) / 2) - Block.X_CENTER);
        }
        //if only one vision target is detected
        else if (lastFrame.numBlocks() == 1) {
            return lastFrame.getRealDistance(lastFrame.getFrameData().get(0).getX() - Block.X_CENTER);
        }
        //if no vision targets are detected
        return 1000;
    }

    /**
     * @return distance from robot to the end of the peg inches
     */
    public double transverseOffset() {
        return distIn - 10.5;
    }

    /**
     * Finds the median in a List of values.
     *
     * @param values a List of Doubles
     * @return the median
     */
    private double medianFilter(Queue<Double> values) {
        int half = values.size() / 2;
        if (values.size() == 0) {
            return 0.0;
        }
        List<Double> temp = new ArrayList<>(values);
        Collections.sort(temp);
        if (values.size() % 2 == 1) {
            return temp.get(half);
        } else {
            return (temp.get(half - 1) + temp.get(half)) / 2;
        }
    }

    /**
     * Converts a number to Centimeters.
     *
     * @param voltage number to convert
     * @return converted number
     */
    private double toCm(double voltage) {
        return voltage / (SUPPLIED_VOLTAGE / 1024);
    }

    /**
     * Converts from Centimeters to Inches.
     *
     * @param voltage number to convert
     * @return converted number
     */
    private double toIn(double voltage) {
        return toCm(voltage) / 2.54;
    }

    @Override
    public void run() {
        while (enabled) {
            try {
                currentFrame = new Frame(in.readLine().split(","), distIn);
            } catch (Exception e) {
                e.printStackTrace();
            }
            if (currentFrame.numBlocks() >= 2) {
                lastFrame = currentFrame;
            }
            values.add(sensor.getVoltage());
            while (values.size() > WINDOW_SIZE) {
                values.remove();
            }

            distCm = toCm(medianFilter(values));
            distIn = toIn(medianFilter(values));
            edu.wpi.first.wpilibj.Timer.delay(0.1);
        }
    }

    @Override
    public void start() {
        enabled = true;
        if (t == null) {
            System.out.println("starting: " + threadName);
            boolean scanning = true;
            while (scanning) {
                try {
                    soc = new Socket(HOST, PORT);
                    in = new BufferedReader(new InputStreamReader(soc.getInputStream()));
                    scanning = false;
                } catch (IOException e) {
                    System.out.println("Connect failed, waiting and trying again");
                    try {
                        Thread.sleep(1000);
                    } catch (InterruptedException ie) {
                        ie.printStackTrace();
                    }
                }
            }
            System.out.println("Socket connection established");
            t = new Thread(this, threadName);
            t.start();
        }
    }

    public void terminate() {
        try {
            in.close();
            soc.close();
        } catch (IOException e) {
            e.printStackTrace();
        }
        sensor.free();
        enabled = false;
        t = null;
    }

    @Override
    public String toString() {
        if (lastFrame != null) {
            return lastFrame.toString();
        } else {
            return currentFrame.toString();
        }
    }
}
@@ -817,16 +817,60 @@
  values;
+%0A    private boolean blocksDetected = false;
 %0A%0A    /*
@@ -1150,129 +1150,8 @@
 ();%0A
-        String%5B%5D temp = %7B%220 1 2 3 4 5 6%22%7D;%0A        currentFrame = new Frame(temp, .5);%0A        lastFrame = currentFrame;%0A
     
@@ -1660,32 +1660,64 @@
 ts are detected%0A
+        blocksDetected = false;%0A
         return 1
@@ -3132,32 +3132,33 @@
 ;%0A            %7D%0A
+%0A
             if (
@@ -3186,19 +3186,114 @@
 s() %3E= 2
-) %7B
+ %7C%7C (lastFrame == null && currentFrame.numBlocks() == 1)) %7B%0A                blocksDetected = true;
 %0A       
@@ -3333,32 +3333,33 @@
 ;%0A            %7D%0A
+%0A
             valu
@@ -4760,24 +4760,24 @@
 sor.free();%0A
-
         enab
@@ -4781,32 +4781,64 @@
 nabled = false;%0A
+        blocksDetected = false;%0A
         t = null

add blocksDetected flag