/*! \file flock.diderot
 *
 * \author Lamont Samuels
 *
 * This example is an implementation of Chris Reynolds Boids algorithm. It represents an example of using Spatial communication 
 */
/*
 * COPYRIGHT (c) 2012 The Diderot Project (http://diderot-language.cs.uchicago.edu)
 * All rights reserved.
 */
real{} initVelocities = load("data/velocities.nrrd");
int numBoids = length(initVelocities)/2;
int numBoids = length(initVelocities)/2;
int strandSize = 2;
real query_radius = 50; 
int MAX_SPEED = 2;
real MAX_FORCE = 0.05; 
int stepsMax = 10000; 
real boundsFactor = 10.0;
input int width; 
input int height;  
int NORTH = 0; 
int SOUTH  = 1;
int WEST = 2;
int EAST =  3;

strand Boid(int i,real vx, real vy) {
    vec2 pos = [real(width)/2.0,real(height)/2.0]; 
    vec2 vel = [vx,vy]; 
    int steps = 0;
    int id = i; 
    output vec4 boidInfo = [pos[0],pos[1],vel[0],vel[1]]; 
    update {
        vec2 cohereVec = [0.0,0.0];
        vec2 alignVec = [0.0,0.0];  
	    vec2 seperateVec = [0.0,0.0];  
        vec4 wrapDimensions = [-boundsFactor,
                               height + boundsFactor,
                               -boundsFactor,
                               width + boundsFactor]; 
	    int count = 0; 

        foreach(Boid neighbor in sphere(query_radius)){
            real d  =  dist(pos,neighbor.pos);  	     
            
            if(d > 0) {
    	        cohereVec += neighbor.pos; 
    	        alignVec += neighbor.vel;   
    	        seperateVec = seperateVec + (normalize(pos-neighbor.pos)/d);  
	            count+=1;
            }  
        }

        if(count > 0) {
	        cohereVec /= real(count); 
    	    alignVec /= real(count); 
	        seperateVec /= real(count); 
        }

        real isZero = alignVec • alignVec; 

        // limit the force
 	    if ( isZero != 0 && |alignVec| > MAX_FORCE) {
	         alignVec = normalize(alignVec); 
             alignVec *= MAX_FORCE; 
	     }  

      
        vec2 diff = cohereVec - pos; 
        isZero = cohereVec • cohereVec; 
 
        if(|diff| > 0 && isZero != 0){
    
            cohereVec = normalize(cohereVec);  

            if(|diff| < 100.0) {
                cohereVec *= (MAX_SPEED*(|diff|/100.0)); 
      	    }else {
                cohereVec *= MAX_SPEED; 
            }

            cohereVec -= vel; 
 
	        // limit the force
 	        if (|cohereVec| > MAX_FORCE) {
	            cohereVec = normalize(cohereVec); 
                cohereVec *= MAX_FORCE; 
	        }  
   	    }
   	    else{
     	    cohereVec = [0.0,0.0]; 
        } 


        vec2 acceleration = cohereVec + alignVec + seperateVec; 
	    vel += acceleration;

 	    if (|vel| > MAX_SPEED){
	        vel = normalize(cohereVec); 
            vel *= MAX_SPEED; 
	    }   
        
        pos += vel; 
    
        /* Wrapping Code, if a boid goes outside the window */ 
        real posX = wrapDimensions[EAST] if pos[0] < wrapDimensions[WEST]
                                         else wrapDimensions[WEST] if pos[0] > wrapDimensions[EAST]
                                                                else pos[0]; 

        real posY = wrapDimensions[SOUTH] if pos[1] < wrapDimensions[NORTH]
                else wrapDimensions[NORTH] if pos[1] > wrapDimensions[SOUTH]
                                          else pos[1]; 

        pos = [posX,posY]; 

        boidInfo = [pos[0],pos[1], vel[0], vel[1]]; 
   }
}

initially {Boid(i,initVelocities{i*2},initVelocities{i*2+1}) | i in 0 .. numBoids-1 };