root/deus/RegressionPlanner.cpp

#include "RegressionPlanner.h"


int RegressionPlanner::MakeNewPlan(KeyFrameNode* key_frame, PlanNode*& retPlan, int timer)
{
        ClearStacks();
    retPlan = NULL;
        
        // we will assume that the Key-preconds are already satisfied
        // try to satisfy the condition for this key_frame node
        currently_seeking = key_frame;
    
    try{
                int x = ResumePlanning(retPlan, timer, false);
        ClearStacks();
        return x;
        }catch(TimeOutException tout) { 
        regression_root = PlanStack.tail();
        throw tout; 
    }
    regression_root = retPlan;
}


// throws TimeOutException
// returns 1 iff retPlan is set to a valid plan
int RegressionPlanner::ResumePlanning(PlanNode*& retPlan, int timer, bool use_stack)
{
        int ret = 1;
        if(currently_seeking->conditions.satisfied) return 1; // already satisfied
        
        try{
                if(!MakePlan(retPlan, use_stack)) 
                {
                        DeletePlan(retPlan);
                        ret = 0;
                }
                else // p now points to root of plan to achieve op's preconds
                {
                currently_seeking->conditions.satisfied = 1;    
                }
        regression_root = PlanStack.tail();
        }catch(TimeOutException){ return 0; }
        
    ClearStacks();
    regression_root = retPlan;
        return ret;
}

int RegressionPlanner::Replan(PlanNode*& retPlan, int timer)
{
    try{
        if(OpStack.empty())
        {   
            int x = MakeNewPlan(currently_seeking, retPlan, timer);
            regression_root = retPlan;
                        return x;
        }
        else
        {
             int y = ResumePlanning(retPlan, timer);
             regression_root = retPlan;
             return y;
        }
    }catch(TimeOutException tout) { 
        throw tout; 
        regression_root = PlanStack.tail();
    }
}

// private functions

// throws TimeOutException
int RegressionPlanner::MakePlan(PlanNode*& retPlan, bool use_stack)
{
        ResetTimer(); ///~~ use timer variable???

    int ret = 0;
    list<pair<WORLDSTATE,Condition> >::iterator bindings;

    try {
        // check all worldstates in the forward search (leaves only?)
    // for possible variable bindings.  If there are some, then 
    // we want to know which ones are satisfied with what values
    // in what worldstate.
    if(!use_stack)
    {
        possible_bindings = 
            GetWorldStates(currently_seeking->conditions);

            // check for whether there are any worldstates in which
            // all the preconds are satisified
        bindings = FindComplete(possible_bindings, currently_seeking->conditions);
    }
    if(!use_stack && bindings != possible_bindings.end())
    {
        // if any of the possible bindings completely satisfy condition,
        // SUCCEEED 
            AddForwardPart(retPlan, bindings->first);///~!*&
        return 1;
    }
    else
    {
                // go through the possible_bindings and try to plan back to 
                // one of the worldstates to satisfy the other parts of 
                // the condition for which variables do not yet have 
                // values
        if(!use_stack)
            satisfied_counts = CountAllSatisfied();
                
                while(!possible_bindings.empty())
                {
            list<pair<list<pair<WORLDSTATE,Condition> >::iterator, pair<int,int> > >::iterator 
                best = FindBestCount();
            if(PlanOnRest(retPlan, *best->first, best->second.second, use_stack))
            {
                AddForwardPart(retPlan, best->first->first);
                                return 1; // planning success
            }
            use_stack = false; // the rest of the calls to PlanOnRest will cover new territory only
                        possible_bindings.erase(best->first);
            satisfied_counts.erase(best);
                }
                ret = 0;
    }
    
    return ret;
        // if timeout, save EVERYTHING and throw TimeOutException
    }catch(TimeOutException timeout) {
        SaveAll(); throw timeout;
    }
}

list<pair<list<pair<WORLDSTATE,Condition> >::iterator, pair<int,int> > >::iterator 
RegressionPlanner::FindBestCount(void)
{
    list<pair<list<pair<WORLDSTATE,Condition> >::iterator, pair<int,int> > >::iterator 
        b(satisfied_counts.begin()), best(satisfied_counts.begin());
    while(++b != satisfied_counts.end())
        if(b->second.first > best->second.first)
            best =  b;
    return best;
}

list<pair<list<pair<WORLDSTATE,Condition> >::iterator,pair<int,int> > > RegressionPlanner::CountAllSatisfied(void)
{
    list<pair<list<pair<WORLDSTATE,Condition> >::iterator,pair<int,int> > > ret;
    pair<int,int> satisfied;
    list<pair<WORLDSTATE, Condition> >::iterator b = possible_bindings.begin();
        for(;b!=possible_bindings.end(); ++b)
        {
        ret.push_back(pair<list<pair<WORLDSTATE,Condition> >::iterator,pair<int,int> >(b, CountSatisfied(b->second)));
        }
    return ret;
}

// the first in the pair is the number of objects filled in in the 
// best set of bindings of the condition
// the second in the pair is the index of the best set of bindings in the condition
pair<int,int> RegressionPlanner::CountSatisfied(Condition& cond)
{
        int best = 0;
    int num_best = _CountSatisfied(cond, best);
    int tmp;
    for(int s=1; s<cond.vars.size(); ++s)
    {
        tmp = _CountSatisfied(cond, s);
        if(tmp > num_best)
        {
            num_best = tmp;
            best = s;
        }
    }
    return pair<int,int>(num_best, best);
}


int RegressionPlanner::_CountSatisfied(Condition& cond, int which_binding)
{
    int num = 0;
    for(int var = 0; var < cond.vars[which_binding].size(); ++var)
        if(cond.vars[which_binding][var] != NULL) ++num;
    return num;
}

list<pair<WORLDSTATE,Condition> >::iterator RegressionPlanner::
    FindComplete(list<pair<WORLDSTATE,Condition> >& possible_bindings,
    Condition& Conds)
{
        list<pair<WORLDSTATE, Condition> >::iterator b(possible_bindings.begin());
        while(b != possible_bindings.end())
        {
                if(NoNulls(b->second.vars)) return b;///~here
                ++b;
        }
        return b;
}

// we assume that if there is a possible row without nulls, then
// there will not be another row without nulls
int RegressionPlanner::NoNulls(vector<vector<GameObject*> >& vars)
{
        if(vars.size() == 0) return 0;
        for(int v=0; v<vars[0].size(); ++v)
        {
                if(vars[0][v] == NULL) return 0;
        }
        return 1;
}

void RegressionPlanner::SaveAll(void)
{
    ///~~ save everything relevant to the recursive search
    ///~~ We need to be able to pick up exactly where we left off
}

// returns 1 iff retPlan is valued with the root of a plan that 
// starts at the current worldstate, passes through the worldstate
// indicated by the first of the pair, and ends up satisfying the 
// conditions in Conds.
// throws TimeOutException
// DESTRUCTIVE ON retPlan
int RegressionPlanner::PlanOnRest(PlanNode*&retPlan,
               pair<WORLDSTATE,Condition>& bindings, int which_binding, bool use_stack)
{
        // try the which_bindings first.  if no good, continue to 
        // plan with the other bindings and see whether we can 
        // simply add some more operators to make the whole thing
        // work out.  if it doesn't work out, return 0
        
        PlanNode* p = NULL;
        retPlan = NULL;
        
        // Get the operators that will satisfy the remaining conditions
        if(!use_stack) Ops = GetMeOperators(bindings.second, which_binding);
    
        if(Ops.empty()) return 0; // no ops can satisfy condition
    // try to plan for the preconds of the ops for which_binding
    if(!use_stack) { ops = Ops.begin(); done = 0; }
        for(;ops != Ops.end(); ++ops, ++done)
        {
        if(!use_stack) op = ops->begin();
            
        for(op = ops->begin();op!=ops->end(); ++op)
        {
            try{
                        if(!MakeSubPlan(p, op->PreConds, bindings.first, use_stack)) break;
            }catch(TimeOutException tout) { throw tout; }
                    ConcatPlans(p, new PlanNode(*op));
                    if(!MergePlans(retPlan, p)) break;
            use_stack = false;
        }
        if(op != ops->end()) // failed to plan for one of the variables
        {
            ///~write me on second pass
            DeletePlan(p);
            break;
        }
        use_stack = false;
        }
        if(ops != Ops.end()) // failed in step 1
        {
                ///~write me on second pass
                ///~ use done here
                // try to recover by using the parts we've already planned
                // for with the other possible variable bindings
                ///~ feasible???
        DeletePlan(retPlan);
        retPlan = NULL;
                return 0;
        }
        else
                return 1;
        
}



// creates a plan that starts at the given world state and achieves Conds
// returns 1 iff success, else 0
// throws TimeOutException
int RegressionPlanner::MakeSubPlan(PlanNode*& plan, Condition& Conds, WORLDSTATE& state, bool use_stack)
{
    bool success = false, Continue = false;
    if(!use_stack) cond = &Conds;
    while(true)
    {
        // fill in the conds that are satisfied in W
        if(!use_stack) 
        {
            *cond = CheckWorldState(state.first, *cond);

            if(NoNulls(cond->vars)) // conditions completely satisfied in world state
            {
                if(PlanStack.empty()) return 1; // successful planning
                PlanNode* p = NULL, *node;
                do
                {
                    Continue = false;
                    
                    // add the operator this is a precond of to a plan node
                    node = new PlanNode(*OpStack.top().second.second);

                    // and merge it with its other operators' plan nodes (after concating to the NULL)
                    ConcatPlans(p, node);
                    MergePlans(PlanStack.top(), p);

                    // then move to the next operator in that list
                    if(++OpStack.top().second.second == OpStack.top().second.first->end())
                    {
                        if(++OpStack.top().second.first == OpStack.top().first.end())
                        {
                            // reached the end of the operators
                            OpStack.pop();
                            p = PlanStack.pop();
                            if(OpStack.empty()) // out of operators, so we're done
                            {
                                plan = p;
                                return 1; // planning success!!!
                            }
                            Continue = true;
                        }
                        OpStack.top().second.second = OpStack.top().second.first->begin();
                    }

                }while(Continue);
            }
            // find the most promising variable binding
            satisfied = CountSatisfied(*cond);
                
            // recursion part
                        OpStack.push(
                        pair<vector<list<Operator> >, pair<vector<list<Operator> >::iterator, list<Operator>::iterator> >(
                                GetMeOperators(Conds, satisfied.second),
                                pair<vector<list<Operator> >::iterator, list<Operator>::iterator>()));
            PlanStack.push(NULL);
            OpStack.top().second.first = OpStack.top().first.begin();
        }
                
                if(OpStack.top().first.empty()) // no operators found
                {
                        ///~ second pass: try to plan for alternate variable bindings
                        ///~~ handle unable to plan case
                        OpStack.clear(); 
            PlanStack.clear();
            return 0;
                }
                else
                {
                        OpStack.top().second.second = OpStack.top().second.first->begin();
                        pair<vector<list<Operator> >, pair<vector<list<Operator> >::iterator, list<Operator>::iterator> >* cur;
                        
                        cur = &OpStack.top();
                        cond = &cur->second.second->PreConds;
                        
            if(TimesUp()) throw TimeOutException();
                }
                        
        
       use_stack = false;
    }
}




void RegressionPlanner::ConcatPlans(PlanNode*& plan, PlanNode* newplan)
{
        // traverse the plan and redirect all of its leaf nodes to branch to
        // newplan
        if(plan == NULL) plan = newplan;
    else if(newplan == NULL) return;
    else if(newplan != NULL)
    {
                ResetFlags(plan);
                _ConcatPlans(plan, newplan);
        }
}

void RegressionPlanner::_ConcatPlans(PlanNode* plan, PlanNode* newplan)
{
        if(plan == newplan) return; // should never be encountered
        if(PrevsSatisfied(plan))
        {
                plan->num = 1;
                if(plan->isLeaf())
                {
                        MakeLink(plan, newplan);
                }
                else
                {
                        
                        for(int b=0; b<plan->branches.size(); ++b)
                                _ConcatPlans(plan->branches[b], newplan);
                }
        }
}

void RegressionPlanner::ResetFlags(PlanNode* plan)
{
        plan->num = 0;
        ///~ horribly redundant, but is there a better way?
        for(int b=0; b<plan->branches.size(); ++b)
                ResetFlags(plan->branches[b]);
}

int RegressionPlanner::PrevsSatisfied(PlanNode* plan)
{
        for(int p=0; p<plan->prevs.size(); ++p)
                if(plan->prevs[p]->num == 0) return 0;
        return 1;
}


///~ is this function even meaningful in a plan where variable values are
///~ ignored?   Maybe there is a situation where the precond of one op
///~ is explicitly the negation of the postcond of another op, but this
///~ will certainly be rare, and there are bigger fish to fry, so this can wait
// returns 1 iff all threats resolved successfully
int RegressionPlanner::ResolveThreats(PlanNode*& plan1, PlanNode*& plan2)
{
        ///~stub
        return 1;
        ///~stub
}

// returns 1 iff the plans are successfully merged and 
// the result pointed to by plan1
int RegressionPlanner::MergePlans(PlanNode*& plan1, PlanNode*& plan2)
{
    if(plan1 == NULL){
                plan1 = plan2;
                return 1;
        }
    if(plan2 == NULL) {
        return 1;
    }
        if(!ResolveThreats(plan1, plan2)) return 0; // fail

        MergeRoots(plan1, plan2); // recursive helper

        return 1;
}

// plan1's pointer will point to the new root after call
void RegressionPlanner::MergeRoots(PlanNode*& plan1, PlanNode*& plan2)
{
        if(plan2 == NULL) return;
    if(plan1 == NULL) {
        plan1 = plan2; return;
    }
    if(plan1->prevs.size() && plan2->prevs.size())
        {
                PlanNode* dummy_root = new PlanNode(Operator());
                MakeLink(dummy_root, plan1);
                MakeLink(dummy_root, plan2);
                plan1 = dummy_root;
        }
        else if(plan1->prevs.size())
        {
                if(!Member(plan1, plan2->branches))
                {
                        // make plan2 the new root
                        MakeLink(plan2, plan1);
                }
                plan1 = plan2;
        }
        else if(!Member(plan2, plan1->branches))
        {
                // make plan1 the new root
                MakeLink(plan1, plan2);
        }
}


// adds the sequence of operators that was executed in the forward search
// in order to get to the world state argument
void RegressionPlanner::AddForwardPart(PlanNode*& plan, WORLDSTATE& state)
{
    ///~ needs alteration to work with acual forward search stuctures
    ///~ be sure to check for plan==NULL
        ///~
    PlanNode* w = state.second;
    PlanNode* newnode;
    while(!w->prevs.empty())  // while not at the current worldstate
    {   // work back to it
        newnode = new PlanNode(w->Op); // assume holds op that was used to get to it
        MakeLink(newnode, plan);
        plan = newnode;
                w = w->prevs[0];
    }
}

void DeletePlan(PlanNode* plan)
{
        PlanNode* br; bool other;
        if(plan != NULL)
        {
                for(int b=0; b<plan->branches.size(); ++b)
                {
                        br = plan->branches[b];
                        other = false;
                        for(int p=0; p<br->prevs.size(); ++p)
                        {
                                if(br->prevs[p] == plan) br->prevs[p] = NULL;
                                else if(br->prevs[p] != NULL) other = true;
                        }
                        if(!other) DeletePlan(br);
                }
                delete plan;
        }
}