#include "particle.h"
#include "ofMain.h"
#define OF_ADDON_USING_OFXVECTORMATH
#include "ofAddons.h"

//------------------------------------------------------------
particle::particle(){
	setInitialCondition(0,0,0,0,0,0);
	damping = 0.025f;
	radius = 15;
	maxTrailLength = 20;
	counter = 0;
	distanceToNearestConnection = 999999;
	bDraw = false;
	bAlive = true;
	lifespan = 400;
	
	/*
	seperation.distance		= 35;
	alignment.distance		= 80;
	 seperation.strength		= .005;
	 alignment.strength		= .005;

	
	cohesion.distance		= 90;
	cohesion.strength		= .005;
	*/
}

//------------------------------------------------------------
void particle::resetForce(){
    // we reset the forces every frame
    frc.set(0,0,0);
}

//------------------------------------------------------------
void particle::addForce(float x, float y, float z){
    // add in a force in X and Y for this frame.
    frc.x = frc.x + x;
    frc.y = frc.y + y;
	frc.z = frc.z + z;
}

//------------------------------------------------------------
void particle::addRepulsionForce(float x, float y, float z, float radius, float scale){
    
	// ----------- (1) make a vector of where this position is: 
	
	ofxVec3f posOfForce;
	posOfForce.set(x,y,z);
	
	// ----------- (2) calculate the difference & length 
	
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
	
	// ----------- (3) check close enough
	
	bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
	
	// ----------- (4) if so, update force
    
	if (bAmCloseEnough == true){
		if(bDraw == true){
			glBegin(GL_LINES);
			glVertex3f(pos.x,pos.y,pos.z);
			glVertex3f(x,y,z);
			glEnd();
		}
		float pct = 1 - (length / radius);  // stronger on the inside
        diff.normalize();
		frc.x = frc.x + diff.x * scale * pct;
        frc.y = frc.y + diff.y * scale * pct;
		frc.z = frc.z + diff.z * scale * pct;
    }
}

//------------------------------------------------------------
void particle::addAttractionForce(float x, float y, float z, float radius, float scale){
    
	// ----------- (1) make a vector of where this position is: 
	
	ofxVec3f posOfForce;
	posOfForce.set(x,y,z);
	
	// ----------- (2) calculate the difference & length 
	
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
	
	// ----------- (3) check close enough
	
	bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
	
	// ----------- (4) if so, update force
    
	if (bAmCloseEnough == true){
		if(bDraw == true){
			glBegin(GL_LINES);
			glVertex3f(pos.x,pos.y,pos.z);
			glVertex3f(x,y,z);
			glEnd();
		}
		float pct = 1 - (length / radius);  // stronger on the inside
		diff.normalize();
		frc.x = frc.x - diff.x * scale * pct;
        frc.y = frc.y - diff.y * scale * pct;
		frc.z = frc.z - diff.z * scale * pct;
    }
}

//------------------------------------------------------------
void particle::addCounterClockwiseForce(float x, float y, float z, float radius, float scale){
	
	ofxVec3f posOfForce;
	posOfForce.set(x,y,z);
		
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
		
	bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
	    
	if (bAmCloseEnough == true){
		if(bDraw == true){
			glBegin(GL_LINE_LOOP);
				glVertex3f(pos.x,pos.y,pos.z);
				glVertex3f(posOfForce.x,posOfForce.y,posOfForce.z);
			glEnd();
		}
		float pct = 1 - (length / radius);
		diff.normalize();
		frc.x = frc.x + diff.y * scale * pct;
        frc.y = frc.y;// - diff.x * scale * pct;
		frc.z = frc.z - diff.z * scale * pct;
    }
}


//------------------------------------------------------------
void particle::addRepulsionForce(particle &p, float radius, float scale){
	
	// ----------- (1) make a vector of where this particle p is: 
	ofxVec3f posOfForce;
	posOfForce.set(p.pos.x,p.pos.y,p.pos.z);
	
	// ----------- (2) calculate the difference & length 
	
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
	//float length	= diff.approximateLength();
	
	//cout << length << ":" << approxLength << endl;
	
	// ----------- (3) check close enough
	
	bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
	
	// ----------- (4) if so, update force
	
	if (bAmCloseEnough == true){
		if(bDraw == true){
			glBegin(GL_LINES);
			glVertex3f(pos.x,pos.y,pos.z);
			glVertex3f(posOfForce.x,posOfForce.y,posOfForce.z);
			glEnd();
		}
		float pct = 1 - (length / radius);  // stronger on the inside
		diff.normalize();
		frc.x = frc.x + diff.x * scale * pct;
        frc.y = frc.y + diff.y * scale * pct;
		frc.z = frc.z + diff.z * scale * pct;
		p.frc.x = p.frc.x - diff.x * scale * pct;
        p.frc.y = p.frc.y - diff.y * scale * pct;
		p.frc.z = p.frc.z - diff.z * scale * pct;
    }
}

//------------------------------------------------------------
void particle::addAttractionForce(particle & p, float radius, float scale){
	
	// ----------- (1) make a vector of where this particle p is: 
	ofxVec3f posOfForce;
	posOfForce.set(p.pos.x,p.pos.y,p.pos.z);
	
	// ----------- (2) calculate the difference & length 
	
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
	
	// ----------- (3) check close enough
	
	bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
	
	// ----------- (4) if so, update force
    
	if (bAmCloseEnough == true){
		if(bDraw == true){
			glBegin(GL_LINES);
			glVertex3f(pos.x,pos.y,pos.z);
			glVertex3f(p.pos.x,p.pos.y,p.pos.z);
			glEnd();
		}
		float pct = 1 - (length / radius);  // stronger on the inside
		diff.normalize();
		frc.x = frc.x - diff.x * scale * pct;
        frc.y = frc.y - diff.y * scale * pct;
		frc.z = frc.z - diff.z * scale * pct;
		p.frc.x = p.frc.x + diff.x * scale * pct;
        p.frc.y = p.frc.y + diff.y * scale * pct;
		p.frc.z = p.frc.z + diff.z * scale * pct;
    }
	
}

//------------------------------------------------------------
void particle::addClockwiseForce(particle &p, float radius, float scale){
	
	// ----------- (1) make a vector of where this particle p is: 
	ofxVec3f posOfForce;
	posOfForce.set(p.pos.x,p.pos.y,p.pos.z);
	
	// ----------- (2) calculate the difference & length 
	
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
	
	// ----------- (3) check close enough
	
	bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
	
	// ----------- (4) if so, update force
    
	if (bAmCloseEnough == true){
		float pct = 1 - (length / radius);  // stronger on the inside
		diff.normalize();
		if(bDraw == true){
			glBegin(GL_LINES);
			glVertex3f(pos.x,pos.y,pos.z);
			glVertex3f(p.pos.x,p.pos.y,p.pos.z);
			glEnd();
		}
		frc.x = frc.x - diff.y * scale * pct;
        frc.y = frc.y + diff.x * scale * pct;
		p.frc.x = p.frc.x + diff.y * scale * pct;
        p.frc.y = p.frc.y - diff.x * scale * pct;
    }
}

//------------------------------------------------------------
void particle::addCounterClockwiseForce(particle &p, float radius, float scale){
	
	ofxVec3f posOfForce;
	posOfForce.set(p.pos.x,p.pos.y,p.pos.z);
		
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
		
	bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
	    
	if (bAmCloseEnough == true){
		float pct = 1 - (length / radius);  // stronger on the inside
		diff.normalize();
		if(bDraw == true){
			glBegin(GL_LINES);
			glVertex3f(pos.x,pos.y,pos.z);
			glVertex3f(p.pos.x,p.pos.y,p.pos.z);
			glEnd();
		}
		frc.x = frc.x + diff.y * scale * pct;
        frc.y = frc.y - diff.x * scale * pct;
		p.frc.x = p.frc.x - diff.y * scale * pct;
        p.frc.y = p.frc.y + diff.x * scale * pct;
    }
}
//------------------------------------------------------------
void particle::addRepulsionForceOnAxis(float x, float y, float z, float radius, float scaleX, float scaleY, float scaleZ){
		
	ofxVec3f posOfForce;
	posOfForce.set(x,y,z);
		
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
		
	bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
	    
	if (bAmCloseEnough == true){
		float pct = 1 - (length / radius);
        diff.normalize();
		if(bDraw == true){
			glBegin(GL_LINES);
			glVertex3f(pos.x,pos.y,pos.z);
			glVertex3f(x,y,z);
			glEnd();
		}
		frc.x = frc.x + diff.x * scaleX * pct;
        frc.y = frc.y + diff.y * scaleY * pct;
		frc.z = frc.z + diff.z * scaleZ * pct;
    }
	
}

//------------------------------------------------------------
void particle::addCounterClockwiseForceOnAxis(float x, float y, float z, float radius, float scaleX, float scaleY, float scaleZ){

	ofxVec3f posOfForce;
	posOfForce.set(x,y,z);
		
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
		
	bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
	
	if (bAmCloseEnough == true){
		float pct = 1 - (length / radius);
		diff.normalize();
		if(bDraw == true){
			glBegin(GL_LINES);
			glVertex3f(pos.x,pos.y,pos.z);
			glVertex3f(x,y,z);
			glEnd();
		}
		
		if(scaleX > scaleY && scaleX > scaleZ){
			frc.x = frc.x + diff.y * scaleX * pct;
			frc.y = frc.y - diff.x * scaleY * pct;
			frc.z = frc.z;// - diff.z * scaleZ * pct;
		} else if(scaleY > scaleX && scaleY > scaleZ){
			frc.x = frc.x - diff.y * scaleX * pct;
			frc.y = frc.y + diff.x * scaleY * pct;
			frc.z = frc.z;// - diff.z * scaleZ * pct;
		} else if(scaleZ > scaleX && scaleZ > scaleY){
			frc.x = frc.x;// - diff.y * scaleX * pct;
			frc.y = frc.y - diff.x * scaleY * pct;
			frc.z = frc.z + diff.z * scaleZ * pct;
		}
	}
}



//------------------------------------------------------------
float particle::distanceTo(particle &p){
	ofxVec3f posOfForce;
	posOfForce.set(p.pos.x,p.pos.y,p.pos.z);
	ofxVec3f diff	= pos - posOfForce;
	float length	= diff.length();
	return length;
}

/*/------------------------------------------------------------
void particle::addForFlocking(particle &p){
	
	ofxVec3f diff, diffNormalized;
	float distance;
	
	diff			= p.pos - pos;
	distance		= diff.length();
	diffNormalized	= diff;
	diffNormalized.normalize();
	
	if( distance > 0 && distance < seperation.distance ){
		seperation.sum += diffNormalized;
		seperation.count++;
	}
	
	if( distance > 0 && distance < alignment.distance ){
		alignment.sum += p.vel.normalized();
		alignment.count++;
	}
	
	if( distance > 0 && distance < cohesion.distance ){
		cohesion.sum += p.pos;
		cohesion.count++;
	}
}

//------------------------------------------------------------
void particle::addFlockingForce(){
	
	// seperation
	if(seperation.count > 0){
		seperation.sum /= (float)seperation.count;
	}
	
	// alignment
	if(alignment.count > 0){
		alignment.sum /= (float)alignment.count;
	}
	
	// cohesion
	if(cohesion.count > 0){
		cohesion.sum /= (float)cohesion.count;
		cohesion.sum -= pos;
	}
	
	float sepFrc 	= seperation.strength;
	float cohFrc 	= cohesion.strength;
	float alignFrc 	= alignment.strength;
	
	frc -= (seperation.sum.normalized()		    * sepFrc);
	frc += (alignment.sum.normalized()			* alignFrc);
	frc += (cohesion.sum.normalized()			* cohFrc);
}*/


//------------------------------------------------------------
void particle::addDampingForce(){
	
	// the usual way to write this is  vel *= 0.99
	// basically, subtract some part of the velocity 
	// damping is a force operating in the oposite direction of the 
	// velocity vector
	
    frc.x = frc.x - vel.x * damping;
    frc.y = frc.y - vel.y * damping;
	frc.z = frc.z - vel.z * damping;
}

//------------------------------------------------------------
void particle::setInitialCondition(float px, float py, float pz, float vx, float vy, float vz){
    pos.set(px,py,pz);
	vel.set(vx,vy,vz);
}

//------------------------------------------------------------
void particle::update(){	
	vel = vel + frc;
	pos = pos + vel;
	
	ofxVec3f center(0,0,0);
	ofxVec3f diff = center - pos;
	diff.normalize();
	diff = 1 - diff;
	
	radius = vel.length()*4 + 5;
	
	if(counter%3==0){
		addPointToTrail(pos.x,pos.y,pos.z);
	}
	
	
	counter++;
	if(counter > lifespan){
		bAlive = false;
	} else {
		bAlive = true;	
	}
}
//------------------------------------------------------------
void particle::addPointToTrail(float x, float y, float z){
	if(trail.size() > maxTrailLength){
		trail.erase(trail.begin());
	}
	ofxVec3f myPoint = ofxVec3f(x,y,z);
	trail.push_back(myPoint);
}


//------------------------------------------------------------
void particle::draw(){
	ofSetColor(255,255,255);
	glPushMatrix();
		glTranslatef(pos.x,pos.y,pos.z);
		//glutSolidSphere(radius,20,1);
	ofRect(0,0,1,1);
	glPopMatrix();
}

void particle::drawSphere(){
	ofSetColor(0,0,0,255);
	glPushMatrix();
	glTranslatef(pos.x,pos.y,pos.z);
		glutSolidSphere(radius,20,20);
		//ofRect(0,0,1,1);
	glPopMatrix();
}


void particle::drawWithTexture(ofTexture &tex){
	ofSetColor(255,255,255);
	glPushMatrix();
	glTranslatef(pos.x-(radius),pos.y-(radius),pos.z);
		tex.draw(0,0,radius*2,radius*2);
		//glRotatef(90,1,0,0);
		//glTranslatef(0,32,32);
		//tex.draw(0,0,64,64);
	glPopMatrix();
}

void particle::drawWithTextureScaledByVelocity(ofTexture &tex, int target){
	ofSetColor(255,255,255);
	glPushMatrix();
		glTranslatef(pos.x-(target/2),pos.y-(target/2),pos.z);
		tex.draw(0,0,target*vel.normalized().length(),target*vel.normalized().length());
		//glRotatef(90,1,0,0);
		//glTranslatef(0,32,32);
		//tex.draw(0,0,64,64);
	glPopMatrix();
}


//------------------------------------------------------------
void particle::drawTrail(ofTexture &line, ofTexture &smoke){
	float xp, yp, zp;
    float xOff, yOff, zOff;
    
    glBegin(GL_QUAD_STRIP);
    
    for (int i=1; i < trail.size(); i++){
		float per     = (float)i/(float)(trail.size()-1);
		xp            = trail[i].x;
		yp            = trail[i].y;
		zp            = trail[i].z;
		
		if ( i > 1 ){
			ofxVec3f perp0 = trail[i] - trail[i-1];
			ofxVec3f one = ofxVec3f();
			one.set(0,1,0);
			ofxVec3f perp1 = perp0.crossed(one);
			perp1.normalize();
			ofxVec3f perp2 = perp0.crossed(perp1);
			perp2.normalize();
			perp1 = perp0.cross( perp2 ).normalize();
			
			xOff        = perp1.x * radius * per * .05;
			yOff        = perp1.y * radius * per * .05;
			zOff        = perp1.z * radius * per * .05;
			
			glColor4f( per, per*.5, 1.5 - per, per);
			//ofSetColor(255,255,255,150);
			glVertex3f( xp - xOff, yp - yOff, zp - zOff );
			glVertex3f( xp + xOff, yp + yOff, zp + zOff );
		}
		
		
    }
	float per     = 0;
	ofxVec3f perp0 = pos - trail[trail.size()];
	ofxVec3f one = ofxVec3f();
	one.set(0,1,0);
	ofxVec3f perp1 = perp0.crossed(one);
	perp1.normalize();
	ofxVec3f perp2 = perp0.crossed(perp1);
	perp2.normalize();
	perp1 = perp0.cross( perp2 ).normalize();
	
	xOff        = perp1.x * radius * per * .05;
	yOff        = perp1.y * radius * per * .05;
	zOff        = perp1.z * radius * per * .05;
	
	glColor4f( per, per*.5, 1.5 - per, per);
	//ofSetColor(255,255,255,150);
	glVertex3f( xp - xOff, yp - yOff, zp - zOff );
	glVertex3f( xp + xOff, yp + yOff, zp + zOff );
	
	
    glEnd();
	
	for (int i=1; i < trail.size(); i++){
		if(i%20 == 0){
			//smoke.draw(trail[i].x,trail[i].y,256,256);
		}
	}

}


//------------------------------------------------------------
void particle::drawConnection(){
	ofSetColor(255,255,255,25);
	glLineWidth(1);
	glBegin(GL_LINE_STRIP);
		glVertex3f(pos.x,pos.y,pos.z);
		glVertex3f(connectedTo.x,connectedTo.y,connectedTo.z);
	glEnd();
}


//------------------------------------------------------------
void particle::bounceOffWalls(){
	
	// sometimes it makes sense to damped, when we hit
	bool bDampedOnCollision = true;
	bool bDidICollide = false;
	
	// what are the walls
	float minx = 0;
	float miny = 0;
	float minz = 0;
	float maxx = ofGetWidth();
	float maxy = ofGetHeight();
	float maxz = ofGetHeight();
	
	if (pos.x > maxx){
		pos.x = maxx; // move to the edge, (important!)
		vel.x *= -1;
		bDidICollide = true;
	} else if (pos.x < minx){
		pos.x = minx; // move to the edge, (important!)
		vel.x *= -1;
		bDidICollide = true;
	}
	
	if (pos.y > maxy){
		pos.y = maxy; // move to the edge, (important!)
		vel.y *= -1;
		bDidICollide = true;
	} else if (pos.y < miny){
		pos.y = miny; // move to the edge, (important!)
		vel.y *= -1;
		bDidICollide = true;
	}
	
	if (pos.z > maxz){
		pos.z = maxz; // move to the edge, (important!)
		vel.z *= -1;
		bDidICollide = true;
	} else if (pos.z < minz){
		pos.z = minz; // move to the edge, (important!)
		vel.z *= -1;
		bDidICollide = true;
	}
	
	if (bDidICollide == true && bDampedOnCollision == true){
		vel *= 0.3;
	}
	
}

//------------------------------------------------------------
void particle::bounceOff3dBounds(ofxVec3f xMin, ofxVec3f xMax, ofxVec3f yMin, ofxVec3f yMax, ofxVec3f zMin, ofxVec3f zMax){
	bool bDampedOnCollision = false;
	bool bDidICollide = false;
	
	if (pos.x > xMax.x){
		pos.x = xMax.x;
		vel.x *= -1;
		bDidICollide = true;
	} else if (pos.x < xMin.x){
		pos.x = xMin.x;
		vel.x *= -1;
		bDidICollide = true;
	}
	
	if (pos.y > yMax.y){
		pos.y = yMax.y;
		vel.y *= -1;
		bDidICollide = true;
	} else if (pos.y < yMin.y){
		pos.y = yMin.y;
		vel.y *= -1;
		bDidICollide = true;
	}
	
	if (pos.z > zMax.z){
		pos.z = zMax.z;
		vel.z *= -1;
		bDidICollide = true;
	} else if (pos.z < zMin.z){
		pos.z = zMin.z;
		vel.z *= -1;
		bDidICollide = true;
	}
	
	if (bDidICollide == true && bDampedOnCollision == true){
		vel *= 0.3;
	}
}