import os,math
import numpy as np

# WGS-84经纬度转Web墨卡托
def wgs_to_mercator(x, y):
    y = 85.0511287798 if y > 85.0511287798 else y
    y = -85.0511287798 if y < -85.0511287798 else y

    x2 = x * 20037508.34 / 180.0
    y2 = math.log(math.tan((90.0 + y) * math.pi / 360.0)) / (math.pi / 180.0)
    
    #print( ' y:',y, "  before Log:",math.tan((90.0 + y) * math.pi / 360.0),  ' log:'  , math.log(math.tan((90.0 + y) * math.pi / 360.0)))
    y2 = y2 * 20037508.34 / 180.0
    return x2, y2
def mercator_to_wgs(x, y):
    """
    将墨卡托投影坐标转换为WGS-84经纬度坐标
    :param x: 墨卡托投影的X坐标
    :param y: 墨卡托投影的Y坐标
    :return: 经度（longitude）和纬度（latitude）
    """
    # 地球半径（米）
    R = 6378137.0
    # 墨卡托投影的X坐标转换为经度
    lon = x / R * 180.0 / math.pi
    # 墨卡托投影的Y坐标转换为纬度
    lat = math.atan(math.sinh(y / R)) * 180.0 / math.pi
    return lon, lat


def ImageCorToCamCor(p0,w=1920,h=1080):
    x,y=p0[0:2]
    return  x-w/2.,(h-y)-h/2.

def wgs84_to_gcj02(lat, lon):  
    """将 WGS-84 坐标转换为 GCJ-02 坐标 (高德地图坐标)"""  
    A = 6378245.0  # 长半轴  
    EE = 0.00669342162296594323  # 偏心率平方  
    if out_of_china(lat, lon):  
        return lat, lon  # 如果在中国以外，直接返回 WGS-84 坐标  

    # 坐标转换  
    dlat = transform_lat(lon - 105.0, lat - 35.0)  
    dlon = transform_lon(lon - 105.0, lat - 35.0)  
    radlat = lat / 180.0 * math.pi  
    magic = math.sin(radlat)  
    magic = 1 - EE * magic * magic  
    sqrt_magic = math.sqrt(magic)  
    dlat = (dlat * 180.0) / (A * (1 - EE) / (magic * sqrt_magic) * math.pi)  
    dlon = (dlon * 180.0) / (A / sqrt_magic * math.cos(radlat) * math.pi)  
    
    mg_lat = lat + dlat  
    mg_lon = lon + dlon  

    return mg_lat, mg_lon  

def out_of_china(lat, lon):  
    """检查坐标是否在中国以外"""  
    return lon < 72.004 or lon > 137.8347 or lat < 0.8293 or lat > 55.8271  

def transform_lat(lon, lat):  
    """辅助函数: 进行纬度转换"""  
    ret = (-100.0 + 2.0 * lon + 3.0 * lat + 0.2 * lat * lat +   
           0.1 * lon * lat + 0.2 * math.sqrt(abs(lon)))  
    ret += (20.0 * math.sin(6.0 * lon * PI) + 20.0 * math.sin(2.0 * lon * PI)) * 2.0 / 3.0  
    ret += (20.0 * math.sin(lat * PI) + 40.0 * math.sin(lat / 3.0 * PI)) * 2.0 / 3.0  
    ret += (160.0 * math.sin(lat / 12.0 * PI) + 320.0 * math.sin(lat * PI / 30.0)) * 2.0 / 3.0  
    return ret  

def transform_lon(lon, lat):  
    """辅助函数: 进行经度转换"""  
    ret = (300.0 + lon + 2.0 * lat + 0.1 * lon * lon +   
           0.1 * lon * lat + 0.1 * math.sqrt(abs(lon)))  
    ret += (20.0 * math.sin(6.0 * lon * PI) + 20.0 * math.sin(2.0 * lon * PI)) * 2.0 / 3.0  
    ret += (20.0 * math.sin(lon * PI) + 40.0 * math.sin(lon / 3.0 * PI)) * 2.0 / 3.0  
    ret += (150.0 * math.sin(lon / 12.0 * PI) + 300.0 * math.sin(lon / 30.0 * PI)) * 2.0 / 3.0  
    return ret
    
def cam2word(p0,pUAV,yaw,delta=1.55e-3 * 4056.0/1920,pitch=-45,f=4.5,camH=50e3,igW=1920,igH=1080):
    
    pitch = pitch/180.0*np.pi
    sinp = np.sin(pitch );cosp= np.cos(pitch)
    p0_new = ImageCorToCamCor(p0,igW,igH)
    Xc0 = p0_new[0]*delta;Zc0 = p0_new[1]*delta;
    
    #(Zw0,Xw0)--相对于光心，X，Z并未校正到正东和正北。
    #f=4.5*f/24.00
    Zw0=camH*(  -f*sinp  + Zc0*cosp )/( f*cosp + Zc0*sinp)*1e-3
    Xw0= camH*Xc0/(f*cosp + Zc0*sinp)*1e-3
    #print(' %4.0f    %4.0f    %4.8f   %4.8f   %4.8f   %4.8f   f:%.2f'%( p0[0],p0[1],  Xc0, Zc0,Xw0,Zw0,f ) )
    #yaw定义为拍摄方向，即图片的高方位（Z方向）偏离正北的方向，北偏东为正。
    yaw_rad = yaw/180.0*np.pi
    siny=np.sin(yaw_rad);cosy=np.cos(yaw_rad)
    Zx0_rot = Xw0*cosy + Zw0*siny + pUAV[0]
    Zw0_rot = -Xw0*siny + Zw0*cosy + pUAV[1]
    

    return Zx0_rot,Zw0_rot

def location( point,igW,igH,PlanWgs84,PlanH,yaw,delta,pitch,focal,outFormat='wgs84'):
    '''
        输入图像中点的X，Y坐标，及无人机相关信息、相机相关信息，输出该点的Wgs84经纬度坐标
        point--点在图像上的坐标，左上角为（0,0），X方向为宽度方向，Y方向为高度方向
        igW--图像的宽度
        igH--图像的高度
        PlanWgs84--无人机的Wgs84坐标，(lon,lat),(经度，纬度)
        PlanH--无人机拍照时的相对高度，用mm表示
        yaw--云台的yaw
        delta--单个像素的长度值，用mm表示。
        pitch--无人的pitch
        focal--真实焦距，用mm表示
    '''    
    PlanX,PlanY = wgs_to_mercator(PlanWgs84[0],PlanWgs84[1])
    #print('location:',PlanX,PlanY)
    #PlanX,PlanY--东西、南北方向的墨卡托投影坐标
    #print( 'line268:',point,PlanX,PlanY,yaw, delta,pitch,focal,PlanH,igW,igH )
    cor_world = cam2word( point,(PlanX,PlanY),yaw, delta,pitch,focal,PlanH,igW,igH)
    cor_world = mercator_to_wgs(cor_world[0], cor_world[1])
    if outFormat=='GCJ02' or outFormat=='gcj02':
        cor_world = wgs84_to_gcj02(cor_world[0], cor_world[1])  
    
    return cor_world
    
def locate_byMqtt(box,igW,igH,camParas,outFormat='wgs84'):
    #camParas--{'lon': 3479.8250608, 'lat': 3566.7630802, 'gpssingal': 4, 'satcount': 6896, 'alt': 3.256, 'hspeed': 86.0, 'vspeed': 4.447911, 'ysingal': 0, 'tsingal': 0, 'voltage': 24.971, 'flytime': 0, 'datetime': 1739315683895, 'yaw': 70.243252, 'roll': -0.89436062, 'pitch': 0.89897547, 'armed': 'false', 'mode': 'stabilize', 'distToHome': 7.132033, 'deviceid': 'THJSQ03A2302KSPYGJ2G', 'mileage': '0', 'altasl': 21.26, 'altasl2': -20.74, 'landing_target_x': 0, 'landing_target_y': 0, 'landing_target_z': 0}
    #模型输出的点的格式是-[(486, 264), (505, 264), (505, 290), (486, 290)]
    
    box_np = np.array(box);  
    point = int(np.mean( box_np[:,0] )) , int(np.mean( box_np[:,1] ))
    PlanWgs84 = (float(camParas['lon']),float(camParas['lat'])) #
    PlanH = float(camParas['alt'])#
    yaw = float(camParas['camerayaw'])
    #delta = camParas['']
    delta = 1.55e-3 * 4056.0/1920
    pitch = float(camParas['camerapitch'])
    #focal = camParas['']
    focal = 3.5 
    
    out = location( point,igW,igH,PlanWgs84,PlanH,yaw,delta,pitch,focal,outFormat='wgs84')
    return out

if __name__=="__main__":
    srt="videos/DJI_20221220133918_0001_W_0.SRT"
    videoUrl = "videos/DJI_20221220133918_0001_W.MP4"
    imgOut= "videos/imgs"
    fpbeg=17273;fpend=17830
    nums = list(range(fpbeg,fpend,16))
    #generateNewSRT(srt)  
    #captureImages(videoUrl,nums,imgOut)    
    
    process(videoUrl,srt,nums,imW=1920,imH=1080,txtDir='videos/labels' )
    #draw_results()
    
    #rotate_example()
    #rotate_example3()