package MainMenu;

import java.io.IOException;
import java.util.ArrayList;

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

public class ManualSorter {

    public void manualLoop(RegulatedMotor mRanni, RegulatedMotor mJalusta,
            ArrayList<int[]> kalibroidutVarit, SampleProvider colorProvider, float[] sample) throws IOException, ClassNotFoundException {

        boolean manualMode = true;
        int[] syotettyVari = new int[3];
        double[] variEtaisyys = new double[4];
        int[] variArvot = {0,0,0};

        String start = "";
        while(start.contains("Start")) start = (String) Main.OBJECTIN.readObject();
        System.out.println("Manual: Started.");

        while (manualMode) {

            mJalusta.setSpeed(50);

            switch (buttonPressed(sensorIR)) {
            // Skannaa ja tiputtaa palikan
            case SCANEJECT:
                System.out.println("READING...");
                // Lue värejä
                colorProvider.fetchSample(sample, 0);
                for (int i = 0; i < syotettyVari.length; i++)
                    syotettyVari[i] = Math.round(sample[i] * 765);

                // Tarkista onko väri lähellä kalibroituja värejä
                for (int i = 0; i < variEtaisyys.length; i++)
                    variEtaisyys[i] = variTarkastus(kalibroidutVarit.get(i), syotettyVari);

                // TODO: laske lyhin etäisyys. Palauttaa indeksin
                int shortest = minDistance(variEtaisyys);

                // TODO: tunnista väri taulukon indeksistä
                if(shortest < variArvot.length) variArvot[shortest]++;
                System.out.println(Main.VARINIMET[shortest] + " color");

                // TODO: update databaseen
                mRanni.rotate(-180);
                mRanni.rotate(180);
                break;
            case CALIBRATE:
                luePalikat(mRanni, kalibroidutVarit, colorProvider, sample);
                break;
            case ROTATELEFT:
                System.out.println(mJalusta.getLimitAngle());
                if(mJalusta.getLimitAngle() > -60) mJalusta.rotate(-60, true);
                while(mJalusta.isMoving());
                break;
            case ROTATERIGHT:
                System.out.println(mJalusta.getLimitAngle());
                if(mJalusta.getLimitAngle() < 60) mJalusta.rotate(60, true);
                while(mJalusta.isMoving());
                break;
            case ESCAPE:
                manualMode = false;
                break;
            default:
                break;
            }
        }
        for(int i: variArvot)
            Main.OBJECTOUT.writeObject(i);

        mJalusta.rotateTo(0, true);
        mRanni.rotateTo(0, true);
        while(mRanni.isMoving() || mJalusta.isMoving());
        Main.mainLoop();
    }


    private static void luePalikat(RegulatedMotor mRanni, ArrayList<int[]> kalibroidutVarit, SampleProvider colorProvider, float[] sample) {
        // Kalibroidaan palikan väri
        System.out.println("Calibration starts in 3 sec...");
        Delay.msDelay(3000);

        for(int i = 0; i < kalibroidutVarit.size(); i++) {
            System.out.print(Main.VARINIMET[i] + ": ");
            kalibroidutVarit.set(i, Main.kalibroi(Main.KALIBROINTITARKKUUS, colorProvider, sample));
            System.out.println("Success!");

            mRanni.rotate(-180, true);
            while (mRanni.isMoving());
            mRanni.rotate(180, true);
            while (mRanni.isMoving());
            Delay.msDelay(750);
        }

        System.out.println("Ready!");
        Delay.msDelay(500);
    }

    // Vertaa kahta väri. True = värit ovat samat, false = värit ovat eri
        private static double variTarkastus(int[] vari1, int[] vari2) {
            // Muuttujat
            int[] variEtaisyysItseisarv = new int[vari1.length];
            int summa = 0;

            // Laksetaan värien etäisyys ja korotetaan neliöön
            for (int i = 0; i < vari1.length; i++) {
                variEtaisyysItseisarv[i] = vari1[i] - vari2[i];
                variEtaisyysItseisarv[i] *= variEtaisyysItseisarv[i];
            }

            // Summataan etäisyyden neliöt ja lasketaan summan neliöjuuri
            for (int i : variEtaisyysItseisarv)
                summa += i;

            return Math.sqrt((double) summa);
        }

        // Palauttaa lyhimmän arvon indeksiluvun
        private static int minDistance(double[] taulukko) {
            int pienin = taulukko[0] < taulukko[1] ? 0 : 1;
            pienin = taulukko[pienin] < taulukko[2] ? pienin : 2;
            return taulukko[pienin] < taulukko[3] ? pienin : 3;
        }

    public int[] kalibroi(int accuracy, SampleProvider colorProvider, float[] sample) {
        // Muuttujat
        int[] rgb = { 0, 0, 0 };

        // Lasketaan R-, G-, ja B-arvojen kertymä n-kertaa
        for (int i = 0; i < accuracy; i++) {
            colorProvider.fetchSample(sample, 0);
            for (int j = 0; j < rgb.length; j++)
                rgb[j] += Math.round(sample[j] * 765);
        }

        // Jaetaan kertymä n-kerralla. Saadaan jäsenien keskiarvo
        for (int i = 0; i < rgb.length; i++)
            rgb[i] = Math.round(rgb[i] / accuracy);

        // Palautetaan kolmiarvoinen int-tyyppinen lista
        return rgb;
    }

    // Palauttaa komennon riippuen painetusta näppäimestä
        // Näppäin voi olla kauko-ohjaimesta tai EV3:sta
        static Commands buttonPressed(SensorModes sensorIR) {
            int channel = 0;
            int command = ((EV3IRSensor) sensorIR).getRemoteCommand(channel);
            if (Button.UP.isDown() || command == IR_DOWNRIGHT) {
                return Commands.CALIBRATE;
            } else if (Button.DOWN.isDown() || command == IR_DOWNLEFT) {
                return Commands.SCANEJECT;
            } else if (Button.LEFT.isDown() || command == IR_UPLEFT) {
                return Commands.ROTATELEFT;
            } else if (Button.RIGHT.isDown() || command == IR_UPRIGHT) {
                return Commands.ROTATERIGHT;
            } else if (Button.ENTER.isDown()) {
                return Commands.SCAN;
            } else if (Button.ESCAPE.isDown()) {
                return Commands.ESCAPE;
            }
            return Commands.NONE;
        }

        //Manual mode commands
        private static enum Commands {
            CALIBRATE, SCANEJECT, ROTATELEFT, ROTATERIGHT, SCAN, ESCAPE, NONE
        }

        // IR -ohjaimen näppäinten arvoja vastaavat muuttujat
        private static final int IR_UPLEFT = 1;
        private static final int IR_UPRIGHT = 2;
        private static final int IR_DOWNLEFT = 3;
        private static final int IR_DOWNRIGHT= 4;

        // Käynnistetään IR -anturi
        private static Port portIR = LocalEV3.get().getPort("S2");
        private static SensorModes sensorIR = new EV3IRSensor(portIR);
}