#include"stdio.h"
#include "math.h"
#include "stdlib.h"
#include "time.h"
#define N 100  //最大粒子數 
#define D 10  //最大問題維數 

double fitness(double x[],int n);
typedef double (*funType)(double [],int n);
double pso(funType fp,int n,double c1,double c2,double w,int m,int d,double xbest[]);

int main(){
	double x[D];
	double f;
	f = pso(fitness,40,1.5,1.5,0.6,2000,1,x);
	printf("最優的x為：");
	for(int i=0;i<1;i++)
		printf("%f ",x[i]); 
	printf("\n");
	printf("函式的最優值為：%f\n",f);
	return 0;

}
double fitness(double x[],int n){  //待優化函式 
	double f = 0;
	f = x[0]*x[0]-2*x[0]+1;  // f(x) = x.^2-2*x+1
	return f;	
}

/*
	fp: 待優化的目標函式
	n: 粒子數目
	c1,c2: 學習因子1，學習因子2
	w: 慣性權重
	M: 最大迭代次數
	d: 問題的維數
	x: 返回的最優解 
*/
double pso(funType fp,int n,double c1,double c2,double w,int m,int d,double xbest[]){
	int i,j,t;
	double x[N][D],v[N][D];
	double pg[D]; //全域性最優解
	double y[N];  //粒子適應度
	double p[N][D];  
	//step1.初始化種群的個體
	srand((int)time(0)); 
	for(i=0;i<n;i++)          	
		for(j=0;j<d;j++){
			x[i][j] = rand()*1.0/RAND_MAX;
			v[i][j] = rand()*1.0/RAND_MAX;
		}
	//step2. 計算粒子適應度，初始化p和pg 
	for(i=0;i<d;i++)
		pg[i] = x[n-1][i]; // pg為全域性最優 
	for(i=0;i<n;i++){
		y[i] = fp(x[i],d);
		for(j=0;j<d;j++)
			p[i][j] = x[i][j];
	}
	for(i=0;i<n-1;i++){
		if(y[i]<y[n-1]){
			for(j=0;j<d;j++)
				pg[j] = x[i][j];
		}		
	}
	//step3. 進入主迴圈，按照公式依次迭代 	
	for(t=0;t<m;t++){         
		for(i=0;i<n;i++){
			for(j=0;j<d;j++){
				v[i][j] = w*v[i][j] + c1*(rand()/RAND_MAX)*(p[i][j]-x[i][j]) + c2*(rand()/RAND_MAX)*(pg[j]-x[i][j]); //更新 
				x[i][j] = x[i][j] + v[i][j];
			}
			if(fp(x[i],d)<y[i]){
				y[i] = fp(x[i],d);
				for(j=0;j<d;j++)
					p[i][j] = x[i][j];
			}
			if(y[i]<fp(pg,d)){
				for(j=0;j<d;j++)
					pg[j] = p[i][j];
			}
		} 
		
	}
	
	for(i=0;i<d;i++) //最優解 
		xbest[i] = pg[i];  
	return fp(pg,d);
}