#include <stdlib.h>
#include "util.h"
#include "boids.h" 
#include <unistd.h>
#ifdef __APPLE__
#  include <GLUT/glut.h>
#else
#  include <GL/glut.h>
#endif

#define PI 3.1415926535897932384626433832795

/* pixel dimensions of output */
#define WIDTH   800
#define HEIGHT  600

#define NUMBOIDS 

/* Wrapping dimensions and indices */ 
#define NORTH 0 
#define SOUTH 1
#define WEST 2
#define EAST 3
#define STEP_SIZE       1
#define SLEEP_MS        250

int wrapDimensions[4] = {-10,
                        600 + 10,
                        -10,
                        800 + 10};

BOID_World_t * wrld; 

bool isInitial = true; 
int count = 0; 
// nrrd for getting computational state
Nrrd *nData; 

void setUpView(void)
{
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(0,      // left
            WIDTH,  // right
            HEIGHT, // bottom
            0,      // top
            0,      // zNear
            1       // zFar
            );
}
void shouldWrap(float * b)
{
    if(b[0] > wrapDimensions[WEST])
        b[0] = wrapDimensions[EAST];
    else if(b[0] > wrapDimensions[EAST])
          b[0] = wrapDimensions[WEST];
    
    if(b[1] < wrapDimensions[NORTH])
        b[1] = wrapDimensions[SOUTH];
    else if(b[1] > wrapDimensions[SOUTH])
        b[1] = wrapDimensions[NORTH];
}
void DrawBoids(Nrrd *positions)
{
    glMatrixMode(GL_MODELVIEW);
    int r = 5;   // "radius" of the triangle

    float *boids = (float *)(positions->data);
    unsigned int nBoids = positions->axis[1].size;

    for(int i = 0; i < nBoids; i++) 
    {
        int idx = i * 4; 

        float theta = (-1 * atan2(-1 * boids[idx + 2], boids[idx + 3])) + (PI/2.0);
        float degTheta = (theta * 180/PI); 
        glColor3f(0.0f, 0.0f, 1.0f);
        glLoadIdentity();
        glPushMatrix();
        glTranslatef(boids[idx],boids[idx+1],0.0f);
        glRotatef(degTheta,0.0f,0.0f,1.0f);
        glBegin(GL_TRIANGLES);
        glVertex2f(0, -1 * r * 2);
        glVertex2f(-1 * r, r * 2);
        glVertex2f(r, r * 2);
        glEnd(); 
        glPopMatrix();
     }
 
}
void GetData (BOID_World_t *wrld, Nrrd *nData)
{
  // get snapshot of state
    if (BOID_Snapshot_boidInfo(wrld, nData)) {
      // error
        fprintf(stderr, "Error getting nrrd data\n");
        exit(1);
    }
}
void flock (int value) 
{ 
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glClear(GL_COLOR_BUFFER_BIT);     // Clear The Screen And The Depth Buffer    
    setUpView();

    // get and render initial state
    if(isInitial){
        GetData (wrld, nData);
        DrawBoids(nData);
        isInitial = false; 
        glutSwapBuffers();
    }else { 
        if(BOID_NumActive(wrld) > 0){
            // step the computation
            BOID_Run(wrld, STEP_SIZE);
            // get and render the state
            GetData (wrld, nData);
            DrawBoids(nData); 
            glutSwapBuffers();
        }
    }
    glutTimerFunc(50,flock, 0);
}  
void Display(void)
{
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glClear(GL_COLOR_BUFFER_BIT);     // Clear The Screen And The Depth Buffer    
    setUpView();
}
int main(int argc, char** argv)
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
    glutInitWindowSize(WIDTH, HEIGHT);
    glutCreateWindow("Flocking Diderot Program");
    glutDisplayFunc(&Display);

    // initialize the Diderot program
    wrld = BOID_Init();
    nData = nrrdNew();
    BOID_InVarSet_width (wrld, WIDTH); 
    BOID_InVarSet_height(wrld, HEIGHT); 
    BOID_Initially (wrld);

    glutTimerFunc(50,flock, 0);

    glutMainLoop();

    return 0;
}