/*
 * Mind.java
 *
 * Created on 1 of February 2006, 2:06
 */

/**
 *
 * @author Paulo Roque Silva
 */

/**
 * 
	TouchSensor(SensorPort.S1);
	SoundSensor(SensorPort.S2);
	LightSensor(SensorPort.S3);
	UltrasonicSensor(SensorPort.S4);

	Motor.A  Left leg
	Motor.B  Right leg
 */
//import icommand.nxt.Battery;
//import icommand.nxt.LightSensor;
//import icommand.nxt.SensorPort;
//import icommand.nxt.TouchSensor;
//import icommand.nxt.UltrasonicSensor;
//import icommand.nxt.SoundSensor;
//import icommand.nxt.Motor;

import java.util.concurrent.CyclicBarrier;
import java.awt.Color;
import java.util.concurrent.BrokenBarrierException;
import icommand.nxt.comm.NXTCommand;
import icommand.nxt.LightSensor;
import icommand.nxt.SensorPort;
import icommand.nxt.TouchSensor;
import icommand.nxt.UltrasonicSensor;
import icommand.nxt.SoundSensor;
import icommand.nxt.Motor;

public class Mind extends Thread {
	private final int PROFOUNDITY = 6; // number of think()
    Brain            brain; // Liberu with a body and a brain
// LIBERU BODY CONSTANTS
    private final int FeelWall = 30; // if less then 20cm feels wall
    static final int Power = 150; // intensity of a Standard sense signal - to remove with analog
    static final int BANDS = 1; // formica 7 NXT 1 # sound bands frequencies
    static final int TimeWait  = 64; // Time in milliseconds to the nodes wait for all listen the bus
    public static Signal []Action; // Action of the robot
    public static int Tact[], // tact from robot
    				NStep; // Step movement of the robot
	TouchSensor		bottom; // Button of NXT
	LightSensor		ligth; // Light Sensor of NXT
	UltrasonicSensor ultraSonic; // Ultra Sonic Sensor of NXT
	SoundSensor     sound; // Sound Sensor of NXT
	boolean			feelPain; // if robot feels pain
    int 			senteTaste[]; // Senses of the robot's body Taste
    int             senteVision[]; // Senses of the robot's body Vision
    int             senteTact[]; // Senses of the robot's body Tact
    int 			senteSound[]; // Senses of the robot's body Sound
    int 			senteSmell[]; // Senses of the robot's body Smell
    int             senteProSelf[]; // Senses of the robot's body Proprioception
    int				tasteBefore[]; // taste before
    int				smellBefore[]; // taste before
    int				visionBefore[]; // vision before
    int				pleasurePS[], // level of pleasure
    				pleasureTa[],
    				pleasureTc[],
    				pleasureSm[],
    				pleasureSo[],
    				pleasureVi[];
    int				quantity[]; // level of pleasure
	int				m; // index
	int				profoundity; // index
    public static int Bom; // Good
    public static int exec; // Executable
    private int 	lastExec; // Executable last
    private int 	beforeLastExec; // Executable before last
    float			veloc; // 1- Normal; 2- Accelerate
	public static boolean Anxiety; // Anxiety => 2 (velocity)
	public static boolean Saudade; // Anxiety => 2 (velocity)
    
    CyclicBarrier   stepBarrierAct; // Establish a barrier to the act steps
    CyclicBarrier   stepBarrierDecision; // Establish a barrier to the RunEmotionalUnit steps
    CyclicBarrier   stepBarrierDecision2; // Establish a barrier to the RunEmotionalUnit2 steps
    CyclicBarrier   stepBarrierPast; // Establish a barrier to the Time Present
    CyclicBarrier   stepBarrierCerebellum; // Establish a barrier to the Cerebellum
    CyclicBarrier   stepBarrierSleep; // Establish a barrier to SONO

// SIMULATED CONSTANTS
    int				i; // index
    boolean         pause; // false in continuous running
	public static boolean OpenData; // If is old data
    
    /** Creates a new instance of PlayLiberu */
    public Mind(boolean od) {
        
        System.out.println("Liberu...by Paulo Roque Silva 2004-2016");
        brain        = new Brain(); // Mind of the Robot
        Action      = new Signal[Brain.MOVES];
        for (int i=0; i < Action.length; i++)
            Action[i] = new Signal(i, 0, 0);
        senteTaste  = new int[Brain.NanalogTaste];
        pleasureTa  = new int[Brain.NanalogTaste];
        senteVision = new int[Brain.NanalogVision];
        pleasureVi  = new int[Brain.NanalogVision];
        senteTact   = new int[Brain.NanalogTact];
        pleasureTc  = new int[Brain.NanalogTact];
        senteSound  = new int[Brain.NanalogSound];
        pleasureSo  = new int[Brain.NanalogSound];
        senteSmell  = new int[Brain.NanalogSmell];
        pleasureSm  = new int[Brain.NanalogSmell];
        senteProSelf= new int[Brain.NanalogProSelf];
        pleasurePS  = new int[Brain.NanalogProSelf];
        quantity    = new int[1];
        quantity[0] = 0;
        NStep       = 0;
        pause 		= true;
        Tact		= new int[Brain.NanalogTact];
        tasteBefore = new int[Brain.NanalogTaste];
        tasteBefore[0] = 0;
        visionBefore= new int[Brain.NanalogVision];
    	for (i=0; i<visionBefore.length; i++)
    		visionBefore[i] = 0;
        smellBefore= new int[Brain.NanalogSmell];
    	for (i=0; i<smellBefore.length; i++)
    		smellBefore[i] = 0;
        OpenData    = od;
        veloc 		= 1; // 1- Normal; 2- Fast
        exec = 0;
        lastExec = 0;
        beforeLastExec = 0;
        display(true);
    }
    
    public void step(boolean p) {
        
    	pause = p;
    }
    
    public static int getBom() {
        
        return Math.abs(Bom);
    }
    
    public static void setBom(int b) {
        
        Bom = b;
    }
    
    public void display(boolean ex) {
    	
        Liberu.InterFace.displayRobot(brain, ex);
        Liberu.InterFace.repaint();
    }

    private void actNXT(float veloc) {
    	
		if (Action[Brain.ABACK].active /*&& Action[Brain.BBACK].active*/) {
			Motor.A.setSpeed((int)(veloc * (int)Action[Brain.ABACK].actPower));    // 900 = 100% power
//			Motor.B.setSpeed((int)(veloc * (int)Action[Brain.BBACK].actPower));    // degrees/s
/*    		try {
                Liberu.output.write((byte)112);
                Liberu.output.write('\n');
    		}
        	catch (Exception e) {
                System.err.println(e.toString());
        	}
*/			Motor.A.backward();
//			Motor.B.backward();
		}
		if (Action[Brain.AWALK].active /*&& Action[Brain.BWALK].active*/) {
			Motor.A.setSpeed((int)(veloc * (int)Action[Brain.AWALK].actPower));    // 900 = 100% power
//			Motor.B.setSpeed((int)(veloc * (int)Action[Brain.BWALK].actPower));    // degrees/s
/*    		try {
                Liberu.output.write((byte)111);
                Liberu.output.write('\n');
    		}
        	catch (Exception e) {
                System.err.println(e.toString());
        	}
*/			Motor.A.forward();
//			Motor.B.forward();
		}
		if (/*Action[Brain.ABACK].active &&*/ Action[Brain.BWALK].active) {
//			Motor.A.setSpeed((int)(veloc * (int)Action[Brain.ABACK].actPower));    // 900 = 100% power
			Motor.B.setSpeed((int)(veloc * (int)Action[Brain.BWALK].actPower));    // degrees/s
/*    		try {
                Liberu.output.write((byte)116);
                Liberu.output.write('\n');
    		}
        	catch (Exception e) {
                System.err.println(e.toString());
        	}
*/ //			Motor.A.backward();
			Motor.B.forward();
		}
		if (/*Action[Brain.AWALK].active &&*/ Action[Brain.BBACK].active) {
//			Motor.A.setSpeed((int)(veloc * (int)Action[Brain.AWALK].actPower));    // 900 = 100% power
			Motor.B.setSpeed((int)(veloc * (int)Action[Brain.BBACK].actPower));    // degrees/s
/*    		try {
                Liberu.output.write((byte)115);
                Liberu.output.write('\n');
    		}
        	catch (Exception e) {
                System.err.println(e.toString());
        	}
*/ //			Motor.A.forward();
			Motor.B.backward();
		}
		if (Action[Brain.SPEAK].active) {
/*    		try {
                Liberu.output.write((byte)110);
                Liberu.output.write('\n');
    		}
        	catch (Exception e) {
                System.err.println(e.toString());
        	}
*/		}
    }
    
    private void updateSensesNXT() { // NXT Commands
    	int   m/*, space*/; // index
//    	String s;

        // TACT
//    	s="";
    	for (i=0; i<pleasureTc.length; i++)
    		pleasureTc[i] = 0;
    	Tact[0] = Brain.NO_TACT; // BUTTON & INFRARED - TACT - PAIN
		feelPain = false;
/*		try {
            Liberu.output.write((byte)99);
            Liberu.output.write('\n');
            while (!Liberu.input.ready())
                sleep(1);
            s = Liberu.input.readLine();
		}
    	catch (Exception e) {
            System.err.println(e.toString());
    	}
		if (s.length()>0) {
	    	if (s.contentEquals("1")) { */
		if (Brain.Energy)
			pleasureTc[0] = Power;
		if (bottom.isPressed()) {
	    		feelPain = true;
	            Tact[0] = Brain.TACT;
	    		pleasureTc[0] = -Power;
	         	senteTact[0] = Power; // Sense of tact
	    		if (brain.past.time[0][0].step > Emotions.ActRandom)
	    			brain.decision.emotionPAIN();
	    	}
	    	else
	         	senteTact[0] = (int)Power/2; // No Sense of tact
//		}  //formica

        // TASTE
    	for (i=0; i<pleasureTa.length; i++)
    		pleasureTa[i] = 0;
/*    	s = "";
		try {
			Liberu.output.write((byte)100);
            Liberu.output.write('\n');
            while (!Liberu.input.ready())
                sleep(1);
            s = Liberu.input.readLine();
		}
    	catch (Exception e) {
            System.err.println(e.toString());
    	}
		if (s.length()>0)
	        senteTaste[0] = Integer.parseInt(s); // Sense light Red
*/        senteTaste[0] = ligth.getLightPercent(); // Sense light Red
        Interface.Taste[0] = senteTaste[0];
        if (NStep > 1) {
            if (Brain.BackHome)
                pleasureTa[0] = tasteBefore[0] - senteTaste[0];
            else
                pleasureTa[0] = senteTaste[0] - tasteBefore[0];
        }
        else
        	pleasureTa[0] = 0;
        brain.feelTaste(senteTaste, pleasureTa); // Liberu feels
        tasteBefore[0] = senteTaste[0];
        
    	for (m=0; m < senteTaste.length; m++)
    		senteTaste[m] = 0;
    	
        // SOUND
    	for (i=0; i<pleasureSo.length; i++)
    		pleasureSo[i] = 0;
		System.out.println("Lisen..................:)"); // REMOVE
/*		s = "";
		try {
            Liberu.output.write((byte)103);
            Liberu.output.write('\n');
            while (!Liberu.input.ready())
                sleep(1);
            s = Liberu.input.readLine();
		}
    	catch (Exception e) {
            System.err.println(e.toString());
    	}
		space=0;
*/		for (m=0; m < BANDS; m++) {
/*			if (s.length()>0) {
				space = s.indexOf(' ');
				senteSound[m] = Integer.parseInt(s.substring(0, space));
				s = s.substring(space+1, s.length());
			}
*/			senteSound[m] = sound.getdB();
			Interface.Sound[m] = senteSound[m];
		}
		for (m=0; m < BANDS; m++) {
			if (m != 0)
				pleasureSo[m] = senteSound[m] - senteSound[m-1];
			else
				pleasureSo[0] = 0;
		}
		brain.feelSound(senteSound, pleasureSo); // Liberu feels

    	for (m=0; m < senteSound.length; m++)
    		senteSound[m] = 0;

		// SMELL
    	for (i=0; i<pleasureSm.length; i++)
    		pleasureSm[i] = 0;
/*		s = "";
		try {
            Liberu.output.write((byte)98);
            Liberu.output.write('\n');
            while (!Liberu.input.ready())
                sleep(1);
            s = Liberu.input.readLine();
		}
    	catch (Exception e) {
            System.err.println(e.toString());
    	}
		space=0;
		for (m=0; m < senteSmell.length; m++) {
			if (s.length()>0) {
				space = s.indexOf(' ');
				senteSmell[m] = Integer.parseInt(s.substring(0, space));
				s = s.substring(space+1, s.length());
			}
			Interface.Smell[m] = senteSmell[m];
		} //formica
*/        if (NStep > 1)
        	for (m=0; m < senteSmell.length; m++)
                pleasureSm[m] = senteSmell[m] - smellBefore[m]; // better if shine
        else
        	pleasureVi[0] = 0;
		brain.feelSmell(senteSmell, pleasureSm); // Liberu feels

    	for (m=0; m < senteSmell.length; m++) {
    		smellBefore[m] = senteSmell[m];
    		senteSmell[m] = 0;
    	}

        // VISION
    	for (i=0; i<pleasureVi.length; i++)
    		pleasureVi[i] = 0;
/*		s = "";
		try {
            Liberu.output.write((byte)101);
            Liberu.output.write('\n');
            while (!Liberu.input.ready())
                sleep(1);
            s = Liberu.input.readLine();
		}
    	catch (Exception e) {
            System.err.println(e.toString());
    	}
		if (s.length()>0)
			senteVision[0] = Integer.parseInt(s);
		s = "";
		try {
            Liberu.output.write((byte)102);
            Liberu.output.write('\n');
            while (!Liberu.input.ready())
                sleep(1);
            s = Liberu.input.readLine();
		}
    	catch (Exception e) {
            System.err.println(e.toString());
    	}
		if (s.length()>0)
			senteVision[1] = Integer.parseInt(s);
*/		senteVision[0] = ultraSonic.getDistance();
		for (m=0; m < senteVision.length; m++)
			Interface.Vision[m] = senteVision[m];
        if (NStep > 1)
        	for (m=0; m < senteVision.length; m++)
                pleasureVi[m] = visionBefore[m] - senteVision[m]; // better if close
        else
        	pleasureVi[0] = 0;
    	if (senteVision[0] < FeelWall /*|| senteVision[1] < FeelWall*/) {
    		feelPain = true;
            Tact[0] = Brain.TACT;
         	senteTact[0] = Power; // Sense of tact
    		pleasureTc[0] = -Power;
    		if (brain.past.time[0][0].step > Emotions.ActRandom)
    			brain.decision.emotionPAIN();
    	}
        brain.feelVision(senteVision, pleasureVi); // Liberu feels

    	for (m=0; m < senteVision.length; m++) {
    		visionBefore[m] = senteVision[m];
    		senteVision[m] = 0;
    	}
    	
        brain.feelTact(senteTact, pleasureTc); // Liberu feels

    	for (m=0; m < senteTact.length; m++)
    		senteTact[m] = 0;
    }

    private void senseProSelf() {
    	
		// PROPRIOCEPTION
    	for (i=0; i<pleasurePS.length; i++)
    		pleasurePS[i] = 0;
        pleasurePS[0] = brain.proSelfNXT(senteProSelf, ligth, Power); // Liberu NXT update proprioception
        brain.motivationNXT(senteProSelf, Power); // Liberu NXT motivation
        if (feelPain)
        	pleasurePS[0] += -Power;
        brain.feelProSelf(senteProSelf, pleasurePS); // Liberu feels   pleasure not used... used as actpower

    	for (m=0; m < senteProSelf.length; m++)
    		senteProSelf[m] = 0;
    }

    private void runStepBrain() {
    	int i; // index
    	
        // starts the cycle
        NStep = brain.addStep();
        System.out.print("Passo = "); // REMOVE
        System.out.println(NStep); // REMOVE
    	if (brain.past.time[0][0].step == 0)
    		brain.past.time[0][0].standby2 = true;
        
// BEGINING Slide LIVING
        brain.clearBusesIn(true);
        Emotions.WARNING = false;
        updateSensesNXT();
        senseProSelf();
        display(true); // shows in screen
        // End Real Time

        for (i=0; i<Action.length; i++)	{ // reset actions
            Action[i].setActive(false);
            Action[i].setActPower(0);
            Action[i].setPleasure(0);
        }

        Remember.Lovely = 0;
        Interface.Lovely = 0;
        for (i=0; i<Brain.SENSES; i++)
        	Remember.NewSense[i] = true;
    	// Remember in action         *******************************************
        try {
            stepBarrierPast.await(); // synchronize nodes with the user - Start PastTime nodes 
            while (stepBarrierPast.getNumberWaiting() < stepBarrierPast.getParties() - 1)
                sleep(TimeWait);
        }
        catch (InterruptedException ex) { 
            return;
        }
        catch (BrokenBarrierException ex) { 
            return;
            }
		System.out.println("Past Remember..."); // REMOVE
		Emotions.Good = Remember.RecentMost;
    	brain.clearBusSleepPreviews();
   //     brain.clearBusesIn(false); // Clear senses buses except proSelf 
        // Decision in action *************************************************************
        try {
            stepBarrierDecision.await(); // Start REU Thread 
            while (stepBarrierDecision2.getNumberWaiting() < stepBarrierDecision2.getParties() - 1)
                sleep(TimeWait);
        }
        catch (InterruptedException ex) { 
            return;
        }
        catch (BrokenBarrierException ex) { 
            return;
            }
		System.out.println("Decision acted..."); // REMOVE
    	System.out.print("recent= "); //REMOVE
    	System.out.println(Remember.RecentMost); //REMOVE
    	System.out.print("goodQuality= "); //REMOVE
    	System.out.println(Remember.QualityMax); //REMOVE
    	System.out.print("goodId= "); //REMOVE
    	System.out.println(Remember.QualityWho); //REMOVE
        display(true); // shows in screen
        // Sleep in action ****************************************************************
        brain.clearBusSleepPreviews();
        brain.clearBusSleepPreviews2();
		Anxiety = false;
		Saudade = false;
		Brain.Best = 0;
		setBom(Emotions.Good);
		Brain.BusSleepPreviews.add(new Signal(Bom,
				Cerebellum.ESCape,
				brain.past.time[getBom()][Brain.ProSelf].finalQuality));
    	for (i=0; i<brain.past.time[getBom()][0].learn.size(); i++)
    		Brain.BusSleepPreviews2.add(new Signal(brain.past.time[getBom()][0].getLearn(i).emitter,
    				Bom,
    				brain.past.time[getBom()][Brain.ProSelf].finalQuality));
		profoundity = 0;
		while (profoundity < PROFOUNDITY) {
			brain.pollSleepOut();
			brain.pollSleepOut2();
	        try {
	            stepBarrierSleep.await(); // Start Decision Thread 
	            while (stepBarrierSleep.getNumberWaiting() < stepBarrierSleep.getParties() - 1)
	                sleep(TimeWait);
	        }
	        catch (InterruptedException ex) { 
	        }
	        catch (BrokenBarrierException ex) { 
            }
			profoundity += 1;

	        Saudade = brain.decide(Remember.QualityWho, profoundity);

			brain.transferOutIn();
			
			System.out.println("SONO acting..."); // REMOVE
			Anxiety = brain.decide2(Remember.QualityWho, profoundity);

			brain.transferOutIn2();
		}

		if (brain.past.time[getBom()][Brain.ProSelf].home && Brain.BackHome) {
			Brain.setBackHome(false);
    		Liberu.jButtonHAct.setBackground(Color.ORANGE);
		}
		
   		if (Brain.Energy)
   			brain.past.time[getBom()][Brain.ProSelf].setHome(true);

       	if (Saudade) {
			veloc = 0.75F; // Twice fast
			Interface.Ink = 3;
		}
		if (Anxiety) {
			veloc = 1.5F; // Twice fast
			Interface.Ink = 3;
		}
/*    	if (getBom() != Cerebellum.ESCape &&
	    		brain.past.time[getBom()][Cortex.Senses-1].finalQuality < -Emotions.Normal) {
	    		setBom(Cerebellum.ESCape);
				brain.decision.emotionAfraid();
				brain.decision.emotionSpeak();
	    	}
*/    	exec = Bom;
		brain.send(exec);
    	if (brain.past.time[0][0].step < 2) {
        	if (brain.past.time[0][0].step == 0)
        		lastExec = exec;
        	else
        		if (Math.abs(beforeLastExec) != Cerebellum.ESCape) {
        			brain.past.time[Math.abs(beforeLastExec)][0].standby2 = true;
        			beforeLastExec = lastExec;
        		}
    	}
    	else
    		if (Math.abs(beforeLastExec) != Cerebellum.ESCape) {
    			brain.past.time[Math.abs(beforeLastExec)][0].standby2 = true;
    		}
		beforeLastExec = lastExec;
		lastExec = exec;
        System.out.print("Bom = ");
        System.out.println(Bom);
		// Cerebellum in action *********************************************************
        try {
            stepBarrierCerebellum.await(); // Start REU Thread 
            while (stepBarrierCerebellum.getNumberWaiting() < stepBarrierCerebellum.getParties() - 1)
                sleep(TimeWait);
        }
        catch (InterruptedException ex) { 
            return;
        }
        catch (BrokenBarrierException ex) { 
            return;
            }
		System.out.println("Cerebellum acted..."); // REMOVE
        // Acting Nodes in action *******************************************************
        try {
            stepBarrierAct.await(); // Start Act Nodes 
            while (stepBarrierAct.getNumberWaiting() < stepBarrierAct.getParties() - 1)
                sleep(TimeWait);
        }
        catch (InterruptedException ex) { 
            return;
        }
        catch (BrokenBarrierException ex) { 
            return;
            }
		System.out.println("Acting Nodes..."); // REMOVE
        // Decision in action Parte 2 *********************************************************
        try {
            stepBarrierDecision2.await(); // Start Decision Thread 
            while (stepBarrierDecision.getNumberWaiting() < stepBarrierDecision.getParties() - 1)
                sleep(TimeWait);
        }
        catch (InterruptedException ex) { 
            return;
        }
        catch (BrokenBarrierException ex) { 
            return;
            }
		System.out.println("Decision 2 acted..."); // REMOVE
        
        // Real Time
		brain.act(Action); // Liberu act
        display(true); // shows in screen
        actNXT(veloc);
        veloc = 1;
        // ENDING OF Slice LIVING
    }

    public void continueRun() {
        
        System.out.println("Continue...");
        pause = false;
    }
    
    public void pause() {
        
        System.out.println("Pause...");
        pause = true;
    }
    
    public void setToEnd() {
        
    	stepBarrierAct.reset();
    	stepBarrierDecision.reset();
    	stepBarrierDecision2.reset();
    	stepBarrierPast.reset();
    	stepBarrierCerebellum.reset();
    	stepBarrierSleep.reset();
    }
    
    public void run() {
        
		NXTCommand.open(); // Liberu interact with world - NXT MindStorms LEGO Body
    	if (!OpenData)
    		brain.past.bornTime();
    	
		bottom = new TouchSensor(SensorPort.S1);
		ligth = new LightSensor(SensorPort.S3);
		ultraSonic = new UltrasonicSensor(SensorPort.S4);
		sound = new SoundSensor(SensorPort.S2);

		Motor.A.setSpeed(150); // 900 = 100% power
		Motor.B.setSpeed(150); // degrees/s

        stepBarrierPast  = brain.initiatePastTime();
        stepBarrierDecision   = brain.initiateDecision();
        stepBarrierDecision2  = brain.initiateDecision2();
        stepBarrierCerebellum = brain.initiateCerebellum();
        stepBarrierSleep 	 = brain.initiateSleep();
		stepBarrierAct   = brain.initiateAct();
		try {
            while(!interrupted() && !Neuron.end)
                if (pause)
                    sleep(200); // Waits 200ms by 200ms for the key <Continue> pressed
                else {
                	brain.past.startTime();
                    runStepBrain();
//                    sleep(1500); // adjust
                    brain.past.addTime();
                }
        }
        catch(InterruptedException e) {
        }
		NXTCommand.close();
        System.out.println("End of Mind Thread...");
    }
}