package varintunnistus;

import java.util.ArrayList;
import java.util.Arrays;

import lejos.hardware.ev3.LocalEV3;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.EV3IRSensor;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.hardware.sensor.SensorModes;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;

public class Ohjelma {

	private final static String VARIANTURI_PORTTI = "S2";
	private final static String KOSKETUSANTURI_PORTTI = "S1";
	private final static String INFRAPUNA_PORTTI = "S4";
	private final static String HAETTU_VARI = "punainen";

	private static Aja aja;
	private static ArrayList<Vari> varit;

	public static void main(String[] args) {
		aja = new Aja();
		varit = new ArrayList<>();

		Port colorPort = LocalEV3.get().getPort(VARIANTURI_PORTTI);
		SensorModes colorSensor = new EV3ColorSensor(colorPort);
		EV3ColorSensor cs = (EV3ColorSensor) colorSensor;
		SampleProvider colorProvider = cs.getRGBMode();

		Port touchPort = LocalEV3.get().getPort(KOSKETUSANTURI_PORTTI);
		SensorModes touchSensor = new EV3TouchSensor(touchPort);
		SampleProvider touch = ((EV3TouchSensor) touchSensor).getTouchMode();

		Port infraPort = LocalEV3.get().getPort(INFRAPUNA_PORTTI);
		SensorModes infraSensor = new EV3IRSensor(infraPort);
		SampleProvider infra = ((EV3IRSensor) infraSensor).getDistanceMode();

		kaynnistaVariKalibrointi(cs, colorProvider, touch);
		aja.taaksepain();
		infrapuna(infraSensor, infra);
		aja.eteenpain();
		varinTunnistus(colorProvider);
		aja.pysahdy();
	}

	private static void varinTunnistus(SampleProvider colorProvider) {
		float[] sample = new float[colorProvider.sampleSize()];
		int i = 0;
		while (true) {
			colorProvider.fetchSample(sample, 0);
			Vari v = haeLahinVari(sample);

			if (i == 100) {
				System.out.println("vari: "+v);
				System.out.println("sample: "+Arrays.toString(sample));
				System.out.println("etaisyys: "+v.etaisyys(sample));
				i = 0;
			}

			if (v.toString().equals(HAETTU_VARI)) {
				break;
			}
			i++;
		}
	}

	private static Vari haeLahinVari(float[] sample) {
		Vari v = varit.get(0);

		for (int i = 1; i < varit.size(); ++i) {
			if (varit.get(i).etaisyys(sample) < v.etaisyys(sample)) {
				v = varit.get(i);
			}
		}
		return v;
	}

	private static void infrapuna(SensorModes infraSensor, SampleProvider infra) {
		float[] sample = new float[infra.sampleSize()];

		while (true) {
			infraSensor.fetchSample(sample, 0);

			if (sample[0] < 0.2) {
				break;
			}
		}
	}

	private static void kaynnistaVariKalibrointi(EV3ColorSensor cs, SampleProvider colorProvider,
			SampleProvider touch) {
		lisaaVari("sininen", cs, colorProvider, touch);

		lisaaVari("punainen", cs, colorProvider, touch);

		lisaaVari("lattia", cs, colorProvider, touch);

	}

	private static void lisaaVari(String nimi, EV3ColorSensor cs, SampleProvider colorProvider, SampleProvider touch) {
		System.out.println("aseta varin " + nimi + " kohdalle, ja paina kayttonappia.");
		float[] sample = new float[touch.sampleSize()];

		while (true) {
			touch.fetchSample(sample, 0);

			if (sample[0] > 0.5) {
				break;
			}
		}

		sample = new float[colorProvider.sampleSize()];

		colorProvider.fetchSample(sample, 0);

		for (int i = 0; i < sample.length; ++i) {
			sample[i] *= 765;
		}

		varit.add(new Vari(nimi, sample));
		System.out.println(nimi + " lisatty. Arvot: "+Arrays.toString(sample));
		Delay.msDelay(2000);
	}
}