/*
 * Copyright (c) Contributors, http://opensimulator.org/
 * See CONTRIBUTORS.TXT for a full list of copyright holders.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the OpenSim Project nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

// Overview : do Newtonian physics treating all physical objects as gravitating objects.
//   Don't worry about collisions the way a normal game physics engine would.
//
//  We set G=1, and work in "natural units" (i.e. units of m within the sim), 
//  but we do NOT force mtot=1, but rather force the mass of each and every object to be
//  1.  (That's done in NewtonPrim.cs).  Reason: we want to be able to give intial
//  conditions with the scripting function llApplyImpulse, but that needs to use llGetMass
//  to figure out the given impulse needed for a given velocity, and if we're rescaling
//  the mass to enforce mtot=1, we've got race conditions and inconsistencies.  So, we'll
//  deal with the fact that we don't know that mtot=1;

using System;
using System.Collections.Generic;
using System.Reflection;
using OpenMetaverse;
using Nini.Config;
using OpenSim.Framework;
using OpenSim.Region.Physics.Manager;
using log4net;

namespace OpenSim.Region.Physics.NewtonPlugin
{
    public class NewtonScene : PhysicsScene
    {
        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
        private List<NewtonCharacter> _characters = new List<NewtonCharacter>();
        private List<NewtonPrim> _prims = new List<NewtonPrim>();
        private float[] _heightMap;
        private const float gravity = -9.8f; // Used only for Characters, not prims
        private float eps = 0.1f; // Softening distance. 

        private const int energyInterval = 10;
        private int energyCounter = 0;
        private int shoutCounter = 0;
        private int shoutInterval = 10;

        private float alpha = 1.0f; // Position scale factor: X_phys = alpha*X. 
        private float beta = 1.0f;  // Velocity scale factor---should be 1/Sqrt(alpha)
            
        // protected internal string sceneIdentifier;

        public NewtonScene(String _sceneIdentifier)
        {
            //sceneIdentifier = _sceneIdentifier;
        }

        public override void Initialise(IMesher meshmerizer, IConfigSource config)
        {
            
        }

        public override void Dispose()
        {
        }

        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
        {
            NewtonCharacter act = new NewtonCharacter();
            act.Position = position;
            act.Flying = isFlying;
            _characters.Add(act);
            return act;
        }

        public override void SetWaterLevel(float baseheight)
        {
        }

        public override void RemovePrim(PhysicsActor prim)
        {
            NewtonPrim p = (NewtonPrim) prim;
            if (_prims.Contains(p))
            {
                _prims.Remove(p);
            }
        }

        public override void RemoveAvatar(PhysicsActor character)
        {
            NewtonCharacter act = (NewtonCharacter) character;
            if (_characters.Contains(act))
            {
                _characters.Remove(act);
            }
        }

/*
        public override PhysicsActor AddPrim(Vector3 position, Vector3 size, Quaternion rotation)
        {
            return null;
        }
*/

        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                  Vector3 size, Quaternion rotation)
        {
            return AddPrimShape(primName, pbs, position, size, rotation, false);
        }

        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                  Vector3 size, Quaternion rotation, bool isPhysical)
        {
            // m_log.InfoFormat("[NEWTON] Adding prim at {0}, isPhysical={1}", position, isPhysical);
            NewtonPrim prim = new NewtonPrim();
            prim.Position = position;
            prim.Orientation = rotation;
            prim.Size = size;
            prim.IsPhysical = isPhysical;

            lock (_prims)
            {
                _prims.Add(prim);
            }
            return prim;
        }

        public override void AddPhysicsActorTaint(PhysicsActor prim)
        {
        }

        /// <summary>
        /// Returns the square of the distance between two points
        /// </summary>
        private float distance2(Vector3 a, Vector3 b)
        {
          float dx = a.X - b.X;
          float dy = a.Y - b.Y;
          float dz = a.Z - b.Z;

          // Math.Pow probably is a lot slower than dx*dx.
          return dx*dx + dy*dy + dz*dz;
          // return (float) (Math.Pow(a.X - b.X, 2) + Math.Pow(a.Y - b.Y, 2) + Math.Pow(a.Z - b.Z, 2));
        }

        private float dot(Vector3 a, Vector3 b) 
        {
          return a.X*b.X + a.Y*b.Y + a.Z*b.Z;
        }

        private void calculateAcceleration()
        {
            // Make sure each Prim has a fresh acceleration vector.
            
            for (int i = 0; i < _prims.Count; i++) {
                _prims[i].SetAcceleration(Vector3.Zero);
            }
            
            for (int i = 0; i < _prims.Count; i++) {


                if (!_prims[i].IsPhysical) continue;
                
                NewtonPrim pi = _prims[i];
                
                for (int j = i+1; j < _prims.Count; j++) {
                    if (!_prims[j].IsPhysical) continue;
                    NewtonPrim pj = _prims[j];
                    Vector3 direction = pj.Position - pi.Position;
                    float dist2 = distance2(pj.Position, pi.Position);
                    float r = (float) Math.Sqrt(dist2 + eps*eps);
                    
                    Vector3 acc_over_m = direction / (r*r*r);
                    
                    pi.SetAcceleration(pi.Acceleration + acc_over_m*pj.Mass);
                    pj.SetAcceleration(pj.Acceleration - acc_over_m*pi.Mass);
                }
            }
        }
        

        private void calculateAccAndJerk() {
            for (int i = 0; i < _prims.Count; i++) {
                _prims[i].SetAcceleration(new Vector3());
                _prims[i].Jerk = new Vector3();
            }
            
            for (int i = 0; i < _prims.Count; i++) {
                if (!_prims[i].IsPhysical) continue;
                
                NewtonPrim pi = _prims[i];
                
                for (int j = i+1; j < _prims.Count; j++) {
                    if (!_prims[j].IsPhysical) continue;
                    
                    NewtonPrim pj = _prims[j];
                    
                    Vector3 direction = pj.Position - pi.Position;
                    Vector3 velocity = pj.Velocity - pi.Velocity;
                    
                    float dist2 = distance2(pj.Position, pi.Position);
                    float r = (float) Math.Sqrt(dist2 + eps*eps);
                    float r2 = r*r;
                    float r3 = r2*r;
                    float r5 = r3*r2;
                    
                    Vector3 acc_over_mass = direction / r3;
                    Vector3 jerk_over_mass = velocity/r3 - direction * 3.0f*dot(direction,velocity)/r5;
                    
                    pi.SetAcceleration(pi.Acceleration + acc_over_mass*pj.Mass);
                    pj.SetAcceleration(pj.Acceleration - acc_over_mass*pi.Mass);
                    pi.Jerk += jerk_over_mass * pj.Mass;
                    pj.Jerk -= jerk_over_mass * pi.Mass;
                }
            }
        }
        

        private void savePrimStates() 
        {
          for (int j = 0; j < _prims.Count; j++) 
            {
              _prims[j].SaveState();
            }
        }

        private void predictPrims(float dt) 
        {
          float dt2 = dt*dt;
          float dt3 = dt*dt2;

          for (int j = 0; j < _prims.Count; j++)
            {
              _prims[j].Position = _prims[j].Position + _prims[j].Velocity * dt +
                        _prims[j].Acceleration * 0.5f*dt2 +
                        _prims[j].Jerk * (1.0f/6.0f)*dt3;
              _prims[j].Velocity = _prims[j].Velocity + _prims[j].Acceleration * dt + 
                        _prims[j].Jerk * 0.5f * dt2;
            }
        }

        private void finishPrims(float dt) 
        {
          float dt2 = dt*dt;

          for (int j = 0; j < _prims.Count; j++) 
            {
                // Important to do velocity first, so that more accurate velocity
                // value can be used in position calculation.
                _prims[j].Velocity = _prims[j].OldVelocity +
                    (_prims[j].OldAcceleration + _prims[j].Acceleration) * 0.5f * dt + 
                    (_prims[j].OldJerk - _prims[j].Jerk) * (1.0f/12.0f) * dt2;
                _prims[j].Position = _prims[j].OldPosition + 
                    (_prims[j].OldVelocity + _prims[j].Velocity) * 0.5f * dt + 
                    (_prims[j].OldAcceleration - _prims[j].Acceleration) * (1.0f/12.0f) * dt2;
                _prims[j].Position = periodicBCs(_prims[j].Position);
            }
        }

        private void updatePrimVelocities(float timestep)
        {
            calculateAcceleration();
            lock (_prims)
            {
                for (int i = 0; i < _prims.Count; ++i)
                {
                    if (!_prims[i].IsPhysical)
                        continue;

                    _prims[i].Velocity += _prims[i].Acceleration * timestep;
                }
            }
        }

        // This is a little scary, since the forces are based on
        //   positions within the region-- if one "wraps around",
        //   suddenly the forces from every other prim is in the
        //   opposite direction and of different strength.

        // RKNOP 2009/12/03 -- I ran this on a grid, and was
        //  getting lots of errors about region crossing.
        //  I think that 0.5m is not enough of a buffer, and
        //  the objects were sliding out of the region when
        //  the out-of-Physics code was interpolating the
        //  velocity.  I added the buffersize variable so that
        //  it's cleaner to change, and set it to 10 m initially.
        //  ...hurm, it may actually be that Vector3 is a struct
        //  and thus is being passed by value.  But why no compiler
        //  errors?  Yes, this was the problem, but I guess Mono
        //  is not smart enough to figure it out.

        //TODO: Check Z Axis

        private Vector3 periodicBCs(Vector3 oldpos)
        {
            Vector3 pos = oldpos;
            float buffersize = 10.0f;
            float smallestPos = buffersize;
            float largestPos = Constants.RegionSize - buffersize;
            
            if (pos.X < smallestPos) 
            {
                pos.X = largestPos;
            } else if (pos.X > largestPos) {
                pos.X = smallestPos;
            }
            
            if (pos.Y < smallestPos)
            {
                pos.Y = largestPos;
            } else if (pos.Y > largestPos) {
                pos.Y = smallestPos;
            }
            
            return pos;
        }

        private void updatePrimPositions(float timestep)
        {
            // if (shoutCounter == 0)
            // {
            //     m_log.InfoFormat("[NEWTON] Updating positions for {0} prims", _prims.Count);
            // }

            for (int i = 0; i < _prims.Count; ++i)
            {
                if (!_prims[i].IsPhysical)
                    continue;

                _prims[i].Position += _prims[i].Velocity * timestep;

                _prims[i].Position = periodicBCs(_prims[i].Position);
                
                // if (shoutCounter == 0)
                // {
                //     m_log.InfoFormat("[NEWTON]   Prim {0} : mass = {1}, " +
                //                      "position = {2}, velocity = {3}, accel = {4}", i, 
                //                      _prims[i].Mass, _prims[i].Position, _prims[i].Velocity,
                //                      _prims[i].Acceleration);
                // }

            }
        }

        private void updateCharacter(NewtonCharacter character, float timestep)
        {
            float oldposX = character.Position.X;
            float oldposY = character.Position.Y;
            float oldposZ = character.Position.Z;
            Vector3 newpos = character.Position;

            if (!character.Flying)
            {
                character._target_velocity.Z += gravity * timestep;
            }

            newpos.X += character._target_velocity.X * timestep;
            newpos.Y += character._target_velocity.Y * timestep;

            newpos.X = Util.Clamp(newpos.X, 0.01f,
                                  Constants.RegionSize - 0.01f);
            newpos.Y = Util.Clamp(newpos.Y, 0.01f,
                                  Constants.RegionSize - 0.01f);

            bool forcedZ = false;

            float terrainheight = _heightMap[(int)newpos.Y * 
                                    Constants.RegionSize +
                                  (int)newpos.X];
            if (newpos.Z + (character._target_velocity.Z * timestep)
                 < terrainheight + 2)
            {
                newpos.Z = terrainheight + 1.0f;
                forcedZ = true;
            }
            else
            {
                newpos.Z += character._target_velocity.Z * timestep;
            }

            character._velocity.X = (newpos.X - oldposX) / timestep;
            character._velocity.Y = (newpos.Y - oldposY) / timestep;

            if (forcedZ)
            {
                character._velocity.Z = 0;
                character._target_velocity.Z = 0;
                ((PhysicsActor)character).IsColliding = true;
                character.RequestPhysicsterseUpdate();
            }
            else
            {
                ((PhysicsActor)character).IsColliding = false;
                character._velocity.Z = (newpos.Z - oldposZ) / timestep;
            }

            character.Position = newpos;
        }

        private float simulateCharacters(float timestep) 
        {
          float fps = 0;
          for (int i = 0; i < _characters.Count; ++i) 
          {
            fps++;
            updateCharacter(_characters[i], timestep);
          }

          return fps;
        }

        
        public override float Simulate(float timestep)
        {
            float result = simulateCharacters(timestep);
            
            if (shouldRescale()) {
                rescaleEpsilon();
                rescaleMasses();
                rescalePositionVelocity();
            }           
                
            unSetFirstStep();
            
            float dt = 0.1f*eps;  // Safety factor of 10
            int nsteps = (int) (timestep/dt + 0.5f);
            float actual_dt = timestep/((float) nsteps);
            
            // Make sure we actually *do* something...!
            if (nsteps < 1)
            {
                nsteps = 1;
                actual_dt = timestep;
            }

            // m_log.InfoFormat("[NEWTON] Simulate; timestep={0}, eps={1}, nsteps={2}", 
            //                  timestep, eps, nsteps);

            for (int i = 0; i < nsteps; i++) {
                //Choose which algorithm to use below.
                //SimulateHermite(actual_dt);
                //SimulateKDK(actual_dt);
                //SimulateDKD(actual_dt);
                //SimulateGGL(actual_dt);
                SimulateForwardEuler(actual_dt);
            }
            for (int i = 0 ; i < _prims.Count ; ++i)
                if (_prims[i].IsPhysical) _prims[i].RequestPhysicsterseUpdate();
            
            if (((++energyCounter) % energyInterval) == 0)
            {
            //  m_log.Info(Energy());
                energyCounter = 0;
            }   

            if (--shoutCounter < 0) shoutCounter = shoutInterval;
            
            return result;
        }
        

        private void SimulateForwardEuler(float timestep)
        {          
            updatePrimVelocities(timestep);
            updatePrimPositions(timestep);
        }

        private void SimulateDKD(float timestep) 
        {
          updatePrimPositions(0.5f*timestep);
          updatePrimVelocities(timestep);
          updatePrimPositions(0.5f*timestep);
        }

        // This re-computes the force on each subsequent timestep:
        // (KDK)(KDK)(KDK)....  Usually, you would store the results
        // of the force computation, and re-use them in a
        // "Kick-stored" advance so there is only one force
        // computation per timestep on average: (KDK)(KsDK)(KsDK)....
        private void SimulateKDK(float timestep)
        {
          updatePrimVelocities(0.5f*timestep);
          updatePrimPositions(timestep);
          updatePrimVelocities(0.5f*timestep);

    }

        private void SimulateHermite(float timestep) 
        {
            calculateAccAndJerk();
            savePrimStates();
            predictPrims(timestep);
            calculateAccAndJerk();
            finishPrims(timestep);

    }

        public override void GetResults()
        {
        }

        public override bool IsThreaded
        {
            // for now we won't be multithreaded
            get { return (false); }
        }

        public override void SetTerrain(float[] heightMap)
        {
            _heightMap = heightMap;
        }

        public override void DeleteTerrain()
        {
        }

        public override Dictionary<uint, float> GetTopColliders()
        {
            Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
            return returncolliders;
        }

        private float PairPotential(NewtonPrim p1, NewtonPrim p2) 
        {
          float d2 = distance2(p1.Position, p2.Position);
          float r = (float) Math.Sqrt(d2 + eps*eps);

          return -p1.Mass*p2.Mass/r;
        }

        public float KineticEnergy() 
        {
          float ke = 0.0f;

          for (int i = 0; i < _prims.Count; i++) 
            {
              ke += _prims[i].KineticEnergy();
            }

          return ke;
        }

        public float PotentialEnergy() 
        {
          float pe = 0.0f;

          for (int i = 0; i < _prims.Count; i++) 
            {
              for (int j = i+1; j < _prims.Count; j++) 
                {
                  pe += PairPotential(_prims[i], _prims[j]);
                }
            }

          return pe;
        }

        public float Energy() 
        {
          return KineticEnergy() + PotentialEnergy();
        }
        
        private void gglPredict(float h) {
            float h2 = h*h;
            float h3 = h2*h;
            float h4 = h3*h;
            float h5 = h4*h;
            float h6 = h5*h;
            
            for (int i = 0; i < _prims.Count; i++) {
                float hold = _prims[i].HOld;
                float hold2 = hold*hold;
                float hold3 = hold2*hold;
                float hold4 = hold3*hold;
                Vector3 a0 = _prims[i].A0;
                Vector3 a1 = _prims[i].A1;
                Vector3 a2 = _prims[i].A2;
                Vector3 j0 = _prims[i].J0;
                Vector3 j2 = _prims[i].J2;


                
                _prims[i].OldPosition = _prims[i].Position;
                _prims[i].OldVelocity = _prims[i].Velocity;
                _prims[i].HOld = h;
                
                _prims[i].Position = _prims[i].Position + 
                                        _prims[i].Velocity * 0.5f*h + 
                                        a2 / h2 / 8.0f +
                                        j2 * h3 / 48.0f -
                                        (a0*5.0f - a1*16.0f + a2*11.0f + j0*hold - j2*4.0f*hold)*h4/(96.0f*hold2) - 
                                        (a0*14.0f - a1*32.0f + a2*18.0f + j0*3.0f*hold - 
                                                j2*5.0f*hold)*h5/(192.0f*hold3) - 
                                        (a0*4.0f - a1*8.0f + a2*4.0f + j0*hold - j2*hold)*h6/(192.0f*hold4);
                
                _prims[i].A0 = _prims[i].A2;
                _prims[i].J0 = _prims[i].J0;
                _prims[i].FirstStep = false;
            }
        }
        
        private void gglSaveA0J0() {
            for (int i = 0; i < _prims.Count; i++) {
                _prims[i].A0 = _prims[i].Acceleration;
                _prims[i].J0 = _prims[i].Jerk;
            }
        }
        
        private void gglSaveA1() {
            for (int i = 0; i < _prims.Count; i++) {
                _prims[i].A1 = _prims[i].Acceleration;
            }
        }
        
        private void gglSaveA2() {
            for (int i = 0; i < _prims.Count; i++) {
                _prims[i].A2 = _prims[i].Acceleration;
            }
        }
        
        private void gglSaveA2J2() {
            for (int i = 0; i < _prims.Count; i++) {
                _prims[i].A2 = _prims[i].Acceleration;
                _prims[i].J2 = _prims[i].Jerk;
            }
        }
        
        private void gglFinalPosition(float h) {
            float h2 = h*h;
            
            for (int i = 0; i < _prims.Count; i++) {
                _prims[i].Position = _prims[i].OldPosition + _prims[i].Velocity*h + 
                        (_prims[i].A0/3.0f + _prims[i].A1*2.0f/3.0f) * 0.5f*h2;
            }
        }
        
        private void gglFinalVelocity(float h) {
            for (int i = 0; i < _prims.Count; i++) {
                _prims[i].Velocity = _prims[i].Velocity + 
                        (_prims[i].A0/6.0f + _prims[i].A1*2.0f/3.0f + _prims[i].A2/6.0f) * h;
            }
        }
        
        private void SimulateGGL(float timestep) {
            // Need something for first timestep.
            
            gglPredict(timestep);
            calculateAcceleration();
            gglSaveA1();
            gglFinalPosition(timestep);
            calculateAcceleration();
            gglSaveA2();
            gglFinalVelocity(timestep);
            calculateAccAndJerk();
            gglSaveA2J2();          
        }
        
        private float totalPhysicalMass() {
            float mtot = 0.0f;
            
            for (int i = 0; i < _prims.Count; i++) {
                if (_prims[i].IsPhysical) {
                    mtot += _prims[i].Mass;
                }
            }
            
            return mtot;
    }
        
        // I've commented this out for now.  It wreaks havoc
        //   with the way I've done "AddForce".  It's probably
        //   better just to take out the "mtot = 1" assumption
        //   from the rest of the equations.  Otherwise, we'll
        //   have race conditions between when prims are enabled
        //   as physical, when forces are applied with llApplyImpulse,
        //   the scripted llGetMass, and this rescaling.  rknop 2009/09/17

        private void rescaleMasses() {
            // float mtot = totalPhysicalMass();
            //
            // for (int i = 0; i < _prims.Count; i++) {
            //     if (_prims[i].IsPhysical) {
            //         _prims[i].SetMass(_prims[i].Mass / mtot);
            //     }
            // }
        }
        
        private Vector3 centerOfMass() {
            Vector3 com = new Vector3();

            float mtot = totalPhysicalMass();
            
            for (int i = 0; i < _prims.Count; i++) {
                if (!_prims[i].IsPhysical) continue;
                
                com += _prims[i].Position * _prims[i].Mass / mtot;
            }
            
            return com;
        }
        
        private Vector3 comVelocity() {
            Vector3 vcm = new Vector3();
            float mtot = totalPhysicalMass();
            
            for (int i = 0; i < _prims.Count; i++) {
                if (!_prims[i].IsPhysical) continue;
                
                vcm += _prims[i].Velocity * _prims[i].Mass / mtot;
            }
            
            return vcm;
        }
        
        // I've commented this out for now, as it seems
        //   to be shooting my prims way off to the edge
        //   for N=2; I need to Deep Think about it at
        //   some point. rknop 2009/09/17

        private void rescalePositionVelocity() {
            // float E = Energy();
            // 
            // alpha = -4.0f*E;
            // beta = 1.0f/((float) Math.Sqrt(alpha));
            //
            // Vector3 com = centerOfMass();
            // Vector3 vcm = comVelocity();
            // 
            // for (int i = 0; i < _prims.Count; i++) {
            //     if (!_prims[i].IsPhysical) continue;
            //     
            //     Vector3 relPos = _prims[i].Position - com;
            //     Vector3 relVel = _prims[i].Velocity - vcm;
            //     
            //     _prims[i].Position = com + alpha * relPos;
            //     _prims[i].Velocity = vcm + beta * relVel;
            // }
        }
        

        // Re-scale position and momentum if there is a physical prim in
        //   the scene which is on its first step.

        private bool shouldRescale() {
            for (int i = 0; i < _prims.Count; i++) {
                if (!_prims[i].IsPhysical) continue;
                
                if (_prims[i].FirstStep) return true;
            }
            
            return false;
        }
        
        private void unSetFirstStep() {
            for (int i = 0; i < _prims.Count; i++) {
                if (!_prims[i].IsPhysical) continue;
                
                _prims[i].FirstStep = false;
            }
        }
        
        // This rescales Epsilon to a very small value---one appropriate
        //   for tracking all but the hardest binaries.
        // For small N, set eps to 0.1, otherwise to 4/N.
        //
        // TODO -- re-evaluate whether this still applies when mtot!=1

        private void rescaleEpsilon() {
            int nPhys = 0;
            
            for (int i = 0; i < _prims.Count; i++) {
                if (!_prims[i].IsPhysical) continue;
                
                nPhys++;
            }
            
            if (nPhys <= 40) eps = 0.1f;
            else eps = 4.0f/((float) nPhys);
        }



    }
}