#include "gaTSP.h"




//---------------------TestNumber-----------------------------
//
//	checks if a given integer is already contained in a vector
//	of integers.
//------------------------------------------------------------
bool TestNumber(const vector<int> &vec, const int &number)
{
	for (int i=0; i<vec.size(); ++i)
	{
		if (vec[i] == number)
		{
			return true;
		}
	}

	return false;
}




////////////////////////////////////////////////////////////////////////////////

//---------------------GrabPermutation----------------------
//
//	given an int, this function returns a vector containing
//	a random permutation of all the integers up to the supplied
//	parameter.
//------------------------------------------------------------
vector<int> SGenome::GrabPermutation(int &limit)
{
	vector<int> vecPerm;
	
	for (int i=0; i<limit; i++)
	{
		//we use limit-1 because we want ints numbered from zero
		int NextPossibleNumber = RandInt(0, limit-1);

		while(TestNumber(vecPerm, NextPossibleNumber))
		{
			NextPossibleNumber = RandInt(0, limit-1);
		}

		vecPerm.push_back(NextPossibleNumber);
	}

	return vecPerm;
}




/////////////////////////////////////////////////////////////////////////////


//-----------------------CalculatePopulationsFitness--------------------------
//
//	calculates the fitness of each member of the population, updates the
//	fittest, the worst, keeps a sum of the total fittness scores and the
//	average fitness score of the population (most of these stats are required
//	when we apply pre-selection fitness scaling.
//-----------------------------------------------------------------------------
void CgaTSP::CalculatePopulationsFitness()
{

	for (int i=0; i<m_iPopSize; ++i)
	{

		double TourLength = m_pMap->GetTourLength(m_vecPopulation[i].vecCities);

		m_vecPopulation[i].dFitness = TourLength;
		
		//keep a track of the shortest route found each generation
		if (TourLength < m_dShortestRoute)
		{
			m_dShortestRoute = TourLength;
		}
		
		//keep a track of the worst tour each generation
		if (TourLength > m_dLongestRoute)
		{
			m_dLongestRoute = TourLength;
		}

	}//next chromo

	//Now we have calculated all the tour lengths we can assign
	//the fitness scores
	for (i=0; i<m_iPopSize; ++i)
	{
		m_vecPopulation[i].dFitness = m_dLongestRoute - m_vecPopulation[i].dFitness;
	}

	//calculate values used in selection
	CalculateBestWorstAvTot();

}

//-----------------------CalculateBestWorstAvTot-----------------------	
//
//	calculates the fittest and weakest genome and the average/total 
//	fitness scores
//---------------------------------------------------------------------
void CgaTSP::CalculateBestWorstAvTot()
{
	m_dTotalFitness = 0;
	
	double HighestSoFar = -9999999;
	double LowestSoFar  = 9999999;
	
	for (int i=0; i<m_iPopSize; ++i)
	{
		//update fittest if necessary
		if (m_vecPopulation[i].dFitness > HighestSoFar)
		{
			HighestSoFar	 = m_vecPopulation[i].dFitness;
			
			m_iFittestGenome = i;

			m_dBestFitness	 = HighestSoFar;
		}
		
		//update worst if necessary
		if (m_vecPopulation[i].dFitness < LowestSoFar)
		{
			LowestSoFar = m_vecPopulation[i].dFitness;
			
			m_dWorstFitness = LowestSoFar;
		}
		
		m_dTotalFitness	+= m_vecPopulation[i].dFitness;
		
		
	}//next chromo
	
	m_dAverageFitness = m_dTotalFitness / m_iPopSize;

  //if all the fitnesses are zero the population has converged
  //to a grpoup of identical genomes so we should stop the run
  if (m_dAverageFitness == 0)
  {
    m_dSigma = 0;
  }

}

//-----------------------------FitnessScaleRank----------------------
//
//	This type of fitness scaling sorts the population into ascending
//	order of fitness and then simply assigns a fitness score based 
//	on its position in the ladder. (so if a genome ends up last it
//	gets score of zero, if best then it gets a score equal to the size
//	of the population. 
//---------------------------------------------------------------------
void CgaTSP::FitnessScaleRank(vector<SGenome> &pop)
{
	//sort population into ascending order
	if (!m_bSorted)
	{
		sort(pop.begin(), pop.end());

		m_bSorted = true;
	}

	//now assign fitness according to the genome's position on
	//this new fitness 'ladder'
	for (int i=0; i<pop.size(); i++)
	{
		pop[i].dFitness = i;
	}

	//recalculate values used in selection
	CalculateBestWorstAvTot();
}


//----------------------------- FitnessScaleSigma ------------------------
//
//  Scales the fitness using sigma scaling based on the equations given
//  in Chapter 5 of the book.
//------------------------------------------------------------------------
void CgaTSP::FitnessScaleSigma(vector<SGenome> &pop)
{
  double RunningTotal = 0;

  //first iterate through the population to calculate the standard
  //deviation
  for (int gen=0; gen<pop.size(); ++gen)
  {
    RunningTotal += (pop[gen].dFitness - m_dAverageFitness) *
                    (pop[gen].dFitness - m_dAverageFitness);
  }

  double variance = RunningTotal/(double)m_iPopSize;

  //standard deviation is the square root of the variance
  m_dSigma = sqrt(variance);

  //now iterate through the population to reassign the fitness scores
  for (gen=0; gen<pop.size(); ++gen)
  {
    double OldFitness = pop[gen].dFitness;

    pop[gen].dFitness = (OldFitness - m_dAverageFitness) /
                                    (2 * m_dSigma);
  }

  //recalculate values used in selection
	CalculateBestWorstAvTot();

}   

//------------------------- FitnessScaleBoltzmann ------------------------
//
//  This function applies Boltzmann scaling to a populations fitness
//  scores as described in Chapter 5.
//  The static value Temp is the boltzmann temperature which is reduced
//  each generation by a small amount. As Temp decreases the difference 
//  spread between the high and low fitnesses increases.
//------------------------------------------------------------------------
void CgaTSP::FitnessScaleBoltzmann(vector<SGenome> &pop)
{

  //reduce the temp a little each generation
  m_dBoltzmannTemp -= BOLTZMANN_DT;

  //make sure it doesn't fall below minimum value
  if (m_dBoltzmannTemp< BOLTZMANN_MIN_TEMP)
  {
    m_dBoltzmannTemp = BOLTZMANN_MIN_TEMP;
  }

  //first calculate the average fitness/Temp
  double divider = m_dAverageFitness/m_dBoltzmannTemp;

  //now iterate through the population and calculate the new expected
  //values
  for (int gen=0; gen<pop.size(); ++gen)
  {
    double OldFitness = pop[gen].dFitness;

    pop[gen].dFitness = (OldFitness/m_dBoltzmannTemp)/divider;
  }

  //recalculate values used in selection
	CalculateBestWorstAvTot();
}

//--------------------------FitnessScale----------------------------------
//
//  This is simply a switch statement to choose a selection method
//  based on the user preference
//------------------------------------------------------------------------
void CgaTSP::FitnessScaleSwitch()
{
  switch(m_ScaleType)
  {
  case NONE:

    break;

  case SIGMA:
    
    FitnessScaleSigma(m_vecPopulation);

    break;

  case BOLTZMANN:
    
    FitnessScaleBoltzmann(m_vecPopulation);

    break;

  case RANK:
    
    FitnessScaleRank(m_vecPopulation);

    break;
  }
}
//-------------------------GrabNBest----------------------------------
//
//	This works like an advanced form of elitism by inserting NumCopies
//  copies of the NBest most fittest genomes into a population vector
//--------------------------------------------------------------------
void CgaTSP::GrabNBest(int				      NBest,
					             const int        NumCopies,
					             vector<SGenome>	&vecNewPop)
{
	//first we need to sort our genomes
	if (!m_bSorted)
	{
		sort(m_vecPopulation.begin(), m_vecPopulation.end());

		m_bSorted = true;
	}

	//now add the required amount of copies of the n most fittest 
	//to the supplied vector
	while(NBest--)
	{
		for (int i=0; i<NumCopies; ++i)
		{
			vecNewPop.push_back(m_vecPopulation[(m_iPopSize - 1) - NBest]);
		}
	}
}

//--------------------------RouletteWheelSelection----------------------
//
//	selects a member of the population by using roulette wheel selection
//	as described in the text.
//-----------------------------------------------------------------------
SGenome& CgaTSP::RouletteWheelSelection()
{
	double fSlice	= RandFloat() * m_dTotalFitness;
	
	double cfTotal	= 0.0;
	
	int	SelectedGenome = 0;
	
	for (int i=0; i<m_iPopSize; ++i)
	{
		
		cfTotal += m_vecPopulation[i].dFitness;
		
		if (cfTotal > fSlice) 
		{
			SelectedGenome = i;
			
			break;
		}
	}
	
	return m_vecPopulation[SelectedGenome];
}

//----------------------- SUSSelection -----------------------------------
//
//  This function performs Stochasitic Universal Sampling.
//
//  SUS uses N evenly spaced hands which are spun once to choose the 
//  new population. As described in chapter 5.
//------------------------------------------------------------------------
void CgaTSP::SUSSelection(vector<SGenome> &NewPop)
{
  //this algorithm relies on all the fitnesses to be positive so
  //these few lines check and adjust accordingly (in this example
  //Sigma scaling can give negative fitnesses
  if (m_dWorstFitness < 0)
  {
    //recalculate
    for (int gen=0; gen<m_vecPopulation.size(); ++gen)
    {
      m_vecPopulation[gen].dFitness += fabs(m_dWorstFitness);
    }

    CalculateBestWorstAvTot();
  }

  int curGen = 0;
  double sum = 0;

  //NumToAdd is the amount of individuals we need to select using SUS.
  //Remember, some may have already been selected through elitism
  int NumToAdd = m_iPopSize - NewPop.size();

  //calculate the hand spacing
  double PointerGap = m_dTotalFitness/(double)NumToAdd;

  //choose a random start point for the wheel
  float ptr = RandFloat() * PointerGap;

	while (NewPop.size() < NumToAdd)
  {
	  for(sum+=m_vecPopulation[curGen].dFitness; sum > ptr; ptr+=PointerGap)
    {
	     NewPop.push_back(m_vecPopulation[curGen]);

       if( NewPop.size() == NumToAdd)
       {
         return;
       }
    }

    ++curGen;
  }
}


//---------------------------- TournamentSelection -----------------
//
//  performs standard tournament selection given a number of genomes to
//  sample from each try.
//------------------------------------------------------------------------
SGenome& CgaTSP::TournamentSelection(int N)
{
  double BestFitnessSoFar = 0;
  
  int ChosenOne = 0;

  //Select N members from the population at random testing against 
  //the best found so far