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

# define M 4096
# define N 256
# define L 16
# define POPSIZE 2
# define DIMENSION N*L
# define MAXLOOP 17
# define TEAM 2

float c1=0.3,c2=0.5;
float w=0.1;
float Vmax=255.0,Xmax=255.0,Xmin=0.0;

float G[DIMENSION];
float gbest;

typedef struct
{ float position[DIMENSION];
	float speed[DIMENSION];
	float best_position[DIMENSION];
	float fitness;
	float pbest;
}prtc;
 
prtc particle[POPSIZE];

struct dis
{ float data;
   int list;
}D[M];

float udpart[M][TEAM];
int pic[M][L];
int u[M][N];
int p_flag;

void GenVector();
void init();
void update();
void calculate_fitness(int num);
void localbest(int num);
void globalbest(int num);
void do_lbg(int num);

void main()
{   GenVector();
	int i,k,t,time;
	float r1,r2;
	for(time=0;time<TEAM;time++)
	{ init();
		for(i=0;i<MAXLOOP;i++)
		{ for(k=0;k<POPSIZE;k++)
			{for(t=0;t<DIMENSION;t++)
				{ r1=(float)rand()/(RAND_MAX+1);
				   r2=(float)rand()/(RAND_MAX+1);
					
	particle[k].speed[t]=w * particle[k].speed[t]+c1*r1*(particle[k].best_position[t]-particle[k].position[t]+c2*r2*(G[t]-particle[k].position[t]);
     particle[k].position[t]=particle[k].position[t]+particle[k].speed[t];
                       if (particle[k].position[t]>Xmax)
                            particle[k].position[t]=Xmax;
                       if (particle[k].position[t]<Xmin)
                            pa[k].position[t]=Xmin;
			     }
                   do_lbg(k);
				calculate_fitness(k);
                    localbest(k);
		     }
              globalbest(1);
              printf("gbest is %f:",gbest);
		   printf("flag is %d \n",P_flag);
	     }  
		   for(i=0;i<M;i++)
		      udpart[i][time]=particle[p_flag].position[i];
     }   
	update();
	 
	for(i=0;i<MAXLOOP;i++)
	{    for(k=0;k<POPSIZE;k++)
			{for(t=0;t<DIMENSION;t++)
				{ r1=(float)rand()/(RAND_MAX+1);
				   r2=(float)rand()/(RAND_MAX+1);
					
	particle[k].speed[t]=w * particle[k].speed[t]+c1*r1*(particle[k].best_position[t]-particle[k].position[t]+c2*r2*(G[t]-particle[k].position[t]);
     particle[k].position[t]=particle[k].position[t]+particle[k].speed[t];
                       if (particle[k].position[t]>Xmax)
                            particle[k].position[t]=Xmax;
                       if (particle[k].position[t]<Xmin)
                            pa[k].position[t]=Xmin;
			     }
                   do_lbg(k);
				calculate_fitness(k);
                    localbest(k);
		     }
              globalbest(1);
              printf("gbest is %f:",gbest);
		   printf("flag is %d \n",P_flag);
     }  
		float psnr=(float) 10*log10(255*255/gbest)'
		printf("the best psnr is:%f\n",psnr);
		printf("the gbest is :%f\n",gbest);
		printf("the best particles number is part %d:\n",p_flag);
		
		for(k=0;k<M;k++)
		    printf("particle[% d].position[% d] is % f",p_flag,k,particle[p_flag].position[k]);
		for(i=0;i<M;i++)
		    for(l=o;k<N;K++)
		        if(u[i][k]==1)
				   printf("u[% d][% d] is % d", i,k,u[i][k]);
} 
void init()
{int i=0,j=0,k,p,index1,index2;
  srand((unsigned int) time(NULL));
  
  for(k=0;k<N;k++)
     {  index1=(int)(M*rand()/(RAND_MAX+1));
      index2=(int)(M*rand()/(RAND_MAX+1));
	 for(p=0;p<L;p++)
	   { particle[0].position[j]=(float)pic[index1][p];
	    particle[i+1].position[j]=(float)pic[index2][p];
	    j++;
	    }  

     }
   
	for(k=0;k<POPSIZE;k++)
	     for(p=0;p<DIMENSION;p++)
		     particle[k].speed[p]=(float)rand()*255/(RAND_MAX+1);
			
	for(i=0;i<POPSIZE;I++)
      { 
          for(j=0;j<DIMENSION;j++)
           particle[i].best_position[j]=particle[i].position[j];
           calculate_fitness(i);
           particle[i].pbest=particle[i].fitness;
       }
     globalbest(0);
      printf("init gbest is: %f\n",gbest);	
}

void calculate_fitness(int num)
{
   int i,j,k,p=0;
   float sum=0;
   float a[L],b[L],c;
   float part0[N][L];
   float part1[N][L];
   
   if(num==0)
   {
       for(i=0;i<N;i++)
	{  
	     for(k=0;j<L;j++)
		{ 
		   part0[i][j]=particle[0].position[p];
		   p++;
		 }
	}
     for(i=0;i<M;i++) 
	{
	   for(j=0;j<N;j++)
	   {
	     b[j]=0;
		for(k=0;k<L;k++)
		{
		  a[k]=pic[i][k]-part0[j][k];
		  b[j]+=a[k]*a[k];
		  }
		}
	c=b[0];
      for(j=1;j<N;j++)
         {
            if(c>b[j)];
               c=b[j];
           }
     sum+=c;
	}
}
 else
 {
     for(i=0;i<N;i++)
	{
	    for(j=0;j<L;j++)
	    {
	       part1[i][j]=particle[1].position[p];
		  p++;
		}
	}
     for(i=0;i<M;i++)
     {
        for(j=0;j<N;j==)
         {  b[j]=0;
              for(k=0;k<L;k++)
               { 
                   a[k]=pic[i][k]-part1[j][k];
			 }
		}
		c=b[0];
		for(j=1;j<N;j++)
		{
		   if(c>b[j])
		     c=b[j];
		}
          sum+=c;
	}
}
   float x=(float)sum/(M*L);
   particle[sum].fitness=x;
}


void localbest(int num)
{
   int i;
   if(particle[num].pbest>particle[num].fitness)
    {
       particle[num].pbest=particle[num].fitness;
	  for(i=0;i<DIMENSION;i++)
	      particle[num].best_position[i]=particle[num].position[i];
	}
}

void globalbest(int num)
{
   int i,j;
   int min_flag=0;
   float temp;
   if(num==0)
   {
      temp=particle[0],fitness;
	  for(i=1;i<POPSIZE;i++)
	{
	    if(temp>particle[i].fitness)
	     { 
		   temp=particle[i].fitness;
		   min_flag=i;
		}
	}  
	for(j=0;j<DIMENSION;j++)
	{
	  G[j]=particle[min_flag].position[j];
	 }
     gbest=particle[min_flag].fitness;
	
  }
else
{ 
    for(i=0;i<POPSIZE;i++)
    {
       if(gbest>particle[i].pbest)
	    { 
	     gbest=particle[i].pbest;
		for(j=0;j<DIMENSION;j++)
		    G[j]=particle[i].best_position[j];
		}  
	}	
}

     p_flag=0;
	temp=particle[0].pbest;
	for(i=1;i<POPSIZE;i++)
	{
	    if(temp>particle[i].pbest)
	    {
	       temp=particle[i].pbest;
		  p_flag=i;
		}
	}
}


void update()
{
    srand((unsigned int)time(NULL))
    int k,p,j;
    for(j=0;j<DIMENSION;j++)
		partice[0].position[j]=udpart[j][0];
	for(j=0;j<DIMENSION;j++)
          particle[1].position[j].=udpart[j][1];
     for(k=0;k<POPSIZE;k++)
          particle[k].speed[p]=(float)rand()*255/(RAND_MAX+1);
}


void do_lbg(int num)
{
   float v_book[N][L];
   int i=0,j,t,loop,p=0,flag;
   float a,min,max,max_list,b[N],D2[M];
   int sum[N],update_array[L];
   int max_flag[M],r_flag[M];
   
   for(i=0;i<N;i++)
    {
      for(j=0;j<L;j++)
	 {
	    V_book[i][j]=particle[num].position[p];
	    p++;
	  }
	}  
	for(loop=0;loop<3;loop++)
	{
	   for(i=0;i<M;i++)
	   {
	      for(j=0;j<N;j++)
		{
		    b[j]=0;
		    for(t=0;t<L;t++)
		    {
		      a=pic[i][t]-v_book[j][t];
			 b[j]+=a*a;
			 }
		}
	      min=b[0];
		 flag=0;
		 for(j=1;j<N;j++)
		{
		     if(min>b[j])
			{
			   min=b[j];
			   flag=j;
			 }
			u[i][j]=0;
		}
          u[i][flag]=1;
          D[i].data=min;
		D[i].list=i;
	   }
	   
	   for(j=0;j<N;j++)
	   {
	      sum[j]=0;
		 for(i=0;i<M;i++)
		 {
		  sum[j]+=u[i][j];
		  }
	   }
	   for(j=0;j<N;j++)
        {
             if(sum[j]>1)
              {  
                for(t=0;t<L;t++)
                     update_array[t]=0;
                for(i=0;i<M;i++)
                    if(u[i][j]==1)	
                        for(t=0;t<L;t++)
                             update_array[t]+=pic[i][t];
                 for(t=0;t<L;t++)
                     v_book[j][t]=(float)update_array[t]/sum[j];
                }
          }
      }
 
      
          for(i=0;i<M;i++)
	      {
		    max=D[i].data;
		    max_list=D[i].list;
		    max_flag[i]=i;
		    
		    
		    for(j=i+1;j<M;j++)
		    {
		         if(D[j].data>max)
			    {
			      max=D[j].data;
				 max_list=D[j].list;
				 max_flag=j;
				 }
			}
			
			int row=max_flag[i];
               D[row].data=D[i].data;
               D[row].list=D[i].list;
			D[i].data=max;
			D[i].list=max_list;
		}


		int top=0;
		for(j=0;j<N;j++)
		{
		     if(sum[j]==0)
			{
			  int pic_row=D[top].list;
			  for(t=0;t<16;t++)
			  {
			    v_book[j][t]=(float)pic[pic_row][t];
			  }
			    top++;
			 }
		}
		 
		 p=0;
		 for(i=0;i<N;i++)
		 {
		    for(j=0;j<L;j++)
		    {
		       particle[num].position[p]=(float)v_book[i][j];
			  P++;
			}
		}	
		
}


void GenVector()
{
    ifstream in_file;
    int i=0;
    int temp,total[65536],ori_pic[256][256];
    in_file.open("lena.data.txt",ios::in);
    if(!in_file)
    {
       cout<<"打開文件失敗"<<endl;
	  exit(1);
	 }
	 

	while(!(in_file>>temp).eof())
	{
	   total[i]=temp;
	    i++;
	 }
     int j,k;
     i=0;
     for(k=0;k<256;k++)
     {  
        for(j=0;j<256;j++)
          { 
				ori_pic[k][j]=total[i];
				i++;
		}
	}
	
	int m,n;
	for(i=0;i<64;i++)
	    for(j=0;j<64;j++)
	         for(m=0;m<4;m++)
			for(n=0;n<4;n++)
			   pic[64*i+j][4*m+n]=ori_pic[4*i+m][4*j+n];
	in_file.close();
}