// simple 3D Termites class - based on code from dan shiffman
// roland snooks | kokkugia.com | 2007


class kTermites{
  
 kVec         acc;
 kVec         vel;
 kVec         pos;
 kVec         vec;
 float        maxVel;
 float        maxForce;
 float        wanScale;
 float        avoScale;
 float        attScale;
 int          pheremoneRate; // how many frames a pheremone is released
 float        rangeOfVision;

 float        wandertheta;
 float        ht = envSize*2;
 int          drawColor;
 String       TermitesType = "kTermites";
 kVec         sumSep = new kVec(0,0,0);
 boolean      seekMud;
 float        closestMudSeekDist;
 int          closestSeekMud;
 float        closestDudSeekDist;
 int          closestSeekDud;
 float        closestPheremoneSeekDist;
 int          closestSeekPheremone;
 
 // constructor simple
 kTermites(
     kVec _pos, 
     kVec _vec, 
     float _maxVel,
     float _maxForce){
   
   acc = new kVec(0,0,0);
   vel = new kVec(random(-1,1),random(-1,1),random(-1,1));    
   pos = kVec.clone(_pos);
   vec = kVec.clone(_vec);
   maxVel = _maxVel;
   maxForce = _maxForce;
   
   wanScale = .2;
   avoScale = 8;
   attScale = 10;
 
   pheremoneRate = 1;
    
   rangeOfVision = 70; 
   drawColor = 255;
   seekMud = true;
 }
 
 // constructor complete
  kTermites(
     kVec _pos, 
     kVec _vec, 
     float _maxVel,
     float _maxForce,
     kVec _vel,
    float _wanScale,
     float _avoScale,
     float _attScale,
     int _pheremoneRate,
     float _rangeOfVision){
   
   acc = new kVec(0,0,0);
   vel = kVec.clone(_vel);    
   pos = kVec.clone(_pos);
   vec = kVec.clone(_vec);
   maxVel = _maxVel;
   maxForce = _maxForce;
   wanScale = _wanScale;
   avoScale = _avoScale;
   attScale = _attScale;
   
   pheremoneRate = _pheremoneRate;
   
   rangeOfVision = _rangeOfVision; 
   drawColor = 255;
   seekMud = true;
 }
 
 
 // calculates new location
 void step(kWorld world1){ 
   
    changeState(world1.mudpop);
    depositDud(world1.dudpop); 
   
    // decide what to seek based on seekMud boolean
    if (seekMud == true){
      // seek mud + pheremones
      kVec mudSeekVec = mudSeek(world1.mudpop);
      kVec pherSeekVec = pherSeek(world1.pheremones);
      mudSeekVec.scale(1);
      pherSeekVec.scale(.5);
      println(mudSeekVec);
      acc.plus(mudSeekVec);
      acc.plus(pherSeekVec);
    }else{
      // seek dud
      kVec dudSeekVec = dudSeek(world1.dudpop);
      dudSeekVec.scale(1);
      acc.plus(dudSeekVec);
    }        


   
    //updatePop(world1.population);  //THIS CALLS ALL THE FLOCKING STUFF
    //updateObs(world1.mudpop); ///////////////BIG PROBLEM WITH THIS
    
    vel.plus(acc);
    vel.limit(maxVel);  
    pos.plus(vel);
    acc = new kVec(0,0,0);  // reset acc to 0 each iteration
    
    releasePheremone();
    
    borders(envSize);
    
 }
 
 
 void updatePop(ArrayList pop){  
    
    // call population functions
    kVec wan = this.wander();
    
    // weight vector
    wan.scale(wanScale);
    
    // add the vectors to acceleration
    acc.plus(wan);
    
    
  }    
 
 
  void updateObs(ArrayList obs){  
    kVec avo = avoid(obs);
    avo.scale(avoScale);
    acc.plus(avo);
  } 
  
  
  
  // seek pheremone
  kVec pherSeek(ArrayList pheremones){
kVec sum = new kVec(0,0,0);
    int count = 0;
    
    for (int i = 0 ; i < pheremones.size(); i++) {
      kPheremone other = (kPheremone) pheremones.get(i);
      float dist = pos.distance(other.pos);
      if ((dist > 0) && (dist < rangeOfVision)) {
        //sum.plus(other.vel);
        count++;
       // find closest pheremone
         if (count == 0){
            closestPheremoneSeekDist = dist;
            closestSeekPheremone = 0;
         }else{
           
            if(dist < closestMudSeekDist){
              closestPheremoneSeekDist = dist;
              closestSeekPheremone = i;
            }           
           
         }
         
      }
    }


    if(count == 0){
      return new kVec(0,0,0);
    }else{
      kPheremone other = (kPheremone) pheremones.get(closestSeekPheremone);
      return steer(kVec.clone(other.pos), ht); 
    }
    
  }
  
  
  // seek mud
  kVec mudSeek(ArrayList mud) {
    kVec sum = new kVec(0,0,0);
    int count = 0;
    closestMudSeekDist = 10000;
    
    for (int i = 0 ; i < mud.size(); i++) {
      kMud other = (kMud) mud.get(i);
      float dist = pos.distance(other.pos);
      if ((dist > 0) && (dist < rangeOfVision)) {
        count++;
         // find closest mud
         if (count == 0){
            closestMudSeekDist = dist;
            closestSeekMud = 0;
         }else{
           
            if(dist < closestMudSeekDist){
              closestMudSeekDist = dist;
              closestSeekMud = i;
            }           
           
         }
         
      }
    }


    if(count == 0){
      return new kVec(0,0,0);
    }else{
      kMud other = (kMud) mud.get(closestSeekMud);
      return steer(kVec.clone(other.pos), ht); 
    }
    
  }
  
  
  // seek dud 
  kVec dudSeek(ArrayList duds){
    //loop to find closest mud and return kVec
   kVec sum = new kVec(0,0,0);
    int count = 0;
    for (int i = 0 ; i < duds.size(); i++) {
      kDud other = (kDud) duds.get(i);
      float dist = pos.distance(other.pos);
      if ((dist > 0) && (dist < rangeOfVision*10)) {
       // sum.plus(other.vel);
        count++;
          // find closest mud
         if (count == 0){
            closestDudSeekDist = dist;
            closestSeekDud = 0;
         }else{
           
            if(dist < closestDudSeekDist){
              closestDudSeekDist = dist;
              closestSeekDud = i;
            }           
           
         }
         
      }
    }


    if(count == 0){
      return new kVec(0,0,0);
    }else{
      kDud other = (kDud) duds.get(closestSeekDud);
      return steer(kVec.clone(other.pos), ht);  
    }
    
  }
  
  // change state
  void changeState(ArrayList mudpop){
    if (seekMud == true){
      // loop through all muds and see if any are within range
      for (int i = 0; i < mudpop.size(); i++) {
        kMud mud = (kMud) mudpop.get(i);
        // distance between agent and mud
        float mudDist = mud.pos.distance(pos);
        if (mudDist < 10){
          seekMud = false;
          mud.depleteMud();
          drawColor = 0;
          pheremoneRate = 1;
        }      
      }
    }
    
  }
  
  
  // deposit dud
  void depositDud(ArrayList dudpop){   
    if (seekMud == false){
      boolean depositedAlready = false; // USE THIS SO IT ONLY DROPS ONE DUD PER FRAME
      // loop through all muds and see if any are within range
      for (int i = 0; i < dudpop.size(); i++) {
        kDud dud = (kDud) dudpop.get(i);
        // distance between agent and mud
        float dudDist = dud.pos.distance(pos);
        
        if (dudDist > 5 && dudDist < 30 && depositedAlready == false){  // USE THE MIN AND MAX TO HELP CHANGE HOW THE TERMITE MOUND GROWS
          
          world1.addDud(new kDud(kVec.clone(pos)));
          depositedAlready = true;
          seekMud = true;
          drawColor = 255;
          pheremoneRate = 100000000;
          
        }      
      }
    }
    
  }
  
  
  


 // steer
 kVec steer(kVec target, float threshold) {
    target.minus(pos); 
    float dist = target.length();

    if (dist > 0 && dist < threshold) {
      target.normalize();
      target.scale(maxVel);
      target.minus(vel); 
      target.limit(maxForce); 
    } 
    else {
      target = new kVec(0,0,0);
    }
    return target;
  }



  // seekmud
  kVec mudseek(kVec target, float threshold) {
    return steer(target, threshold);
  }
   
   
   
  // seekdud  
  kVec dudseek(kVec target, float threshold){
    return steer(target, threshold);
  }   
  
  
  
  // avoid
  kVec avoid(ArrayList mudpop){
    float checkStep = 20; // factor of current velocity to check ahead
    // calculate the projected future position
    kVec futurePos = kVec.clone(pos);
    kVec oVel = kVec.clone(vel);
    oVel.scale(checkStep);
    futurePos.plus(oVel);

    // find the closest mud
    for (int i = 0; i < mudpop.size(); i++) {
      kMud mud = (kMud) mudpop.get(i);
      kVec MudTarget = kVec.clone(mud.pos);
      float SeekMudDist = MudTarget.distance(futurePos);
           
      if (i != 0){
        if(closestMudSeekDist > SeekMudDist){
          closestMudSeekDist = SeekMudDist;
          closestSeekMud = i;  
        }
      }else{
        closestMudSeekDist = SeekMudDist;
        closestSeekMud = 0;
      }
    }   
 
    // if the future position is within the radius of the mud then apply lateral force
    kMud mud = (kMud) mudpop.get(closestSeekMud);   
    if(closestMudSeekDist < mud.rad){

      // oVec to = length to mud center
      float mudDist = pos.distance(mud.pos);
      oVel.normalize();
      oVel.scale(mudDist);
      oVel.plus(pos);
      oVel.minus(mud.pos); 
      oVel.normalize();
      oVel.scale(maxForce);
      
      // render line if Termites future vector is within mud
      kLine lnOb1 = new kLine(pos, futurePos); 
      lnOb1.setColor(255,0,0);
      space.append(lnOb1);

    }else{
      
      oVel = new kVec(0,0,0);
    }
    
    return oVel;
    
  }
 
 
  
  // wander - currently only working in 2d
  kVec wander() {
    // make this 3D
    float wanderR = 8;         
    float wanderD = 60;         
    float change = 0.1;
    wandertheta += random(-change,change);     

    kVec circleloc = kVec.clone(vel); 
    circleloc.normalize();            
    circleloc.scale(wanderD);          
    circleloc.plus(pos);              

    kVec circleOffSet = new kVec(wanderR*cos(wandertheta),wanderR*sin(wandertheta));
    circleOffSet.plus(circleloc);
    return steer(circleOffSet, ht); 
  }  



  void releasePheremone(){
    if(frameCount % pheremoneRate <= 0){
          world1.addPheremone(new kPheremone(kVec.clone(pos)));
    }  
  }


  void render() {

    // draw vectors  
    float vecDisScale = 2;
    
    kLine ln = new kLine(pos, vel, vel.length()*vecDisScale);
    ln.setColor(255,0,0);
    //space.append(ln);

    kLine ln1 = new kLine(pos, sumSep, sumSep.length()*vecDisScale);
    ln1.setColor(0,0,255);
    //space.append(ln1);
    
    // draw 
    kLine ln2 = new kLine(pos, vel, 3);
    ln2.setColor(drawColor);
    space.append(ln2);

  }
  
  
  void borders(float envSize) {
    if (pos.x < -envSize) pos.x = envSize;
    if (pos.y < -envSize) pos.y = envSize;
    if (pos.z < -envSize) pos.z = envSize;
    if (pos.x > envSize) pos.x = -envSize;
    if (pos.y > envSize) pos.y = -envSize;
    if (pos.z > envSize) pos.z = -envSize;
    
  }

}