#include once "InitGL.bi"

#define USEDEBUG
#include once "TrueAxis.bi"

#include once "SandBox.bi"

sub _AppInit_ constructor
  InitGL
  PhysicsCreateInstance
  InitSandBox
end sub

sub _AppExit_ destructor
  PhysicsDestroyInstance
end sub



function CreateOpenGLWheel() as integer     
  const as single M_PI = atn(1)*4
  const as integer SEGMENTS  = 15
  const as single US = (M_PI*2) / SEGMENTS
  const as single VS =  M_PI    / SEGMENTS
  const NPOINTS = (SEGMENTS + 1) * (SEGMENTS + 1)
  static as integer wheel
  dim A(2) as single 
  dim B(2) as single
  dim P(NPOINTS*3-1) as single
  dim UW  as single
  
  
  for i as integer=0 to SEGMENTS
    P(i*2+0)=cos(UW)*0.5 ' x
    P(i*2+1)=sin(UW)*0.5 ' z
    UW=UW+US
  next

  if wheel then return wheel

  wheel=glGenLists(1)
  glNewList (wheel, GL_COMPILE)
    glRotatef(90,0,0,1)

    glBegin(GL_TRIANGLE_FAN)
      glColor3f(0,0,0):glNormal3f(0,1,0):glVertex3f(0,0.5,0) ' center
      for i as integer=SEGMENTS to 0 step-1
        if (i and 4) then
          glColor3f(0,0,0)
        else
          glColor3f(0.5,0.5,0.5)
        end if
        glVertex3f(P(i*2+0),0.5,P(i*2+1))
      next
    glEnd()

    glBegin(GL_TRIANGLE_FAN)
      glColor3f(0,0,0):glNormal3f(0,-1,0):glVertex3f(0,-0.5,0) ' center
      for i as integer=0 to SEGMENTS
       if (i and 4) then
          glColor3f(0,0,0)
        else
          glColor3f(0.5,0.5,0.5)
        end if
        glVertex3f(P(i*2+0),-0.5,P(i*2+1))
      next
    glEnd()

    glBegin(GL_TRIANGLE_STRIP)
      glColor3f(0,0,0)
      for i as integer=0 to SEGMENTS
        dim as integer j=i+1 :j=j mod SEGMENTS
        'A=Normalize(vec3(P(j*2+0),-CH2#,P(j*2+1))-vec3(P(i*2+0), CH2#,P(i*2+1)))
        'B=Normalize(vec3(P(i*2+0), CH2#,P(i*2+1))-vec3(P(i*2+0),-CH2#,P(i*2+1)))
        'glNormal3fv(Crossproduct(B,A))
        glVertex3f(P(i*2+0),-0.5,P(i*2+1))
        glVertex3f(P(i*2+0), 0.5,P(i*2+1))
      next
    glEnd()
  glEndList()
  
  return wheel
End function

const as single fWheelSize   =  0.6
const as single fWheelWidth  =  0.2
const as single bWheelSize   =  0.6
const as single bWheelWidth  =  0.4


function CreateSimpleCar(carwidth as single=2,carheight as single=1.2,carlength as single=4) as DynamicObjectCar ptr
  ' create a car from CarTemplate
  dim as CarTemplate ptr CarDesc = CarTemplateCreateNew() 
  CarTemplateBeginInitialise(CarDesc) 

    ' add car CollisionObject (a simple box in this case)
    dim as CollisionObjectConvex ptr Convex = CollisionObjectConvexCreateNew()
    CollisionObjectConvexInitialiseAsABox Convex,0,CarHeight/4,0,CarWidth,CarHeight,CarLength
    CarTemplateAddCollisionObjectConvex CarDesc,Convex
    ' we no longer need the collision box here.
    CollisionObjectConvexRelease Convex

    ' add wheels
  
    ' steering front wheels
    const as single fWheelTop    =   0
    const as single fWheelBottom =  -0.7
    dim   as single fWheelXPos   =  (CarWidth -fWheelWidth/2)/2
    dim   as single fWheelZPos   =  (CarLength-fWheelSize   )/2
    ' rear wheels
    const as single bWheelTop    =  0
    const as single bWheelBottom = -0.7
    dim   as single bWheelXPos   =  (CarWidth -fWheelWidth/2)/2
    dim   as single bWheelZPos   = -(CarLength-fWheelSize   )/2
    ' front steering wheels
    CarTemplateWheelSetRadius fWheelSize/2 ' radius
    CarTemplateWheelSetSteeringFlag  1
    CarTemplateWheelSetDrivingFlag   1
    CarTemplateWheelSetSuspensionTop    fWheelXPos,fWheelTop   ,fWheelZPos
    CarTemplateWheelSetSuspensionBottom fWheelXPos,fWheelBottom,fWheelZPos
    CarTemplateAddWheel CarDesc
    CarTemplateWheelSetSuspensionTop    -fWheelXPos,fWheelTop,   fWheelZPos
    CarTemplateWheelSetSuspensionBottom -fWheelXPos,fWheelBottom,fWheelZPos
    CarTemplateAddWheel CarDesc
    ' back driving wheels
    CarTemplateWheelSetRadius bWheelSize/2 ' radius
    CarTemplateWheelSetSteeringFlag  0
    CarTemplateWheelSetDrivingFlag   1
    if 0 then   '  6 wheel vehicle
      for i as integer=1 to 1
        CarTemplateWheelSetSuspensionTop    bWheelXPos,bWheelTop   ,bWheelZPos+(bWheelSize+0.1)*i
        CarTemplateWheelSetSuspensionBottom bWheelXPos,bWheelBottom,bWheelZPos+(bWheelSize+0.1)*i
        CarTemplateAddWheel CarDesc
        CarTemplateWheelSetSuspensionTop    -bWheelXPos,bWheelTop   ,bWheelZPos+(bWheelSize+0.1)*i
        CarTemplateWheelSetSuspensionBottom -bWheelXPos,bWheelBottom,bWheelZPos+(bWheelSize+0.1)*i
        CarTemplateAddWheel CarDesc
      next
    end if 
    ' Rear wheels
    CarTemplateWheelSetSuspensionTop    bWheelXPos,bWheelTop   ,bWheelZPos
    CarTemplateWheelSetSuspensionBottom bWheelXPos,bWheelBottom,bWheelZPos
    CarTemplateAddWheel CarDesc
    CarTemplateWheelSetSuspensionTop    -bWheelXPos,bWheelTop   ,bWheelZPos
    CarTemplateWheelSetSuspensionBottom -bWheelXPos,bWheelBottom,bWheelZPos
    CarTemplateAddWheel CarDesc

  CarTemplateEndInitialise CarDesc
  ' create a DynamicObjectCar from our CarTemplate
  dim as DynamicObjectCar ptr NewCar = DynamicObjectCarCreateNew()
  DynamicObjectCarInitialise NewCar,CarDesc
  ' we no longer need a reference to the CarTemplate here.
  CarTemplateRelease CarDesc

  return NewCar
end function

'
' main
'
' create a static ground plane
PhysicsAddStaticGround

const NCARS = 50
dim as DynamicObjectCar ptr cars(NCARS-1)
' create some a cars
for i as integer = 0 to NCARS-1
  cars(i) = CreateSimpleCar()
  DynamicObjectSetCenterOffset cars(i),0,-0.5,0
  dim as single x=rnd*SandBoxSize()/2,y=0,z=rnd*SandBoxSize()/2
  if PhysicsTestLineForCollision(x,50,z, x,-50,z) then
    CollisionGetPosition x,y,z
  end if
  DynamicObjectSetPosition cars(i),x,y+1,z
  ' add the car to the physics simulation
  PhysicsAddDynamicObject cars(i)
  
next

' car controls
dim as single Steer=-.05,Accel=0.3,Brake,HandBrake,TractionControl=1
dim as integer bSteeringHelp=0,nGear=-1 ' -1=auto shift 0=reverse 1=neutral  2=first gear 3=second 4=third ...
dim as integer WheelList = CreateOpenGLWheel()
dim as single  camx,camy,camz,cx,cy,cz,carx,cary,carz
dim as integer bChangeSteering=1


' update car controls
for i as integer=0 to NCARS-1
  DynamicObjectCarUpdateControls Cars(i),Steer,Accel,Brake,HandBrake,bSteeringHelp,TractionControl,nGear
next

' render and simualtion loop
dim as integer carcam=NCARS\2
dim as integer frames
while inkey=""
  
  ' one step in simulation 
  PhysicsUpdate(1.0/75.0)
  if frames mod 3 then
    glClear GL_COLOR_BUFFER_BIT or GL_DEPTH_BUFFER_BIT
    glLoadIdentity
    ' look target
    DynamicObjectGetPosition cars(carcam), carx,cary,carz
    ' look source x and z
    DynamicObjectTransform   cars(carcam),0,8,-25, cx,cy,cz
    ' look source y
    if PhysicsTestLineForCollision(cx,100,cz, cx,cy,cz,STATIC_OBJECTS) then
      CollisionGetPosition cx,cy,cz : cy+=5
    end if
    camx+=(cx-camx)*0.02
    camy+=(cy-camy)*0.02
    camz+=(cz-camz)*0.02
    if PhysicsTestLineForCollision(camx,100,camz, camx,camz,camz,STATIC_OBJECTS) then
      CollisionGetPosition camx,camy,camz : camy+=5
    end if
    LookAT camx,camy,camz, _
           carx,cary,carz, _
              0,   1,   0

    ' render all objects (only in debug mode) 
    PhysicsRender()
    'PhysicsRenderBoundingBoxes()
    for i as integer=0 to NCARS-1
      ' render the wheels
      glPushMatrix()  
      DynamicObjectMultOpenGLMatrix(cars(i))
      for w as integer=0 to DynamicObjectCarGetNumWheels(cars(i))-1
        glPushMatrix()
          DynamicObjectCarWheelMultLocalOpenGLMatrix(cars(i),w)
          ' front or rear wheels 
          if w<2 then
            glScalef(fWheelWidth,fWheelSize,fWheelSize)
          else
            glScalef(bWheelWidth,bWheelSize,bWheelSize)
          end if
          glCallList(WheelList)
        glPopMatrix()
      next
      glPopMatrix()
    next
    flip
  end if
  Sleep(10)
  frames+=1
  if frames mod 1000=0 then
    carcam=(carcam+1) mod NCARS
  end if
wend

