#include <stdio.h>
#include <cstdlib>
#include <time.h>
#include <math.h>
#include <iostream>
#include <gl\glut.h>
#include <Windows.h>

/*DISPLAY*/
#define WIDTH  400
#define HEIGHT  400

/*GENERAL*/
#define GLOBAL  0.3
#define PERSONAL 0.3
#define INERT  0.3
#define DELAY  100

/*SWARM*/
#define N   60 //NUmber of agents
#define K   200 //Number of moves

/* LIMITS */
#define Xmin  -4
#define Xmax  6
#define Ymin  -4
#define Ymax  6

double velocity[N][2];
double swarm[N][2];
double BestPers[N][3];
double BestGlob[3];


float Lrand(float min, float max)
{
 return (min + ((rand() % 10000) / 1e4) * (max - min));
}

void init() //Generate initial positions and directions
{
 srand(time(NULL));
 glClearColor(0.67, 0.2, 0.8, 0);
 glMatrixMode(GL_PROJECTION);
 gluOrtho2D(Xmin, Xmax, Ymin, Ymax);
 for(int i = 0; i < N; i++)
 {
  swarm[i][0] = Lrand(Xmin, Xmax);
  swarm[i][1] = Lrand(Ymin, Ymax);
  velocity[i][0] = Lrand(-1, 1);
  velocity[i][1] = Lrand(-1, 1);
  BestPers[i][2] = 100000;
 }
 BestGlob[2] = 100000;
}

void MoveWasp() //Moves the whole wasp
{
 for(int i = 0; i < N; i++)
 {
  for(int a = 0; a < 2; a++)
  {
   velocity[i][a] = INERT * velocity[i][a] + 
    Lrand(-1, 1) * GLOBAL * (BestGlob[a] - swarm[i][a]) + 
    Lrand(-1, 1) * PERSONAL * (BestPers[i][a] - swarm[i][a]);
   swarm[i][a] = swarm[i][a] + velocity[i][a];
  }
 }
}

int chkBrd(int i) //Verifies agent's position
{
 if((Xmin <= swarm[i][0]) && (swarm[i][0] <= Xmax) && (Ymin <= swarm[i][1]) && (swarm[i][1] <= Ymax))
  return 1;
 else
  return 0;
}

double calculate(int i)
{
 double f;
 double a = swarm[i][0], b = swarm[i][1];
 f = - 0.1 * fabs(1 - b) - 0.1 * fabs(1 - a) - j0(20 * a * a + b * b);
// f = - 0.1 * fabs(1 - b) - 0.1 * fabs(1 - a) - j0(a * a + b * b);
// f = a * sin(4 * a) + 1.1 * b * sin(2 * b);
 return f;
}

void checkBP(int i, double a)
{
 if(a < BestPers[i][2])
 {
  BestPers[i][2] = a;
  BestPers[i][1] = swarm[i][1];
  BestPers[i][0] = swarm[i][0];
 } 
}

void checkBG(int i, double a)
{
 if(a < BestGlob[2])
 {
  BestGlob[2] = a;
  BestGlob[1] = swarm[i][1];
  BestGlob[0] = swarm[i][0];
 }
}

void draw(void)
{
 glColor3f(1, 1, 0);
 glBegin(GL_POINTS);
  for(int i = 0; i < N; i++)
   glVertex2dv(swarm[i]);
  glVertex2d(BestGlob[0], BestGlob[1]);
 glEnd();
 glFlush();
 glutSwapBuffers();
}

void display(int i)
{
 std::cout << i << "\t" << BestGlob[0] << "\t" << BestGlob[1] << "\t" << BestGlob[2]  << "\n";
}

void go()
{
 double a;
 for(int k = 0; k < K; k++)
 {
  for(int i = 0; i < N; i++)
  {
   if(chkBrd(i))
   {
    a = calculate(i);
    checkBP(i, a);
    checkBG(i, a);
   }
  }
  display(k);
  draw();
  MoveWasp();
  glClear(GL_COLOR_BUFFER_BIT);
  Sleep(DELAY);
 }
 return;
}

void main(int argc, char **argv)
{
 glutInit(&argc;, argv);
 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
 glutInitWindowPosition(600,220);
 glutInitWindowSize(WIDTH, HEIGHT);
 glutCreateWindow("Wasps are alive!!!");
 glPointSize(3.0);
 glEnable(GL_POINT_SMOOTH);
 init();
 glutDisplayFunc(go);
 glutIdleFunc(go);
 glutMainLoop();
}