using System;
using System.Collections.Generic;
using System.Linq;
using Clarion;
using Clarion.Framework;
using Clarion.Framework.Core;
using Clarion.Framework.Templates;
using VRepProxy.Objects;
using System.Threading;

namespace ClarionSimulation
{
	public delegate Perceptions InputVisualSensorialInformationEventHandler();
    public delegate void OutputActionChunkEventHandler(ClarionAgentActionType externalAction);

    public enum ClarionAgentActionType
    {
        DO_NOTHING,
        ROTATE,
		MOVE,
		LEFT,
		RIGHT,
		BACK,

    }

    public class ClarionAgent
    {
		#region Constants
        private String DIMENSION_SENSOR = "SensorialInformation";
		private String DIMENSION_TARGET_AHEAD = "TargetAhead";
		private String DIMENSION_SOMETHING_AT_FRONT = "SomethingAtFront";
		private String DIMENSION_SOMETHING_AT_LEFT = "SomethingAtLeft";
		private String DIMENSION_SOMETHING_AT_RIGHT = "SomethingAtRight";
		private String DIMENSION_SOMETHING_AT_BACK = "SomethingAtBack";
        #endregion

        #region Properties

        #region Simulation
        public double MaxNumberOfCognitiveCycles = -1;
        private double CurrentCognitiveCycle = 0;
        public Int32 TimeBetweenCognitiveCycles = 5;
        private Thread runThread;
        #endregion

        #region Agent
        private Clarion.Framework.Agent CurrentAgent;

        /// For each cognitive cycle, this event will 
		/// be called in order to the agent 
		/// receives the current sensorial information
        public event InputVisualSensorialInformationEventHandler OnNewVisualSensorialInformation;

        /// For each cognitive cycle, this event will 
		/// be called when the agent selects one action
        public event OutputActionChunkEventHandler OnNewExternalActionSelected;

        #endregion

        #region Perception Input
		private DimensionValuePair InputTargetAhead;
		private DimensionValuePair InputSomethingAtFront;
		#endregion

        #region Action Output
		private ExternalActionChunk OutputMove;
		private ExternalActionChunk OutputLeft;
		private ExternalActionChunk OutputRight;
		private ExternalActionChunk OutputBack;
        #endregion

        #endregion

        #region Constructor

        public ClarionAgent()
        {
            // Initialize the agent
            CurrentAgent = World.NewAgent("Current Agent");

            // Initialize Input Information
			InputTargetAhead = World.NewDimensionValuePair(DIMENSION_SENSOR, DIMENSION_TARGET_AHEAD);
			InputSomethingAtFront = World.NewDimensionValuePair(DIMENSION_SENSOR, DIMENSION_SOMETHING_AT_FRONT);

            // Initialize Output actions
			OutputMove = World.NewExternalActionChunk(ClarionAgentActionType.MOVE.ToString());

			OutputLeft = World.NewExternalActionChunk(ClarionAgentActionType.LEFT.ToString());
			OutputRight = World.NewExternalActionChunk(ClarionAgentActionType.RIGHT.ToString());
			OutputBack = World.NewExternalActionChunk(ClarionAgentActionType.BACK.ToString());

            //Create thread to simulation
            runThread = new Thread(RunThread);
			Console.WriteLine("Agent started");
        }

        #endregion

        #region Public Methods
        public void Run()
        {                
			Console.WriteLine ("Running Simulation ...");
            // Setup Agent to run
            if (runThread != null && !runThread.IsAlive)
            {
                SetupAgentInfraStructure();

                if (OnNewVisualSensorialInformation != null)
                {
                    // Start Simulation Thread                
                    runThread.Start(null);
                }
            }
        }
			
        public void Abort(Boolean deleteAgent)
        {   Console.WriteLine ("Aborting ...");
            if (runThread != null && runThread.IsAlive)
            {
                runThread.Abort();
            }

            if (CurrentAgent != null && deleteAgent)
            {
                CurrentAgent.Die();
            }
        }
        #endregion

        #region Setup Agent Methods

        /// <summary>
        /// Setup agent infra structure (ACS, NACS, MS and MCS)
        /// </summary>
        private void SetupAgentInfraStructure()
        {
            // Setup the ACS Subsystem
            SetupACS();                    
        }

        private void SetupMS()
        {            
            //RichDrive
        }

        /// <summary>
        /// Setup the ACS subsystem
        /// </summary>
        private void SetupACS()
        {
            //SupportCalculator avoidCollisionWallSupportCalculator = FixedRuleDelegateToAvoidColissionWall;
            //FixedRule ruleAvoidCollisionWallLeft = AgentInitializer.InitializeActionRule(CurrentAgent, FixedRule.Factory, OutputRotate, avoidCollisionWallSupportCalculator);
			//CurrentAgent.Commit(ruleAvoidCollisionWallLeft);

			SupportCalculator moveSupportCalculator = FixedRuleDelegateMove;
			FixedRule ruleMove = AgentInitializer.InitializeActionRule(CurrentAgent, FixedRule.Factory, OutputMove, moveSupportCalculator);
			CurrentAgent.Commit(ruleMove);

			SupportCalculator avoidCollisionFrontSupportCalculator = FixedRuleDelegateAvoidColissionAtFront;
			FixedRule ruleAvoidCollisionFront = AgentInitializer.InitializeActionRule(CurrentAgent, FixedRule.Factory, OutputBack, avoidCollisionFrontSupportCalculator);
			CurrentAgent.Commit(ruleAvoidCollisionFront);

            // Disable Rule Refinement
            CurrentAgent.ACS.Parameters.PERFORM_RER_REFINEMENT = false;

            // The selectio type will be probabilistic
            CurrentAgent.ACS.Parameters.LEVEL_SELECTION_METHOD = ActionCenteredSubsystem.LevelSelectionMethods.STOCHASTIC;

            // The selection of the action will be fixed (not variable) i.e. only the statement defined above.
            CurrentAgent.ACS.Parameters.LEVEL_SELECTION_OPTION = ActionCenteredSubsystem.LevelSelectionOptions.FIXED;

            // Define Probabilistic values
            CurrentAgent.ACS.Parameters.FIXED_FR_LEVEL_SELECTION_MEASURE = 1;
            CurrentAgent.ACS.Parameters.FIXED_IRL_LEVEL_SELECTION_MEASURE = 0;
            CurrentAgent.ACS.Parameters.FIXED_BL_LEVEL_SELECTION_MEASURE = 0;
            CurrentAgent.ACS.Parameters.FIXED_RER_LEVEL_SELECTION_MEASURE = 0;
        }

		//Robot robot;

        /// <summary>
        /// Make the agent perception. In other words, translate the information that came from sensors to a new type that the agent can understand
        /// </summary>
        /// <param name="sensorialInformation">The information that came from server</param>
        /// <returns>The perceived information</returns>
		private SensoryInformation MakePerceptionFromSensorialInput(Perceptions sensorialInformation)
        {
            SensoryInformation si = World.NewSensoryInformation(CurrentAgent);

			double[] d = sensorialInformation.getPerceivedDistance ();

			//Coligion at Front 
			// 00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15
			//  0,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0
			//Coligion at Left 
			// 00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15
			//  0,  0,  0,  0,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0

			//Coligion at Right 
			// 00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15
			//  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1

			//Go to Target 
			Double minFront = (new Double [6] {d[1], d[2], d[3], d[4], d[5], d[6]}).Min ();

			String color = sensorialInformation.getPerceivedColor ();
			Boolean target = ((color == Shape.SHAPE_BLUE) || (color == Shape.SHAPE_RED) || (color == Shape.SHAPE_PINK)) && (minFront > 0.2);
			double targetAheadActivationValue = target ? CurrentAgent.Parameters.MAX_ACTIVATION : CurrentAgent.Parameters.MIN_ACTIVATION;
			si.Add(InputTargetAhead, targetAheadActivationValue);
		
			Boolean somethingAtFront =  minFront < 0.2 ;
			double somethingAtFrontActivationValue = somethingAtFront ? CurrentAgent.Parameters.MAX_ACTIVATION : CurrentAgent.Parameters.MIN_ACTIVATION;
			si.Add (InputSomethingAtFront, somethingAtFrontActivationValue);

            return si;
        }

        #endregion

        #region Delegate Methods

        #region Fixed Rules

        private double FixedRuleDelegateAvoidColissionAtFront(ActivationCollection currentInput, Rule target)
        {
			return ((currentInput.Contains(InputSomethingAtFront, CurrentAgent.Parameters.MAX_ACTIVATION))) ? 1.0 : 0.0;
        }

		private double FixedRuleDelegateMove(ActivationCollection currentInput, Rule target)
		{
			return ((currentInput.Contains(InputTargetAhead, CurrentAgent.Parameters.MAX_ACTIVATION))) ? 1.0 : 0.0;
		}

        #endregion

        #region Run Thread Method

        private void RunThread(object obj)
        {
			Console.WriteLine("Starting Cognitive Cycle ... press CTRL-C to finish !");
            // Cognitive Cycle starts here getting sensorial information
            while (CurrentCognitiveCycle != MaxNumberOfCognitiveCycles)
            {   
                // Get current sensorial information                    
				Perceptions sensorialInformation = OnNewVisualSensorialInformation();

                // Make the perception
                SensoryInformation si = MakePerceptionFromSensorialInput(sensorialInformation);

                //Perceive the sensory information
                CurrentAgent.Perceive(si);

                //Choose an action
                ExternalActionChunk chosen = CurrentAgent.GetChosenExternalAction(si);

                // Get the selected action
                String actionLabel = chosen.LabelAsIComparable.ToString();
                ClarionAgentActionType actionType = (ClarionAgentActionType)Enum.Parse(typeof(ClarionAgentActionType), actionLabel, true);

                // Call the output event handler
                if (OnNewExternalActionSelected != null)
                {
                    OnNewExternalActionSelected(actionType);
                }

                // Increment the number of cognitive cycles
                CurrentCognitiveCycle++;

                //Wait to the agent accomplish his job
                if (TimeBetweenCognitiveCycles > 0)
                {
                    Thread.Sleep(TimeBetweenCognitiveCycles);
                }
            }
        }

        #endregion

        #endregion
    }
}
