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()


# 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
 
# Gets length of the full pofx representing "Scanner On" from pofx attribute of last point
def CalcDistanceOfPOFX(inputPath):
    pofxPath = inputPath[-1]
    pathsize = os.path.getsize(pofxPath)

    distanceList = []
    chunk =[]
   
    with rdb.rdb_open(pofxPath) as pofx:
        for points in pofx.select():
            output = points.riegl_pof_path_length[-1]

    outputString = str(round(output,2))

    sys.stdout.write(outputString)
 
if __name__ == "__main__":
    CalcDistanceOfPOFX(sys.argv)



