# -*- coding: utf-8 -*-
"""
Created on Fri Oct  2 14:02:52 2020

@author: ppantina
"""

#function Nav = L1A_DataProcessing_Navigation(dir_fn_HKmat, config, TimeUTC)
#% Processing navigation data for L1A
#%
#% Inputs:
#%        dir_fn_HKmat:    location of the navigation/housekeeping data file in '.mat' format
#%        config:          Configuration structure
#%        TimeUTC:         UTC time in L1A
#% Outputs:
#%        Nav:             Navigation data at TimeUTC
#%
#% Jinzheng Peng
#% 2020-04-22

##20230404 Added heading correction line

print ('test')
import numpy as np
import h5py
import sys
import L1A_sub_interpDegree

def L1A_sub_processNavigation (radName, navhkp, config, TimeUTC):
   
    telem       = h5py.File(navhkp, 'r')   
    navigation = ['nav', 'gps']
    telem_dict = {}
    
    for k, nav_type in enumerate(navigation):
        telem_dict[nav_type] = {}
        #% remove bad data and report if bad data can't be removed completely
        if nav_type in telem:
            telem_dict[nav_type]['CPUtime'] = telem[nav_type]['cpusec'][:] + telem[nav_type]['cpunsec'][:]/1e9
            Adiff = np.diff(telem_dict[nav_type]['CPUtime'])
            ind_good = list(np.where(Adiff >  0)[0])
            ind_bad  = list(np.where(Adiff <= 0)[0])
            if len(ind_bad) > 0:
                telem_dict[nav_type]['CPUtime'] = telem_dict[nav_type]['CPUtime'][ind_good]
                for i, j in enumerate(telem[nav_type].keys()):
                    telem_dict[nav_type][j]             = telem[nav_type][j][:][ind_good]                
                Adiff2 = np.diff(telem_dict[nav_type]['CPUtime'])
                ind_bad2  = list(np.where(Adiff2 <= 0)[0])
                if len(ind_bad2) > 0:
                    print('Bad time NOT removed in', nav_type)
            else:
                for i, j in enumerate(telem[nav_type].keys()):
                    telem_dict[nav_type][j]             = telem[nav_type][j][:]   
    Nav                = {}
    Nav['Information'] = {}
    Nav['Data']        = {}
    
    Nav['Information']['PitchCorrection'  ] = [config['L1A_PitchCorrection'  ]]
    Nav['Information']['HeadingCorrection'] = [config['L1A_HeadingCorrection']]
    Nav['Information']['RollCorrection'   ] = [config['L1A_RollCorrection'   ]]
    Nav['Information']['TimeCorrection'   ] = [config['L1A_TimeCorrection'   ]]
    Nav['Information']['Source']            = [config['L1A_navSource'        ]]
    kind = config['L1A_navSource'].upper() 
   
    if  kind == 'IWG':
        L1NAVdata = telem_dict['nav']
        print ( 'Using IWG data from file ' ,navhkp, ' for Navigation')
        
        #%Interpolate the IWG data onto the profile times
        tnav_corr = L1NAVdata['CPUtime'] + Nav['Information']['TimeCorrection']
        Nav['Data']['Drift'        ] = np.interp(TimeUTC, tnav_corr, L1NAVdata['IWG_drift' ])
        Nav['Data']['EastVelocity' ] = np.interp(TimeUTC, tnav_corr, L1NAVdata['IWG_gspeed']*np.sin(np.deg2rad(L1NAVdata['IWG_track'])))
        Nav['Data']['NorthVelocity'] = np.interp(TimeUTC, tnav_corr, L1NAVdata['IWG_gspeed']*np.cos(np.deg2rad(L1NAVdata['IWG_track'])))
        K_vs = 1 
        if 'IMPACTS' in config['experimentName'] : K_vs = 1/60*1.1  #% correction faction to UpVelocity for IMPACTS2020
        Nav['Data']['UpVelocity'   ] = np.interp(TimeUTC, tnav_corr, L1NAVdata['IWG_upspeed']*K_vs)
        Nav['Data']['Track'        ] = L1A_sub_interpDegree.L1A_sub_interpDegree(TimeUTC, tnav_corr, L1NAVdata['IWG_track' ])
        Nav['Data']['Heading'      ] = L1A_sub_interpDegree.L1A_sub_interpDegree(TimeUTC, tnav_corr, L1NAVdata['IWG_head'  ]+ Nav['Information']['HeadingCorrection'] )
        
    elif  kind == 'INSPVA':
        L1NAVdata = telem_dict['gps']        
        print ( 'Using INSPVA data from file ' ,navhkp, ' for Navigation')

        
        #%Interpolate the INSPVA data onto the profile times
        tnav_corr = L1NAVdata['CPUtime'] + Nav['Information']['TimeCorrection']
        Nav['Data']['EastVelocity' ] = np.interp(TimeUTC, tnav_corr, L1NAVdata['INSPVA_evel'   ])
        Nav['Data']['NorthVelocity'] = np.interp(TimeUTC, tnav_corr, L1NAVdata['INSPVA_nvel'   ])
        Nav['Data']['UpVelocity'   ] = np.interp(TimeUTC, tnav_corr, L1NAVdata['INSPVA_upvel'  ])
        
        ##modified heading calculation Oct 2021 to match IWG. Might be a better way to do this
        Nav['Data']['Heading'      ] = np.mod(L1A_sub_interpDegree.L1A_sub_interpDegree(TimeUTC, tnav_corr, L1NAVdata['INSPVA_head'])+Nav['Information']['HeadingCorrection']+180, 360)-180        
        ##modified track calculation Oct 2021 to match IWG. Might be a better way to do this.
        Nav['Data']['Track'        ] = np.mod(np.angle(Nav['Data']['NorthVelocity' ] +1j*Nav['Data']['EastVelocity'   ]) * 180/np.pi, 360)
        Nav['Data']['Drift'        ] = np.mod  (Nav['Data']['Track']   - Nav['Data']['Heading'] + 180, 360) - 180
    else: 
        print ('bad nav source')
        sys.exit()
    
    Nav['Data']['Latitude'        ] = L1A_sub_interpDegree.L1A_sub_interpDegree(TimeUTC, tnav_corr, L1NAVdata[kind + '_lat' ])
    Nav['Data']['Longitude'       ] = L1A_sub_interpDegree.L1A_sub_interpDegree(TimeUTC, tnav_corr, L1NAVdata[kind + '_lon'])
    
    Nav['Data']['Pitch'           ] = np.interp(TimeUTC, tnav_corr, L1NAVdata[kind + '_pitch' ]) + Nav['Information']['PitchCorrection']
    Nav['Data']['Roll'            ] = np.interp(TimeUTC, tnav_corr, L1NAVdata[kind + '_roll'  ]) + Nav['Information']['RollCorrection']
    Nav['Data']['Height'          ] = np.interp(TimeUTC, tnav_corr, L1NAVdata[kind + '_height'])
    
    if radName != 'EXRAD_SCAN':
        Nav['Data']['AntennaAzimuth'  ] = [config['L1A_antennaAzimuth'  ]]
        Nav['Data']['AntennaElevation'] = [config['L1A_antennaElevation']]

    return (Nav)