package dpm.lejos.project;

import dpm.lejos.Sensors.SuperColorSensor;
import lejos.nxt.comm.RConsole;
import lejos.util.Timer;
import lejos.util.TimerListener;

import java.util.LinkedList;

/**
 * Class that polls the down facing light sensor to detect floor lines
 *
 * @author David Lavoie-Boutin
 * @version 1.0
 */
public class LineDetector extends Thread {

    private static final int DEFAULT_PERIOD = 50;
    private static final int DEFAULT_WINDOW = 4;
    private boolean isLine;
    private SuperColorSensor colorSensor;
    private int LIGHT_THRESHOLD;

    private double[] coeffs = new double[] {1/2.0,-1/6.0, -1/6.0, -1/6.0};

    private LinkedList<Integer> list = new LinkedList<Integer>();

    /**
     * default constructor
     * @param colorSensor requires the robot object containing the light sensor
     * @param lightThreshold the normalised light value to use as a threshold
     * @param start boolean to start the timer right away
     */
    public LineDetector(SuperColorSensor colorSensor, int lightThreshold, boolean start){
        this(colorSensor, lightThreshold, DEFAULT_WINDOW, start);
    }

    /**
     * constructor with added period adjustment capability
     * @param colorSensor requires the object light sensor
     * @param start boolean to start the timer right away
     * @param lightThreshold the threshold for line detection
     * @param window the size of the sampling window if different from default
     */
     public LineDetector(SuperColorSensor colorSensor, int lightThreshold, int window, boolean start) {
         LIGHT_THRESHOLD = lightThreshold;
         this.colorSensor = colorSensor;
         colorSensor.setFloodlight(true);
         for (int i = 0; i < window; i++){
             list.add(colorSensor.getNormalizedLightValue());
         }
         if(start) this.start();

     }

    /**
     * callback for the timer timeout
     *
     * checks the change in the value of the read light
     * and compares with the set threshold
     */
    public void run(){

        while(true) {

            list.add(colorSensor.getNormalizedLightValue());
            list.remove(0);
            double diff = Utils.averageList(list, coeffs);


            if (colorSensor.getSensorPort().getId() == 0) {
            //    RConsole.println(String.valueOf(diff));
            } else {
              //  RConsole.println("\t" + String.valueOf(diff));
            }

            if (Math.abs(diff) > LIGHT_THRESHOLD) {
               // RConsole.println("Detected a line edge on sensor " + String.valueOf(colorSensor.getSensorPort().getId()));
                //we detected a significant change
                isLine = diff >= 0;
            }

            if (isLine) RConsole.println("Sensor " + String.valueOf(colorSensor.getSensorPort().getId()) + " on line");


            try {
                Thread.sleep(DEFAULT_PERIOD);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        }
	}

    /**
     * get if the robot is over a line
     *
     * @return return true if sensor currently detecting line
     */
    public boolean isLine(){
        return isLine;
    }

    /**
     * manual start of the timer
     */
    public void startTimer(){
        this.start();
    }

    public int getPastLightValue(){
        return (int) Utils.averageList(list,coeffs);
    }
}//end LineDetector
@@ -408,10 +408,10 @@
 D = 
+2
 5
-0
 ;%0A  
@@ -700,24 +700,59 @@
 nteger%3E();%0A%0A
+    private double pastLight = 0;%0A%0A
     /**%0A    
@@ -2234,24 +2234,54 @@
 , coeffs);%0A%0A
+            pastLight = diff;%0A
 %0A           
@@ -3417,38 +3417,17 @@
 nt) 
-Utils.averageList(list,coeffs)
+pastLight
 ;%0A  

fix for null pointer