# Amira Abdel-Rahman
# (c) Massachusetts Institute of Technology 2020


# BASED ON https://github.com/jonhiller/Voxelyze

function force(i,N_intForce,N_orient,N_force,N_position,currentTimeStep,material,linMom) 
    # forces from internal bonds
    totalForce=Vector3(0,0,0)
    # new THREE.Vector3(node.force.x,node.force.y,node.force.z);
    #  todo 


    totalForce=totalForce+N_intForce

    if(i==10)
        # x=translate.x*1e6
        # y=translate.y*1e6
        # z=translate.z*1e6
        # @cuprintln("translate x $x 1e-6, y $y 1e-6, z $z 1e-6")

        # x=totalForce.x*1e6
        # y=totalForce.y*1e6
        # z=totalForce.z*1e6
        # @cuprintln("totalForce x $x 1e-6, y $y 1e-6, z $z 1e-6")
    end

    totalForce = RotateVec3D(N_orient,totalForce); # from local to global coordinates

    totalForce=totalForce+externalForce(currentTimeStep,N_position,N_force)

    mass=material.mass
    massInverse=material.massInverse

    
    globalDampingTranslateC= material.zetaGlobal*material._2xSqMxExS

    
    # x=totalForce.x*1e6
    # y=totalForce.y*1e6
    # z=totalForce.z*1e6
    # @cuprintln("totalForce 2 x $x 1e-6, y $y 1e-6, z $z 1e-6")
    


    vel=linMom*Vector3((massInverse*globalDampingTranslateC),(massInverse*globalDampingTranslateC),(massInverse*globalDampingTranslateC))
    

    # x=vel.x*1e0/globalDampingTranslateC
    # y=vel.y*1e0/globalDampingTranslateC
    # z=vel.z*1e0/globalDampingTranslateC
    # @cuprintln("vel x $x, y $y, z $z ")

    
    totalForce =totalForce - vel; #global damping f-cv

    

    # other forces
    # if(!static)

        #massInverse=1.0/mass #
        #vel = linMom*Vector3((massInverse),(massInverse),(massInverse))
        #totalForce =totalForce- vel*globalDampingTranslateC(); #global damping f-cv
    gravity=gravityEnabled();
    if(gravity)
        grav=-mass*9.80665*1000.0;
        totalForce =totalForce +Vector3(0,grav,0);
    end
    # end

    # x=totalForce.x*1e6
    # y=totalForce.y*1e6
    # z=totalForce.z*1e6
    # @cuprintln("totalForce 3 x $x 1e-6, y $y 1e-6, z $z 1e-6")

    return totalForce
end

function moment(intMoment,orient,moment,material,angMom) 
    #moments from internal bonds
    totalMoment=Vector3(0,0,0)
    # for (int i=0; i<6; i++){ 
    # 	if (links[i]) totalMoment += links[i]->moment(isNegative((linkDirection)i)); //total force in LCS
    # }

    totalMoment=totalMoment+intMoment
    

    totalMoment = RotateVec3D(orient,totalMoment);
    
    
    totalMoment=totalMoment+moment

    #other moments
    # if (externalExists()) totalMoment += external()->moment(); //external moments
    # totalMoment -= angularVelocity()*mat->globalDampingRotateC(); //global damping

    _2xSqIxExSxSxS = 2.0*sqrt(material.inertia*material.E*material.nomSize*material.nomSize*material.nomSize)
    globalDampingRotateC=material.zetaGlobal*_2xSqIxExSxSxS
    angularVelocity=angMom*Vector3(material.momentInertiaInverse*globalDampingRotateC,material.momentInertiaInverse*globalDampingRotateC,material.momentInertiaInverse*globalDampingRotateC)
    totalMoment= totalMoment-angularVelocity; #global damping


    # x=totalMoment.x
    # y=totalMoment.y
    # z=totalMoment.z
    # @cuprintln("totalMoment x $x, y $y, z $z ")

    return totalMoment
end

################################################################

function floorPenetration(x,y,nomSize)
    # floor=0.001*2.0
    floor=0.0
    p=0.0
    d=10.0
    if(y<floor)
    # if(y<floor&& (x<5.0*d || x>=14.0*d))
        p=floor-y
    end
    # if(y<floor)
    #     p=floor-y
    # end
    return p
end
#Returns the interference (in meters) between the collision envelope of this voxel and the floor at Z=0. Positive numbers correspond to interference. If the voxel is not touching the floor 0 is returned.

function penetrationStiffness(E,nomSize)
    return (2.0*E*nomSize)
end 
#!< returns the stiffness with which this voxel will resist penetration. This is calculated according to E*A/L with L = voxelSize/2.


function globalDampingTranslateC(_2xSqMxExS,zetaGlobal)
    return zetaGlobal*_2xSqMxExS;
end #!< Returns the global material damping coefficient (translation)

function collisionDampingTranslateC(_2xSqMxExS,zetaCollision)
    # _2xSqIxExSxSxS = (2.0f*sqrt(_momentInertia*E*nomSize*nomSize*nomSize));
    return zetaCollision*_2xSqMxExS;
end #!< Returns the global material damping coefficient (translation)

function floorForce!(dt,pTotalForce,pos,linMom,FloorStaticFriction,N_material)
    E=convert(Float64,N_material.E)
    nomSize=convert(Float64,N_material.nomSize)
    mass=convert(Float64,N_material.mass)
    massInverse=convert(Float64,N_material.massInverse)
    muStatic=convert(Float64,N_material.muStatic)*1.0
    muKinetic=convert(Float64,N_material.muKinetic)*100.0
    _2xSqMxExS=convert(Float64,N_material._2xSqMxExS)
    zetaCollision=convert(Float64,N_material.zetaCollision)
    
    CurPenetration = floorPenetration(convert(Float64,pos.x),convert(Float64,pos.y),nomSize); #for now use the average.


    if (CurPenetration>=0.0)
        vel = linMom*Vector3((massInverse),(massInverse),(massInverse)) #Returns the 3D velocity of this voxel in m/s (GCS)
        horizontalVel= Vector3(convert(Float64,vel.x), 0.0, convert(Float64,vel.z));
        
        normalForce = penetrationStiffness(E,nomSize)*CurPenetration;
        pTotalForce=Vector3( pTotalForce.x, convert(Float64,pTotalForce.y) + normalForce - collisionDampingTranslateC(_2xSqMxExS,zetaCollision)*convert(Float64,vel.y),pTotalForce.z)
        #in the z direction: k*x-C*v - spring and damping

        if (FloorStaticFriction) #If this voxel is currently in static friction mode (no lateral motion) 
            # assert(horizontalVel.Length2() == 0);
            surfaceForceSq = convert(Float64,(pTotalForce.x*pTotalForce.x + pTotalForce.z*pTotalForce.z)); #use squares to avoid a square root
            frictionForceSq = (muStatic*normalForce)*(muStatic*normalForce);


            if (surfaceForceSq > frictionForceSq) 
                FloorStaticFriction=false; #if we're breaking static friction, leave the forces as they currently have been calculated to initiate motion this time step
            end

        else  #even if we just transitioned don't process here or else with a complete lack of momentum it'll just go back to static friction
            #add a friction force opposing velocity according to the normal force and the kinetic coefficient of friction
            leng=sqrt((convert(Float64,vel.x) * convert(Float64,vel.x)) + (0.0 * 0.0) + (convert(Float64,vel.z) * convert(Float64,vel.z)))
            if(leng>0)
                horizontalVel= Vector3(convert(Float64,vel.x)/(leng),0.0,convert(Float64,vel.z)/(leng))
            else
                horizontalVel= Vector3(convert(Float64,vel.x)*(leng),0.0,convert(Float64,vel.z)*(leng))

            end   
            pTotalForce = pTotalForce- Vector3(muKinetic*normalForce,muKinetic*normalForce,muKinetic*normalForce) * horizontalVel;
        end

    else 
        FloorStaticFriction=false;
    end
    
    
    
    return pTotalForce,FloorStaticFriction
end


################################################################