//////////////////////////////////////////////////////////////////////
// Implementacia simulovaneho zihania
// Automaticky inkludovane v sa.h


#include <math.h>
#include <time.h>
#include <stdlib.h>

inline double rnd()
{
  return double(rand())/(RAND_MAX+1.0);
}

inline int rnd(int lessThan)
{
  return lessThan != 0 ? rand() % lessThan : 0;
}

///////////////////////////////////////////////////////////////////////////
// Report

template<class OptVect> Report<OptVect>::~Report()
{
  delete valRecord;
  for(int k = 0; k < trace.size(); k++)
  {
    delete trace[k].macroVals;
    delete trace[k].xOpt;
  }
}

template<class OptVect>void Report<OptVect>::endNewStep(const OptVect &optim,
    							double t)
{
  ReportElem elem;
  double recVal, avg, avgSq, entropy, disper;
  double recValCnt; //je to pocet, ale nasobi sa s double
  multiset<double>::iterator recIter,last;
  double total;
  int i;
  
  elem.stepNr = newStepNr;
  elem.t = t;
  elem.xOpt = new OptVect(optim);
  elem.macroVals = new vector<double>(mvCNT,0);
  avg = avgSq = entropy = total = 0;
  
  last = valRecord->end();
  recIter = valRecord->begin();
  while(recIter != last)
  {
    recVal = *(recIter); recValCnt = valRecord->count(recVal);
    avg += recVal * recValCnt;
    avgSq += (recVal * recVal) * recValCnt;
    entropy += recValCnt * log(recValCnt);
    total += recValCnt;
    for(i = 0; i < recValCnt; i++)
      recIter++;
  }
  avg /= total;
  avgSq /= total;
  disper = avgSq - avg * avg;
  (*(elem.macroVals))[mvAVG_VAL] = avg;
  (*(elem.macroVals))[mvAVG_VAL_SQ] = avgSq;
  (*(elem.macroVals))[mvDISPER] = disper;
  (*(elem.macroVals))[mvSPEC_HEAT] = disper / t;
  (*(elem.macroVals))[mvENTROPY] = -(entropy/total - log(total));
  
  trace.push_back(elem);
}

template<class OptVect>int Report<OptVect>::numOfRecords()
{
  return trace.size();
}

template<class OptVect> double Report<OptVect>::getMacroVal(int recNr, int valNr)
{
  return (*(trace[recNr].macroVals))[valNr];
}

template<class OptVect> OptVect &Report<OptVect>::getVector(int recNr)
{
  return *(trace[recNr].xOpt);
}

template<class OptVect> double Report<OptVect>::getTemp(int recNr)
{
  return trace[recNr].t;
}

template<class OptVect> double Report<OptVect>::getPurpFVal(int recNr)
{
  return getVector(recNr).purpFun();
}

template<class OptVect> void Report<OptVect>::printVector(int recNr, ostream &s)
{
  getVector(recNr).print(s);
}

//////////////////////////////////////////////////////////////////////////
// Zakladne simulovane zihanie

template<class OptVect> void SimulatedAnnealing<OptVect>::metropolis
					(OptVect &x, double t, bool doRep)
{
  OptVect xMut;
  double p = 0,
  	 fXMut = 0,
	 fX = x.purpFun();

  for(int k = 0; k < kmax; k++)
  {
    xMut.becomeMutationOf(x);
    fXMut = xMut.purpFun();
    p = exp(-(fXMut - fX) / t);/* mozno prekroci 1, ale to nevadi */
    if (rnd() < p)
    {
      x.copyFrom(xMut);
      fX = fXMut;
      if (doRep)
	rep->record(fX);
    }
  }
  // x sme dostali referenciou, jeho zmeny sa prenesu von
}

template<class OptVect> OptVect &SimulatedAnnealing<OptVect>::anneal(OptVect &x)
{
  double t;
  int step = 0;
  x.randomInit();

  for(t = tMax; t > tMin; t *= annFact)
  {
    if (((repPer > 0) && (step % repPer == 0)) ||
	(t * annFact < tMin))//musi sa urobit zaznam aspon v poslednom kroku
    {
      rep->beginNewStep(step);
      metropolis(x, t, true);
      rep->endNewStep(x,t);
    }
    else
      metropolis(x, t, false);
    step++;
  }
  return x;
}

///////////////////////////////////////////////////////////////////////////
// Elitarske simulovane zihanie

template<class OptVect> void ElitSimulAnnealing<OptVect>::metropolis
					(OptVect &x, double t, bool doRep)
{
  OptVect xMut;
  OptVect xWrk(x);	// tato inicializacia je dolezita
  double p = 0,
  	 fXMut = 0,
	 fX = x.purpFun(),
	 fXWrk = xWrk.purpFun();
  int i;
  for(int k = 0; k < kmax; k++)
  {
    xMut.becomeMutationOf(xWrk);
    fXMut = xMut.purpFun();
    p = exp(-(fXMut - fXWrk) / t);	/* mozno prekroci 1, ale to nevadi */
    if (rnd() < p)
    {
      xWrk.copyFrom(xMut);
      fXWrk = fXMut;
      if (fXMut < fX)	// tu nastupuje elitarstvo
      {
	x.copyFrom(xMut);
	fX = fXMut;
      }
      if (doRep)	// nezabudame zaznamenavat
	rep->record(fX);
    }
  }
}
  
///////////////////////////////////////////////////////////////////////////
// Paralelne simulovane zihanie

template<class OptVect> void 
  ParallelSimulAnnealing<OptVect>::metropolis
					(vector<OptVect> &pool, double t, bool doRep)
{
  OptVect *x, *y,
 	  chld1(pool[0]), chld2(pool[0]);
  int idx1, idx2;
  double p1, p2, f1, f2;

  for(int k = 0; k < kmax; k++)
    if (rnd() > pCross)
    {
      idx1 = rnd(poolSize);
      x = &(pool[idx1]);
      chld1.becomeMutationOf(*x);
#ifdef DEBUG2
	cout<<" "<<f1;
#endif

      f1 = chld1.purpFun();
      p1 = exp(-(f1 - x->purpFun()) / t);
      if (rnd() < p1)
      {
#ifdef DEBUG2
	cout<<"->"<<pool[idx1].purpFun()<<endl;
#endif
	pool[idx1].copyFrom(chld1);
	if (doRep) rep->record(f1);
      }
    }
    else
    {
      idx1 = rnd(poolSize);
      idx2 = rnd(poolSize);
      if (idx1 == idx2)
	idx2 = (idx2 + 1) % poolSize;
      x = &(pool[idx1]);
      y = &(pool[idx2]);
      x->cross(*y,chld1,chld2);
      f1 = chld1.purpFun();
      f2 = chld2.purpFun();
      p1 = exp(-(f1 - x->purpFun()) / t);
      p2 = exp(-(f2 - y->purpFun()) / t);
      if (rnd() < p1)
      {
#ifdef DEBUG2
	cout<<pool[idx2].purpFun()<<"<*"<<f1<<endl;
#endif
	pool[idx1].copyFrom(chld1);
	if (doRep) rep->record(f1);
      }
      if (rnd() < p2)
      {
#ifdef DEBUG2
	cout<<pool[idx2].purpFun()<<"<*"<<f2<<endl;
#endif
	pool[idx2].copyFrom(chld2);
	if (doRep) rep->record(f2);
      }
    }
}

template<class OptVect> OptVect &ParallelSimulAnnealing<OptVect>::anneal(OptVect &x)
{
  vector<OptVect> pool(poolSize,x);
  int k, idxOpt;
  int step = 0;
  double fOpt, fx, t;

  //pool = new OptVect[poolSize](x);
  
  for(k = 0; k < poolSize; k++)
  {
    //pool[k].copyFrom(x);
    pool[k].randomInit();
  }
  
  for(t = tMax; t > tMin; t *= annFact)
  {
    if (((repPer > 0) && (step % repPer == 0)) ||
	(t * annFact < tMin))//musi sa urobit zaznam aspon v poslednom kroku
    {
      rep->beginNewStep(step);
      metropolis(pool, t, true);
      
      fOpt = pool[0].purpFun();
#ifdef DEBUG3
      cout<<fOpt<<" ";
#endif
      idxOpt = 0;
      for(k = 1; k < poolSize; k++)
      {
	fx = pool[k].purpFun();
#ifdef DEBUG3
	cout<<fx<<" ";
#endif
	if (fx < fOpt)
	{
	  idxOpt = k;
	  fOpt = fx;
	}
      }
      
      rep->endNewStep(pool[idxOpt],t);
    }
    else
      metropolis(pool, t, false);
    step++;
  }

  x.copyFrom(pool[idxOpt]);
  return x;
}