
#include "vectorField.h"

vectorField::vectorField(int _fieldw, int _fieldh, float _scalew, float _scaleh){

 fieldw = _fieldw;
 fieldh = _fieldh;
 scalew = _scalew;
 scaleh = _scaleh;
 
 FIELD_SIZE = fieldw * fieldh;
 field = new float*[FIELD_SIZE];
 for (int i = 0; i < FIELD_SIZE; i++){
   field[i] = new float [2];
   field[i][0] = 0.0f;
   field[i][1] = 0.0f;
 }
}
  
	//------------
void vectorField::clear(){
	for (int i = 0; i < FIELD_SIZE; i++){
		field[i][0] = 0;
		field[i][1] = 0;
	}
}
  
//------------
void vectorField::fadeField(float fadeAmount){
  for (int i = 0; i < FIELD_SIZE; i++){
   field[i][0] *= fadeAmount;
   field[i][1] *= fadeAmount;
 }
}

  
void vectorField::ltValToVal(float ltVal, float val){
     for (int i = 0; i < FIELD_SIZE; i++){
      //if (field[i] < ltVal) field[i] = val;
    }
}
  
//------------
void vectorField::randomizeField(float scale){
 for (int i = 0; i < FIELD_SIZE; i++){
   // random between -1 and 1 ---- (Math.random()*2.0f-1.0f) 
   
   float ht = (float)(ofRandomuf()*2.0f-1.0f) * scale;
   field[i][0] += ht;
   ht = (float)(ofRandomuf()*2.0f-1.0f) * scale;
   field[i][1] += ht;
 } 
}
  
void vectorField::draw(){
 
	float stepx = scalew;
	float stepy = scaleh;
	ofVec3f temp(0,0);
 
	 for (int i = 0; i < fieldw; i++){
	   for (int j = 0; j < fieldh; j++){
	   
	     // pos in array
	     int pos = j * fieldw + i;
	     // pos on screen
	     float px = 	i * stepx;
	     float py = 	j * stepy;
	     float pctx = (float)i/(float)(fieldw-1);
	     float pcty = (float)j/(float)(fieldh-1);
	     getVelFromPos(pctx, pcty, temp);
	     ofLine(px, py, px + temp.x*5, py + temp.y*5);
	     //mom.noStroke();
	     //mom.fill(ht*255.0f);
	     //mom.rect(px,py, scalew, scaleh);
	  }
	}
}
 
void vectorField::getVelFromPos(float xpos, float ypos, ofVec3f &v){
 
	// where are we in the array
	int fieldPosX = (int)(xpos * (float)fieldw);
	int fieldPosY = (int)(ypos * (float)fieldh);

	// saftey	
	fieldPosX = MAX(0, MIN(fieldPosX, fieldw-1));
	fieldPosY = MAX(0, MIN(fieldPosY, fieldh-1));

	int pos = (fieldPosY) * fieldw + fieldPosX; 
	float diffx = field[pos][0];
	float diffy = field[pos][1];
	v.set(diffx, diffy,0);
}
  
  
  
void vectorField::erodePos(float xpos, float ypos){
    
    /*
    // where are we in the array
    int fieldPosX = (int)(xpos * (float)fieldw);
    int fieldPosY = (int)(ypos * (float)fieldh);
    
    // saftey	
    fieldPosX = MAX(1, MIN(fieldPosX, fieldw-2));
    fieldPosY = MAX(1, MIN(fieldPosY, fieldh-2));
    
    // pos in array
    
    int pnw = (fieldPosY+1) * fieldw + fieldPosX - 1;
    int pn_ = (fieldPosY+1) * fieldw + fieldPosX; 
    int pne = (fieldPosY+1) * fieldw + fieldPosX + 1;
    int p_w = (fieldPosY) * fieldw + fieldPosX - 1;
    int pos = (fieldPosY) * fieldw + fieldPosX; 
    int p_e = (fieldPosY) * fieldw + fieldPosX + 1;
    int psw = (fieldPosY-1) * fieldw + fieldPosX - 1;
    int ps_ = (fieldPosY-1) * fieldw + fieldPosX; 
    int pse = (fieldPosY-1) * fieldw + fieldPosX + 1;
    
    float val = 0.012f;
    val *= 3.0f;
    field[pnw]   -= val;
    field[pn_]   -= val*2;
    field[pne]   -= val;
    field[p_w]   -= val*2;
    field[pos]   -= val*4;
    field[p_e]   -= val*2;
    field[psw]   -= val;
    field[ps_]   -= val*2;
    field[pse]   -= val;
    
    
    //[zach]
    //v.set(diffx*0.1f, diffy*0.1f);
    //v (field[pos]);
    */
  }
  
  
  //-------------
  void vectorField::addToFieldAnInwardCircleShape(float x, float y, float radius, float strength){
  	
    int fieldPosX = (int)(x * (float)fieldw);
    int fieldPosY = (int)(y * (float)fieldh);
	ofVec3f diff(0,0);;
	
    for (int i = 0; i < fieldw; i++){
      for (int j = 0; j < fieldh; j++){
        int pos = j * fieldw + i;			
        float distance = (float)sqrt((fieldPosX-i)*(fieldPosX-i) + 
		                 (fieldPosY-j)*(fieldPosY-j));
		                 
		
		
        if (distance < radius){
        
			diff.set(fieldPosX,fieldPosY,0);
			diff.x -= i;
			diff.y -= j; //sub(i,j);
			diff.normalize();

			if (distance < 0.001f) distance = 0.001f;
			float pct = 1.0f - (distance / radius);
			float strongness = strength * pct;
			field[pos][0] -= strongness * diff.x;
			field[pos][1] -= strongness * diff.y;
			//make a unit vector between origin and pt
        }
      }	
    }
  } 
  
  
  //-------------
  void vectorField::addToFieldAnOutwardCircleShape(float x, float y, float radius, float strength){
  	
    int fieldPosX = (int)(x * (float)fieldw);
    int fieldPosY = (int)(y * (float)fieldh);
    
    int startX 		= fieldPosX - radius;
    int endX 		= fieldPosX + radius;
    int startY 		= fieldPosY - radius;
    int endY 		= fieldPosY + radius;
    
    
    startX = MAX(0, startX);
    endX = MIN(fieldw-1, endX);
    startY = MAX(0, startY);
    endY = MIN(fieldh-1, endY);
    
    
	ofVec3f diff(0,0);;
	
    for (int i = startX; i < endX; i++){
      for (int j = startY; j < endY; j++){
      
        int pos = j * fieldw + i;			
        float distance = (float)sqrt((fieldPosX-i)*(fieldPosX-i) + 
		                 (fieldPosY-j)*(fieldPosY-j));
        
        
        
        if (distance < radius){
          
          	diff.set(fieldPosX,fieldPosY,0);
			diff.x -= i;
			diff.y -= j; //sub(i,j);
			diff.normalize();
          
          if (distance < 0.001f) distance = 0.001f;
          float pct = 1.0f - (distance / radius);
          float strongness = strength * pct;
          float unit_px = ( fieldPosX - i) / distance;
          float unit_py = ( fieldPosY - j) / distance;
          //field[pos] -=  strongness;
          
          field[pos][0] += strongness * diff.x;
		  field[pos][1] += strongness * diff.y;
          
          //make a unit vector between origin and pt
        }
      }	
    }
    
  }
  
  
  //-------------
  void vectorField::addToFieldASpecialCircleShape(float x, float y, float vx, float vy, float radius, float strength){
  	
    int fieldPosX = (int)(x * (float)fieldw);
    int fieldPosY = (int)(y * (float)fieldh);
	ofVec3f diff(0,0);;
	
	
	  
    int startX 		= fieldPosX - radius;
    int endX 		= fieldPosX + radius;
    int startY 		= fieldPosY - radius;
    int endY 		= fieldPosY + radius;
    
    
    startX = MAX(0, startX);
    endX = MIN(fieldw-1, endX);
    startY = MAX(0, startY);
    endY = MIN(fieldh-1, endY);
	
	
     for (int i = startX; i < endX; i++){
      for (int j = startY; j < endY; j++){
      
        int pos = j * fieldw + i;			
        float distance = (float)sqrt((fieldPosX-i)*(fieldPosX-i) + 
		                 (fieldPosY-j)*(fieldPosY-j));
        if (distance < radius && distance > 0){
          float pct = 1.0f - (distance / radius);
          float strongness = strength * pct;
          float unit_px = ( fieldPosX - i) / distance;
          float unit_py = ( fieldPosY - j) / distance;
          field[pos][0] += vx * strongness;
          field[pos][1] += vy * strongness;
          
          //make a unit vector between origin and pt
        }
      }	
    }
  }
  



void vectorField::addToFieldALine(float x1, float y1, 
							float x2, float y2,
							float vx, float vy, float radius, float strength){
									
	
	int ix1 = (int)(x1 * (float)fieldw);
	int ix2 = (int)(x2 * (float)fieldw);
	int iy1 = (int)(y1 * (float)fieldh);
	int iy2 = (int)(y2 * (float)fieldh);
	
	
	
	ix1 = MAX(0, MIN(ix1, fieldw-1));
	iy1 = MAX(0, MIN(iy1, fieldh-1));
	ix2 = MAX(0, MIN(ix2, fieldw-1));
	iy2 = MAX(0, MIN(iy2, fieldh-1));
	
	
	
	Bresenham(ix1, iy1, ix2, iy2, 1,1, radius, strength, false);
	//Bresenham(ix1, iy1, ix2, iy2, vx, vy, radius, strength, true);
								
}



#define SGN(a)          (((a)<0) ? -1 : 1)


void vectorField::Bresenham(int x1, int y1, int x2, int y2, float vx, float vy, float radius, float strength, bool bOutward){
    
	int d, x, y, ax, ay, sx, sy, dx, dy;

	dx = x2-x1;  ax = ABS(dx)<<1;  sx = SGN(dx);
	dy = y2-y1;  ay = ABS(dy)<<1;  sy = SGN(dy);

	x = x1;
	y = y1;
	if (ax>ay) {  /* x dominant */
			d = ay-(ax>>1);
			counter = 0;
			for (;;) {
	 			//counter++;
	 			//SetPixel(x, y, vx, vy, radius, strength);
	 			 //if (counter % 2 == 0)  
	 			 addToFieldASpecialCircleShape(x/(float)(fieldw), y/(float)(fieldh),vx, vy, radius/2.0f, strength);
	 			
	 
	 if (x==x2) return;
	 if (d>=0) {
	y += sy;
	d -= ax;
	 }
	 x += sx;
	 d += ay;
	}
	}
	else {   /* y dominant */
	d = ax-(ay>>1);
	counter = 0;
	for (;;) {
		//ounter++;
	 	//SetPixel( (float)x, y,  vx, vy, radius, strength);
	 	addToFieldASpecialCircleShape(x/(float)(fieldw), y/(float)(fieldh),vx, vy, radius/2.0f, strength);
	 			
	 if (y==y2) return;
	 if (d>=0) {
	x += sx;
	d -= ay;
	 }
	 y += sy;
	 d += ax;
	}
	}
}

