6 files changed, 146 insertions, 27 deletions

diff --git a/src/explicitsimulation.cpp b/src/explicitsimulation.cpp
index 281e7e5..6a9356b 100644
--- a/src/explicitsimulation.cpp
+++ b/src/explicitsimulation.cpp
@@ -1,17 +1,21 @@
 #include "genetic/explicitsimulation.h"
 #include "genetic/operator.h"
 #include "genetic/fitnessfunction.h"
+#include "genetic/phenotype.h"
 
 #include <bu/random.h>
 #include <bu/sio.h>
+#include <bu/mutexlocker.h>
+
+#include <stdlib.h>
 
 using namespace Bu;
 
 Genetic::ExplicitSimulation::ExplicitSimulation( Genetic::Operator *pOper,
-                Genetic::FitnessFunction *pFunc, int iPopSize, float fKeep,
-                float fRandom, bool bKeepBest ) :
+                Genetic::FitnessFunction *pFunc, int iThreads, int iPopSize,
+                float fKeep, float fRandom, bool bKeepBest ) :
         pOper( pOper ),
-        pFunc( pFunc ),
+        //pFunc( pFunc ),
         iPopSize( iPopSize ),
         fKeep( fKeep ),
         fRandom( fRandom ),
@@ -22,13 +26,30 @@ Genetic::ExplicitSimulation::ExplicitSimulation( Genetic::Operator *pOper,
                 xPop.addPhenotype( pOper->random() );
         }
 
+        if( iThreads < 1 )
+                iThreads = 1;
+
+        int iId = 0;
+        lProcessor.append( new Processor( *this, pFunc, qWork, iId++ ) );
+        while( lProcessor.getSize() < iThreads )
+                lProcessor.append(
+                        new Processor( *this, pFunc->clone(), qWork, iId++ )
+                        );
+
         updateFitness();
 }
 
 Genetic::ExplicitSimulation::~ExplicitSimulation()
 {
         delete pOper;
-        delete pFunc;
+
+        for( ProcessorList::iterator i = lProcessor.begin(); i; i++ )
+                (*i)->setRunning( false );
+        for( ProcessorList::iterator i = lProcessor.begin(); i; i++ )
+        {
+                (*i)->join();
+                delete *i;
+        }
 }
 
 void Genetic::ExplicitSimulation::timestep()
@@ -52,12 +73,15 @@ void Genetic::ExplicitSimulation::timestep()
         for( int j = 0; j < iKeep; j++ )
         {
                 Genetic::PhenotypeId id = selectWeighted();
+                mFitness.lock();
                 lNew.append( xPop.takePhenotype( id ) );
                 hTempFitness.insert( id, hFitness.get( id ) );
                 hFitness.erase( id );
                 dTotalFitness -= hTempFitness.get( id );
+                mFitness.unlock();
         }
 
+        mFitness.lock();
         if( bKeepBest && hFitness.has( uMaxFitness ) )
         {
                 lNew.append( xPop.takePhenotype( uMaxFitness ) );
@@ -65,6 +89,7 @@ void Genetic::ExplicitSimulation::timestep()
                 hFitness.erase( uMaxFitness );
                 dTotalFitness -= hTempFitness.get( uMaxFitness );
         }
+        mFitness.unlock();
 
         // Fill in the remainder with random phenotypes
         while( lNew.getSize() < iPopSize )
@@ -73,7 +98,9 @@ void Genetic::ExplicitSimulation::timestep()
         }
 
         // Refill the population
+        mFitness.lock();
         hFitness = hTempFitness;
+        mFitness.unlock();
         xPop.clear();
         xPop.timestep();
         for( PhenotypeList::iterator i = lNew.begin(); i; i++ )
@@ -86,6 +113,8 @@ void Genetic::ExplicitSimulation::timestep()
 
 Genetic::PhenotypeId Genetic::ExplicitSimulation::selectWeighted()
 {
+        Bu::MutexLocker ml( mFitness );
+
         double dSel = Bu::Random::randNorm()*dTotalFitness;
         double dRun = 0.0;
 
@@ -98,42 +127,93 @@ Genetic::PhenotypeId Genetic::ExplicitSimulation::selectWeighted()
 
         sio << "Genetic::ExplicitSimulation::selectWeighted() - failed, picked max"
                 << sio.nl;
+        sio << "  " << dMinFitness << " - " << dMaxFitness << " -- " << dTotalFitness << " > " << dRun << " > " << dSel << sio.nl;
+        abort();
 
         return uMaxFitness;
 }
 
 void Genetic::ExplicitSimulation::updateFitness()
 {
+        mFitness.lock();
         dMinFitness = -1.0;
         dTotalFitness = 0.0;
+        mFitness.unlock();
+
         for( Population::iterator i = xPop.begin(); i; i++ )
         {
-                double dFitness;
                 if( hFitness.has( i.getKey() ) )
                 {
-                        dFitness = hFitness.get( i.getKey() );
+                        setFitness( i.getKey(), hFitness.get( i.getKey() ) );
                 }
                 else
                 {
-                        dFitness = (*pFunc)( *i );
-                        if( dFitness < 0.0 )
-                                dFitness = 0.0;
-                        hFitness.insert( i.getKey(), dFitness );
-                }
-                dTotalFitness += dFitness;
-                if( dMinFitness < 0.0 )
-                {
-                        dMinFitness = dMaxFitness = dFitness;
-                        uMaxFitness = i.getKey();
-                }
-                else if( dMinFitness > dFitness )
-                {
-                        dMinFitness = dFitness;
-                }
-                else if( dMaxFitness < dFitness )
-                {
-                        dMaxFitness = dFitness;
-                        uMaxFitness = i.getKey();
+                        qWork.enqueue( *i );
                 }
         }
+        
+        for( ProcessorList::iterator i = lProcessor.begin(); i; i++ )
+                (*i)->start();
+
+        for( ProcessorList::iterator i = lProcessor.begin(); i; i++ )
+                (*i)->join();
+}
+
+void Genetic::ExplicitSimulation::setFitness( Genetic::PhenotypeId id,
+                double dFitness )
+{
+        Bu::MutexLocker ml( mFitness );
+        if( dFitness < 0.0 )
+                dFitness = 0.0;
+
+//      sio << id << ": " << dFitness << sio.nl;
+        hFitness.insert( id, dFitness );
+
+        dTotalFitness += dFitness;
+        if( dMinFitness < 0.0 )
+        {
+                dMinFitness = dMaxFitness = dFitness;
+                uMaxFitness = id;
+        }
+        else if( dMinFitness > dFitness )
+        {
+                dMinFitness = dFitness;
+        }
+        else if( dMaxFitness < dFitness )
+        {
+                dMaxFitness = dFitness;
+                uMaxFitness = id;
+        }
+}
+
+Genetic::ExplicitSimulation::Processor::Processor(
+                Genetic::ExplicitSimulation &rSim,
+                Genetic::FitnessFunction *pFunc,
+                Genetic::ExplicitSimulation::WorkQueue &rqWork,
+                int iId ) :
+        rSim( rSim ),
+        pFunc( pFunc ),
+        rqWork( rqWork ),
+        iId( iId )
+{
+}
+
+Genetic::ExplicitSimulation::Processor::~Processor()
+{
+        delete pFunc;
+}
+
+void Genetic::ExplicitSimulation::Processor::run()
+{
+        bRunning = true;
+        while( bRunning && !rqWork.isEmpty() )
+        {
+                Genetic::Phenotype *pPhen = rqWork.dequeue( 0, 250000 );
+                if( pPhen == NULL )
+                        continue;
+
+                double dFitness = (*pFunc)( pPhen );
+                rSim.setFitness( pPhen->getId(), dFitness );
+        }
 }
+
diff --git a/src/explicitsimulation.h b/src/explicitsimulation.h
index ebddd62..c2aeed9 100644
--- a/src/explicitsimulation.h
+++ b/src/explicitsimulation.h
@@ -4,6 +4,10 @@
 #include "genetic/population.h"
 #include "genetic/config.h"
 
+#include <bu/thread.h>
+#include <bu/mutex.h>
+#include <bu/synchroqueue.h>
+
 namespace Genetic
 {
         class Operator;
@@ -13,7 +17,8 @@ namespace Genetic
         {
         public:
                 ExplicitSimulation( Operator *pOper, FitnessFunction *pFunc,
-                                int iPopSize, float fKeep, float fRandom, bool bKeepBest=true );
+                                int iThreads, int iPopSize, float fKeep, float fRandom,
+                                bool bKeepBest=true );
                 virtual ~ExplicitSimulation();
 
                 void timestep();
@@ -25,11 +30,11 @@ namespace Genetic
 
         protected:
                 void updateFitness();
+                void setFitness( Genetic::PhenotypeId, double dFitness );
 
         protected:
                 Population xPop;
                 Operator *pOper;
-                FitnessFunction *pFunc;
 
         private:
                 int iPopSize;
@@ -42,6 +47,32 @@ namespace Genetic
                 double dMaxFitness;
                 double dTotalFitness;
                 PhenotypeId uMaxFitness;
+
+                Bu::Mutex mFitness;
+                typedef Bu::SynchroQueue<Phenotype *> WorkQueue;
+                WorkQueue qWork;
+
+                class Processor : public Bu::Thread
+                {
+                public:
+                        Processor( ExplicitSimulation &rSim, FitnessFunction *pFunc,
+                                        WorkQueue &rqWork, int iId );
+                        virtual ~Processor();
+
+                        void setRunning( bool b ) { bRunning = b; }
+
+                protected:
+                        virtual void run();
+
+                        ExplicitSimulation &rSim;
+                        FitnessFunction *pFunc;
+                        WorkQueue &rqWork;
+                        int iId;
+                        bool bRunning;
+                };
+
+                typedef Bu::List<Processor *> ProcessorList;
+                ProcessorList lProcessor;
         };
 };
 
diff --git a/src/fitnessfunction.h b/src/fitnessfunction.h
index c41f733..0f852fa 100644
--- a/src/fitnessfunction.h
+++ b/src/fitnessfunction.h
@@ -12,6 +12,7 @@ namespace Genetic
                 virtual ~FitnessFunction();
 
                 virtual double operator()( Phenotype *pTest )=0;
+                virtual FitnessFunction *clone() const=0;
         };
 };
 
diff --git a/src/tests/maxima/fitnessfunctioneq.cpp b/src/tests/maxima/fitnessfunctioneq.cpp
index 5694507..c64e87a 100644
--- a/src/tests/maxima/fitnessfunctioneq.cpp
+++ b/src/tests/maxima/fitnessfunctioneq.cpp
@@ -23,3 +23,8 @@ double FitnessFunctionEq::operator()( Genetic::Phenotype *pTest )
         return -1.8*(x*x*x*x) + 0.86*(x*x*x) + 4.0*(x*x);
 }
 
+Genetic::FitnessFunction *FitnessFunctionEq::clone() const
+{
+        return new FitnessFunctionEq();
+}
+
diff --git a/src/tests/maxima/fitnessfunctioneq.h b/src/tests/maxima/fitnessfunctioneq.h
index 7139532..d1a64c5 100644
--- a/src/tests/maxima/fitnessfunctioneq.h
+++ b/src/tests/maxima/fitnessfunctioneq.h
@@ -10,6 +10,7 @@ public:
         virtual ~FitnessFunctionEq();
 
         virtual double operator()( Genetic::Phenotype *pTest );
+        virtual Genetic::FitnessFunction *clone() const;
 };
 
 #endif
diff --git a/src/tests/maxima/main.cpp b/src/tests/maxima/main.cpp
index ec02a41..db11f66 100644
--- a/src/tests/maxima/main.cpp
+++ b/src/tests/maxima/main.cpp
@@ -23,6 +23,7 @@ int main( int argc, char *argv[] )
                         0.05
                         ),
                 new FitnessFunctionEq(),
+                4, 
                 1000,
                 .1, .1
                 );