package org.neurostruct.script.devel;

import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Iterator;

import org.catacomb.be.Position;
import org.catacomb.report.E;


public class Navigator {

   
   HashMap  sasHM = new HashMap();
   double stepLength;
   
   HashSet goals = new HashSet();
    
   Action lastAction;
   
   State[] stateQueue = new State[10000];
   
   int errno = 0;
   
   Position lpos;
   
   static final double EPS = 1.e-6; // small number, not important exactly how small;
   
   public double goalFactor; // 0 for random exploration, 1 for totally goal directed
   
   
   
   public Navigator() {
	   stepLength = 1.;
	   goalFactor = 0.5;
   }
    
   
   public void setStepLength(double d) {
	   stepLength = d;
   }
   
   public void setGoalFactor(double d) {
	   goalFactor = d;
   }
   
   
    
   

   public double  getNextStepBearing(Position pos) {
      // get an object containing the possible actions for this position;
      State sas = getStateActionSet(pos);
      if (goals.contains(sas)) {
          E.info("fishy - already at goal? " + sas);
       }
      
      
      lpos = pos.copy();
      
      // if there is a known goal, propagate back through the actions
      // incrementing the values of the actions that lead to it, and 
      // then the ones that lead to them and so on
      Collection vals = sasHM.values();
      for (Iterator it = sasHM.values().iterator(); it.hasNext(); ) {
          ((State)it.next()).clearValue();
      }
     
     
      int nsas = 0;
      for (Iterator it = goals.iterator(); it.hasNext(); ) {
         State g = (State)(it.next());
    	 g.setValue(1.);
         stateQueue[nsas++] = g;
      }
      queuePropagate(stateQueue, nsas);
     

      double[] valueRange = sas.getValueRange();
      
      double dvr = valueRange[1] - valueRange[0];
      if (dvr < EPS) {
    	  dvr = EPS;
      }
      double vmin = valueRange[0];
      
      
      double vmax = -1.;
      Action atake = null;
      
      for (Iterator it = sas.getActionsIterator(); it.hasNext(); ) {
          Action a = (Action)it.next();
    	  double v = a.getValue();
    	  double ra = goalFactor * (v - vmin) / dvr + (1. - goalFactor) * Math.random();
    	  if (ra > vmax) {
    		  vmax = ra;
    		  atake = a;
    	  }
      }
    
       
      // update the states weight for the action that got us here
      if (lastAction != null) {
         sas.updateSource(lastAction);
      }
      lastAction = atake;
      return atake.getBearing();
   }
      
   
   
    public void disableAttemptedAction() {
      lastAction.getFrom().remove(lastAction);
   }
   
   
   
   public void reachedReward(Position pos) {
      State sas = getStateActionSet(pos);
      sas.updateSource(lastAction);
      
      
      E.info("reached reward " + pos.getX() + " " + pos.getY() + " " + sas);
      E.info("coming from " + lpos.getX() + " " + lpos.getY() + " " + lastAction.getBearing());
      
      if (goals.contains(sas)) {
         // already been there once - nothing more to do;
      } else {
         if (goals.size() > 0) {
            E.info("duplicate goals??");
         } else {
            goals.add(sas);
            //E.info("goal at " + pos.getX() + " " + pos.getY() + " " + goals.size());
         }
      }
   }
   
   

   private State getStateActionSet(Position pos) {
      // this allocates place fields based on the position and 
      // step length. If the position has already been visited, 
      // the existing actions are returned, otherwise a new set 
      // is made.
      
       long ix = Math.round(pos.getX() / stepLength);
       long iy = Math.round(pos.getY() / stepLength);
       String key = "" +  ix + ":" + iy;
       State ret = null;
       if (sasHM.containsKey(key)) {
          ret = (State)sasHM.get(key);
       } else {
          ret = new State();
          sasHM.put(key, ret);
       }
       return ret;
   }

   
   
   
   private void queuePropagate(State[] sq, int nInQueue) {
   int isq = 0;
   int nsq = nInQueue;
   while (isq < nsq) {
      State csas = stateQueue[isq];
      double d = csas.getValue() - 0.01;
      
      
      for (Iterator it = csas.getInActionsIterator(); it.hasNext(); ) {
    	 Action a = (Action)it.next();
      
         State src = a.getFrom();
         if (src.doneValue()) {
            // already done;
         } else {
            src.setValue(d);
            stateQueue[nsq++] = src;
         }
      }
      isq += 1;
   }
   }
   
}