<commit_before>package org.metacsp.examples.meta;

import java.awt.Rectangle;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.Vector;
import java.util.logging.Level;

import org.metacsp.dispatching.DispatchingFunction;
import org.metacsp.framework.Constraint;
import org.metacsp.framework.ConstraintNetwork;
import org.metacsp.framework.ValueOrderingH;
import org.metacsp.framework.VariableOrderingH;
import org.metacsp.meta.hybridPlanner.FluentBasedSimpleDomain;
import org.metacsp.meta.hybridPlanner.MetaOccupiedConstraint;
import org.metacsp.meta.hybridPlanner.MetaSpatialAdherenceConstraint;
import org.metacsp.meta.hybridPlanner.SensingSchedulable;
import org.metacsp.meta.hybridPlanner.SimpleHybridPlanner;
import org.metacsp.meta.simplePlanner.ProactivePlanningDomain;
import org.metacsp.meta.simplePlanner.SimplePlanner;
import org.metacsp.meta.simplePlanner.SimpleDomain.markings;
import org.metacsp.meta.symbolsAndTime.Schedulable;
import org.metacsp.multi.activity.Activity;
import org.metacsp.multi.activity.ActivityNetworkSolver;
import org.metacsp.multi.allenInterval.AllenIntervalConstraint;
import org.metacsp.multi.spatial.rectangleAlgebra.RectangleConstraint;
import org.metacsp.multi.spatial.rectangleAlgebra.RectangularRegion;
import org.metacsp.multi.spatial.rectangleAlgebra.UnaryRectangleConstraint;
import org.metacsp.multi.spatial.rectangleAlgebraNew.toRemove.OntologicalSpatialProperty;
import org.metacsp.multi.spatioTemporal.SpatialFluent;
import org.metacsp.multi.spatioTemporal.SpatialFluentSolver;
import org.metacsp.sensing.ConstraintNetworkAnimator;
import org.metacsp.sensing.Controllable;
import org.metacsp.spatial.utility.SpatialAssertionalRelation;
import org.metacsp.spatial.utility.SpatialRule;
import org.metacsp.time.APSPSolver;
import org.metacsp.time.Bounds;
import org.metacsp.utility.logging.MetaCSPLogging;
import org.metacsp.utility.timelinePlotting.TimelinePublisher;
import org.metacsp.utility.timelinePlotting.TimelineVisualizer;



public class TestHybridPlanningWithSensingAndDispatching {

	static int pad = 0;    
	static long duration = 1000; //both for activity and location
	static long tick = 1000;
	static HashMap<String, SpatialAssertionalRelation> currentObservation = new HashMap<String, SpatialAssertionalRelation>();

	static MetaOccupiedConstraint metaOccupiedConstraint = null;
	static MetaSpatialAdherenceConstraint metaSpatialAdherence  = null;
	static SensingSchedulable sensingSchedulable = null;
	
	static int counter = 0;
	public static void main(String[] args) {

		//Create planner
		final SimpleHybridPlanner simpleHybridPlanner = new SimpleHybridPlanner(0,100000,0);
		//MetaCSPLogging.setLevel(planner.getClass(), Level.FINEST);

		FluentBasedSimpleDomain.parseDomain(simpleHybridPlanner, "domains/testSensingBeforePickAndPlaceDomain.ddl", FluentBasedSimpleDomain.class);
//		FluentBasedSimpleDomain.parseDomain(simpleHybridPlanner, "domains/After.ddl", FluentBasedSimpleDomain.class);
//      FluentBasedSimpleDomain.parseDomain(simpleHybridPlanner, "domains/tray.ddl", FluentBasedSimpleDomain.class);
//		FluentBasedSimpleDomain.parseDomain(simpleHybridPlanner, "domains/1.ddl", FluentBasedSimpleDomain.class);
		
		ConstraintNetworkAnimator animator = new ConstraintNetworkAnimator(simpleHybridPlanner, tick);

		//Most critical conflict is the one with most activities 
		VariableOrderingH varOH = new VariableOrderingH() {
			@Override
			public int compare(ConstraintNetwork arg0, ConstraintNetwork arg1) {
				return arg1.getVariables().length - arg0.getVariables().length;
			}
			@Override
			public void collectData(ConstraintNetwork[] allMetaVariables) { }
		};
		// no value ordering
		ValueOrderingH valOH = new ValueOrderingH() {
			@Override
			public int compare(ConstraintNetwork o1, ConstraintNetwork o2) { return 0; }
		};
		metaSpatialAdherence = new MetaSpatialAdherenceConstraint(varOH, valOH);
		final SpatialFluentSolver groundSolver = (SpatialFluentSolver)simpleHybridPlanner.getConstraintSolvers()[0];

		MetaCSPLogging.setLevel(SimpleHybridPlanner.class, Level.FINEST);
		MetaCSPLogging.setLevel(MetaSpatialAdherenceConstraint.class, Level.FINEST);
		//#################################################################################################################
		//add metaOccupiedConstraint
		metaOccupiedConstraint = new MetaOccupiedConstraint(null, null);
		metaOccupiedConstraint.setPad(pad);
		//#################################################################################################################
		//add sensor schedulable
		sensingSchedulable = new SensingSchedulable(null, null);
		
		//#################################################################################################################
		//add spatial general rule to MetaSpatialFluentConstraint
		Vector<SpatialRule> srules = new Vector<SpatialRule>();
		getSpatialKnowledge(srules);
		metaSpatialAdherence.setSpatialRules(srules.toArray(new SpatialRule[srules.size()]));

		//#################################################################################################################
		//Set initial situation
		metaSpatialAdherence.setInitialGoal(new String[]{"at_cup1_table1"});


		Activity act = getCreatedActivty(groundSolver, "atLocation::at_cup1_table1()--(0,0,0,0)++true");
		act.setMarking(markings.UNJUSTIFIED);

		
		Activity two = (Activity)groundSolver.getConstraintSolvers()[1].createVariable("atLocation");
		two.setSymbolicDomain("at_robot1_counter1()");
		two.setMarking(markings.JUSTIFIED);
		
		long releaseTime = 1000;
		releaseActivity(groundSolver, releaseTime, two);
		addDurationToActivity(groundSolver, duration, two);
		
		//##############################################################################################################
		//add meta constraint to hybrid planner
//		simpleHybridPlanner.addMetaConstraint(sensingSchedulable);
		
		
		simpleHybridPlanner.addMetaConstraint(metaOccupiedConstraint);		
		simpleHybridPlanner.addMetaConstraint(metaSpatialAdherence);

		//################################################################################################################
		Controllable contrallableAtLocation = new Controllable();
		
		//Two culprit
		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_cup1_counter1()--(1,2,3,4)++true");
		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_table1_table1()--(0,60,0,99)++false");
		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_fork1_table1()--(20,26,13,32)++true");
		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_knife1_table1()--(30,36,10,33)++true");

		//One culprit
//		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_cup1_counter1()--(1,2,3,4)++true");
//		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_table1_table1()--(0,100,0,99)++false");
//		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_fork1_table1()--(31,37,13,32)++true");
//		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_knife1_table1()--(40,46,10,33)++true");

		
		final Vector<String> ctrls = contrallableAtLocation.getContrallbaleSymbols();

		//#######################################################################
		final Vector<Activity> executingActs = new Vector<Activity>();

		Vector<DispatchingFunction> dispatches = new Vector<DispatchingFunction>();
		DispatchingFunction df = new DispatchingFunction("RobotAction") {
			@Override
			public void dispatch(Activity act) {
				System.out.println(">>>>>>>>>>>>>> Dispatched " + act);
//				//#####################################################################################################################
//				ActivityNetworkSolver actSolver = ((ActivityNetworkSolver)((SpatialFluentSolver)simpleHybridPlanner.getConstraintSolvers()[0]).getConstraintSolvers()[1]);
//				//sort Activity based on the start time for debugging purpose
//				HashMap<Activity, Long> starttimes = new HashMap<Activity, Long>();
//				for (int i = 0; i < actSolver.getVariables().length; i++) {
//					starttimes.put((Activity) actSolver.getVariables()[i], ((Activity)actSolver.getVariables()[i]).getTemporalVariable().getStart().getLowerBound());                       
//				}
//
//				//          Collections.sort(starttimes.values());
//				starttimes =  sortHashMapByValuesD(starttimes);
//				for (Activity act0 : starttimes.keySet()) {
//					System.out.println(act0 + " --> " + starttimes.get(act0));
//				}
//				//#####################################################################################################################
				
				executingActs.add(act);

			}
		};
		dispatches.add(df);

		DispatchingFunction dfSense = new DispatchingFunction("RobotSense") {
			@Override
			public void dispatch(Activity act) {
				System.out.println(">>>>>>>>>>>>>> Dispatched " + act);
				executingActs.add(act);
				if(counter == 0){
//					getCreatedActivty(groundSolver, ctrls.get(0));
					releaseActivity(groundSolver, act.getTemporalVariable().getLST() + 1000, getCreatedActivty(groundSolver, ctrls.get(0)));
					Vector<SpatialAssertionalRelation> saRelations = new Vector<SpatialAssertionalRelation>(); 
					for (String st : currentObservation.keySet()) saRelations.add(currentObservation.get(st));
					metaSpatialAdherence.setSpatialAssertionalRelations(saRelations);
					counter ++;
				}
				else if(counter == 1){
//					ConstraintNetwork.draw(((SpatialFluentSolver)simpleHybridPlanner.getConstraintSolvers()[0]).getConstraintSolvers()[0].getConstraintNetwork(), "RA Constraint Network");					
//					getCreatedActivty(groundSolver, ctrls.get(1));
//					getCreatedActivty(groundSolver, ctrls.get(2));
//					getCreatedActivty(groundSolver, ctrls.get(3));
					
					releaseActivity(groundSolver, act.getTemporalVariable().getLST() , getCreatedActivty(groundSolver, ctrls.get(1)));
					releaseActivity(groundSolver, act.getTemporalVariable().getLST() , getCreatedActivty(groundSolver, ctrls.get(2)));
					releaseActivity(groundSolver, act.getTemporalVariable().getLST() , getCreatedActivty(groundSolver, ctrls.get(3)));
					Vector<SpatialAssertionalRelation> saRelations = new Vector<SpatialAssertionalRelation>(); 
					for (String st : currentObservation.keySet()) saRelations.add(currentObservation.get(st));
					metaSpatialAdherence.setSpatialAssertionalRelations(saRelations);	
					counter++;
				}
			}
		}; 
		dispatches.add(dfSense);

		animator.addDispatchingFunctions(simpleHybridPlanner, dispatches.toArray(new DispatchingFunction[dispatches.size()]));

		//######################################################################
		//Timeline
		ActivityNetworkSolver actSolver = ((ActivityNetworkSolver)((SpatialFluentSolver)simpleHybridPlanner.getConstraintSolvers()[0]).getConstraintSolvers()[1]);<commit_msg>change initial condition of hybrid test case --iran<commit_after>package org.metacsp.examples.meta;

import java.awt.Rectangle;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.Vector;
import java.util.logging.Level;

import org.metacsp.dispatching.DispatchingFunction;
import org.metacsp.framework.Constraint;
import org.metacsp.framework.ConstraintNetwork;
import org.metacsp.framework.ValueOrderingH;
import org.metacsp.framework.VariableOrderingH;
import org.metacsp.meta.hybridPlanner.FluentBasedSimpleDomain;
import org.metacsp.meta.hybridPlanner.MetaOccupiedConstraint;
import org.metacsp.meta.hybridPlanner.MetaSpatialAdherenceConstraint;
import org.metacsp.meta.hybridPlanner.SensingSchedulable;
import org.metacsp.meta.hybridPlanner.SimpleHybridPlanner;
import org.metacsp.meta.simplePlanner.ProactivePlanningDomain;
import org.metacsp.meta.simplePlanner.SimplePlanner;
import org.metacsp.meta.simplePlanner.SimpleDomain.markings;
import org.metacsp.meta.symbolsAndTime.Schedulable;
import org.metacsp.multi.activity.Activity;
import org.metacsp.multi.activity.ActivityNetworkSolver;
import org.metacsp.multi.allenInterval.AllenIntervalConstraint;
import org.metacsp.multi.spatial.rectangleAlgebra.RectangleConstraint;
import org.metacsp.multi.spatial.rectangleAlgebra.RectangularRegion;
import org.metacsp.multi.spatial.rectangleAlgebra.UnaryRectangleConstraint;
import org.metacsp.multi.spatial.rectangleAlgebraNew.toRemove.OntologicalSpatialProperty;
import org.metacsp.multi.spatioTemporal.SpatialFluent;
import org.metacsp.multi.spatioTemporal.SpatialFluentSolver;
import org.metacsp.sensing.ConstraintNetworkAnimator;
import org.metacsp.sensing.Controllable;
import org.metacsp.spatial.utility.SpatialAssertionalRelation;
import org.metacsp.spatial.utility.SpatialRule;
import org.metacsp.time.APSPSolver;
import org.metacsp.time.Bounds;
import org.metacsp.utility.logging.MetaCSPLogging;
import org.metacsp.utility.timelinePlotting.TimelinePublisher;
import org.metacsp.utility.timelinePlotting.TimelineVisualizer;



public class TestHybridPlanningWithSensingAndDispatching {

	static int pad = 0;    
	static long duration = 1000; //both for activity and location
	static long tick = 1000;
	static HashMap<String, SpatialAssertionalRelation> currentObservation = new HashMap<String, SpatialAssertionalRelation>();

	static MetaOccupiedConstraint metaOccupiedConstraint = null;
	static MetaSpatialAdherenceConstraint metaSpatialAdherence  = null;
	static SensingSchedulable sensingSchedulable = null;
	
	static int counter = 0;
	public static void main(String[] args) {

		//Create planner
		final SimpleHybridPlanner simpleHybridPlanner = new SimpleHybridPlanner(0,100000,0);
		//MetaCSPLogging.setLevel(planner.getClass(), Level.FINEST);

		FluentBasedSimpleDomain.parseDomain(simpleHybridPlanner, "domains/testSensingBeforePickAndPlaceDomain.ddl", FluentBasedSimpleDomain.class);
//		FluentBasedSimpleDomain.parseDomain(simpleHybridPlanner, "domains/After.ddl", FluentBasedSimpleDomain.class);
//      FluentBasedSimpleDomain.parseDomain(simpleHybridPlanner, "domains/tray.ddl", FluentBasedSimpleDomain.class);
//		FluentBasedSimpleDomain.parseDomain(simpleHybridPlanner, "domains/1.ddl", FluentBasedSimpleDomain.class);
		
		ConstraintNetworkAnimator animator = new ConstraintNetworkAnimator(simpleHybridPlanner, tick);

		//Most critical conflict is the one with most activities 
		VariableOrderingH varOH = new VariableOrderingH() {
			@Override
			public int compare(ConstraintNetwork arg0, ConstraintNetwork arg1) {
				return arg1.getVariables().length - arg0.getVariables().length;
			}
			@Override
			public void collectData(ConstraintNetwork[] allMetaVariables) { }
		};
		// no value ordering
		ValueOrderingH valOH = new ValueOrderingH() {
			@Override
			public int compare(ConstraintNetwork o1, ConstraintNetwork o2) { return 0; }
		};
		metaSpatialAdherence = new MetaSpatialAdherenceConstraint(varOH, valOH);
		final SpatialFluentSolver groundSolver = (SpatialFluentSolver)simpleHybridPlanner.getConstraintSolvers()[0];

		MetaCSPLogging.setLevel(SimpleHybridPlanner.class, Level.FINEST);
		MetaCSPLogging.setLevel(MetaSpatialAdherenceConstraint.class, Level.FINEST);
		//#################################################################################################################
		//add metaOccupiedConstraint
		metaOccupiedConstraint = new MetaOccupiedConstraint(null, null);
		metaOccupiedConstraint.setPad(pad);
		//#################################################################################################################
		//add sensor schedulable
		sensingSchedulable = new SensingSchedulable(null, null);
		
		//#################################################################################################################
		//add spatial general rule to MetaSpatialFluentConstraint
		Vector<SpatialRule> srules = new Vector<SpatialRule>();
		getSpatialKnowledge(srules);
		metaSpatialAdherence.setSpatialRules(srules.toArray(new SpatialRule[srules.size()]));

		//#################################################################################################################
		
		metaSpatialAdherence.setInitialGoal(new String[]{"at_cup1_table1"});


		Activity act = getCreatedActivty(groundSolver, "atLocation::at_cup1_table1()--(0,0,0,0)++true");
		act.setMarking(markings.UNJUSTIFIED);

		//################################################################################################
		//Set initial situation
//		Activity two = (Activity)groundSolver.getConstraintSolvers()[1].createVariable("atLocation");
//		two.setSymbolicDomain("at_robot1_counter1()");
//		two.setMarking(markings.JUSTIFIED);
		

		Activity two = (Activity)groundSolver.getConstraintSolvers()[1].createVariable("atLocation");
		two.setSymbolicDomain("at_robot1_table1()");
		two.setMarking(markings.JUSTIFIED);
		
		long releaseTime = 1000;
		releaseActivity(groundSolver, releaseTime, two);
		addDurationToActivity(groundSolver, duration, two);
		
		//##############################################################################################################
		//add meta constraint to hybrid planner
//		simpleHybridPlanner.addMetaConstraint(sensingSchedulable);
		
		
		simpleHybridPlanner.addMetaConstraint(metaOccupiedConstraint);		
		simpleHybridPlanner.addMetaConstraint(metaSpatialAdherence);

		//################################################################################################################
		Controllable contrallableAtLocation = new Controllable();
		
		//Two culprit
//		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_cup1_counter1()--(1,2,3,4)++true");
//		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_table1_table1()--(0,60,0,99)++false");
//		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_fork1_table1()--(20,26,13,32)++true");
//		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_knife1_table1()--(30,36,10,33)++true");

		//One culprit
		//contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_cup1_counter1()--(1,2,3,4)++true");
		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_cup1_tray1()--(1,2,3,4)++true");
		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_table1_table1()--(0,100,0,99)++false");
		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_fork1_table1()--(31,37,13,32)++true");
		contrallableAtLocation.registerSymbolsFromControllableSensor("atLocation::at_knife1_table1()--(40,46,10,33)++true");

		
		final Vector<String> ctrls = contrallableAtLocation.getContrallbaleSymbols();

		//#######################################################################
		final Vector<Activity> executingActs = new Vector<Activity>();

		Vector<DispatchingFunction> dispatches = new Vector<DispatchingFunction>();
		DispatchingFunction df = new DispatchingFunction("RobotAction") {
			@Override
			public void dispatch(Activity act) {
				System.out.println(">>>>>>>>>>>>>> Dispatched " + act);
//				//#####################################################################################################################
//				ActivityNetworkSolver actSolver = ((ActivityNetworkSolver)((SpatialFluentSolver)simpleHybridPlanner.getConstraintSolvers()[0]).getConstraintSolvers()[1]);
//				//sort Activity based on the start time for debugging purpose
//				HashMap<Activity, Long> starttimes = new HashMap<Activity, Long>();
//				for (int i = 0; i < actSolver.getVariables().length; i++) {
//					starttimes.put((Activity) actSolver.getVariables()[i], ((Activity)actSolver.getVariables()[i]).getTemporalVariable().getStart().getLowerBound());                       
//				}
//
//				//          Collections.sort(starttimes.values());
//				starttimes =  sortHashMapByValuesD(starttimes);
//				for (Activity act0 : starttimes.keySet()) {
//					System.out.println(act0 + " --> " + starttimes.get(act0));
//				}
//				//#####################################################################################################################
				
				executingActs.add(act);

			}
		};
		dispatches.add(df);

		DispatchingFunction dfSense = new DispatchingFunction("RobotSense") {
			@Override
			public void dispatch(Activity act) {
				System.out.println(">>>>>>>>>>>>>> Dispatched " + act);
				executingActs.add(act);
				if(counter == 0){
//					releaseActivity(groundSolver, act.getTemporalVariable().getLST() + 1000, getCreatedActivty(groundSolver, ctrls.get(0)));
//					Vector<SpatialAssertionalRelation> saRelations = new Vector<SpatialAssertionalRelation>(); 
//					for (String st : currentObservation.keySet()) saRelations.add(currentObservation.get(st));
//					metaSpatialAdherence.setSpatialAssertionalRelations(saRelations);
					
					releaseActivity(groundSolver, act.getTemporalVariable().getLST() + 1000, getCreatedActivty(groundSolver, ctrls.get(0)));
					Vector<SpatialAssertionalRelation> saRelations = new Vector<SpatialAssertionalRelation>(); 
					for (String st : currentObservation.keySet()) saRelations.add(currentObservation.get(st));
					metaSpatialAdherence.setSpatialAssertionalRelations(saRelations);

					counter ++;
				}
				else if(counter == 1){
//					ConstraintNetwork.draw(((SpatialFluentSolver)simpleHybridPlanner.getConstraintSolvers()[0]).getConstraintSolvers()[0].getConstraintNetwork(), "RA Constraint Network");					
//					getCreatedActivty(groundSolver, ctrls.get(1));
//					getCreatedActivty(groundSolver, ctrls.get(2));
//					getCreatedActivty(groundSolver, ctrls.get(3));
					
					releaseActivity(groundSolver, act.getTemporalVariable().getLST() , getCreatedActivty(groundSolver, ctrls.get(1)));
					releaseActivity(groundSolver, act.getTemporalVariable().getLST() , getCreatedActivty(groundSolver, ctrls.get(2)));
					releaseActivity(groundSolver, act.getTemporalVariable().getLST() , getCreatedActivty(groundSolver, ctrls.get(3)));
					Vector<SpatialAssertionalRelation> saRelations = new Vector<SpatialAssertionalRelation>(); 
					for (String st : currentObservation.keySet()) saRelations.add(currentObservation.get(st));
					metaSpatialAdherence.setSpatialAssertionalRelations(saRelations);	
					counter++;
				}
			}
		}; 
		dispatches.add(dfSense);

		animator.addDispatchingFunctions(simpleHybridPlanner, dispatches.toArray(new DispatchingFunction[dispatches.size()]));

		//######################################################################
		//Timeline
		ActivityNetworkSolver actSolver = ((ActivityNetworkSolver)((SpatialFluentSolver)simpleHybridPlanner.getConstraintSolvers()[0]).getConstraintSolvers()[1]);
{'fraction_non_alphanumeric': 0.20251075105338603, 'fraction_numerical': 0.01229312366969289, 'mean_word_length': 6.9358841778697, 'pattern_counts': {'":': 0, '<': 37, '<?xml version=': 0, '>': 93, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3232976', 'n_tokens_mistral': 7532, 'n_tokens_neox': 6679, 'n_words': 897}