/*
 *  lump.cpp
 *  simplespring
 *
 *  Created by Robert Tubb on 01/06/2011.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */
#include "lump.h"
#include "testApp.h"
#include <iostream>
#include "globalForces.h"
#include "globalUI.h"
extern GlobalForces globalForces;
extern GlobalUI globalUI;
//int Lump::numLumps = 0;
//--------------------------------------------------------------
// default constr
Lump::Lump(){
	//cout << "constructing a default lump" << endl;
	mass = 10.0;
	inverseMass = 1.0/mass; // not needed - used csquared for force
	friction = 0.996;
	position.x = 0.5;
	position.y = 0.5;
	velocity.x = 0.0;
	velocity.y = 0.0;
	accel.x = 0.0;
	accel.y = 0.0;	
	numAttachedSprings = 0;
	grabbed = false;
	highlighted = false;
	constrained = false;
	totalForceMag = 0.0;
	size = 3; //sqrt(mass/3.0);
    isInScanPath = false;
    isScanPathStart = false;
    isScanPathEnd = false;
    
    previousPosition.setCoord(0.5,0.5);
    zeroRefPos.setCoord(0.0,0.0);
    constrainMode = NOT_CONSTRAINED;

    
    grabID = -1;
	
}
//--------------------------------------------------------------
// arg constructor
Lump::Lump(double aMass,double aFriction, double positionX, double positionY){
		// set members

}

//--------------------------------------------------------------
Lump::~Lump(){

}
//--------------------------------------------------------------
void Lump::attachSpring(Spring* aSpring){
	
    
    attachedSprings.push_back(aSpring);
    numAttachedSprings++;

}
//--------------------------------------------------------------
Spring * Lump::checkConnectedTo(Lump * otherLump){
    // loop thru all attached springs looking at other end
    for(int i = 0; i<attachedSprings.size(); i++){
        if(attachedSprings[i]->getLumpOnOtherEnd(this) == otherLump){
            return attachedSprings[i];
        }
    }
    return NULL;
}
//--------------------------------------------------------------
void Lump::constrain(){
    // TODO1 different constrain modes
	constrained = true;
    constrainMode = CONSTRAIN_XY;
    setZeroRefPos();
	//cout << "constraining lump "<< endl;
	
}
//--------------------------------------------------------------
void Lump::constrain(ConstrainMode aconstrainMode){
	constrained = true;
    constrainMode = aconstrainMode;
    setZeroRefPos();
	//cout << "constraining lump "<< endl;
	
}
void Lump::setInvMass(double aInvMass){
	// use csquared
	inverseMass = aInvMass;
}
//--------------------------------------------------------------

void Lump::draw(){
/*


 */

    if(grabbed){

        int xpos = position.x * ofGetHeight() + globalUI.borderSize;
        int ypos = position.y * ofGetHeight();
		// draw a circle round it
		ofSetColor(255, 0, 0);
		ofNoFill();
		ofCircle(xpos, ypos, 35.0);
		ofFill();
	}else if(isInScanPath){
        ofSetColor(0, 200, 20);
        int xpos = position.x * ofGetHeight() + globalUI.borderSize;
        int ypos = position.y * ofGetHeight();
        ofEllipse(xpos,ypos, 6, 6);
        if(isScanPathEnd){
            ofSetColor(255, 255, 0);
            ofNoFill();
            ofCircle(xpos, ypos, 6.0);
            ofFill();
        }
        if(isScanPathStart){
            ofSetColor(0, 255, 255);
            ofNoFill();
            ofCircle(xpos, ypos, 7.0);
            ofFill();
        }
        // code to display restpos and displacement
        /*
        ofSetColor(0, 0, 0);
        int rxpos = zeroRefPos.x * ofGetHeight() + 128;
        int rypos = zeroRefPos.y * ofGetHeight();
        ofEllipse(rxpos,rypos, 6, 6);  
        
        ofSetColor(100, 100, 100);

        ofLine(rxpos,rypos,xpos,ypos);  
         */
        
    }else if(highlighted){
		ofSetColor(200, 0, 0);
        int xpos = position.x * ofGetHeight() + globalUI.borderSize;
        int ypos = position.y * ofGetHeight();
        ofEllipse(xpos,ypos, 2, 2);
	}else if (constrained){
		ofSetColor(200,5,5);
        int xpos = position.x * ofGetHeight() + globalUI.borderSize;
        int ypos = position.y * ofGetHeight();
        ofEllipse(xpos,ypos, 2, 2);
	}else{
        // dont draw 'normal ' lumps
        return;
		//ofSetColor(23, 23, 200);
	}
}

//--------------------------------------------------------------
 
TwoVector Lump::applyForce(){

    
	if(grabbed || constrainMode == CONSTRAIN_XY){
        
        // don't bother
        return position;
    }
	// called LOTS so optimise
	// use spring force to calc accel - vel - pos

	TwoVector springForce(0.0,0.0);
	TwoVector totalForce(0.0,0.0);
	
	// sum up force from each attached spring
	for(int i = 0;i<attachedSprings.size(); i++){

        springForce = (attachedSprings[i])->getForce(this);
        
		//cout << "spring number " << i << " force x " << springForce.x << endl;
		totalForce.x += springForce.x;
		totalForce.y += springForce.y;
	}

    // get the global forces, gravity and so on
    totalForce.x += globalForces.getAllForceAt(position.x,position.y).x;
    totalForce.y += globalForces.getAllForceAt(position.x,position.y).y;

    if (constrainMode != CONSTRAIN_X){
        accel.x = totalForce.x*inverseMass;	
    }else{
        accel.x = 0.0;
    }
    if(constrainMode != CONSTRAIN_Y){
        accel.y = totalForce.y*inverseMass;	
    }else{
        accel.y = 0.0;
    }
    
    // DIFFERENCE EQUATIONS HERE. This is the bit that controls the movement!

    // Heun
    
    double pvx = velocity.x*friction + accel.x*2/3;
    double pvy = velocity.y*friction + accel.y*2/3;
    
    
    velocity.x = 0.75*pvx + 0.25*velocity.x;
    velocity.y = 0.75*pvy + 0.25*velocity.y;

    double px = position.x + velocity.x*2/3;
    double py = position.y + velocity.y*2/3;
    
    
    position.x = 0.75*px + 0.25*position.x;
    position.y = 0.75*py + 0.25*position.y;
     

    	// Newton's 2nd law
/*
        velocity.x += accel.x;
        velocity.x *= friction;
        position.x += velocity.x;

        velocity.y += accel.y;
        velocity.y *= friction;
        position.y += velocity.y;           
*/


    // WALLS
    if (position.x < 0.0){
        position.x = -position.x;
        velocity.x = -velocity.x * globalForces.wallBounce;
        
    }  else  if (position.x > 1.0){
        position.x = 2.0 - position.x;
        velocity.x = -velocity.x * globalForces.wallBounce;
        
    }
    if (position.y < 0.0){
        position.y = -position.y;
        velocity.y = -velocity.y * globalForces.wallBounce;
        
    }  else  if (position.y > 1.0){
        position.y = 2.0 - position.y;
        velocity.y = -velocity.y * globalForces.wallBounce;
        
    }    
	return position;
	
}
//---------------------------------------------
void Lump::homingFilter(double amt){
    // includes a little bit of the zero pos in the position, so sound will exponentially decay 

    if (constrained || grabbed) return;

    position.x = (1 - amt)*position.x + amt*zeroRefPos.x;
    position.y = (1 - amt)*position.y + amt*zeroRefPos.y;
    
}
//---------------------------------------------
void Lump::averagingFilter(double amt){
    // NOT USED AVERAGING FILTER NOW IN MESH
    // amt is between 0 and 1
    if (constrained || grabbed) return;
    double avx = 0.0, avy = 0.0;
    // average the position of all the attached lumps
    for(int i = 0;i<attachedSprings.size(); i++){
        
        Lump* otherLump = attachedSprings[i]->getLumpOnOtherEnd(this);
        avx += otherLump->position.x;
        avy += otherLump->position.y;
	}
    avx /= attachedSprings.size();
    avy /= attachedSprings.size();
    
    // mix in the average with the 'real'
    position.x = (1 - amt)*position.x + amt*avx;
    position.y = (1 - amt)*position.y + amt*avy;
    
}
//---------------------------------------------
TwoVector Lump::averageOfConnected(){
    TwoVector av;
    if (constrained || grabbed) return position; // don't want constrained ones moving
    //TODO what if edges unconstrained? this is why filtered unconstrained just ends up as a line...

    // average the position of all the attached lumps
    for(int i = 0;i<attachedSprings.size(); i++){
        
        Lump* otherLump = attachedSprings[i]->getLumpOnOtherEnd(this);
        av.x += otherLump->position.x;
        av.y += otherLump->position.y;
	}
    av.x /= attachedSprings.size();
    av.y /= attachedSprings.size();
    
    return av;
    
}
//--------------------------------------------------------------
void Lump::setPosition(double ax, double ay){
    // set INITIAL position
    // Called from mesh set up. not used for updates
	
	position.x = ax;
	position.y = ay;
	zeroRefPos.x = ax;
	zeroRefPos.y = ay;
}
//--------------------------------------------------------------
void Lump::setVelocity(double ax, double ay){
	
	velocity.x = ax;
	velocity.y = ay;
    
}
//--------------------------------------------------------------

void Lump::setFriction(double aF){
    friction = aF;
}
//--------------------------------------------------------------

void Lump::setZeroRefPos(){
    // sets the reference point from which displacement is measured for scan amplitudes
    zeroRefPos = position;
}
//--------------------------------------------------------------
double Lump::getTotalForceMag(){
	return totalForceMag;
}
//--------------------------------------------------------------
double Lump::scanDisplacement(){
    // returns the absolute distance from 'home'
    return position.distanceTo(zeroRefPos);
}
//--------------------------------------------------------------
double Lump::scanLumpSpeed(){
    // returns the absolute magnitude of the lumps velocity
    return velocity.norm();
}
//--------------------------------------------------------------
double Lump::scanYPos(){
    // returns the y displ
    return position.y - zeroRefPos.y;
}
//--------------------------------------------------------------
double Lump::scanXPos(){
    // returns the x displ
    return position.x - zeroRefPos.x;
}
// -------------------
double Lump::scanRadialDisplacement(){
    // returns the distance from circle zero line
    // need to know where the centre point of the circle is and default radius

    //return position.y - 0.5;
    return position.distanceTo(zeroRefPos);
    
}
//--------------------------------------------------------------
void Lump::addToScanPath(){
    isInScanPath = true;
    setZeroRefPos();
}
//--------------------------------------------------------------
void Lump::removeFromScanPath(){
    isInScanPath = false;

}
//--------------------------------------------------------------
void Lump::grab(int aGrabID){
	// hover hilight doesn't work on touchscreens if(highlighted) grabbed = true;
    grabbed = true;
    grabID = aGrabID;
    velocity.x = 0.0;
    velocity.y = 0.0;
}
//--------------------------------------------------------------
void Lump::drag(double ax, double ay, int aGrabID){
	if(aGrabID == grabID){
        //cout << "dragging lump ID: " << grabID << endl;
        position.x = ax;
        position.y = ay;
        velocity.x = previousPosition.x - position.x;
        velocity.y = previousPosition.y - position.y;
        previousPosition = position; // sets velocity too          
    } 
}
//--------------------------------------------------------------
void Lump::unGrab(){
    cout << "ungrabbed something\n";
	grabbed = false;
    grabID = -1;
    velocity.x = 0.0;
    velocity.y = 0.0;
}
//--------------------------------------------------------------
void Lump::unconstrain(){
	constrained = false;
    constrainMode = NOT_CONSTRAINED;
}


//--------------------------------------------------------------

bool Lump::isGrabbed(){
	return grabbed;
}

//--------------------------------------------------------------

void Lump::highlight(){
	highlighted = true;
}

//--------------------------------------------------------------

void Lump::unhighlight(){
	highlighted = false;
}
//--------------------------------------------------------------
