#include <iomanip>
#include <sstream>
#include "OpenSteer/SimpleVehicle.h"
#include "OpenSteer/OpenSteerDemo.h"
#include "OpenSteer/Color.h"

#define kWorldRadius                  140.0
#define kMaxObstacleCount             100
#define kFixedFrameElapse             0.1
#define kReportInterval               500
#define kReportsPerRun                3000000
#define kNumRuns                      30
#define kTimeScale                    10.0

#define kPreyCount                    50
#define kPreyMaxSpeed                 5.0
#define kPreyMaxForce                 7.0
#define kPreyVisionRange              20.0
#define kPreyTooCloseRadius           5.0
#define kCosineOfPreyVisionAngle     -0.707
#define kCosineOfPreyMaxSteerAngle    0.707
#define kPreyMaxAliveTime             8000
#define kPreyBumpRecoverRate          0.05
#define kPreyNumWeights               5
#define kPreyInitialSD                0.5
#define kPreyMutateSD                 0.5

#define kPreySigToNoiseIn             1.0
#define kPreySigToNoiseOut            1.0

#define kPredatorCount                2
#define kPredatorMaxSpeed             8.0
#define kPredatorMaxForce             10.0
#define kPredatorVisionRange          20.0
#define kPredatorTooCloseRadius       5.0
#define kPredatorStayAliveTime        2000
#define kPredatorBumpRecoverRate      0.01
#define kPredatorEatRecoverRate       0.01
#define kPredatorNumWeights           8
#define kPredatorInitialSD            0.5
#define kPredatorMutateSD             0.5

#define kLargeNumber                  1000000

namespace {
  
  using namespace OpenSteer;
  
  typedef std::vector<SphereObstacle*> SOG;  // SphereObstacle group
  typedef SOG::const_iterator SOI;           // SphereObstacle iterator
  
  // ----------------------------------------------------------------------------
  // This PlugIn uses two vehicle types: Prey and Predator.  They have a
  // common base class: Critters which is a specialization of SimpleVehicle.
  
  class Critters : public SimpleVehicle {
  public:
    // constructor
    Critters () {reset ();}
    // reset state
    void reset (void);
    // draw this character/vehicle into the scene
    void draw (void);
    Vec3 addNoise (Vec3, float, float);
    // gaussian random number generator (mu, sigma)
    double gaussRnd (double, double);
    void randomizeStartingPositionAndHeading (void);
    // for draw method
    Color bodyColor;
    // store steer sub-state for annotation
    bool avoiding;
    unsigned int aliveTime;
    // store collision state for annotation
    bool bumping;
    // dynamic obstacle registry
    static void initializeObstacles (void);
    static void addOneObstacle (void);
    static void removeOneObstacle (void);
    float minDistanceToObstacle (const Vec3 point);
    static int obstacleCount;
    static SOG allObstacles;
  };
  
  
  class Prey : public Critters {
  public:
    // constructor
    Prey () {reset ();}
    // reset state
    void reset (void);
    // mutate weights for single regenerated prey
    void mutateWeights (void);
    // sense behaviour tailored for prey
    Vec3 avgNearestObstacle (void);
    // per frame simulation update
    void update (const float currentTime, const float elapsedTime);
    // weights for 'neural' vector sum
    float neuronWeights[kPreyNumWeights];
    // evolving steering behaviour
    Vec3 steeringForPrey (void);
    void draw (void);
    bool evading; // store steer sub-state for annotation
  };
  
  
  class Predator : public Critters {
  public:
    // constructor
    Predator () {reset ();}
    // reset state
    void reset (void);
    // sense behaviour tailored for predator
    Vec3 avgNearestObstacle (void);
    // mutate weights for all predators
    void mutateAll (void);
    // weights for 'neural' vector sum
    float neuronWeights[kPredatorNumWeights];
    float prevNeuronWeights[kPredatorNumWeights];
    // store collision state for annotation
    bool eating;
    // per frame simulation update
    void update (const float currentTime, const float elapsedTime);
    void draw (void);
    // how many eaten?
    unsigned int score, prevScore;
  };
  
  
  // ----------------------------------------------------------------------------
  // globals
  
  bool showGraphics = true;
  Prey* prey [kPreyCount];
  Predator* predators [kPredatorCount + (kPredatorCount == 0)];
  
  unsigned int runNum = 1;
//  char* execLoc = "/Users/danielsayers/Downloads/critters/macosx/build/Development/OpenSteerDemo.app/Contents/MacOS/OpenSteerDemo";
//  char* logLoc = "/Users/danielsayers/Downloads/latexpaper/log.txt";
  //Predator* predators[1];

  unsigned int numPreyDead, numPredatorsDead;
  
  // ----------------------------------------------------------------------------
  // reset state
  
  void Critters::reset (void) {
    SimpleVehicle::reset ();  // reset the vehicle 
    setSpeed (0);             // speed along Forward direction.
    setMaxForce (kPreyMaxForce);        // steering force is clipped to this magnitude
    setMaxSpeed (kPreyMaxSpeed);        // velocity is clipped to this magnitude
    randomizeStartingPositionAndHeading ();  // new starting position
    avoiding = false;         // not actively avoiding
    aliveTime = 0;
    //    clearTrailHistory ();     // prevent long streaks due to teleportation
  }
  
  
  void Prey::reset (void) {
    Critters::reset ();
    // bodyColor.set (0.5f, 0.6f, 1.0f); // blue
//    bodyColor.set (0.2f, 0.2f, 0.2f); // dark grey
    bodyColor.set (0.0f, 0.0f, 0.0f); // black
    evading = false;
    bumping = false;
    for (int j = 0; j < kPreyNumWeights; j++)
      neuronWeights[j] = gaussRnd(0, kPreyInitialSD);
    // we know these weights will evolve to be strongly negative
    // as they direct avoidance of fellow prey, predators and obstacles
    // so we speed up evolution ...
//    neuronWeights[2] -= 10;
//    neuronWeights[3] -= 10;
//    neuronWeights[4] -= 10;
  }
  
  
  void Predator::reset (void) {
    //Critters::reset ();
    aliveTime = 0;
    setMaxForce (kPredatorMaxForce);        // steering force is clipped to this magnitude
    setMaxSpeed (kPredatorMaxSpeed);        // velocity is clipped to this magnitude
//    bodyColor.set (1.0f, 0.5f, 1.0f); // purple
    bodyColor.set (0.0f, 0.0f, 0.0f); // black
    eating = false;
    for (int i = 0; i < kPredatorNumWeights; i++) {
      prevNeuronWeights[i] = neuronWeights[i];
      prevScore = score;
    }
    score = 0;
    for (int j = 0; j < kPredatorNumWeights; j++)
      neuronWeights[j] = gaussRnd(0, kPredatorInitialSD);
  }
  
  
  // ----------------------------------------------------------------------------
  // general critter behaviours
  
  
  void Critters::draw (void) {
    drawBasic2dCircularVehicle (*this, bodyColor);
    //    drawTrail ();
  }
  
  
  // ----------------------------------------------------------------------------
  
  
  Vec3 Critters::addNoise (Vec3 vect, float ratio, float scale) {
    //    if (vect == Vec3::zero) return vect;
    //    printf("%1.5f\n", vect.length());
    return vect * ratio + RandomVectorInUnitRadiusSphere().setYtoZero() * (1 - ratio) * scale;
  }
  
  
  // ----------------------------------------------------------------------------
  
  
  double Critters::gaussRnd (double mu=0.0, double sigma=1.0) {
    static bool deviateAvailable=false;//flag
    static float storedDeviate;//deviate from previous calculation
    double polar, rsquared, var1, var2;
    
    // If no deviate has been stored, the polar Box-Muller transformation is
    // performed, producing two independent normally-distributed random
    // deviates.One is stored for the next round, and one is returned.
    if (!deviateAvailable) {
      
      // choose pairs of uniformly distributed deviates, discarding those
      // that don't fall within the unit circle
      do {
        var1 = 2.0 * ( double(rand())/double(RAND_MAX) ) - 1.0;
        var2 = 2.0 * ( double(rand())/double(RAND_MAX) ) - 1.0;
        rsquared=var1*var1+var2*var2;
      } while ( rsquared>=1.0 || rsquared == 0.0);
      
      // calculate polar tranformation for each deviate
      polar=sqrtXXX(-2.0*log(rsquared)/rsquared);
      
      // store first deviate and set flag
      storedDeviate=var1*polar;
      deviateAvailable=true;
      
      // return second deviate
      return var2*polar*sigma + mu;
    }
    
    // If a deviate is available from a previous call to this function, it is
    // returned, and the flag is set to false.
    else {
      deviateAvailable=false;
      return storedDeviate*sigma + mu;
    }
  }
  
  
  // ----------------------------------------------------------------------------
  
  
  void Critters::randomizeStartingPositionAndHeading (void) {
    // randomize position on a ring between inner and outer radii
    // centered around the home base
    const float rRadius = frandom2 (0, kWorldRadius);
    const Vec3 randomOnRing = RandomUnitVectorOnXZPlane () * rRadius;
    setPosition (randomOnRing);
    
    // are we are too close to an obstacle?
    if (minDistanceToObstacle (position()) < radius()*5) {
      // if so, retry the randomization (this recursive call may not return
      // if there is too little free space)
      randomizeStartingPositionAndHeading ();
    } else {
      // otherwise, if the position is OK, randomize 2D heading
      randomizeHeadingOnXZPlane ();
    }
  }
  
  
  // ----------------------------------------------------------------------------
  // prey behaviours
  
  
  void Prey::mutateWeights (void) {
    int reproducer = -1;
    unsigned int aliveTimes[kPreyCount];
    unsigned int totAlive = 0;
    
    // get average weights (for reporting) and alive times (for probability weighting of selection)
    for (int i = 0; i < kPreyCount; i++) {
      aliveTimes[i] = prey[i]->aliveTime;
      totAlive += aliveTimes[i];
    }
    
    // make random number between 0 and 1
    // then loop through weighted probabilities (scaled alive times) until we pass this number
    float probPos = (float)rand() / (float)RAND_MAX;
    float probGot = 0.0;
    for (int i = 0; i < kPreyCount; i++) {
      probGot += (float)aliveTimes[i]/(float)totAlive;
      if (probGot > probPos) {
        reproducer = i;
        break;
      }
    }
    
    // missed it? recurse
    if (reproducer == -1) return mutateWeights();    
    for (int j = 0; j < kPreyNumWeights; j++)
      neuronWeights[j] = prey[reproducer]->neuronWeights[j] + gaussRnd(0, kPreyMutateSD);
    
    aliveTime = 0;
    numPreyDead++;
  }
  
  
  // ----------------------------------------------------------------------------
  
  
  Vec3 Prey::steeringForPrey (void) {
    // set up vars
    Vec3 steer (0, 0, 0);
    Vec3 avgNearPredatorLoc (0, 0, 0);
    Vec3 avgNearBoidsLoc (0, 0, 0);
    Vec3 avgNearBoidsVel (0, 0, 0);
    Vec3 tooCloseBoidsLoc (0, 0, 0);
    float numNear = 0.0;
    float numTooClose = 0.0;
    float numPredatorNear = 0.0;
    evading = false;
    
    if (aliveTime > kPreyMaxAliveTime)
      mutateWeights();
    
    // has bumped into something? get faster
    if (bumping) {
      float ms = maxSpeed();
      ms += kPreyBumpRecoverRate;
      if (ms > kPreyMaxSpeed) {
        // not bumping any more
        ms = kPreyMaxSpeed;
        bumping = false;
      }
      setMaxSpeed(ms);
    }
    
    // get average fellow prey positions & headings within near and "too close" limits
    for (int i = 0; i < kPreyCount; i++) {
      if(prey[i] == this) continue;
      Prey& e = *prey[i];
      const Vec3 eOffset = e.position() - position();
      const float eDistance = eOffset.length();
      const Vec3 direction = eOffset / eDistance;
      float cosineOfAngle = eOffset.normalize().dot(velocity().normalize());
      if (eDistance < kPreyVisionRange) {
        if (eDistance < kPreyTooCloseRadius) {
          if (cosineOfAngle >= kCosineOfPreyVisionAngle) {
            // too close
            numTooClose++;
            
            // scale "too close" locations so that vector is bigger for nearer boids
            tooCloseBoidsLoc += eOffset.normalize() * (kPreyTooCloseRadius / eDistance - 1);
          }
          
          // actually bumping into each other ...
          if (eDistance < 2 * radius()) {
            setMaxSpeed(0.5);
            bumping = true;
          }
        } else {
          if (cosineOfAngle >= kCosineOfPreyVisionAngle) {
            // near
            numNear++;
            avgNearBoidsVel += e.velocity();
            avgNearBoidsLoc += eOffset;
          }
        }
      }
    }
    
    // detect average predator presence within circle
    for (int i = 0; i < kPredatorCount; i++) {
      Predator& e = *predators[i];
      const Vec3 eOffset = e.position() - position();
      const float eDistance = eOffset.length();
      float cosineOfAngle = eOffset.normalize().dot(velocity().normalize());
      if (eDistance < kPreyVisionRange && cosineOfAngle >= kCosineOfPreyVisionAngle) {
        evading = true;
        numPredatorNear++;
        avgNearPredatorLoc += eOffset.normalize() * (kPreyVisionRange / eDistance - 1);
      }
    }
    
    // get nearest point of any obstacle wall
    const Vec3 nearestObstacle = avgNearestObstacle();
    
    // saved for annotation
    avoiding = (nearestObstacle != Vec3::zero);
    
    //neuronWeights[2] = neuronWeights[0] = 0;
    
    // scale to average, multiply by neuron weights
    if (numNear > 0.0) {
      // nearby prey location factor
      steer += neuronWeights[0] * 0.10 * addNoise(avgNearBoidsLoc / numNear, kPreySigToNoiseIn, kPreyVisionRange);
      
      // nearby prey velocity factor
      steer += neuronWeights[1] * addNoise(avgNearBoidsVel / numNear, kPreySigToNoiseIn, kPreyMaxSpeed);
    } else {
      // still use noise signal even if no nearby prey present
      steer += neuronWeights[0] * 0.10 * addNoise(Vec3::zero, kPreySigToNoiseIn, kPreyVisionRange);
      steer += neuronWeights[1] * addNoise(Vec3::zero, kPreySigToNoiseIn, kPreyMaxSpeed);
    }
    
    // "too close" prey location factor
    if (numTooClose > 0.0)
      steer += neuronWeights[2] * 1.00 * addNoise(tooCloseBoidsLoc / numTooClose, kPreySigToNoiseIn, kPreyTooCloseRadius);
    else
      steer += neuronWeights[2] * 1.00 * addNoise(Vec3::zero, kPreySigToNoiseIn, kPreyTooCloseRadius);
    
    // annotate steering thus far generated (group response) in green
//    annotationLine (position(), position() + steer * 0.1, gGreen);
    
    // obstacle location factor
    Vec3 addVect = neuronWeights[3] * 1.00 * addNoise(nearestObstacle, kPreySigToNoiseIn, kPreyVisionRange);
    steer += addVect;
    
    // annotate obstacle steering in orange
//    annotationLine (position(), position() + addVect * 0.1, gOrange);
    
    // predator location factor
    if (numPredatorNear > 0.0) {
      Vec3 addVect = neuronWeights[4] * 1.00 * addNoise(avgNearPredatorLoc / numPredatorNear, kPreySigToNoiseIn, kPreyVisionRange);
      steer += addVect;
      // annotate predator steering in red
//      annotationLine (position(), position() + addVect * 0.1, gRed);
    } else {
      Vec3 addVect = neuronWeights[4] * 1.00 * addNoise(Vec3::zero, kPreySigToNoiseIn, kPreyVisionRange);
      steer += addVect;
    }
    
    steer = addNoise(steer, kPreySigToNoiseOut, kPreyMaxForce / kFixedFrameElapse);
    
    // limit maximum steering angle at 45 degrees
    steer = limitMaxDeviationAngle (steer, kCosineOfPreyMaxSteerAngle, forward());
    
    return steer;
    
  }
  
  
  // ----------------------------------------------------------------------------
  void Predator::draw (void) {
    Critters::draw();
    drawXZCircle (radius() + 2, position(), gBlack, 40);
  }
  
  void Prey::draw (void) {
    // first call the draw method in the base class
    Critters::draw();
    
    // annotation (remove for speed)
    if (0) {
      // select string describing current prey state
      char* preyStateString = "";
      if (evading)
        preyStateString = "evade";
      else if (bumping)
        preyStateString = "ouch!";
      else if (avoiding)
        preyStateString = "avoid";
      else
        preyStateString = "";
      
      // annote prey with its state as text
      const Vec3 textOrigin = position() + Vec3 (0, 0.25, 0);
      std::ostringstream annote;
      annote << preyStateString << std::endl;
      annote << std::setprecision(2) << std::setiosflags(std::ios::fixed)
      << std::ends;
      draw2dTextAt3dLocation (annote, textOrigin, gWhite, drawGetWindowWidth(), drawGetWindowHeight());
      
      // display status in the upper left corner of the window
      std::ostringstream status;
      status << obstacleCount << " obstacles [F1/F2]" << std::ends;
      const float h = drawGetWindowHeight ();
      const Vec3 screenLocation (10, h-50, 0);
      draw2dTextAt2dLocation (status, screenLocation, gGray80, drawGetWindowWidth(), drawGetWindowHeight());
    }
  }
  
  
  // ----------------------------------------------------------------------------
  
  
  void Prey::update (float currentTime, const float elapsedTime) {
    currentTime++;
    // determine and apply steering/braking forces
    Vec3 steer (0, 0, 0);
    steer = steeringForPrey ();
    applySteeringForce (steer, elapsedTime);
    // annotation
//    annotationVelocityAcceleration ();
//    annotationLine (position(),position()+steer, gRed);
//    annotationLine (position(),position()+velocity(), gGray80);
    annotationLine (position(),position()+velocity(), gBlack);
    //    recordTrailVertex (currentTime, position());
    aliveTime++;
  }
  
  
  // ----------------------------------------------------------------------------
  
  
  Vec3 Prey::avgNearestObstacle (void) {
    Vec3 where (0, 0, 0);
    int numObst = 0;
    
    for (SOI so = allObstacles.begin(); so != allObstacles.end(); so++) {
      if (so == allObstacles.begin()) {
        // first obstacle is actually the perimeter boundary
        float howFarOut = position().length();
        const float eSensorOverlap = howFarOut + kPreyVisionRange - kWorldRadius;
        if (eSensorOverlap > 0) {
          // perimeter is within sensing radius
          numObst++;
          // scale vector to be larger when nearer
          where += position().normalize() * (kPreyVisionRange / eSensorOverlap - 1);
          if (howFarOut + radius() > kWorldRadius) {
            // bumped into perimeter
            float bump = 1.0 - (howFarOut + radius() - kWorldRadius);
            if (bump < 0.5) {
              // very hard - die
              randomizeStartingPositionAndHeading ();  // new starting position
              //              clearTrailHistory ();     // prevent long streaks due to teleportation
              mutateWeights();
            } else {
              // slow down
              setMaxSpeed(bump);
              bumping = true;
            }
          }
        }
      } else {
        // regular (small) obstacle
        const Vec3 eOffset = (**so).center - position();
        const float eDistance = eOffset.length();
        const float eSensorOverlap = kPreyVisionRange + (**so).radius - eDistance;
        if (eSensorOverlap > 0) {
          // obstacle boundary within sensing radius
          numObst++;
          
          where += eOffset.normalize() * (kPreyVisionRange / eSensorOverlap - 1);
        }
        if (eDistance < radius() + (**so).radius) {
          // bumped into it
          float bump = 1.0 - (radius() + (**so).radius - eDistance);
          if (bump < 0.5) {
            // hard - die
            randomizeStartingPositionAndHeading ();  // new starting position
            //            clearTrailHistory ();     // prevent long streaks due to teleportation
            mutateWeights();
          } else {
            // slow down
            setMaxSpeed(bump);
            bumping = true;
          }
        }
      }
    }
    
    // average out
    if (numObst > 0) where /= (float)numObst;
    
    return where;
  }
  
  
  // ----------------------------------------------------------------------------
  // predator behaviours
  
  
  void Predator::mutateAll (void) {
    float avgWeights[kPredatorNumWeights];
    float bestWeights[kPredatorNumWeights];
    for (int j = 0; j < kPredatorNumWeights; j++) avgWeights[j] = bestWeights[j] = 0;
    int bestScore = 0;
    
    // find best-scoring predator weights from current and previous generation
    for (int i = 0; i < kPredatorCount; i++) {
      Predator& e = *predators[i];
      int eScore = e.score;
      if (eScore >= bestScore) {
        bestScore = eScore;
        for (int j = 0; j < kPredatorNumWeights; j++)
          bestWeights[j] = e.neuronWeights[j];
      }
      eScore = e.prevScore;
      if (eScore >= bestScore) {
        bestScore = eScore;
        for (int j = 0; j < kPredatorNumWeights; j++)
          bestWeights[j] = e.prevNeuronWeights[j];
      }
    }
    //    printf("b %1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f;\n", bestWeights[0], bestWeights[1], bestWeights[2], bestWeights[3], bestWeights[4], bestWeights[5], bestWeights[6], bestWeights[7]);
    
    // set all predator weights to mutations of best weights found
    for (int i = 0; i < kPredatorCount; i++) {
      Predator& e = *predators[i];
      e.reset();
      for (int j = 0; j < kPredatorNumWeights; j++) {
        e.neuronWeights[j] = bestWeights[j] + gaussRnd(0, kPredatorMutateSD);
        avgWeights[j] += e.neuronWeights[j]/(float)kPredatorCount;
      }
    }
    numPredatorsDead += kPredatorCount;
    // report averages
    //    printf("d %1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f;\n", avgWeights[0], avgWeights[1], avgWeights[2], avgWeights[3], avgWeights[4], avgWeights[5], avgWeights[6], avgWeights[7]);
  }
  
  
  // ----------------------------------------------------------------------------
  
  
  void Predator::update (float currentTime, const float elapsedTime) {
    currentTime++;
    Prey* gPrey; // target prey
    float minDist = kLargeNumber;
    float scaleProxBySpeed = 0.7; // how much to favour slow-moving prey
    
    // periodically mutate from best predator in current and previous generations
    if (aliveTime > kPredatorStayAliveTime)
      mutateAll();
    
    // find "nearest" prey (favouring slower-moving targets)
    for (int i = 0; i < kPreyCount; i++) {
      Prey& e = *prey[i];
      const Vec3 eOffset = e.position() - position();
      
      // offset distance by prey speed
      const float eDistance = eOffset.length() *
      ((1 - scaleProxBySpeed) * kPreyMaxSpeed + scaleProxBySpeed * e.speed()) /
      kPreyMaxSpeed;
      if (eDistance < minDist) {
        minDist = eDistance;
        gPrey = &e;
      }
    }
    
    // has bumped into something? get faster
    if (eating) {
      float ms = maxSpeed();
      ms += kPredatorEatRecoverRate;
      
      // back to normal ...
      if (ms > kPredatorMaxSpeed) {
        ms = kPredatorMaxSpeed;
        eating = false;
      }
      setMaxSpeed(ms);
    } else if (bumping) {
      float ms = maxSpeed();
      ms += kPredatorBumpRecoverRate;
      
      // back to normal ...
      if (ms > kPredatorMaxSpeed) {
        ms = kPredatorMaxSpeed;
        bumping = false;
      }
      setMaxSpeed(ms);
    }
    
    // determine steering (pursuit, obstacle avoidance, inter-predator interaction)
    Vec3 steer (0, 0, 0);
    Vec3 avgNearBoidsLoc (0, 0, 0);
    Vec3 avgNearBoidsVel (0, 0, 0);
    Vec3 tooCloseBoidsLoc (0, 0, 0);
    float numNear = 0.0;
    float numTooClose = 0.0;
    const float sumOfRadii = 2*radius();// + gPrey->radius();
    
    // get average fellow predator positions & headings within near and "too close" limits
    for (int i = 0; i < kPredatorCount; i++) {
      if(predators[i] == this) continue;
      Predator& e = *predators[i];
      const Vec3 eOffset = e.position() - position();
      const float eDistance = eOffset.length();
      
      if (eDistance < kPredatorVisionRange) {
        if (eDistance < kPredatorTooCloseRadius) {
          // too close
          numTooClose++;
          
          // scale "too close" locations so that vector is bigger for nearer predators
          tooCloseBoidsLoc += eOffset.normalize() * (kPredatorTooCloseRadius / eDistance - 1);
          
          // actually bumping into each other ...
          if (eDistance < sumOfRadii) {
            setMaxSpeed(0.5);
            bumping = true;
            
            // way too close ... reset position and heading
            if (eDistance < sumOfRadii/2) {
              score = 0;
              e.score = 0;
              randomizeStartingPositionAndHeading();
            }
          }
        } else {
          // nearby
          numNear++;
          avgNearBoidsVel += e.velocity();
          avgNearBoidsLoc += eOffset;
        }
      }
    }
    
    // preyPos, preyVel, pursuit, predict, obstacle, predPos, predVel, predClose
    
    const Vec3 eOffset = gPrey->position() - position();
    const Vec3 eVel = gPrey->velocity();
    const Vec3 nearestObstacle = avgNearestObstacle();
    
    //neuronWeights[2] = neuronWeights[1] = 0;
    // calculate steering as sum of weighted vectors
    steer = neuronWeights[0] * eOffset + neuronWeights[1] * eVel +
    neuronWeights[2] * steerForPursuit (*gPrey, 1.0) +
    neuronWeights[4] * 1.00 * nearestObstacle;
    
    // scale to average, multiply by neuron weights
    if (numNear > 0.0) {
      // nearby prey location factor
      steer += neuronWeights[5] * 0.10 * avgNearBoidsLoc / numNear;
      
      // nearby prey velocity factor
      steer += neuronWeights[6] * avgNearBoidsVel / numNear;
    }
    
    // "too close" prey location factor
    if (numTooClose > 0.0) steer += neuronWeights[7] * 1.00 * tooCloseBoidsLoc / numTooClose;
    
    applySteeringForce (steer, elapsedTime);
    
    // annotation
//    annotationVelocityAcceleration ();
//    annotationLine (position(),position()+steer, gRed);
//    annotationLineXXX (position(),position()+velocity(), gBlack, 0xAAAA);
    annotationLineXXX (position(),gPrey->position(), gGray10, 0xAAAA);
//    annotationLine (position(),position()+velocity(), gBlack);
//    annotationLine (position(),position()+velocity(), gGray80);
    //    recordTrailVertex (currentTime, position());
    
    // detect and record interceptions ("tags") of prey
    const float preyToMeDist = Vec3::distance (position(), gPrey->position());
    
    // caught one!
    if (preyToMeDist < sumOfRadii) {
      eating = true;
      setMaxSpeed(0.5);
      score++;
      
      // regenerate it ...
      gPrey->randomizeStartingPositionAndHeading ();  // new starting position
      //      gPrey->clearTrailHistory ();     // prevent long streaks due to teleportation
      gPrey->mutateWeights();
    }
    
    aliveTime++;
  }
  
  
  // ----------------------------------------------------------------------------
  
  
  Vec3 Predator::avgNearestObstacle (void) {
    Vec3 where (0, 0, 0);
    int numObst = 0;
    
    for (SOI so = allObstacles.begin(); so != allObstacles.end(); so++) {
      // the first obstacle is the perimeter boundary
      if (so == allObstacles.begin()) {
        float howFarOut = position().length();
        const float eSensorOverlap = howFarOut + kPredatorVisionRange - kWorldRadius;
        if (eSensorOverlap > 0) {
          // perimeter is within sensing radius
          numObst++;
          // scale vector to be larger when nearer
          where += position().normalize() * (kPredatorVisionRange / eSensorOverlap - 1);
          if (howFarOut + radius() > kWorldRadius) {
            // bumped into perimeter
            float bump = 1.0 - (howFarOut + radius() - kWorldRadius);
            if (bump < 0.5) {
              // very hard - die
              score = 0;
              randomizeStartingPositionAndHeading();
            } else {
              // slow down
              setMaxSpeed(bump);
              bumping = true;
            }
          }
        }
      } else {
        // normal obstacle
        const Vec3 eOffset = (**so).center - position();
        const float eDistance = eOffset.length();
        const float eSensorOverlap = kPredatorVisionRange + (**so).radius - eDistance;
        if (eSensorOverlap > 0) {
          // obstacle boundary within sensing radius
          numObst++;
          
          where += eOffset.normalize() * (kPredatorVisionRange / eSensorOverlap - 1);
        }
        if (eDistance < radius() + (**so).radius) {
          // bumped into perimeter
          float bump = 1.0 - (radius() + (**so).radius - eDistance);
          if (bump < 0.5) {
            // very hard - die
            score = 0;
            randomizeStartingPositionAndHeading();
          } else {
            // slow down
            setMaxSpeed(bump);
            bumping = true;
          }
        }
      }
    }
    
    // average out ...
    if (numObst > 0) where /= (float)numObst;
    
    return where;
  }
  
  
  // ----------------------------------------------------------------------------
  // dynamic obstacle registry
  
  
  int Critters::obstacleCount = -1; // this value means "uninitialized"
  SOG Critters::allObstacles;
  
  
#define testOneObstacleOverlap(radius, center) {         \
float d = Vec3::distance (c, center);                    \
float clearance = d - (r + (radius));                    \
if (minClearance > clearance) minClearance = clearance;}
  
  
  void Critters::initializeObstacles (void) {
    // start with one 'obstacle' - the enclosure
    if (obstacleCount == -1) {
      obstacleCount = 0;
      addOneObstacle ();
    }
  }
  
  
  void Critters::addOneObstacle (void) {
    if (obstacleCount < kMaxObstacleCount) {
      // pick a random center and radius,
      // loop until no overlap with other obstacles and the home base
      float r;
      Vec3 c (0, 0, 0);
      if (obstacleCount == 0) r = kWorldRadius;
      else {
        float minClearance;
        const float requiredClearance = prey[0]->radius() * 4; // 2 x diameter
        do {
          r = frandom2 (1.5, 4);
          c = randomVectorOnUnitRadiusXZDisk () * kWorldRadius;
          minClearance = FLT_MAX;
          
          for (SOI so = allObstacles.begin(); so != allObstacles.end(); so++) {
            if (so == allObstacles.begin()) continue;
            testOneObstacleOverlap ((**so).radius, (**so).center);
          }
          
        }
        while (minClearance < requiredClearance);
      }
      
      // add new non-overlapping obstacle to registry
      SphereObstacle *newSphere = new SphereObstacle (r, c);
      if (obstacleCount == 0) newSphere->setSeenFrom(Obstacle::inside);
      allObstacles.push_back (newSphere);
      obstacleCount++;
    }
  }
  
  
  float Critters::minDistanceToObstacle (const Vec3 point) {
    float r = 0;
    Vec3 c = point;
    float minClearance = FLT_MAX;
    for (SOI so = allObstacles.begin(); so != allObstacles.end(); so++) {
      if (so == allObstacles.begin()) continue;
      testOneObstacleOverlap ((**so).radius, (**so).center);
    }
    return minClearance;
  }
  
  
  void Critters::removeOneObstacle (void) {
    if (obstacleCount > 0) {
      obstacleCount--;
      allObstacles.pop_back();
    }
  }
  
  
  // ----------------------------------------------------------------------------
  // PlugIn for OpenSteerDemo
  
  
  class CtfPlugIn : public PlugIn {
  public:
    
    const char* name (void) {return "Critter";}
    
    float selectionOrderSortKey (void) {return 0.01f;}
    
    virtual ~CtfPlugIn() {} // be more "nice" to avoid a compiler warning
    
    void open (int argc, char **argv) {
/*
      printf("%d arguments\n", argc);
      for (int i = 0; i < argc; i++) {
        printf("arg %d: %s\n", i, argv[i]);
      }
 */
//      OpenSteer::toggleAnnotationState ();
      if (argc > 1)
        runNum = atoi(argv[1]);
//      printf("run number: %d\n", runNum);
      numPreyDead = 0;
      numPredatorsDead = 0;
      
      // create the prey ("hero"/"attacker")
      for (int i = 0; i<kPreyCount; i++) {
        prey[i] = new Prey;
        all.push_back (prey[i]);
      }
      
      // create the specified number of predators, 
      // storing pointers to them in an array.
      for (int i = 0; i<kPredatorCount; i++) {
        predators[i] = new Predator;
        all.push_back (predators[i]);
      }
      
      // initialize camera
      OpenSteerDemo::init2dCamera (*prey[0]);
      OpenSteerDemo::camera.mode = Camera::cmFixed;
      OpenSteerDemo::camera.fixedTarget.set (0, -0.4*kWorldRadius, 0);
      OpenSteerDemo::camera.fixedPosition.set (1.49065*kWorldRadius, 1.02560*kWorldRadius, -0.06145*kWorldRadius);
      
      Critters::initializeObstacles ();
    }
    
    void update (const float currentTime, float elapsedTime) {
      elapsedTime *= kTimeScale;
//      elapsedTime = kFixedFrameElapse;
      // update the prey
      for (int i = 0; i < kPreyCount; i++)
        prey[i]->update (currentTime, elapsedTime);
      
      // update each predator
      for (int i = 0; i < kPredatorCount; i++)
        predators[i]->update (currentTime, elapsedTime);
      /*
      char displaytime[11];
      sprintf (displaytime, "%d%d:%d%d:%d%d.%d%d", int(currentTime) / 36000 % 10, int(currentTime) / 3600 % 10, int(currentTime) / 360 % 6, int(currentTime) / 60 % 10, int(currentTime) / 10 % 6, int(currentTime) % 10, int(currentTime * 10) % 10, int(currentTime * 100) % 10);
      std::ostringstream status;
      status << "dead predators: " << numPredatorsDead << "\n" << "dead prey: " << numPreyDead << "\n" << displaytime << std::ends;
      const float h = drawGetWindowHeight ();
      const Vec3 screenLocation (284, h-141, 0);
      draw2dTextAt2dLocation (status, screenLocation, gGray80, drawGetWindowWidth(), drawGetWindowHeight());
      */
    }
    
    void printStats (float elapsedTime) {
      static unsigned int reportNum = 0;
      float avgDistance = 0;
      float avgDotProd = 0;
      reportNum++;
      
      for (int i = 0; i < kPreyCount; i++) {
        Prey& p1 = *prey[i];
        float minDist = kLargeNumber;
        float thisDot = 0;
        for (int j = 0; j < kPreyCount; j++) {
          if (i == j) continue;
          Prey& p2 = *prey[j];
          const Vec3 eOffset = p1.position() - p2.position();
          const float eDistance = eOffset.length();
          if (eDistance < minDist) {
            minDist = eDistance;
            
            thisDot = p1.velocity().normalize().dot(p2.velocity().normalize());
          }
        }
        avgDistance += minDist;
        avgDotProd += thisDot;
      }
      
      avgDistance /= (float)kPreyCount;
      avgDotProd /= (float)kPreyCount;
      
      printf("avgDist(%d,%d) = %1.9f;\n", runNum, reportNum, avgDistance);
      printf("avgDot(%d,%d) = %1.9f;\n", runNum, reportNum, avgDotProd);
      
      float preyAvgWeights[kPreyNumWeights];
      float preySDWeights[kPreyNumWeights];
      for (int j = 0; j < kPreyNumWeights; j++) preyAvgWeights[j] = preySDWeights[j] = 0;
      unsigned int preyTotAlive = 0;
      
      // get average weights (for reporting) and alive times (for probability weighting of selection)
      for (int i = 0; i < kPreyCount; i++) {
        for (int j = 0; j < kPreyNumWeights; j++)
          preyAvgWeights[j] += prey[i]->neuronWeights[j]/(float)kPreyCount;
        preyTotAlive += prey[i]->aliveTime;
      }
      
      for (int i = 0; i < kPreyCount; i++)
        for (int j = 0; j < kPreyNumWeights; j++)
          preySDWeights[j] += (preyAvgWeights[j] - prey[i]->neuronWeights[j]) * (preyAvgWeights[j] - prey[i]->neuronWeights[j]);
      for (int j = 0; j < kPreyNumWeights; j++) {
        preySDWeights[j] /= kPreyCount;
        preySDWeights[j] = sqrtXXX(preySDWeights[j]);
      }
      
      const float preyAvgAliveTime = (float)preyTotAlive / (float)kPreyCount;
      printf("avgLife(%d,%d) = %1.9f;\n", runNum, reportNum, preyAvgAliveTime);

      printf("preyAvgWeights(%d,%d,:) = [", runNum, reportNum);
//      for (int j = 0; j < 2; j++) {
      for (int j = 0; j < kPreyNumWeights; j++) {
        printf("%1.9f", preyAvgWeights[j]);
//        if (j+1 == 2) break;
        if (j+1 == kPreyNumWeights) break;
        printf(", ");
      }
      printf("];\n");
      
      printf("preySDWeights(%d,%d,:) = [", runNum, reportNum);
      for (int j = 0; j < kPreyNumWeights; j++) {
        printf("%1.9f", preySDWeights[j]);
        if (j+1 == kPreyNumWeights) break;
        printf(", ");
      }
      printf("];\n");
      
      float predatorAvgWeights[kPredatorNumWeights];
      float predatorSDWeights[kPredatorNumWeights];
      for (int j = 0; j < kPredatorNumWeights; j++) predatorAvgWeights[j] = predatorSDWeights[j] = 0;
      
      // get average weights (for reporting) and alive times (for probability weighting of selection)
      for (int i = 0; i < kPredatorCount; i++)
        for (int j = 0; j < kPredatorNumWeights; j++)
          predatorAvgWeights[j] += predators[i]->neuronWeights[j]/(float)kPredatorCount;
      
      for (int i = 0; i < kPredatorCount; i++)
        for (int j = 0; j < kPredatorNumWeights; j++)
          predatorSDWeights[j] += (predatorAvgWeights[j] - predators[i]->neuronWeights[j]) * (predatorAvgWeights[j] - predators[i]->neuronWeights[j]);
      for (int j = 0; j < kPredatorNumWeights; j++) {
        predatorSDWeights[j] /= kPredatorCount;
        predatorSDWeights[j] = sqrtXXX(predatorSDWeights[j]);
      }

      printf("predatorAvgWeights(%d,%d,:) = [", runNum, reportNum);
      for (int j = 0; j < kPredatorNumWeights; j++) {
        printf("%1.9f", predatorAvgWeights[j]);
        if (j+1 == kPredatorNumWeights) break;
        printf(", ");
      }
      printf("];\n");

      printf("predatorSDWeights(%d,%d,:) = [", runNum, reportNum);
      for (int j = 0; j < kPredatorNumWeights; j++) {
        printf("%1.9f", predatorSDWeights[j]);
        if (j+1 == kPredatorNumWeights) break;
        printf(", ");
      }
      printf("];\n");

      printf("numPreyDead(%d,%d) = %d;\n", runNum, reportNum, numPreyDead);
      printf("numPredatorsDead(%d,%d) = %d;\n", runNum, reportNum, numPredatorsDead);

      if (reportNum == 1000) {
        for (int i = 0; i < 20; i++)
          Critters::addOneObstacle ();
      }

      if (reportNum >= kReportsPerRun) {
        /*
        char buffer [512];
        int n;
        if (runNum <= kNumRuns)
        n = sprintf (buffer, "%s %d >> %s &", execLoc, runNum+1, logLoc);
//        printf("%s %d >> %s\n", execLoc, runNum+1, logLoc);
        system(buffer);
        exit(EXIT_SUCCESS);

        numPreyDead = 0;
        numPredatorsDead = 0;
        reportNum = 0;
        runNum++;
        reset();
        close();
        open();
        printf("%% new report %%\n");
        printf("%% time %f %%\n", elapsedTime);
*/
      }
      
    }
    
    void redraw (const float currentTime, float elapsedTime) {
      static unsigned int frameTimeSinceReport = 0;
      frameTimeSinceReport++;
      if (frameTimeSinceReport > kReportInterval) {
        printStats(currentTime);
        frameTimeSinceReport = 0;
      }
      
      // f3 key toggles graphics (for speed)
      if (!showGraphics) return;
      
      // elapsed time constant between frames. remove for real-time performance
//      elapsedTime = kFixedFrameElapse;
      elapsedTime *= kTimeScale;
      
      // selected vehicle (user can mouse click to select another)
      AbstractVehicle& selected = *OpenSteerDemo::selectedVehicle;
      
      // vehicle nearest mouse (to be highlighted)
      AbstractVehicle& nearMouse = *OpenSteerDemo::vehicleNearestToMouse ();
      
      // update camera
      OpenSteerDemo::updateCamera (currentTime, elapsedTime, selected);
      
      // draw the prey, obstacles and home base
      for (int i = 0; i < kPreyCount; i++) prey[i]->draw ();
      drawObstacles ();
      
      // draw each predator
      for (int i = 0; i < kPredatorCount; i++) predators[i]->draw ();
      
      // highlight vehicle nearest mouse
      OpenSteerDemo::highlightVehicleUtility (nearMouse);
    }
    
    void close (void) {
      // delete prey
      for (int i = 0; i < kPreyCount; i++) {
        delete (prey[i]);
        prey[i] = NULL;
      }
      
      // delete each predator
      for (int i = 0; i < kPredatorCount; i++) {
        delete (predators[i]);
        predators[i] = NULL;
      }
      
      // clear the group of all vehicles
      all.clear();
    }
    
    void reset (void) {
      // reset the prey ("hero"/"attacker") and predators
      for (int i = 0; i<kPreyCount; i++) prey[i]->reset ();
      for (int i = 0; i<kPredatorCount; i++) predators[i]->reset ();
      
      // reset camera position
      OpenSteerDemo::position2dCamera (*prey[0]);
      
      // make camera jump immediately to new position
      OpenSteerDemo::camera.doNotSmoothNextMove ();
    }
    
    void handleFunctionKeys (int keyNumber) {
      switch (keyNumber) {
        case 1: Critters::addOneObstacle ();    break;
        case 2: Critters::removeOneObstacle (); break;
        case 3: showGraphics = !showGraphics; break;
          //        case 4: Vec3 pos = OpenSteerDemo::camera.fixedPosition;
          //          printf("pos = (%1.3f, %1.3f, %1.3f)\n", pos.x, pos.y, pos.z); break;
      }
    }
    
    void printMiniHelpForFunctionKeys (void) {
      std::ostringstream message;
      message << "Function keys handled by ";
      message << '"' << name() << '"' << ':' << std::ends;
      OpenSteerDemo::printMessage (message);
      OpenSteerDemo::printMessage ("  F1     add one obstacle.");
      OpenSteerDemo::printMessage ("  F2     remove one obstacle.");
      OpenSteerDemo::printMessage ("  F3     toggle graphics.");
      OpenSteerDemo::printMessage ("");
    }
    
    const AVGroup& allVehicles (void) {return (const AVGroup&) all;}
    
    void drawObstacles (void) {
//      const Color color (0.8f, 0.6f, 0.4f); // orange
      const Color color (0.0f, 0.0f, 0.0f); // black
//      const Color color (0.4f, 0.4f, 0.4f); // grey
      const SOG& allSO = Critters::allObstacles;
      for (SOI so = allSO.begin(); so != allSO.end(); so++) {
        drawXZCircle ((**so).radius, (**so).center, color, 40);
        drawXZCircle ((**so).radius-0.1, (**so).center, color, 40);
        drawXZCircle ((**so).radius-0.2, (**so).center, color, 40);
      }
    }
    
    // a group (STL vector) of all vehicles in the PlugIn
    std::vector<Critters*> all;
  };
  
  
  CtfPlugIn gCtfPlugIn;
  
  
  // ----------------------------------------------------------------------------
  
  
} // anonymous namespace