SCREEN_SIZE = (500,800)

from math import radians 
from numpy import array
from solver import *


from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
from OpenGL.GLE import *

import pygame
from pygame.locals import *


def resize(width, height):
    
    glViewport(0, 0, width, height)
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()

    gluPerspective(45.0, float(width)/height, .1, 1000.)
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    gluLookAt(0,20,0, 0,0,0, 0,0,1)


def init():
    global clock
    clock = pygame.time.Clock()
    glEnable(GL_DEPTH_TEST)
    
    glShadeModel(GL_FLAT)
    glClearColor(1.0, 1.0, 1.0, 0.0)

    glEnable(GL_COLOR_MATERIAL)
    
    glEnable(GL_LIGHTING)
    glEnable(GL_LIGHT0)        
    glLight(GL_LIGHT0, GL_POSITION,  (0, 1, 1, 0))    
    
amp = 4
def f( x, y):
    g = -9.8
    z_dd = amp * math.sin(x)
    l = 4.0
    return array([  y[1], -(g + z_dd)*math.sin(y[0])/l  ])    

def update(f, h, x, y, threshold):
    x, y, h = r_k_adaptive_step(f, h, x, y, threshold)
    #x, y, h = r_k_step(f, h, x, y, threshold)
    #x, y, h = euler_step(f, h, x, y, threshold)
    return x, y, h
            

def run():
    glutInit()
    pygame.init()
    screen = pygame.display.set_mode(SCREEN_SIZE, HWSURFACE|OPENGL|DOUBLEBUF)
    
    resize(*SCREEN_SIZE)
    init()
    
    clock = pygame.time.Clock()      
    glMaterial(GL_FRONT, GL_AMBIENT, (0.1, 0.1, 0.1, 1.0))    
    glMaterial(GL_FRONT, GL_DIFFUSE, (1.0, 1.0, 1.0, 1.0))
    x = 0.0
    y = array([1, 0])
    #y = array([.00001, 0])
    #y = array([math.pi -.2, 0])
    h = .1
    threshold = 0.0000001
    pos_hist = []
    t = 0
    while True:  
        for event in pygame.event.get():
            if event.type == QUIT:
                return
            if event.type == KEYUP and event.key == K_ESCAPE:
                return

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        # Light must be transformed as well
        glLight(GL_LIGHT0, GL_POSITION,  (0, 1.5, 1, 0)) 

        # Render the pendulum
        x, y, h = update(f, h, x, y, threshold)
        disp = amp * math.sin(x)
        glPushMatrix()
        glColor3f(0.2,0.2,0.2)
        glTranslatef(0, 0 ,disp)
        glutSolidSphere(0.1,6,6)
        glPopMatrix()

        glPushMatrix()
        glColor(0.9,0.9,0.9)
        height, side = 4*math.cos(y[0]) + disp , 4 *math.sin(y[0])
        pos_hist.append((side, height))
        glTranslatef(side, 0, height)
        glutSolidSphere(0.5,16,16)
        glPopMatrix()

        #render the rod
        glDisable(GL_TEXTURE_2D);
        glColor3f(0.2,0.2,0.2)
        
        glLineWidth(2)
        glBegin(GL_LINE_STRIP)
        glVertex3f(side, 0, height)
        glVertex3f(0, 0 ,disp)
        glEnd()     
        
        #render the path
        glColor3f(0,0,0)
        glLineWidth(1)
        glBegin(GL_LINE_STRIP)
        for hor, ver in pos_hist:
            glVertex3f(hor, 0, ver)
        glEnd()        

        # Show the screen
        pygame.display.flip()
        passed = clock.tick_busy_loop()
       # print passed, h
        if int(300*h) - passed < 0:
            print int(300*h) - passed
        pygame.time.wait(max(int(300*h) - passed, 0))
        clock.tick_busy_loop()
        #pygame.image.save(screen, "png/"+"0"+str(t)+'.png')
        t += 1
        
run()