import os
import sys
print("os.path.abspath(__file__) = ",os.path.dirname(os.path.abspath(__file__)))
from VegetationHeightPrePro import PreProcess as Pp   # 此行放在下面会报错，最好放在上面
from tool.algorithm.xml.AlgXmlHandle import ManageAlgXML, CheckSource
# from AlgXmlHandle import ManageAlgXML, CheckSource  # 导入xml文件读取与检查文件
from VegetationHeightAlg import PlantHeightAlg, ROIAlg  # 此处的ROIAlg可以对多波段数据进行掩膜，与tool文件中的ROIAlg不同
from VegetationHeightOrthoAlg import IndirectOrthorectification as Inor
from tool.algorithm.image.ImageHandle import ImageHandler
from tool.algorithm.algtools.logHandler import LogHandler
from AHVToPolSarPro import AHVToPolSarProS2
from tool.algorithm.xml.CreatMetafile import CreateMetafile
from VegetationHeightXmlInfo import CreateDict, CreateStadardXmlFile
from Orthorectification import fine_registration,Orthorectification_RD, header_file_read_angle
import numpy as np

import shutil
import logging
import datetime
import sys
import tarfile
import glob
import gc
import math
import xmltodict, json
import VegetationHeightMain 


# polsarpro
AHVToPolSarProS2 = AHVToPolSarProS2()
PlantHeightAlg = PlantHeightAlg()

workspace_preprocessed2_path=r"D:\forestHeight\Result\\" # 结果文件夹
workspace_master_slc_path=r"D:\forestHeight\Input\RS2_OK31134_PK305834_DK273463_FQ20W_20120627_100751_HH_VV_HV_VH_SLC\\"
workspace_slave_slc_path=r"D:\forestHeight\Input\RS2_OK31594_PK309303_DK276153_FQ20W_20120907_100751_HH_VV_HV_VH_SLC\\"

master_s2 = workspace_preprocessed2_path + "master_s2""\\"
os.makedirs(master_s2)   
slave_s2 = workspace_preprocessed2_path + "slave_s2""\\"
os.makedirs(slave_s2)   

#  主影像->s2
# 输出文件夹
AHVToPolSarProS2.api_ahv_to_polsarpro_s2(master_s2, workspace_master_slc_path) # 全极化影像转S2矩阵
#  辅影像->s2
# 输出文件夹
AHVToPolSarProS2.api_ahv_to_polsarpro_s2(slave_s2, workspace_slave_slc_path) # 全极化影像转S2矩阵
print("slc->s2 finish ")

# 读取轨道节点，生产flat kz
master_orbit_path=r"D:\forestHeight\Input\orbit\oribit0907.xml"
slave_orbit_path=r"D:\forestHeight\Input\orbit\oribit0627.xml"
with open(master_orbit_path) as f:
       master_orbit_dict = xmltodict.parse(f.read())['orbitInformation']['stateVector']
with open(slave_orbit_path) as f:
       slave_orbit_dict = xmltodict.parse(f.read())['orbitInformation']['stateVector']
GPSTime_Format="%Y-%m-%dT%H:%M:%S.%fZ"
GPSNodeNames_list=['timeStamp', 'xPosition', 'yPosition', 'zPosition', 'xVelocity', 'yVelocity', 'zVelocity']
# master
master_orbits=[]
for GPSPoint in master_orbit_dict:
    GPSPoint=[
            datetime.datetime.strptime(GPSPoint[GPSNodeNames_list[0]],GPSTime_Format).timestamp(), # TimeStamp
            float(GPSPoint[GPSNodeNames_list[1]]['#text']), # Xp
            float(GPSPoint[GPSNodeNames_list[2]]['#text']), # Yp
            float(GPSPoint[GPSNodeNames_list[3]]['#text']), # Zp
            float(GPSPoint[GPSNodeNames_list[4]]['#text']), # Vx
            float(GPSPoint[GPSNodeNames_list[5]]['#text']), # Vy
            float(GPSPoint[GPSNodeNames_list[6]]['#text'])] # VZ     
    master_orbits.append(GPSPoint)
# slave
slave_orbits=[]
for GPSPoint in slave_orbit_dict:
    GPSPoint=[
            datetime.datetime.strptime(GPSPoint[GPSNodeNames_list[0]],GPSTime_Format).timestamp(), # TimeStamp
            float(GPSPoint[GPSNodeNames_list[1]]['#text']), # Xp
            float(GPSPoint[GPSNodeNames_list[2]]['#text']), # Yp
            float(GPSPoint[GPSNodeNames_list[3]]['#text']), # Zp
            float(GPSPoint[GPSNodeNames_list[4]]['#text']), # Vx
            float(GPSPoint[GPSNodeNames_list[5]]['#text']), # Vy
            float(GPSPoint[GPSNodeNames_list[6]]['#text'])] # VZ   
    slave_orbits.append(GPSPoint)
# 设置起始时间
master_start_time=min([p[0] for p in master_orbits])-1
slave_start_time=min([p[0] for p in slave_orbits])-1
for i in range(len(master_orbits)):
    master_orbits[i][0]=master_orbits[i][0]-master_start_time
for i in range(len(slave_orbits)):
    slave_orbits[i][0]=slave_orbits[i][0]-slave_start_time
master_orbits=np.array(master_orbits)
slave_orbits=np.array(slave_orbits)
# master
master_orbits_fit_dict={}
master_orbits_fit_dict['x']=np.polynomial.hermite.Hermite.fit(master_orbits[:,0],master_orbits[:,1],3)
master_orbits_fit_dict['y']=np.polynomial.hermite.Hermite.fit(master_orbits[:,0],master_orbits[:,2],3)
master_orbits_fit_dict['z']=np.polynomial.hermite.Hermite.fit(master_orbits[:,0],master_orbits[:,3],3)
master_orbits_fit_dict['vx']=np.polynomial.hermite.Hermite.fit(master_orbits[:,0],master_orbits[:,4],3)
master_orbits_fit_dict['vy']=np.polynomial.hermite.Hermite.fit(master_orbits[:,0],master_orbits[:,5],3)
master_orbits_fit_dict['vz']=np.polynomial.hermite.Hermite.fit(master_orbits[:,0],master_orbits[:,6],3)
master_orbits_fit=lambda t:[master_orbits_fit_dict['x'](t),
                           master_orbits_fit_dict['y'](t),
                           master_orbits_fit_dict['z'](t),
                           master_orbits_fit_dict['vx'](t),
                           master_orbits_fit_dict['vy'](t),
                           master_orbits_fit_dict['vz'](t)]
# slave
slave_orbits_fit_dict={}
slave_orbits_fit_dict['x']=np.polynomial.hermite.Hermite.fit(slave_orbits[:,0],slave_orbits[:,1],3)
slave_orbits_fit_dict['y']=np.polynomial.hermite.Hermite.fit(slave_orbits[:,0],slave_orbits[:,2],3)
slave_orbits_fit_dict['z']=np.polynomial.hermite.Hermite.fit(slave_orbits[:,0],slave_orbits[:,3],3)
slave_orbits_fit_dict['vx']=np.polynomial.hermite.Hermite.fit(slave_orbits[:,0],slave_orbits[:,4],3)
slave_orbits_fit_dict['vy']=np.polynomial.hermite.Hermite.fit(slave_orbits[:,0],slave_orbits[:,5],3)
slave_orbits_fit_dict['vz']=np.polynomial.hermite.Hermite.fit(slave_orbits[:,0],slave_orbits[:,6],3)
slave_orbits_fit=lambda t:[slave_orbits_fit_dict['x'](t),
                           slave_orbits_fit_dict['y'](t),
                           slave_orbits_fit_dict['z'](t),
                           slave_orbits_fit_dict['vx'](t),
                           slave_orbits_fit_dict['vy'](t),
                           slave_orbits_fit_dict['vz'](t)]

master_image_time=[datetime.datetime.strptime(i,GPSTime_Format).timestamp() for i in  ["2012-09-07T10:07:52.415268Z","2012-09-07T10:07:56.843662Z"]]
slave_image_time=[datetime.datetime.strptime(i,GPSTime_Format).timestamp() for i in  ["2012-06-27T10:07:51.501578Z","2012-06-27T10:07:55.933788Z"]]
master_image_time=np.array(master_image_time)-master_start_time
slave_image_time=np.array(slave_image_time)-slave_start_time

master_lines=6764
master_cols=5804
slave_lines=6764
slave_cols=5809

# 成像时间
master_delta=(np.max(master_image_time)-np.min(master_image_time))/(master_lines-1)
slave_delta=(np.max(slave_image_time)-np.min(slave_image_time))/(slave_lines-1)
master_times=np.array(list(range(master_lines)))*master_delta+np.min(master_image_time)
slave_times=np.array(list(range(slave_lines)))*slave_delta+np.min(slave_image_time)

# 构建成像轨道节点
master_orbits=np.array(master_orbits_fit(master_times)).transpose(1,0)
slave_orbits=np.array(slave_orbits_fit(slave_times)).transpose(1,0)
# 成像中心点
master_center_orbits=np.array(master_orbits_fit(np.mean(master_times)))
slave_center_orbits=np.array(slave_orbits_fit(np.mean(slave_times)))
# 构建向量
B_orbits=slave_orbits-master_orbits
B=np.mean(B_orbits,axis=0)[:3] # 基线向量的平均值
B_=slave_center_orbits-master_center_orbits

# 求解水平角
alpha =math.pi-math.acos(np.dot(master_center_orbits[:3],B[:3])/(np.linalg.norm(master_center_orbits[:3])*np.linalg.norm(B[:3])))
B_h=np.linalg.norm(B[:3])*math.cos(alpha)
B_v=np.linalg.norm(B[:3])*math.sin(alpha)
print(alpha,B_h,B_v)


master_fer = workspace_preprocessed2_path + "master_fer""\\"
slave_fer = workspace_preprocessed2_path + "slave_fer""\\"
os.makedirs(master_fer)
os.makedirs(slave_fer)

master_fer = master_s2
master_slave_t6 = workspace_preprocessed2_path + "master_slave_t6""\\"

current_path=r"D:\forestHeight\vegetationHeight-L-SAR-V1.0-源码\vegetationHeight-L-SAR-V1.0"
# 3、s2+s2->t6
data_convert_s2_t6_path = os.path.join(current_path, "data_convert_MLK_S2_T6.exe")
master_slave_t6 = workspace_preprocessed2_path + "master_slave_t6""\\"
#os.makedirs(master_slave_t6)
PlantHeightAlg.s2_to_t6(master_fer, slave_fer, data_convert_s2_t6_path, master_slave_t6)


# 4、T6->boxcar_filter->T6
boxcar_filter_tool_path = os.path.join(current_path, "boxcar_filter_T6.exe")
# boxcar_filter_tool_path = os.path.join(current_path, "lee_refined_filter_T6.exe")
master_slave_t6_box = workspace_preprocessed2_path + "master_slave_t6_box""\\"
#os.makedirs(master_slave_t6_box)
PlantHeightAlg.polsar_boxcar_filter(master_slave_t6_box, boxcar_filter_tool_path,
                                                 master_slave_t6, *(1, 21, 0, 0))

master_slave_t6_box=master_slave_t6
master_slave_t6_box_coh=workspace_preprocessed2_path + "master_slave_t6_box_coh""\\"
#os.makedirs(master_slave_t6_box_coh)

# 5、 T6->coherence_estimation->T6
coherence_estimation_path = os.path.join(current_path, "complex_coherence_estimation_T6.exe")
#coherence_opt_estimation_path = os.path.join(current_path, "complex_coherence_opt_estimation_T6.exe")
#master_slave_t6_box_coh = master_slave_t6_box
PlantHeightAlg.complex_coherence_estimation_T6(master_slave_t6_box,
                                                            coherence_estimation_path,
                                                            master_slave_t6_box_coh, * (7, 7, 7, 7))

out_kz_path=r"D:\forestHeight\Result\flat_remove\kz.bin"
master_slave_t6_box_coh_rvog = workspace_preprocessed2_path + "master_slave_t6_box_coh_rvog""\\"


# 6、  T6->RVOG->产品
height_estimation_inversion_rvog_path = os.path.join(current_path,
                                                             "height_estimation_inversion_procedure_RVOG.exe")
file_gamma_high_file = master_slave_t6_box_coh + "cmplx_coh_HV.bin"
file_gamma_low_file = master_slave_t6_box_coh + "cmplx_coh_HHmVV.bin"
# file_gamma_high_file = master_slave_t6_box_coh + "cmplx_coh_avg_HV.bin"
# file_gamma_low_file = master_slave_t6_box_coh + "cmplx_coh_avg_HHmVV.bin"

PlantHeightAlg.height_estimation_RVOG(master_slave_t6_box_coh, height_estimation_inversion_rvog_path,
                                                   file_gamma_high_file, file_gamma_low_file,
                                                   master_slave_t6_box_coh_rvog, out_kz_path, *(11, 0.4))