#include <stdlib.h>
#include "..\\Utils.h"
#include "..\\TSPDataSet.h"
#include "PSO.h"
static void PSO_MendCycle( TParticle *Particle )
{
    int tLooper1, tLooper2;
int HasFound;
tLooper1 = 1;
while( tLooper1 < Particle->Dimension )
{
  //查找是否有重復節點
  HasFound = 0;
  for( tLooper2=0; tLooper2<tLooper1; tLooper2++ )
  {
   if( Particle->X[ tLooper2 ] == Particle->X[ tLooper1 ] )
   {
    HasFound = 1;
    break;
   }
  }
  if( HasFound == 1 )
  {
   Particle->X[ tLooper1 ] = ( Particle->X[ tLooper1 ] + 1 ) % Particle->Dimension;
  }
  else
  {
   tLooper1++;
  }
}
}
//適合度計算方法，必須定義
static double PSO_GetFit( CTSPDataSet *dat, TParticle *Particle ) 
{
int tResult = 0;
tResult = dat->GetCycleLength( Particle->Dimension, Particle->X );
return (double)tResult;
}
//計算群體各個微粒適合度
static void PSO_CalFit( CTSPDataSet *dat, TPSO *PSO )
{
if(!PSO)
{
  return;
}
for(int i=0; i<PSO->PNumber; i++)
{
  PSO->Particle[i].Fitness = PSO_GetFit( dat, &(PSO->Particle[i]));
}
}
//初始化數據
static void PSO_InitializeData( CTSPDataSet *dat, TPSO *PSO, int PNumber, int PDimension )
{
if(!PSO) 
{
  return;
}
static int kk=(unsigned)time(NULL);
srand((unsigned)time(NULL)+kk++);
PSO->GBestIndex = 0;
for(int i=0; i<PNumber; i++)
{
  for(int j=0; j<PSO->Particle[i].Dimension; j++)
  {
   PSO->Particle[i].X[j] = (int)(rand()/(double)RAND_MAX*(PSO->Xup[j]-PSO->Xdown[j])+PSO->Xdown[j]);//初始化坐標
   PSO->Particle[i].Velocity[j] = rand()/(double)RAND_MAX*PSO->Vmax[j]-PSO->Vmax[j]/2;//初始化速度
  }
  PSO_MendCycle(&(PSO->Particle[i]));
  for(j=0; j<PSO->Particle[i].Dimension; j++)
  {
   PSO->Particle[i].XBest[j] = PSO->Particle[i].X[j];
  }
  PSO->Particle[i].Fitness = PSO_GetFit( dat, &(PSO->Particle[i])); //計算該微粒適合度
  PSO->Particle[i].FitnessBest = PSO->Particle[i].Fitness; //設最優適合度初值
  if(PSO->Particle[i].Fitness<PSO->Particle[PSO->GBestIndex].Fitness) 
  {
   PSO->GBestIndex = i;//查找群體最優微粒
  }
}
}
//初始化數據結構
static PSO_InitializeDataStruce( TPSO *PSO, int PNumber, int PDimension )
{
int tLooper;
PSO->GBestIndex = -1;
PSO->PNumber = PNumber;
PSO->PDimension = PDimension;
PSO->Xup = CC_SAFE_MALLOC( PDimension, int );
PSO->Xdown = CC_SAFE_MALLOC( PDimension, int );
for( tLooper=0; tLooper<PDimension; tLooper++ )
{
     PSO->Xup[ tLooper ] = PDimension - 1;
  PSO->Xdown[ tLooper ] = 0;
}
PSO->Vmax = CC_SAFE_MALLOC( PDimension, double );
for( tLooper=0; tLooper<PDimension; tLooper++ )
{
     PSO->Vmax[ tLooper ] = ( PSO->Xup[ tLooper ] - PSO->Xdown[ tLooper ] ) * VelocityCoefficent;
}
PSO->Particle = CC_SAFE_MALLOC( PNumber, TParticle );
for( tLooper=0; tLooper<PNumber; tLooper++ )
{
     PSO->Particle[ tLooper ].Dimension = PDimension;
  PSO->Particle[ tLooper ].X = CC_SAFE_MALLOC( PDimension, int );
  PSO->Particle[ tLooper ].Velocity = CC_SAFE_MALLOC( PDimension, double );
  PSO->Particle[ tLooper ].XBest = CC_SAFE_MALLOC( PDimension, int );
  PSO->Particle[ tLooper ].Fitness = -1;
  PSO->Particle[ tLooper ].FitnessBest = -1;
}
}
static void PSO_ReleaseDataStructure( TPSO *PSO )
{
int tLooper;
for( tLooper=0; tLooper<PSO->PDimension; tLooper++ )
{
  CC_FREE( PSO->Particle[ tLooper ].XBest, int );
  CC_FREE( PSO->Particle[ tLooper ].Velocity, double );
  CC_FREE( PSO->Particle[ tLooper ].X, int );
}
CC_FREE( PSO->Particle, TParticle );
CC_FREE( PSO->Vmax, double );
CC_FREE( PSO->Xdown, int );
CC_FREE( PSO->Xup, int );
}
//微粒飛翔,產生新一代微粒
static void PSO_ParticleFly( CTSPDataSet *dat, TPSO *PSO )
{
static double FitBak[100];
if(!PSO)
{
  return;
}
static int tt=(unsigned)time(NULL);
srand((unsigned)time(NULL)+tt++);
//整個群體飛向新的位置
for(int i=0; i<PSO->PNumber; i++)
{
  //處理速度
  for(int j=0; j<PSO->Particle[i].Dimension; j++)
  {
   double tTemp;
   tTemp = (PSO->Particle[i]).Velocity[j];
   //tTemp = WEIGHT * tTemp;
   tTemp += rand()/(double)RAND_MAX*C1*(PSO->Particle[i].XBest[j]-PSO->Particle[i].X[j]);
   tTemp += rand()/(double)RAND_MAX*C2*(PSO->Particle[PSO->GBestIndex].XBest[j]-PSO->Particle[i].X[j]);
   (PSO->Particle[i]).Velocity[j] = tTemp;
  }
  //檢查速度最大值
  for(j=0; j<PSO->Particle[i].Dimension; j++) 
  {
   if(PSO->Particle[i].Velocity[j]>PSO->Vmax[j]) 
   {
    PSO->Particle[i].Velocity[j] = PSO->Vmax[j];
   }
   if(PSO->Particle[i].Velocity[j]<-PSO->Vmax[j]) 
   {
    PSO->Particle[i].Velocity[j] = -PSO->Vmax[j];
   }
  }
  //粒子飛翔
  for(j=0; j<PSO->Particle[i].Dimension; j++)
  {
   //修改坐標
   PSO->Particle[i].X[j] = (int)(PSO->Particle[i].Velocity[j] + PSO->Particle[i].X[j]) % PSO->Xup[j]; 
   //保護
   if(PSO->Particle[i].X[j]>PSO->Xup[j]) 
   {
    PSO->Particle[i].X[j]=PSO->Xup[j];
   }
   if(PSO->Particle[i].X[j]<PSO->Xdown[j]) 
   {
    PSO->Particle[i].X[j]=PSO->Xdown[j];
   }
  }
  //對粒子飛翔后產生的新的個體進行修復
  PSO_MendCycle( &(PSO->Particle[i]) );
}
//計算各微粒適合度
PSO_CalFit( dat, PSO );
for(i=0; i<PSO->PNumber; i++) 
{
  FitBak[i] = PSO->Particle[i].Fitness;
}
//設置新的個體最好位置
for(i=0; i<PSO->PNumber; i++)
{
  if(PSO->Particle[i].Fitness<=PSO->Particle[i].FitnessBest)
  {
   PSO->Particle[i].FitnessBest = PSO->Particle[i].Fitness;
   for(int j=0; j<PSO->Particle[i].Dimension; j++)
   {
    PSO->Particle[i].XBest[j] = PSO->Particle[i].X[j];
   }
  }
}

//設置新的最優個體
PSO->GBestIndex = 0;
for(i=0; i<PSO->PNumber; i++)
{
  if(PSO->Particle[i].FitnessBest<=PSO->Particle[PSO->GBestIndex].FitnessBest && i!=PSO->GBestIndex) 
  {
   PSO->GBestIndex = i;
  }
}
}
//返回最佳個體
static void PSO_GetBest( TPSO *PSO, int *elist )
{
for(int i=0; i<PSO->Particle[PSO->GBestIndex].Dimension; i++)
{
  elist[i*2] = PSO->Particle[PSO->GBestIndex].XBest[i];
  elist[i*2+1] = PSO->Particle[PSO->GBestIndex].XBest[(i+1)%PSO->Particle[PSO->GBestIndex].Dimension];
}
}
//主程序
void PSO_Main( CTSPDataSet *dat, int PSONumber, int ecount, int *elist )
{
TPSO *PSO;
int counter=0;
PSO = CC_SAFE_MALLOC( 1, TPSO );
PSO_InitializeDataStruce( PSO, 20, ecount );
    PSO_InitializeData( dat, PSO, 20, ecount );
do
{
  PSO_ParticleFly( dat, PSO );
  counter++;
} while ( counter<PSONumber );
PSO_GetBest( PSO, elist );
PSO_ReleaseDataStructure( PSO );
CC_FREE( PSO, TPSO );
}