#!/usr/bin/env python

import math
from terrain_coords import vertex

inited = 0

dbcenter = [0,0,0]
dbradius = 0

def getDatabaseCenterRadius():
    n=0.0
    dbcenter = [0,0,0]
    center = [ 0.0, 0.0, 0.0 ]

    cnt = 39 * 38

    for i in range(cnt):
        center[0] += vertex[i][0]
        center[1] += vertex[i][1]
        center[2] += vertex[i][2]
        n = n + 1.0

    center[0] /= n
    center[1] /= n
    center[2] /= n

    r = 0.0

    #    for( i = 0; i < sizeof( vertex ) / (sizeof( float[3] )); i++ )
    for i in range(cnt):
        d = math.sqrt(
            ((vertex[i][0] - center[0]) * (vertex[i][0] - center[0])) +
            ((vertex[i][1] - center[1]) * (vertex[i][1] - center[1])) +
            ((vertex[i][2] - center[2]) * (vertex[i][2] - center[2]))  )

        if d > r:
            r = d

    dbradius = r
    dbcenter[0] = center[0]
    dbcenter[1] = center[1]
    dbcenter[2] = center[2]

    index = 19 * 39 + 19
    dbcenter[0] = vertex[index][0] - 0.15
    dbcenter[1] = vertex[index][1]
    dbcenter[2] = vertex[index][2] + 0.35
    return dbradius, dbcenter


def init():
    global inited, dbradius, dbcenter
    dbradius, dbcenter = getDatabaseCenterRadius()
    inited = 1

def getNormal( v1, v2, v3, n ):
    V1= [0,0,0,0]
    V2= [0,0,0,0]

    # Two vectors v2->v1 and v2->v3

    for i in range(3):
        V1[i] = v1[i] - v2[i]
        V2[i] = v3[i] - v2[i]

    # Cross product between V1 and V2

    n[0] = (V1[1] * V2[2]) - (V1[2] * V2[1])
    n[1] = -((V1[0] * V2[2]) - ( V1[2] * V2[0] ))
    n[2] = (V1[0] * V2[1] ) - (V1[1] * V2[0] )

    # /* Normalize */

    f = math.sqrt( ( n[0] * n[0] ) + ( n[1] * n[1] ) + ( n[2] * n[2] ) )
    n[0] /= f
    n[1] /= f
    n[2] /= f


def Hat(x, y, z ):
    tri = [[0,0,0], [0,0,0], [0,0,0]]
    norm = [0,0,0]

    global inited
    if inited == 0:
        init()


    # m = columns
    # n = rows
    m = len( vertex ) / 39
    n = 39

    i = 0
    while  i < ((m-1)*39) and x > (vertex[i+n][0] - dbcenter[0]):
        i += n

    j = 0

    while j < n-1 and y > (vertex[i+j+1][1] - dbcenter[1]):
        j += 1

    tri[0][0] = vertex[i+0+j+0][0] - dbcenter[0]
    tri[0][1] = vertex[i+0+j+0][1] - dbcenter[1]
    #tri[0][2] = vertex[i+0+j+0][2] - dbcenter[2]
    tri[0][2] = vertex[i+0+j+0][2]

    tri[1][0] = vertex[i+n+j+0][0] - dbcenter[0]
    tri[1][1] = vertex[i+n+j+0][1] - dbcenter[1]
    #tri[1][2] = vertex[i+n+j+0][2] - dbcenter[2]
    tri[1][2] = vertex[i+n+j+0][2]

    tri[2][0] = vertex[i+0+j+1][0] - dbcenter[0]
    tri[2][1] = vertex[i+0+j+1][1] - dbcenter[1]
    #tri[2][2] = vertex[i+0+j+1][2] - dbcenter[2]
    tri[2][2] = vertex[i+0+j+1][2]

    getNormal( tri[0], tri[1], tri[2], norm )

    d = (tri[0][0] * norm[0]) + \
        (tri[0][1] * norm[1]) + \
        (tri[0][2] * norm[2])

    d *= -1
    pz = (-(norm[0] * x) - (norm[1] * y) - d)/norm[2]

    return z - pz

if __name__ == "__main__":
    print Hat(0,0,0)
    # print Hat(10,10,5)