#include <windows.h>
#include <time.h>
#include <math.h>
#include "Intrface\CDDraw.h"
#include "Intrface\Prims2D.h"
#include "Utility\Vector.h"
#include "Utility\GenMath.h"

using namespace GameLib;

#define NUMBOIDS  3
#define ACCEL     1.0
#define DECEL     1.0
#define MAXTURN   4
#define MAXSPEED  6

struct Boid_s
{ double x, y, speed, omaxspd, maxspd, slowing, dir;
  void setdir(double a) { while(a<0) a+= 256; while(a>255) a-=256; dir = a; }

  void seek(double ang);
  void seek(double tx, double ty)
  { if(x!=tx && y!=ty) seek(Math::FindAngle(x, y, tx, ty)); }
  void flee(double ang)
  { seek(ang+128); }
  void flee(double tx, double ty)
  { if(x!=tx && y!=ty) seek(Math::FindAngle(x, y, tx, ty)+128); }
  void arrive(double tx, double ty);
  void pursue(Boid_s *b);
  void evade(Boid_s *b);
  void move()
  { Vector<double> v = RotatedZ(Vector<double>(0, -speed), (int)dir);
    x += v.X, y += v.Y;
  }
  void accel()
  { speed += ACCEL;
    if(speed>maxspd) speed=maxspd;
  }
  void decel()
  { speed -= DECEL;
    if(speed<0) speed=0;
  }
} Boids[NUMBOIDS];

void Boid_s::seek(double ang)
{ ang -= dir;
  if(ang>128) ang-=256;
  else if(ang<-127) ang+=256;
  if(ang<-MAXTURN) ang = -MAXTURN;
  else if(ang>MAXTURN) ang = MAXTURN;
  setdir(dir+ang);
}

void Boid_s::arrive(double tx, double ty)
{ if(x==tx && y==ty && speed<=DECEL)
  { speed=0;
    return;
  }
  double xd=tx-x, yd=ty-y, dist=xd*xd+yd*yd;
  if(dist<=slowing*slowing)
  { maxspd = omaxspd*sqrt(dist)/slowing;
    decel();
  }
  else maxspd = omaxspd;
  seek(tx, ty);
}

void Boid_s::pursue(Boid_s *b)
{ double xd=b->x-x, yd=b->y-y, dist=sqrt(xd*xd+yd*yd), T = dist/speed;
  Vector<double> v = RotatedZ(Vector<double>(0, -b->speed*T), (int)b->dir);
  seek(b->x+v.X, b->y+v.Y);
}

void Boid_s::evade(Boid_s *b)
{ double xd=b->x-x, yd=b->y-y, dist=sqrt(xd*xd+yd*yd), T = dist/b->speed;
  Vector<double> v = RotatedZ(Vector<double>(0, -b->speed*T), (int)b->dir);
  flee(b->x+v.X, b->y+v.Y);
}

CDDSurface back;
CDDraw     cdd;
int TargX, TargY;
bool       bQuit;

inline static Vertex2D VectVert(int i, Vector<double> v) { return Vertex2D((int)(v.X+Boids[i].x), (int)(v.Y+Boids[i].y)); }

static void DrawBoids()
{ back.Fill();
  back.Lock();
  Circle2D(&back, Vertex2DC(TargX, TargY, CDDRGB16(128, 128, 128)), 10);
  Line2D(&back, Vertex2D(TargX, TargY-15), Vertex2D(TargX, TargY+15), CDDRGB16(128,128,128));
  Line2D(&back, Vertex2D(TargX-15, TargY), Vertex2D(TargX+15, TargY), CDDRGB16(128,128,128));
  for(int i=0;i<NUMBOIDS;i++)
  { Vector<double> v = RotatedZ(Vector<double>(0,-10), (int)Boids[i].dir);
    Tri2D(&back, VectVert(i, v), VectVert(i, RotatedZ(v, 90)), VectVert(i, RotatedZ(v, 180)),
          i?CDDRGB16(0,240,16):CDDRGB(128,128,128), ST_FLAT);
    Circle2D(&back, Vertex2DC((int)(v.X+Boids[i].x), (int)(v.Y+Boids[i].y), CDDRGB16(200,0,0)), 3);
  }
  back.Unlock();
  back.Flip();
}

static void UpdateBoids()
{ int i;
  for(i=0;i<NUMBOIDS;i++) Boids[i].accel();
  Boids[0].arrive(TargX, TargY);
  Boids[1].pursue(&Boids[0]);
  Boids[2].evade(&Boids[0]);
  for(i=0;i<NUMBOIDS;i++)
  { Boids[i].move();
    if(Boids[i].x<0) Boids[i].x=0;
    if(Boids[i].x>799) Boids[i].x=799;
    if(Boids[i].y<0) Boids[i].y=0;
    if(Boids[i].y>599) Boids[i].y=599;
  }
}

static long CALLBACK WindowProc(HWND hWnd, UINT iMsg, WPARAM wParam, LPARAM lParam);

int WINAPI WinMain(HINSTANCE hInst, HINSTANCE hPrev, PSTR szCmd, int iCmdShow)
{ WNDCLASS wc;
  MSG      msg;
  HWND     hwnd;

  wc.style = CS_HREDRAW | CS_VREDRAW;
  wc.lpfnWndProc    = WindowProc;
  wc.cbClsExtra     = 0;
  wc.cbWndExtra     = 0;
  wc.hInstance      = hInst;
  wc.hIcon          = LoadIcon(NULL, IDI_APPLICATION);
  wc.hCursor        = LoadCursor(NULL, IDC_ARROW);
  wc.hbrBackground  = NULL;
  wc.lpszMenuName   = NULL;
  wc.lpszClassName  = "boids";
  if (!RegisterClass(&wc)) return 1;
  
  hwnd = CreateWindow("boids", "Boids", WS_OVERLAPPEDWINDOW & ~WS_MAXIMIZEBOX,
                      CW_USEDEFAULT, CW_USEDEFAULT, 810, 630, NULL, NULL, hInst, 0);
  if(!hwnd) return 2;
  ShowWindow(hwnd, iCmdShow);
  UpdateWindow(hwnd);  

  cdd.Init(hwnd);
  cdd.SetMode(800, 600, CDD_16BIT, CDD_FULLSCREEN);
  cdd.CreatePrimary(CDD_SINGLE);
  back.Init(&cdd, 800, 600, CDD_SYSTEM);

  srand(time(0));
  for(int i=0;i<NUMBOIDS;i++)
  { Boids[i].x      = rand()%800;
    Boids[i].y      = rand()%600;
    Boids[i].speed  = rand()%MAXSPEED+1;
    Boids[i].dir    = rand()%256;
    Boids[i].maxspd = Boids[i].omaxspd = rand()%MAXSPEED+1;
  }
  { Boids[0].omaxspd = Boids[0].maxspd = MAXSPEED+1;
    double s=Boids[0].omaxspd, t=s;
    while(s>0) t+=s, s-=DECEL;
    Boids[0].slowing = t;
  }

  while(!bQuit)
  { if(PeekMessage(&msg, hwnd, 0, 0, PM_REMOVE))
    { if(msg.message == WM_QUIT) break;
      TranslateMessage(&msg);
      DispatchMessage(&msg);
    }
    UpdateBoids();
    DrawBoids();
  }
  
  back.Deinit();
  cdd.Deinit();
  return 0;
}

static long CALLBACK WindowProc(HWND hWnd, UINT iMsg, WPARAM wParam, LPARAM lParam)
{ switch(iMsg)
  { case WM_MOUSEMOVE:
      ShowCursor(FALSE);
      TargX = LOWORD(lParam);
      TargY = HIWORD(lParam);
      return 0;
    case WM_MOVE:
    case WM_SIZE:
      cdd.AutoViewport();
      return 0;
    case WM_DESTROY:
      PostQuitMessage(0);
      bQuit = true;
      return 0;
  }
  return DefWindowProc(hWnd, iMsg, wParam, lParam);
}