// PsoVrptw.cpp: implementation of the CPsoVrptw class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "VRP.h"
#include "math.h"
#include "PsoVrptw.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CPsoVrptw::CPsoVrptw()
{

}

CPsoVrptw::~CPsoVrptw()
{

}

void CPsoVrptw::reserial(CPsoVrptw::Node *PNode)
{
	int i,j,count;
	double data;

	for(i=1;i<=TASKTW;i++)
	{
		data=PNode->Xr[i];
		count=1;
		for(j=1;j<=TASKTW;j++)
		{
			if(PNode->Xv[i]==PNode->Xv[j]&&data-PNode->Xr[j]>=0.000001)
				count++;
			if(PNode->Xv[i]==PNode->Xv[j]&&(fabs(data-PNode->Xr[j])<0.000001)&&i>j)
				count++;
		}
		PNode->iXr[i]=count;
	}
    
	return;
}

double CPsoVrptw::Eval(CPsoVrptw::PNODE *PNode)
{
	int i,j,k,step;
	double dist[VECHI+1];
	double weight[VECHI+1];
	double All_weight,All_dist;
    int pre;
	double pe=50,pl=50,speed=50;
	double time[VECHI+1];



label1:
	reserial(PNode);

	for(i=1;i<=VECHI;i++)
	{
		dist[i]=0;
		time[i]=0;
	    weight[i]=0;
	}

	for(i=1;i<=VECHI;i++)
	{
		step=1;
		pre=0;
		do
		{
		  for(j=1;j<=TASKTW;j++)
			if(PNode->Xv[j]==i&&PNode->iXr[j]==step)
				break;
		  if(j<=TASKTW)
			//找到車輛i的step次任務點j
		  {
		    dist[i]+=dist_M[pre][j];
		    weight[i]+=GPNode.weight[j];
			time[i]+=dist_M[pre][j]/speed;
			if(time[i]>GPNode.end_time[j])
			{
				dist[i]+=pl*(time[i]-GPNode.end_time[j]);
				time[i]+=GPNode.time[j];
			}else
			if(time[i]<GPNode.start_time[j])
			{
				dist[i]+=pe*(GPNode.start_time[j]-time[i]);
				time[i]=GPNode.start_time[j]+GPNode.time[j];
			}else
				time[i]+=GPNode.time[j];
			pre=j;
		  }
		  step=step+1;
		}while(j<=TASKTW);
        dist[i]+=dist_M[pre][0];
	}
	 All_weight=0;
	 for(i=1;i<=VECHI;i++)
		 All_weight+=weight[i];
	 All_dist=0;
	 for(i=1;i<=VECHI;i++)
		 All_dist+=dist[i];
   PNode->value=All_dist;
   for(i=1;i<=VECHI;i++)
	   if(weight[i]>8.0)
	   {
		   do{
			   j=rand()%VECHI+1;
		   }while(j==i);
		   do{
			   k=rand()%TASKTW+1;
		   }while(PNode->Xv[k]!=i);
		   //從車輛中找出一輛取代i
		   PNode->Xv[k]=j;
		   goto label1;
	   }

   return PNode->value;
}

void CPsoVrptw::initialize()
{
  int i,j;
  /*VNode[0].Node.Xv[1]=2;
  VNode[0].Node.Xv[2]=2;
  VNode[0].Node.Xv[3]=2;
  VNode[0].Node.Xv[4]=1;
  VNode[0].Node.Xv[5]=3;
  VNode[0].Node.Xv[6]=1;
  VNode[0].Node.Xv[7]=3;
  VNode[0].Node.Xv[8]=3;
  //
  VNode[0].Node.iXr[1]=2;
  VNode[0].Node.iXr[2]=3;
  VNode[0].Node.iXr[3]=1;
  VNode[0].Node.iXr[4]=2;
  VNode[0].Node.iXr[5]=2;
  VNode[0].Node.iXr[6]=1;
  VNode[0].Node.iXr[7]=3;
  VNode[0].Node.iXr[8]=1;

  Eval(&VNode[0].Node);*/


  for(i=0;i<PopSize;i++)
	{
		for(j=1;j<=TASKTW;j++)
		{
			VNode[i].Node.Xv[j]=rand()%VECHI+1;
			VNode[i].Node.Xr[j]=rand()%TASKTW+1;
            VNode[i].Node.Vv[j]=(rand()%10>5)?rand()%VECHI:(-(rand()%VECHI));
            VNode[i].Node.Vr[j]=(rand()%10>5)?rand()%TASKTW:(-(rand()%TASKTW));
		}
        Eval(&VNode[i].Node);
  }
  for(i=0;i<PopSize;i++)
  {
	  VNode[i].LocalNode=VNode[i].Node;
  }
  //生成局部最小
  j=0;
  double minvalue=MAX;
  for(i=0;i<PopSize;i++)
  {
	  if(VNode[i].Node.value<minvalue)
	  {
		  minvalue=VNode[i].Node.value;
		  j=i;
	  }
  }
  GBest=VNode[j].Node;
  //全局最小值 
}

void CPsoVrptw::localbest(int Pos)
{
	if(VNode[Pos].Node.value<VNode[Pos].LocalNode.value)
		VNode[Pos].LocalNode=VNode[Pos].Node;
}

void CPsoVrptw::globalbest(int Pos)
{
	if(VNode[Pos].Node.value<GBest.value)
		GBest=VNode[Pos].Node;
}

void CPsoVrptw::comput_Pso(int Pos)
{
	int i;
	double c1=0.729,r1=1.49445,r2=1.49445;
	double s;
	CPsoVrptw::PNODE PNode;
	double rand1,rand2;
	rand1=double(rand()%100)/100;
	rand2=double(rand()%100)/100;

	PNode=VNode[Pos].Node;
	for(i=1;i<=TASKTW;i++)
	{
		PNode.Vv[i]=(int)(c1*PNode.Vv[i]+r1*rand1*(VNode[Pos].LocalNode.Xv[i]-PNode.Xv[i])+r2*rand2*(GBest.Xv[i]-PNode.Xv[i]));
		if(PNode.Vv[i]>MaxVSpeed)
		  PNode.Vv[i]=MaxVSpeed;
        if(PNode.Vv[i]<-MaxVSpeed)
		  PNode.Vv[i]=-MaxVSpeed;
		
		PNode.Vr[i]=c1*PNode.Vr[i]+r1*rand1*(VNode[Pos].LocalNode.Xr[i]-PNode.Xr[i])+r2*rand2*(GBest.Xr[i]-PNode.Xr[i]);
		if(PNode.Vr[i]>MaxRTWSpeed)
		  PNode.Vr[i]=MaxRTWSpeed;
        if(PNode.Vr[i]<-MaxRTWSpeed)
		  PNode.Vr[i]=-MaxRTWSpeed;
	}
    
	for(i=1;i<=TASKTW;i++)
	{
        s=PNode.Xv[i]+PNode.Vv[i];
		PNode.Xv[i]=s>(int)s?(int)s+1:(int)s;

      	if(PNode.Xv[i]>VECHI)
		  PNode.Xv[i]=VECHI;
		if(PNode.Xv[i]<1)
		  PNode.Xv[i]=1;
        PNode.Xr[i]=PNode.Xr[i]+PNode.Vr[i];
	}
	Eval(&PNode);
	if(PNode.value>VNode[Pos].Node.value)
		VNode[Pos].Node=PNode;

}

double CPsoVrptw::Vrp_Pso(int *Try_step)
{
	int Step,i;
	double prebest;

	initialize();
	prebest=GBest.value;
for(Step=0;Step<200;Step++)
	{
		for(i=0;i<PopSize;i++)
		{
			comput_Pso(i);
			localbest(i);
			globalbest(i);
		}
		if(fabs(prebest-GBest.value)<0.000001) {Step++;prebest=GBest.value;}
		else
		{Step=0;prebest=GBest.value;}
	}//while(Step<20);
		
    *Try_step=Step;
	return GBest.value;

}