#include <stdio.h>
#include <stdlib.h>
#define _USE_MATH_DEFINES
#include <math.h>
#define pi M_PI
#define FREEGLUT_STATIC
#include <GL/freeglut.h>
#include <GL/gl.h>
#include <gl/glu.h>
#include <windows.h>


struct __verlet_node__{
    double __x, __y, __z, __xp, __yp, __zp, __mass, __damp, __grav;
    __verlet_node__()
    {
        __x = 0; __y = 0; __z = 0; __xp = __x; __yp = __y; __zp = __z;
        __mass = 1; __damp = 1; __grav = .1;
    }
    __verlet_node__( double x, double y, double z, double mass = 1, double damp = 1 )
    {
        __x = x; __y = y; __z = z; __xp = __x; __yp = __y; __zp = __z;
        __mass = mass; __damp = damp; __grav = .1;
    }
    __verlet_node__( double mass, double damp = 1 )
    {
        __x = 0; __y = 0; __z = 0; __xp = __x; __yp = __y; __zp = __z;
        __mass = mass; __damp = damp; __grav = .1;
    }
    void move()
    {
        double xp2 = __x, yp2 = __y, zp2 = __z;
        __x += ( __x - __xp ) * __damp * (1 - 1 / (__mass+50));
        __y += ( __y - __yp ) * __damp * (1 - 1 / (__mass+50));
        __z += ( __z - __zp ) * __damp * (1 - 1 / (__mass+50)) + __grav;
        __xp = xp2; __yp = yp2; __zp = zp2;
    }
    void moveTo( double x, double y, double z )
    {
        __x = x;
        __y = y;
        __z = z;
    }
    void moveRel( double x, double y, double z )
    {
        __x += x;
        __y += y;
        __z += z;
    }
    void moveDir( double latDir, double vertDir, double distance )
    {
        __x += cos( latDir ) * cos( vertDir );
        __y += sin( latDir ) * cos( vertDir );
        __z += sin( vertDir );
    }
};
struct __verlet_constraint__{
    struct __verlet_node__ *n1;
    struct __verlet_node__ *n2;
    double __len;
    int __ret_time;
    __verlet_constraint__( struct __verlet_node__ *_n1, struct __verlet_node__ *_n2 )
    {
        n1 = _n1;
        n2 = _n2;
        __ret_time = 1;
        double dx = n2->__x - n1->__x;
        double dy = n2->__y - n1->__y;
        double dz = n2->__z - n1->__z;
        __len = sqrt( dx*dx+dy*dy+dz*dz );
    }
    __verlet_constraint__( struct __verlet_node__ *_n1, struct __verlet_node__ *_n2, double len )
    {
        n1 = _n1;
        n2 = _n2;
        __ret_time = 1;
        __len = len;
    }

    void satisfy()
    {
        double dx = n2->__x - n1->__x;
        double dy = n2->__y - n1->__y;
        double dz = n2->__z - n1->__z;
        double h = sqrt( dx*dx+dy*dy+dz*dz);
        double scale = (__len - h) / h;
        double massRatio1 = n2->__mass / (n1->__mass+n2->__mass);
        double massRatio2 = n1->__mass / (n1->__mass+n2->__mass);

        n1->moveRel( -( dx * scale ) / __ret_time * massRatio1, -( dy * scale ) / __ret_time * massRatio1, -( dz * scale ) / __ret_time * massRatio1 );
        n2->moveRel(  ( dx * scale ) / __ret_time * massRatio2,  ( dy * scale ) / __ret_time * massRatio2,  ( dz * scale ) / __ret_time * massRatio2 );
    }
};

typedef struct __verlet_constraint__ CONSTRAINT;
typedef struct __verlet_node__ NODE;


// These variables set the dimensions of the rectanglar region we wish to view.
const double Xmin = 0.0, Xmax = 3.0;
const double Ymin = 0.0, Ymax = 3.0;

// glutKeyboardFunc is called below to set this function to handle
//      all "normal" ascii key presses.
// Only space bar and escape key have an effect.


int mouse_x = 320, mouse_y= 100, mvd = 0;
void setM( int x, int y){
    mouse_x = x;
    mouse_y = y;
}
void getM( int x, int y){
    mvd = 0;
    if( x != mouse_x || y != mouse_y )
        mvd = 1;
}


NODE* n1;




void myKeyboardFunc( unsigned char key, int x, int y )
{
    switch ( key ) {
    case ' ':
        printf( "X: %f; Y: %f; MX = %i; MY = %i;\n", n1->__x, n1->__y, mouse_x, mouse_y );
        break;
    case 27:                                    // "27" is theEscape key
        exit(1);

    }
}

#include<vector>
std::vector<NODE*> Nodes;
std::vector<CONSTRAINT*> Cons;

NODE* new_node( double x, double y, double z, double mass = 1, double damp = 1 )
{
    NODE* a = new NODE( x, y, z, mass, damp );
    Nodes.push_back( a );
    return a;
}
CONSTRAINT* constrain( NODE* n1, NODE* n2 )
{
    CONSTRAINT* a = new CONSTRAINT( n1, n2 );
    Cons.push_back( a );
    return a;
}
CONSTRAINT* constrain( NODE* n1, NODE* n2, double len )
{
    CONSTRAINT* a = new CONSTRAINT( n1, n2, len );
    Cons.push_back( a );
    return a;
}





void init(){

    #define R 9
    #define RAD 5
    NODE* nodes[160][320];

    int j = 0;
    int i = 0;
    for( double rv = pi/2; rv > -pi/2; rv -= pi/R, i ++ ){
        if( rv == pi/2) continue;
        j = 0;
        for( double rl = 0; rl < 2*pi; rl += pi/R, j ++ ){
            nodes[i][j] = new_node( RAD * cos( rl ) * cos( rv ), RAD * sin( rl ) * cos(rv), RAD * sin( rv ), 1, .999 );
        }
    }
    printf( "Made Nodes\n" );
    nodes[0][0] = new_node( 0, 0, RAD, 1000, .999 );
    for( int i = 1; i < 2*R; i ++ ) nodes[0][i] = nodes[0][0];
    nodes[R][0] = new_node( 0, 0, -RAD, 1, .999 );
    printf( "Constraining\n" );
    for( int i = 1; i < R; i ++ ){
        printf(" %i\t", i );
        for( int j = 0; j < 2*R; j ++ ){
            printf(" %i\t", j );
            constrain( nodes[i][j], nodes[i-1][j] );
            constrain( nodes[i][j], nodes[i][(j+R)%(2*R)] );

            if( i != R-i) constrain( nodes[i][j], nodes[R-i][j] );

            constrain( nodes[i][j], nodes[i-1][(j+1)%(2*R)] );

            //constrain( nodes[i][j], nodes[i-1][(j+2*R-1)%(2*R)] );

            constrain( nodes[i][j], nodes[i][(j+1)%(2*R)] );
        }
    }
    printf( "Constraining Bottom\n" );
    for( int j = 0; j < 2*R; j ++ ){
            constrain( nodes[R][0], nodes[R-1][j] );
    }
    constrain( nodes[R][0], nodes[0][0] );
    printf( "Constrained\n" );
    n1 = nodes[0][0];




}

#include <deque>

void myIdleFunction()
{
    double mx = -(640 - mouse_x), my = -(480 - mouse_y);
    double x2 = 640/2-mouse_x;
    double y2 = -(480/2-mouse_y);
    if( !mvd ){
        n1->moveTo( x2/25, 0, y2/25 );
        n1->__xp = n1->__x;
        n1->__yp = n1->__y;
        n1->__zp = n1->__z;}

        for( int i = 0; i < Nodes.size(); i ++ ){
            Nodes[i]->move();
        }

        /*std::deque<int> d;
        for( int j = 0; j < 10; j ++ ){
        for( int i = 0; i < Cons.size(); i ++ )
            d.push_front( i );
        while( d.size() > 0 )
        {
            int k = rand() % (d.size()*2);
            while( k > 0 ){
                k--;
                d.push_back( d.front());
                d.pop_front();
            }
            Cons[d.front()]->satisfy();
            d.pop_front();
        }}*/

        for( int k = 0; k < 90; k ++ ){
        for( int i = 0; i < Cons.size(); i ++ ){
            Cons[i]->satisfy();
        }
        /*for( int i = Cons.size()-1; i >= 0; i -- ){
            Cons[i]->satisfy();
        }*/
        }

}



void drawScene(void)
{
    myIdleFunction();
    // Clear the rendering window
glEnable(GL_LIGHT1);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // Set drawing color to white
    glColor3f( 1.0, 1.0, 1.0 );
        glBegin( GL_LINES );
        for( int i = 0; i < Cons.size(); i ++ ){
            glVertex3d( Cons[i]->n1->__x, Cons[i]->n1->__y, Cons[i]->n1->__z );
            glVertex3d( Cons[i]->n2->__x, Cons[i]->n2->__y, Cons[i]->n2->__z );
        }

        glEnd();

    glFlush();
    glutSwapBuffers();
    Sleep( 20 );
    glutPostRedisplay();
}

void initRendering()
{

    //glEnable ( GL_DEPTH_TEST );
  /* Setup the view of the cube. */

    // Uncomment out the first block of code below, and then the second block,
    //      to see how they affect line and point drawing.

    // The following commands should cause points and line to be drawn larger
    //  than a single pixel width.
    glPointSize(8);
    glLineWidth(2);



    // The following commands should induce OpenGL to create round points and
    //  antialias points and lines.  (This is implementation dependent unfortunately).
    /*glEnable(GL_POINT_SMOOTH);
    glEnable(GL_LINE_SMOOTH);
    glHint(GL_POINT_SMOOTH_HINT, GL_NICEST);    // Make round points, not square points
    glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);     // Antialias the lines
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);*/


}


void resizeWindow(int w, int h)
{
    double scale, center;
    double windowXmin, windowXmax, windowYmin, windowYmax;

    // Define the portion of the window used for OpenGL rendering.
    glViewport( 0, 0, w, h );   // View port uses whole window

    // Set up the projection view matrix: orthographic projection
    // Determine the min and max values for x and y that should appear in the window.
    // The complication is that the aspect ratio of the window may not match the
    //      aspect ratio of the scene we want to view.
    w = (w==0) ? 1 : w;
    h = (h==0) ? 1 : h;
    if ( (Xmax-Xmin)/w < (Ymax-Ymin)/h ) {
        scale = ((Ymax-Ymin)/h)/((Xmax-Xmin)/w);
        center = (Xmax+Xmin)/2;
        windowXmin = center - (center-Xmin)*scale;
        windowXmax = center + (Xmax-center)*scale;
        windowYmin = Ymin;
        windowYmax = Ymax;
    }
    else {
        scale = ((Xmax-Xmin)/w)/((Ymax-Ymin)/h);
        center = (Ymax+Ymin)/2;
        windowYmin = center - (center-Ymin)*scale;
        windowYmax = center + (Ymax-center)*scale;
        windowXmin = Xmin;
        windowXmax = Xmax;
    }

    // Now that we know the max & min values for x & y that should be visible in the window,
    //      we set up the orthographic projection.
    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();
    gluPerspective( 90, scale, 0, 1000 );
    gluLookAt( 0, -10, 0, 0, 0, 0, 0, 0, -1 );


    //glOrtho( windowXmin, windowXmax, windowYmin, windowYmax, -1, 1 );

}

GLfloat light1_diffuse[] =
{300, 300, 300, 300};
GLfloat light1_position[] =
{1.0, 1.0, 1.0, 0.0};

int main( int argc, char** argv )
{
    init();
    glutInit(&argc,argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH );

    // Window position (from top corner), and size (width and hieght)
    glutInitWindowPosition( 20, 60 );
    glutInitWindowSize( 640, 480 );
    glutCreateWindow( "Verlet Draw" );

    // Initialize OpenGL as we like it..
    initRendering();

      glLightfv(GL_LIGHT1, GL_DIFFUSE, light1_diffuse);
  glLightfv(GL_LIGHT1, GL_POSITION, light1_position);

    // Set up callback functions for key presses
    glutKeyboardFunc( myKeyboardFunc );         // Handles "normal" ascii symbols
    // glutSpecialFunc( mySpecialKeyFunc );     // Handles "special" keyboard keys

    // Set up the callback function for resizing windows
    glutReshapeFunc( resizeWindow );

    // Call this for background processing
     //glutIdleFunc( myIdleFunction );

    // call this whenever window needs redrawing
    glutDisplayFunc( drawScene );
    //glutPassiveMotionFunc(getM);
    glutMotionFunc( setM );


    GLfloat fogcolour[4]={0.0,0.0,0.0,1.0};         /* BLACK fog */

  glFogfv(GL_FOG_COLOR,fogcolour);              /* Define the fog colour */
  glFogf(GL_FOG_DENSITY,0.1);                   /* How dense */
  glFogi(GL_FOG_MODE,GL_LINEAR);                   /* exponential decay */
  glFogf(GL_FOG_START,0);                   /* Where wwe start fogging */
  glFogf(GL_FOG_END,10.0);                       /* end */
  //glHint(GL_FOG_HINT, GL_FASTEST);              /* compute per vertex */
  glEnable(GL_FOG);                             /* ENABLE */


    // Start the main loop.  glutMainLoop never returns.
    glutMainLoop(  );

    return(0);  // This line is never reached.
}