// Amira Abdel-Rahman
// (c) Massachusetts Institute of Technology 2019

function initialize(setup){
	var voxCount = setup.nodes.length;
	for(var i=0;i<voxCount;i++){
		setup.nodes[i].currPosition=new THREE.Vector3(setup.nodes[i].position.x,setup.nodes[i].position.y,setup.nodes[i].position.z);
    } 
}



function doTimeStep(setup,dt){
    var voxCount = setup.nodes.length;
	for(var i=0;i<voxCount;i++){
        timeStep(dt,setup.nodes[i]);
    } 

}

function timeStep(dt,node){
	// var previousDt = dt;
	// var linMom=node.linMom.clone();
    // var angMom=node.angMom.clone();
    // var orient=node.orient.clone();
	// var pos=new THREE.Vector3(node.currPosition.x,node.currPosition.y,node.currPosition.z);
    var massInverse=1.0;//todo ?? later change

    //Translation
    var curForce = force(node);
    
    // Calculate acceleration ( F = ma )
    var A = curForce.clone().multiplyScalar(massInverse);
    
    // Integrate to get velocity
    node.velocity.add(A.clone().multiplyScalar(dt));
    
    // Integrate to get position
    node.currPosition.add(node.velocity.clone().multiplyScalar(dt));

	
	//var fricForce = curForce.clone();

	//if (isFloorEnabled()) floorForce(dt, &curForce); //floor force needs dt to calculate threshold to "stop" a slow voxel into static friction.

	//fricForce = curForce - fricForce;

	//assert(!(curForce.x != curForce.x) || !(curForce.y != curForce.y) || !(curForce.z != curForce.z)); //assert non QNAN
	// linMom.add(curForce).multiplyScalar(dt);

	// var translate=linMom.clone().multiplyScalar(dt*massInverse);//??massInverse

    // //	we need to check for friction conditions here (after calculating the translation) and stop things accordingly
	// if (isFloorEnabled() && floorPenetration() >= 0){ //we must catch a slowing voxel here since it all boils down to needing access to the dt of this timestep.
	// 	double work = fricForce.x*translate.x + fricForce.y*translate.y; //F dot disp
	// 	double hKe = 0.5*mat->_massInverse*(linMom.x*linMom.x + linMom.y*linMom.y); //horizontal kinetic energy

	

    // pos.add(translate);
	// node.currPosition=pos.clone();
	// node.displacement={
	// 	x:translate.x+node.displacement.x,
	// 	y:translate.y+node.displacement.y,
	// 	z:translate.z+node.displacement.z};
	
	// pos += translate;

	//Rotation
	
}

function force(node) {
    //forces from internal bonds
    var inverseMass=1.0;
    var gravity=new THREE.Vector3(0,-9.81,0);
    var Cd = 0.47; // Dimensionless
    var rho = 1.22; // kg / m^3
    var v=1; //volume or area
    
    var totalForce=new THREE.Vector3(0,0,0);
    
    // Drag force: 
    // var drag=

    

    totalForce.add(new THREE.Vector3(node.force.x,node.force.y,node.force.z));
    totalForce.add(gravity);

    

    var Fd = node.velocity.clone().multiplyScalar(-1/2 * Cd * rho*v ).multiply( node.velocity );
    
    //totalForce.add(Fd);
	
	
	totalForce.add(node.intForce);

	// for (int i=0; i<6; i++){ 
	// 	if (links[i]) totalForce += links[i]->force(isNegative((linkDirection)i)); //total force in LCS
	// }
	// totalForce = RotateVec3D(node.orient,totalForce); //from local to global coordinates

	//assert(!(totalForce.x != totalForce.x) || !(totalForce.y != totalForce.y) || !(totalForce.z != totalForce.z)); //assert non QNAN

	//other forces
    
    
    

	// if (externalExists()) totalForce += external()->force(); //external forces
	// totalForce -= velocity()*mat->globalDampingTranslateC(); //global damping f-cv
	// totalForce.z += mat->gravityForce(); //gravity, according to f=mg

	// if (isCollisionsEnabled()){
	// 	for (std::vector<CVX_Collision*>::iterator it=colWatch->begin(); it!=colWatch->end(); it++){
	// 		totalForce -= (*it)->contactForce(this);
	// 	}
	// }
	//todo make internal forces 0 again
	node.intForce=new THREE.Vector3(0,0,0);
	// node.force.x=0;
	// node.force.y=0;
	// node.force.z=0;
	return totalForce;
}