import math
import numpy as np
from src.tools.WGS84displace import WGS84displace


def getGeo(x_img,y_img,range,lat,lon,dep,yaw,speed=0.0) -> (float,float,float):
    '''
    returns (lat,lon,depth)
    '''
    # Get geo coordinates of both ends of the line
    c = 1500.0
#    startTime= 0.0
#    endTime = 10.0
#    millis= (endTime - y_img * (endTime-startTime)) # in millisekunden vom bildrand
#    micros= ((range * 2 * x_img - range) * 1e6/c )  # in microsekunden von der Mitte
    
#    horizontal_distance = micros * c/1e6
#    rp = horizontal_distance
    rp = (range * 2 * x_img - range)
    r = math.fabs(rp)
    a = dep
    
    if math.fabs(r) < a :
        h = np.sign(rp)
    else:
        h= np.sign(rp) * np.sqrt(r*r - a*a) # korrigierter abstand zum Mittelpunkt

    if h < 0.0:
        angle = np.pi
    else:
        angle = 0.0

    angle = angle - (-1)* yaw
    north = math.fabs(h) * math.sin(angle)
    east = math.fabs(h) * math.cos(angle)
    lat_p,lon_p,d_p = WGS84displace(lat,lon,0,north,east,0.0)

    return (lat_p,lon_p,d_p)