import riegl.rdb as rdb
import os
import sys
import math

# Create text file from the whole project pofx 
def CreateTxtFromPOFX(pofxPathList):
    pofxPath = pofxPathList[-2]
    txtPath = pofxPathList[-1]
    txtfullpath = txtPath + "latlong.txt"
    f = open(txtfullpath, "w")
    f.write("ID, LATTITUDE, LONGITUDE, HEIGHT\n")
    
    count = 0
    pathsize = os.path.getsize(pofxPath)

    with rdb.rdb_open(pofxPath) as rdb:
        for points in rdb.select(
            chunk_size=int(pathsize/9000) # number of points to load in one step OLD -> (pathsize/3000)
        ):
                f.writelines("{0}, {1}, {2}, {3}\n".format(points.riegl_id[0], points.riegl_pof_latitude[0], points.riegl_pof_longitude[0], points.riegl_pof_height[0]))                
    f.close()

    sys.stdout.write(txtfullpath)


# Create text file of points from the rdbx individual pofx files
def CreateTxtFromRDBXPOFX(pofxPathList):
    pofxPath = pofxPathList[-2]
    txtPath = pofxPathList[-1]
    txtfullpath = txtPath
    f = open(txtfullpath, "w")
    f.write("ID; LONGITUDE; LATTITUDE;  HEIGHT\n")
    
    count = 0
    pathsize = os.path.getsize(pofxPath)

    listart = []
    listop=[]

    with rdb.rdb_open(pofxPath) as pofx:
        totalPoints = pofx.stat.point_count_total
        chunk = int(pathsize/1000)
        chunkPiece = chunk - 11
        for points in pofx.select("(riegl.id > 10)", 
            chunk_size=chunk # number of points to load in one step OLD -> (pathsize/3000)
        ): 
            if (points.riegl_id[0] + chunkPiece) < totalPoints:
                f.writelines("{0}; {1}; {2}; {3}\n".format(points.riegl_id[0], points.riegl_pof_longitude[0], points.riegl_pof_latitude[0], points.riegl_pof_height[0]))
                listart.append(points.riegl_pof_height[0])
                listop.append(points.riegl_pof_height[-1])
                # Changed the above so it will work even if German Windows with , as decimal
                #             if (points.riegl_id[0] + chunkPiece) < totalPoints:
                # f.writelines("{0}, {1}, {2}, {3}\n".format(points.riegl_id[0], points.riegl_pof_longitude[0], points.riegl_pof_latitude[0], points.riegl_pof_height[0]))
                # listart.append(points.riegl_pof_height[0])
                # listop.append(points.riegl_pof_height[-1])
                
        # adds the last point in the list to the text file and gets the accumulative length for the pofx
        for allpoints in pofx.select():
            pofxLength = allpoints.riegl_pof_path_length[-1]
                
    f.close()
    
    aveheight = (sum(listart) + sum(listop))/(len(listart) + len(listop))
       
    outputString = txtfullpath + ","+ str(listart[0]) + "," + str(listop[-1]) + "," + str(aveheight) + "," + str(round(pofxLength, 2))
    sys.stdout.write(outputString)


# Haversine formula to calculate the great circle distance between two lat/long coordinates
def CalcDistanceGeo(LatN1, LongE1, LatN2, LongE2):
    # Equatorial radius of the Earth in metres
    RadiusEarth = 6378e3

    # difference in latitudes and longitudes between two points
    deltaLatN = math.radians(LatN2 - LatN1)
    deltaLongE = math.radians(LongE2 - LongE1)

    # latitudes to radians
    radLatN1 = math.radians(LatN1)
    radLatN2 = math.radians(LatN2)
    
    # square of half the chord length between the points
    a = math.sin(deltaLatN/2)**2 + math.cos(radLatN1)*math.cos(radLatN2)*(math.sin(deltaLongE/2)**2)
    
    # angular distance in radians
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
    
    # calculated distance between the two points in metres
    distanceBetweenPoints = RadiusEarth * c
    return distanceBetweenPoints


# Calculate length of the full pofx representing "Scanner On"
def CalcDistanceOfPOFX(inputPath):
    pofxPath = inputPath[-1]
    pathsize = os.path.getsize(pofxPath)

    with rdb.rdb_open(pofxPath) as rdb:
        for points in rdb.select(
            chunk_size=int(pathsize/9000) # number of points to load in one step OLD -> (pathsize/3000)
        ):
            chunk.append((points.riegl_pof_latitude[0], points.riegl_pof_longitude[0]))

    for i in range(len(chunk)-1):
       distanceList.append(CalcDistanceGeo(chunk[i][0], chunk[i][1], chunk[i+1][0], chunk[i+1][1]))

    outputString = str((sum(distanceList)))

    sys.stdout.write(outputString)


# Creates text file of points from the rdbx boundary
def CreateTxtFromRDBX(rdbxPathList):
    rdbxPath = rdbxPathList[-2]
    txtPath = rdbxPathList[-1]
    txtfullpath = txtPath + "rdbx.txt"
    f = open(txtfullpath, "w")
    f.write("ID, X, Y, Z\n")
    
    large = 0.0
    small = 0.0
    with rdb.rdb_open(rdbxPath) as rdb:
        for points in rdb.select():            
            selected = points.riegl_scan_angle[0]

            if selected > large:
                large = selected               
                
            if selected < small:
                small = selected
        
        for points in rdb.select("(riegl.scan_angle >= {0}) || (riegl.scan_angle <= {1})".format(large, small)):
            f.writelines("{0}, {1}, {2}, {3}\n".format(points.riegl_id[0], points.riegl_xyz[0][0], points.riegl_xyz[0][1], points.riegl_xyz[0][2]))                
                                     
    f.close()
    
    sys.stdout.write(txtfullpath)
    
 
if __name__ == "__main__":
    #CreateTxtFromPOFX(sys.argv)
    CreateTxtFromRDBXPOFX(sys.argv)
  

