import java.lang.*;
import java.util.*;
import java.awt.*;

/* here's a sequential plan. It ignores percepts, but tries to find a
   solution to a MazeWorld problem using a sequence of actions, despite
   any robot position uncertainty */
public class SequentialPlanner
{   
    int numActions;
    TextArea textArea;
    
    int maze[][][]; /* initial description of MazeWorld,
                            so we don't have to pass around the array */
    public SequentialPlanner(MazeWorld theWorld, TextArea textArea) {
        this.maze = theWorld.maze;
        this.numActions = 3;
        this.textArea = textArea;
    }
    
    public Vector doSeqPlan(Vector inits, Vector goals, 
                            int curDepth, int maxDepth) {
        Vector returnPlan;
        if (subsetOf(inits, goals)) return new Vector(); /* Done! null plan */
        if (curDepth == maxDepth) return null; /* failure */
        
        for (int i=0; i < numActions; i++) {
            returnPlan = doSeqPlan(acts(inits, i), goals, curDepth+1, maxDepth);
            if (returnPlan != null) {
                returnPlan.addElement(new Integer(i));
                return returnPlan;
            }
        } /* for */
        return null;  
    }
    
    /* this is the top-level function you should call 
        this does depth-first iterative deepening */
    public Vector SequentialPlan(Vector inits, Vector goals, int maxDepth) {
        Vector returnPlan;
        //printStateSet(inits);
        //printStateSet(goals);
        for (int i=0; i <= maxDepth; i++) {
            textArea.append("Searching depth = " + i+"\n");
            returnPlan = doSeqPlan(inits, goals, 0, i);
            if (returnPlan != null) { textArea.append("Plan returned!\n");
                // printPlan(returnPlan);
                return reverse(returnPlan);
            }            
        } /* end for */
        textArea.append("Darn. No plan found.\n");
        return null;
    }
    
    /* helper functions************************** */
    
    public boolean memberOf(int oneState[], Vector stateSet) {
        int memberState[];
        for (int i=0; i < stateSet.size(); i++) {
            memberState = (int [])stateSet.elementAt(i);
            if ((oneState[0] == memberState[0]) &&
                (oneState[1] == memberState[1]) &&
                (oneState[2] == memberState[2])) {
                    return true;
            } /* end if */
        } /* end for */
        return false;
    } /* memberOf() */
    
    public boolean subsetOf(Vector testSet, Vector bigSet) {
      int memberState[];
      for (int i=0; i < testSet.size(); i++) {
        memberState = (int [])testSet.elementAt(i);
        if (!(memberOf(memberState,bigSet))) {
            return false;
        }
      } /* end for */
      return true;
    } /* subsetOf() */
    
    public Vector reverse(Vector inVector) {
      Vector revVector = new Vector();
      for (int i= inVector.size() - 1; i >= 0; i--) {
        revVector.addElement(inVector.elementAt(i));
      } /* for */
      return revVector;
    } /* reverse() */
    
    public void printState(int theState[]) {
        textArea.append("Robot x: " + theState[0] + " y: " + theState[1] +
                            " dir: " + theState[2]+"\n");
    }
    
    public void printStateSet(Vector theStates) {
        int theState[];
        textArea.append("Printing state set:\n");
        for (int i= 0; i < theStates.size(); i++) {
            theState = (int []) theStates.elementAt(i);
            printState(theState);
        }
        textArea.append("\n");
    }
    
    public void printPlan(Vector thePlan) {
        Integer planStep;
        
        textArea.append("Printing out plan...\n");
        for (int i=0; i < thePlan.size(); i++) {
            planStep = (Integer) thePlan.elementAt(i);
            textArea.append("Action " + i + ": " + planStep.intValue()+"\n");
        } /* for */            
    } /* printPlan() */
    
    
    /* *************** ACT and ACTS, very important functions! ******** */
    
    /* takes a single state in the form of an array robot{x,y,dir}
        and returns a new state based on maze walls and action taken.
        Action 0 = forward, 1 = left, 2 = right */
    public int[] act(int roboState[], int action) {
        int resultState[] = new int[3];
        if (action == 1) {
            resultState[0] = roboState[0];
            resultState[1] = roboState[1];
            resultState[2] = roboState[2] + 1;
            if (resultState[2] == 4) resultState[2] = 0;
        } else if (action == 2) {
            resultState[0] = roboState[0];
            resultState[1] = roboState[1];
            resultState[2] = roboState[2] - 1;
            if (resultState[2] == -1) resultState[2] = 3;
        } else { /* action = 0 -- go forward */
            if (maze[roboState[0]][roboState[1]][roboState[2]] == 0) {
                /* successful move forward */
                resultState[0] = roboState[0];
                resultState[1] = roboState[1];
                resultState[2] = roboState[2];                
                switch (roboState[2]) {
                    case 0: resultState[1]++; break;
                    case 1: resultState[0]--; break;
                    case 2: resultState[1]--; break;
                    case 3: resultState[0]++; break;
                }
            } else {
                /* hit a wall! */
                resultState[0] = roboState[0];
                resultState[1] = roboState[1];
                resultState[2] = roboState[2];                
            }
        } /* endelse */  
        return resultState;
    } /* end of function act() */
    
    /* this function takes a state set (a vector) and an action
        and returns a new state set that is the result of doing
        that action in each state in the input state set */
    public Vector acts(Vector curStates, int action) {
        Vector returnStates = new Vector();
        int newState[];
        for (int i=0; i < curStates.size(); i++) {
            newState = act((int [])curStates.elementAt(i), action);
            if (! (memberOf(newState, returnStates))) {
                returnStates.addElement(newState);
            }
        } /* end for loop */ 
        return returnStates;
    } /* end of function acts() */
    
}