1 files changed, 105 insertions, 25 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 );
+        }
 }
+