package MainMenu;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.io.ObjectInputStream;
import java.io.ObjectOutputStream;
import java.net.ServerSocket;
import java.net.Socket;
import java.util.ArrayList;

import lejos.hardware.Button;
import lejos.hardware.ev3.LocalEV3;
import lejos.hardware.motor.MindsensorsGlideWheelMRegulatedMotor;
import lejos.hardware.port.MotorPort;
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.RegulatedMotor;
import lejos.robotics.SampleProvider;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import lejos.utility.Delay;

public class Main {
    public static final String[] VARINIMET = {"Red", "Gray", "Blue", "X"};

    public static ObjectInputStream OBJECTIN;
    public static ObjectOutputStream OBJECTOUT;

    // Muuttujat Automodelle
    public static boolean backToMenu = true;
    public static boolean LAJITTELE;
    public static boolean KALIBROI;
    public static boolean STOP;
    public static boolean EMSTOP;
    public static Arbitrator arby;

    // Alustetaan sensorit ja moottorit
    // Määritellään muuttujat
    public static final int KALIBROINTITARKKUUS = 10;

    private static ArrayList<int[]> kalibroidutVarit = new ArrayList<>();

    // Käynnistetään kosketusanturi
    private static Port touchPort = LocalEV3.get().getPort("S3");
    private static SensorModes touchSensor = new EV3TouchSensor(touchPort);
    private static SampleProvider touch = ((EV3TouchSensor) touchSensor).getTouchMode();
    private static float[] touchSample = new float[touch.sampleSize()];
    private static EmStopBtn emStopBtn = new EmStopBtn(touch, touchSample);

    // Käynnistetään värianturi
    private static Port portRGB = LocalEV3.get().getPort("S1");
    private static SensorModes sensorRGB = new EV3ColorSensor(portRGB);
    private static SampleProvider colorProvider = (((EV3ColorSensor) sensorRGB).getRGBMode());
    private static float[] sample = new float[colorProvider.sampleSize()];

    // Käynnistetään moottori
    private static RegulatedMotor mRanni = new MindsensorsGlideWheelMRegulatedMotor(MotorPort.C);
    private static RegulatedMotor mJalusta = new MindsensorsGlideWheelMRegulatedMotor(MotorPort.B);

    public static void main(String[] args) throws Exception {

        System.out.println("Waiting for\nconnection...");

        ServerSocket serv1 = new ServerSocket(1111);
        Socket s1 = serv1.accept(); //Odotetaan yhteyden muodostusta
        DataInputStream in = new DataInputStream(s1.getInputStream());
        OBJECTIN = new ObjectInputStream(in);

        ServerSocket serv2 = new ServerSocket(1112);
        Socket s2 = serv2.accept(); //Odotetaan yhteyden muodostusta
        DataOutputStream out = new DataOutputStream(s2.getOutputStream());
        OBJECTOUT = new ObjectOutputStream(out);

        OBJECTOUT.writeObject("Connected to EV3");

        for(int i = 0; i < 3; i++) {
            int[] iArr = (int[]) OBJECTIN.readObject();
            kalibroidutVarit.add(i, iArr);
        }
        //Rännin väri
        int a[] = { 36, 14, 8 };
        kalibroidutVarit.add(3, a);

        // Asetetaan jalustan moottoreille haluttu nopeus
        mJalusta.setSpeed(250);
        mRanni.setSpeed(750);

        mainLoop();
    }

    public static 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;
    }

    public static void mainLoop() throws ClassNotFoundException, IOException {
        System.out.println("Choose mode");

        while (true) {
            String command = (String) OBJECTIN.readObject();
            if(command.contains("Demo")) {
                // TODO: Virhetilojen havaitseminen sensoreissa ja moottoreille
                Behavior dB1 = new DemoColorSorting(mRanni, mJalusta, colorProvider, sample, kalibroidutVarit);
                Behavior dB2 = new EmStop(mRanni, mJalusta, emStopBtn);

                Behavior[] demobArray = { dB1, dB2 };

                // Looppaa käyttäytymisiä
                arby = new Arbitrator(demobArray);
                arby.go();
            }else if(command.contains("Auto")) {
                /*// Testataan hätäseisnapin toimivuutta
                System.out.println("Press Em.STOP \n"
                                    + "to continue.");
                while (!emStopBtn.painettuna());
                Delay.msDelay(1000);*/

                Behavior aB1 = new AutoWaitForCommand();
                Behavior aB2 = new AutoCalibration(colorProvider, mRanni, sample, kalibroidutVarit, emStopBtn);
                Behavior aB3 = new AutoColorSorting(mRanni, mJalusta, colorProvider, sample, kalibroidutVarit);
                Behavior aB4 = new EmStop(mRanni, mJalusta, emStopBtn);

                Behavior[] autobArray = { aB1, aB2, aB3, aB4 };

                // Looppaa käyttäytymisiä
                arby = new Arbitrator(autobArray);
                arby.go();
            }else if(command.contains("Manual")) {
                ManualSorter manual = new ManualSorter();
                manual.manualLoop(mRanni, mJalusta, kalibroidutVarit, colorProvider, sample);
            }else if(command.contains("Exit")) {
                System.out.println("Program stopping");
                Delay.msDelay(2000);
                System.exit(1);
            }
        }
    }
}