<reponame>MarcelDogotari/HSRW_PG02_Laser-Scanner
#!/usr/bin/env python
# coding: utf-8

"""
    This script should parse the pcap and csv files and find
    the required info for each return from the .xyz files.
    Then save it in 1 place...
"""

import sys, os, logging, re, datetime
import numpy as np
import pandas as pd
from vlp16_tables import *
from scipy.interpolate import interp1d

log_dir = 'p_logging'
txt_dir_in = 'p_01_apx_csv_shapefile'
txt_in_base_len = len(txt_dir_in)
pcap_dir_in = 'p_22_likely_gcp_pcap'
pcap_in_base_len = len(pcap_dir_in)
xyz_dir_in = 'p_20_likely_gcp'
xyz_dir_in_ref = 'p_21_refined_gcp'
out_dir_ascii = 'p_24_likely_gcp_infos'
out_ascii_base_len = len(out_dir_ascii)
fn_keyword = 'hi_freq_apx'

def remove_min_sec(ts):
    return (int(ts) // 3600) * 3600

# ### Function to calculate the gaps between given azimuths. Needed to interpolate azimuths that are not given.

def get_azim_gap(azimuths, dual = True, preserve_shape = False):
    """
        Only works for dual returns now.
        
        preserve_shape is relevant for dual, where the azimuths repeat.
        if False: return only unique gaps.
        if True: return same shape as azimuths
    """
    if dual:
        azimuths_gap_flat = np.zeros_like(azimuths[0::2]).flatten()
        azimuths_gap_flat[:-1] = ((azimuths[0::2].flatten()[1:] - azimuths[0::2].flatten()[:-1]) % 36000)
        azimuths_gap_flat[-1] = azimuths_gap_flat[-2]
        azimuths_gap = azimuths_gap_flat.reshape(azimuths[0::2].shape)
        if preserve_shape:
            #either of the following lines should work the same. only use one.
            #azimuths_gap = np.hstack((azimuths_gap.reshape((azimuths_gap.size,1)), azimuths_gap.reshape((azimuths_gap.size,1)))).flatten().reshape(azimuths.shape)
            azimuths_gap = np.tile(azimuths_gap,2)
        return azimuths_gap
    else:
        raise NotImplementedError

def get_micros_pulses(micros, dual = True, preserve_shape = False):
    """
        preserve_shape is relevant for dual, where the azimuths repeat.
        if False: return only unique gaps.
        if True: return same shape as azimuths
    """
    if dual:
        if preserve_shape:
            micros_pulses = np.expand_dims(micros, axis=1) + TIMING_OFFSETS_DUAL.T.flatten() * 1e6
        else:
            micros_pulses = np.expand_dims(micros, axis=1) + TIMING_OFFSETS_DUAL.T[0::2,:].flatten() * 1e6
    else:
        micros_pulses = np.expand_dims(micros, axis=1) + TIMING_OFFSETS_SINGLE.T.flatten() * 1e6
    
    MICROS_PER_HOUR = np.array([1e6 * 3600]).astype(np.int64)[0]
    micros_pulses %= MICROS_PER_HOUR
    return micros_pulses

def get_nanos_pulses(micros, dual = True, preserve_shape = False):
    """
        Get the nanoseconds from the top of the hour for each laser pulse. Need for boresight!
        preserve_shape is relevant for dual, where the azimuths repeat.
        if False: return only unique gaps.
        if True: return same shape as azimuths
    """
    nanos = np.copy(micros) 
    nanos *= 1000
    if dual:
        if preserve_shape:
            nanos_pulses = np.expand_dims(nanos, axis=1) + (TIMING_OFFSETS_DUAL_nano.T.flatten()).astype(np.int64)
        else:
            nanos_pulses = np.expand_dims(nanos, axis=1) + (TIMING_OFFSETS_DUAL_nano.T[0::2,:].flatten()).astype(np.int64)
    else:
        nanos_pulses = np.expand_dims(nanos, axis=1) + (TIMING_OFFSETS_SINGLE_nano.T.flatten()).astype(np.int64)
    NANOS_PER_HOUR = np.array([1e9 * 3600]).astype(np.int64)[0]
    nanos_pulses %= NANOS_PER_HOUR
    return nanos_pulses

def get_precision_azimuth(az_simple, azimuths_gap, dual = True, minimal_shape = True):
    az_simple = az_simple[np.newaxis, :]
    azimuths_gap = azimuths_gap[np.newaxis,:]
    if dual:
        timing_offsets_within_block = TIMING_OFFSETS_DUAL[:,0]
        az_pulses = np.tile(az_simple,(LASERS_PER_DATA_BLOCK)).reshape(az_simple.shape[0], LASERS_PER_DATA_BLOCK, az_simple.shape[1])
        az_pulses = az_pulses.transpose((0,2,1))
        precision_azimuth = az_pulses[:,:,:] + timing_offsets_within_block / (2 * T_CYCLE) * np.expand_dims(azimuths_gap, axis=2)
        precision_azimuth = precision_azimuth % 36000
        if not minimal_shape:
            precision_azimuth = np.tile(precision_azimuth.transpose((0,2,1)), (1,2,1)).transpose((0,2,1))
        precision_azimuth = precision_azimuth.reshape((precision_azimuth.shape[0], precision_azimuth.shape[1] * precision_azimuth.shape[2]))
        return precision_azimuth[0,:]
    else:
        raise NotImplementedError

def get_azimuth_of_interest(loc_raw_data, loc_packet_pos, loc_pulse_pos):
    #azimuth
    azimuths = np.zeros((DATA_BLOCKS, 1), dtype = np.int32)
    azim_data = azimuths.view(dtype = np.uint8)
    azim_data[:,0:2] = loc_raw_data[loc_packet_pos, :-6].reshape(DATA_BLOCKS, 100)[:, 2:4]
    azim_data = azim_data.reshape((DATA_BLOCKS * 4))
    azimuths = azim_data.view(dtype= np.int32)
    azimuths_gap = get_azim_gap(azimuths)
    # ### Calculate the azimutzhs for each datapoint
    #Use the following simplified array if in dual mode
    #Otherwise can still refer to it, but it's just the original array
    mode_hypothesis = loc_raw_data[0, RETURN_MODE_OFFSET]
    if mode_hypothesis == RETURN_MODE_DUAL:
        # az_simple = azimuths[:,0::2]
        az_simple = azimuths[0::2]
    else:
        az_simple = azimuths
    prec_az = get_precision_azimuth(az_simple, azimuths_gap, True, True)
    the_azimuth_rad = np.deg2rad(prec_az[loc_pulse_pos] / 100)
    return the_azimuth_rad

def make_puck_ranges_mm(channel_data):
    puck_ranges_mm = np.zeros((DATA_BLOCKS * LASERS_PER_DATA_BLOCK), dtype = np.uint32)
    puck_range_bytes = puck_ranges_mm.view(dtype = np.uint8)
    puck_range_bytes[0::4] = channel_data[0::3]
    puck_range_bytes[1::4] = channel_data[1::3]
    puck_ranges_mm *= 2
    return puck_ranges_mm

def make_puck_intensities(channel_data):
    puck_intens = np.zeros((DATA_BLOCKS * LASERS_PER_DATA_BLOCK), dtype = np.uint8)
    puck_intens[:] = channel_data[2::3]
    return puck_intens

def puck_shperical_to_cartesian(puck_range, alpha, omega, vert_corr):
    """
        input: range, alpha, omega, vert_corr
        output:
        xyz in puck coordinates
    """
    X_puck = puck_range * np.cos(omega) * np.sin(alpha)
    Y_puck = puck_range * np.cos(omega) * np.cos(alpha)
    Z_puck = puck_range * np.sin(omega)
    Z_puck += vert_corr
    return np.vstack((X_puck, Y_puck, Z_puck)).T

def puck_to_uav_coords(XYZ_puck):
    # first rotate into XYZ of the drone!
    # x_roll = -90 #degrees
    # y_pitch = 0
    # z_yaw = -90
    #rotation from puck to uav coordinates:
    R_01 = np.array([[0., 1., 0.],
                    [0., 0., 1.],
                    [1., 0., 0.]])

    XYZ_uav = np.matmul(XYZ_puck, R_01)
    return XYZ_uav

def pcap_to_uav_coord(pcap_file_in, pcap_dir_in, xyz_str):
    """
        Process a point upto the return coordinates in uav frame
        Input:
            1 pcap file, where the point of interest is roughly in the middle
            pcap directory name
            string corresponding to the point, coming from the xyz file
    """
    with open(os.path.join(pcap_dir_in, pcap_file_in), "rb") as fh:
        raw_pcap = fh.read()
        #24 bytes = global header, 16+42 bytes = packet header, 1200 bytes = vlp returns, 2 bytes = vlp factory bytes
        raw_data = np.frombuffer(raw_pcap, dtype = np.uint8)[24:].reshape((len(raw_pcap)//1264,1264))[:,16+42:]

    target_nano = (np.array([xyz_str.split()[9]])).astype(np.int64)[0]
    target_channel = int(xyz_str.split()[10])
    target_intensity = int(xyz_str.split()[5])


    # #### Take Ethernet timestamp of (1st) packet, discard sub-hour info and add replace it with that from GNSS µs timestamp
    # 
    # What happens when the capture rolls over a full hour?
    # 
    # **Need to deal with this when such data is captured!**
    # 
    # # Solution below!
    micros = np.zeros((raw_data.shape[0],), dtype = np.int64)
    micro_bytes = micros.view(dtype = np.uint8)
    micro_bytes[0::8] = raw_data[:, DATA_PACK_TIMESTAMP_OFFSET + 0]
    micro_bytes[1::8] = raw_data[:, DATA_PACK_TIMESTAMP_OFFSET + 1]
    micro_bytes[2::8] = raw_data[:, DATA_PACK_TIMESTAMP_OFFSET + 2]
    micro_bytes[3::8] = raw_data[:, DATA_PACK_TIMESTAMP_OFFSET + 3]

    # #### Another problem could be that the UDP packets are not guaranteed to arrive in order.
    # 
    # An assumption that is made for the following calculations is that this does not happen.
    # 
    # **Need to deal with this when such data is captured!**
    while (micros[1:] < micros[:-1]).sum() > 0:
        jump_position = np.where((micros[1:] < micros[:-1]))[0][0] + 1
        micros[jump_position:] += int(3.6e9)

    nanos_pulses = get_nanos_pulses(micros)

    precise_location = np.where(nanos_pulses == target_nano)
    if precise_location[0].size == 0: #wrong packet?!
        return None
    
    the_packet_pos = precise_location[0][0]
    the_pulse_pos = precise_location[1][0]
    assert target_channel == the_pulse_pos % 16, f'Target channel does not match. xyz_str: {xyz_str}. the_pulse_pos: {the_pulse_pos}'

    # the following contains only channel data (i.e. no timestamp, factory bytes or azimuth)
    channel_data = raw_data[the_packet_pos,:-6].reshape((1, DATA_BLOCKS, 100))[:,:,4:]
    # channel_data = channel_data.reshape((1, DATA_BLOCKS * LASERS_PER_DATA_BLOCK * 3))
    channel_data = channel_data.reshape((DATA_BLOCKS * LASERS_PER_DATA_BLOCK * 3))

    #puck ranges in mm
    puck_ranges = make_puck_ranges_mm(channel_data)

    #intensities as 1 byte
    puck_intens = make_puck_intensities(channel_data)


    # strongest and last ranges (meters) + intensities
    strongest_return_ranges = puck_ranges.reshape((DATA_BLOCKS, LASERS_PER_DATA_BLOCK))[1::2].flatten() / 1000
    strongest_return_intensities = puck_intens.reshape((DATA_BLOCKS, LASERS_PER_DATA_BLOCK))[1::2].flatten()
    last_return_ranges = puck_ranges.reshape((DATA_BLOCKS, LASERS_PER_DATA_BLOCK))[0::2].flatten() / 1000
    last_return_intensities = puck_intens.reshape((DATA_BLOCKS, LASERS_PER_DATA_BLOCK))[0::2].flatten()

    #range of the point of interest
    the_return_id = int(xyz_str.split()[4])
    if the_return_id==1:
        the_precise_range = strongest_return_ranges[the_pulse_pos]
        assert strongest_return_intensities[the_pulse_pos]==target_intensity,\
            f'Target intensity not matched! xyz_str: {xyz_str}. This intensity: {strongest_return_intensities[the_pulse_pos]}'
    elif the_return_id==2:
        the_precise_range = last_return_ranges[the_pulse_pos]
        assert last_return_intensities[the_pulse_pos]==target_intensity,\
            f'Target intensity not matched! xyz_str: {xyz_str}. This intensity: {last_return_intensities[the_pulse_pos]}'
    else:
        raise ValueError(f'return ID > 2! ({the_return_id}). xyz_str: {xyz_str}')
    assert the_precise_range > 0, f'Range is not larger than 0! xyz_str: {xyz_str}'

    the_azimuth = get_azimuth_of_interest(raw_data, the_packet_pos, the_pulse_pos)
    elev_angle_and_corr = elevation_and_vert_corr_by_laser_id[target_channel]
    the_elev_angle = elev_angle_and_corr[0]
    the_vert_corr = elev_angle_and_corr[1]

    puck_xyz = puck_shperical_to_cartesian(the_precise_range, the_azimuth, the_elev_angle, the_vert_corr)
    uav_xyz = puck_to_uav_coords(puck_xyz)

    return uav_xyz

def apx_df_to_interp_funcs(apx_df):
    """
        Makes 6 functions that are used to interpolate lon, lat, ele,
        rol, pitch and yaw for any time that is covered by the apx dataset.
    """
    f_lat = interp1d(apx_df.index, apx_df["lat_EPSG32632"], kind = 'cubic', fill_value="extrapolate")
    f_lon = interp1d(apx_df.index, apx_df["lon_EPSG32632"], kind = 'cubic', fill_value="extrapolate")
    f_ele = interp1d(apx_df.index, apx_df["elevation"], kind = 'cubic', fill_value="extrapolate")
    f_yaw = interp1d(apx_df.index, apx_df["heading_continuous"], kind = 'cubic', fill_value="extrapolate")
    f_rol = interp1d(apx_df.index, apx_df["roll"], kind = 'cubic', fill_value="extrapolate")
    f_pit = interp1d(apx_df.index, apx_df["pitch"], kind = 'cubic', fill_value="extrapolate")
    return f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit

def apx_funcs_to_interp_values(xyz_str, f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit):
    """
        Uses the 6 functions made above to provide lon, lat, ... for a specific timestamp
    """
    the_time = float(xyz_str.split()[7]) + 1e9
    lat = f_lat(the_time)
    lon = f_lon(the_time)
    ele = f_ele(the_time)
    yaw = -np.deg2rad(f_yaw(the_time) % 360)
    rol = -np.deg2rad(f_rol(the_time))
    pit = -np.deg2rad(f_pit(the_time))
    return lat, lon, ele, yaw, rol, pit

def yaw_correction_to_interp_func():
    """
        Makes 1 function that is used to interpolate yaw correction is present.
        'yaw_correction.csv' must be present in p_03_pcap!
    """
    if 'yaw_correction.csv' in os.listdir('p_03_pcap'):
        yaw_df = pd.read_csv(os.path.join('p_03_pcap', 'yaw_correction.csv'), index_col=0)
    else:
        return None

    f_yaw_corr = interp1d(yaw_df.index, yaw_df["smooth_yaw_err"], kind = 'cubic', fill_value="extrapolate")

    return f_yaw_corr

def yaw_corr_func_to_interp_value(xyz_str, f_yaw_corr):
    """
        Uses the function made above to provide yaw correction for a specific timestamp
    """
    if f_yaw_corr is None:
        return 0
    
    the_time = float(xyz_str.split()[7]) + 1e9
    yaw_corr = np.deg2rad((f_yaw_corr(the_time) +360) % 360)
    
    return yaw_corr

if __name__=='__main__':
    this_script = sys.argv[0]

    if len(sys.argv)<=1:
        print("ERROR\n\nPlease provide the absolute path of the campaign's top directory.\n\nEXITING")
        sys.exit(0)
    else:
        try: top_dir = sys.argv[1].rstrip(os.path.sep)
        except Exception as e: print(e); sys.exit(0)

    if sys.byteorder == 'little':
        print(f"System endianness: {sys.byteorder}.\nGood to go :)")
    else:
        print(f"System endianness: {sys.byteorder}.\nSTOP right now!")
        sys.exit(0)

    os.chdir(top_dir)
    if os.getcwd() != top_dir:
        print("Something went wrong.")
        print(f"cwd: {os.getcwd()}")
        sys.exit(0)
    
    # if refined files are available
    if xyz_dir_in_ref in os.listdir():
        xyz_dir_in = xyz_dir_in_ref
    
    if log_dir not in os.listdir():
        os.mkdir(log_dir)

    if out_dir_ascii not in os.listdir():
        os.mkdir(out_dir_ascii)

    logging.basicConfig(format='<%(asctime)s>   <%(levelname)-8s>   <%(message)s>',
                        level=logging.ERROR,
                        filename=os.path.join(top_dir, log_dir, f'{datetime.date.today().strftime("%Y.%m.%d")}_{os.path.split(this_script)[-1]}.log'),
                        datefmt='%Y-%m-%d %H:%M:%S')
    logging.info(f"{this_script} has started and received a valid top directory name")

    potential_dirs_in = len(re.findall(txt_dir_in, " ".join(os.listdir())))
    if potential_dirs_in == 0:
        print("No valid input directories found. Make sure to run script 00_join_pcap_.. first\nEXITING!")
        sys.exit(0)
    elif potential_dirs_in > 1:
        print("Multiple input directories found. Choose one:")
        valid_choices = dict()
        for i, d_name in enumerate(os.listdir()):
            if os.path.isfile(d_name):
                continue
            elif txt_dir_in not in d_name:
                continue
            else:
                print(f"{i}: {d_name}")
                valid_choices[i] = d_name

        dir_in_selected = False
        while not dir_in_selected:
            try:
                dir_in_id = int(input("Which directory do we use? Type a number and press enter.\n"))
                if dir_in_id in valid_choices.keys():
                    txt_dir_in = valid_choices[dir_in_id]
                    dir_in_selected = True
            except:
                print("Invalid choice!")
    potential_pcap_dirs_in = len(re.findall(pcap_dir_in, " ".join(os.listdir())))
    if potential_pcap_dirs_in == 0:
        print("No valid PCAP input directories found. Make sure to run the other scripts first\nEXITING!")
        sys.exit(0)
    elif potential_pcap_dirs_in > 1:
        print("Multiple input directories found. Choose one:")
        valid_choices = dict()
        for i, d_name in enumerate(os.listdir()):
            if os.path.isfile(d_name):
                continue
            elif pcap_dir_in not in d_name:
                continue
            else:
                print(f"{i}: {d_name}")
                valid_choices[i] = d_name

        dir_in_selected = False
        while not dir_in_selected:
            try:
                dir_in_id = int(input("Which directory do we use? Type a number and press enter.\n"))
                if dir_in_id in valid_choices.keys():
                    pcap_dir_in = valid_choices[dir_in_id]
                    dir_in_selected = True
            except:
                print("Invalid choice!")

    print(f"Using {txt_dir_in} and {pcap_dir_in} as input")
    logging.info(f"Using {txt_dir_in} and {pcap_dir_in} as input")

    print("Making APX dataframe...")
    csv_files = [f for f in sorted(os.listdir(txt_dir_in)) if fn_keyword in f]
    print(f"Found {len(csv_files)} CSV files.")
    big_apx_df = pd.concat((pd.read_csv(os.path.join(txt_dir_in, f), sep = ";", index_col = 0, na_values = "NAN") for f in csv_files))
    big_apx_df = big_apx_df[["lon_EPSG32632", "lat_EPSG32632", "elevation", "heading_continuous", "roll", "pitch"]]
    
    # print(big_apx_df.head())
    # somehow there might be duplicate times??
    big_apx_df = big_apx_df[~big_apx_df.index.duplicated(keep='first')]
    bad_rows = np.isnan(big_apx_df).sum(axis=1) > 0
    big_apx_df = big_apx_df[~bad_rows]

    f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit = apx_df_to_interp_funcs(big_apx_df)
    f_yaw_corr = yaw_correction_to_interp_func() # returns interpolation function or None
    if f_yaw_corr is None:
        print('NO yaw correction')
    else:
        print ('USING yaw correction')

    print(f"Done concatenating {len(csv_files)} CSV files into dataframe.")
    print("\n\n\n")
    logging.info(f"Used {csv_files} from {txt_dir_in}")



    # pcap_to_uav_coord('line_001_point_0000.pcap', pcap_dir_in, '343040.693 5709133.784 28.559   1 1 171 1 614249080.422688961 -18.780 -12.399 8 28.559')
    # curr_str = '343040.693 5709133.784 28.559   1 1 171 1 614249080.422688961 -18.780 1880422689024 8 28.559'
    # uav_coords = pcap_to_uav_coord('line_001_point_0000.pcap', pcap_dir_in, curr_str)
    # print(uav_coords)

    # lat, lon, ele, yaw, rol, pit = apx_funcs_to_interp_values(curr_str, f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit)
    # print(lat, lon, ele, yaw, rol, pit)
    # pcap_to_uav_coord('line_001_point_0001.pcap', pcap_dir_in, '343040.717 5709133.886 28.578   1 1 177 1 614249080.122606993 -18.267 1880122606904 12 28.578')

    pcap_files_to_process = sorted([fn for fn in os.listdir(pcap_dir_in) \
                            if len(fn)==24 and fn.endswith('.pcap')])
    xyz_lines_to_process = []
    xyz_files_to_process = sorted([fn for fn in os.listdir(xyz_dir_in)\
                            if len(fn)==12 and fn.endswith('.xyz')])
    for xyz_file in xyz_files_to_process:
        with open(os.path.join(xyz_dir_in, xyz_file), 'r') as fh:
            curr_lines = fh.readlines()
        xyz_lines_to_process.extend(curr_lines)

    assert len(pcap_files_to_process)==len(xyz_lines_to_process), f'nr of PCAP files and strings do not match ({len(pcap_files_to_process)} vs {len(xyz_lines_to_process)})'
    n_rows = len(pcap_files_to_process)
    n_cols = 3 + 3 + 6 + 1 #xyz pulse, xyz drone, sin/cos rotation, yaw correction
    output = np.ones((n_rows, n_cols), dtype = np.float64) * np.nan
    temp_angles = np.ones((n_rows, 3), dtype = np.float64) * np.nan
    for pcap_file, xyz_line, row_number in zip(pcap_files_to_process, xyz_lines_to_process, np.arange(n_rows)):
        print(row_number)
        uav_coords = pcap_to_uav_coord(pcap_file, pcap_dir_in, xyz_line)
        if uav_coords is not None:
            uav_coords = uav_coords.flatten()
            lat, lon, ele, yaw, rol, pit = apx_funcs_to_interp_values(xyz_line, f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit)
            output[row_number][:3] = uav_coords
            output[row_number][3:6] = lat, lon, ele
            temp_angles[row_number][:] = yaw, rol, pit
            yaw_correction = yaw_corr_func_to_interp_value(xyz_line, f_yaw_corr)
            output[row_number][12] = yaw_correction
    for i in range(temp_angles.shape[1]):
        #6 columns with xyz uav and lon, lat, elev
        col0 = 6 + i*2
        col1 = col0 + 1
        print(col0, col1, temp_angles[0,i])
        output[:,col0] = np.sin(temp_angles[:,i])
        output[:,col1] = np.cos(temp_angles[:,i])
    
    if np.isnan(output).any():
        print('WARNING bad output.')
    np.savetxt(os.path.join(out_dir_ascii, 'human_readable.txt'), output)
    np.save(os.path.join(out_dir_ascii, 'point_infos.npy'), output, False)
#!/usr/bin/env python
# coding: utf-8

"""
    This script should parse the pcap and csv files and find
    the required info for each return from the .xyz files.
    Then save it in 1 place...
"""

import sys, os, logging, re, datetime
import numpy as np
import pandas as pd
from vlp16_tables import *
from scipy.interpolate import interp1d

log_dir = 'p_logging'
txt_dir_in = 'p_01_apx_csv_shapefile'
txt_in_base_len = len(txt_dir_in)
pcap_dir_in = 'p_22_likely_gcp_pcap'
pcap_in_base_len = len(pcap_dir_in)
xyz_dir_in = 'p_20_likely_gcp'
xyz_dir_in_ref = 'p_21_refined_gcp'
out_dir_ascii = 'p_24_likely_gcp_infos'
out_ascii_base_len = len(out_dir_ascii)
fn_keyword = 'hi_freq_apx'

def remove_min_sec(ts):
    return (int(ts) // 3600) * 3600

# ### Function to calculate the gaps between given azimuths. Needed to interpolate azimuths that are not given.

def get_azim_gap(azimuths, dual = True, preserve_shape = False):
    """
        Only works for dual returns now.
        
        preserve_shape is relevant for dual, where the azimuths repeat.
        if False: return only unique gaps.
        if True: return same shape as azimuths
    """
    if dual:
        azimuths_gap_flat = np.zeros_like(azimuths[0::2]).flatten()
        azimuths_gap_flat[:-1] = ((azimuths[0::2].flatten()[1:] - azimuths[0::2].flatten()[:-1]) % 36000)
        azimuths_gap_flat[-1] = azimuths_gap_flat[-2]
        azimuths_gap = azimuths_gap_flat.reshape(azimuths[0::2].shape)
        if preserve_shape:
            #either of the following lines should work the same. only use one.
            #azimuths_gap = np.hstack((azimuths_gap.reshape((azimuths_gap.size,1)), azimuths_gap.reshape((azimuths_gap.size,1)))).flatten().reshape(azimuths.shape)
            azimuths_gap = np.tile(azimuths_gap,2)
        return azimuths_gap
    else:
        raise NotImplementedError

def get_micros_pulses(micros, dual = True, preserve_shape = False):
    """
        preserve_shape is relevant for dual, where the azimuths repeat.
        if False: return only unique gaps.
        if True: return same shape as azimuths
    """
    if dual:
        if preserve_shape:
            micros_pulses = np.expand_dims(micros, axis=1) + TIMING_OFFSETS_DUAL.T.flatten() * 1e6
        else:
            micros_pulses = np.expand_dims(micros, axis=1) + TIMING_OFFSETS_DUAL.T[0::2,:].flatten() * 1e6
    else:
        micros_pulses = np.expand_dims(micros, axis=1) + TIMING_OFFSETS_SINGLE.T.flatten() * 1e6
    
    MICROS_PER_HOUR = np.array([1e6 * 3600]).astype(np.int64)[0]
    micros_pulses %= MICROS_PER_HOUR
    return micros_pulses

def get_nanos_pulses(micros, dual = True, preserve_shape = False):
    """
        Get the nanoseconds from the top of the hour for each laser pulse. Need for boresight!
        preserve_shape is relevant for dual, where the azimuths repeat.
        if False: return only unique gaps.
        if True: return same shape as azimuths
    """
    nanos = np.copy(micros) 
    nanos *= 1000
    if dual:
        if preserve_shape:
            nanos_pulses = np.expand_dims(nanos, axis=1) + (TIMING_OFFSETS_DUAL_nano.T.flatten()).astype(np.int64)
        else:
            nanos_pulses = np.expand_dims(nanos, axis=1) + (TIMING_OFFSETS_DUAL_nano.T[0::2,:].flatten()).astype(np.int64)
    else:
        nanos_pulses = np.expand_dims(nanos, axis=1) + (TIMING_OFFSETS_SINGLE_nano.T.flatten()).astype(np.int64)
    NANOS_PER_HOUR = np.array([1e9 * 3600]).astype(np.int64)[0]
    nanos_pulses %= NANOS_PER_HOUR
    return nanos_pulses

def get_precision_azimuth(az_simple, azimuths_gap, dual = True, minimal_shape = True):
    az_simple = az_simple[np.newaxis, :]
    azimuths_gap = azimuths_gap[np.newaxis,:]
    if dual:
        timing_offsets_within_block = TIMING_OFFSETS_DUAL[:,0]
        az_pulses = np.tile(az_simple,(LASERS_PER_DATA_BLOCK)).reshape(az_simple.shape[0], LASERS_PER_DATA_BLOCK, az_simple.shape[1])
        az_pulses = az_pulses.transpose((0,2,1))
        precision_azimuth = az_pulses[:,:,:] + timing_offsets_within_block / (2 * T_CYCLE) * np.expand_dims(azimuths_gap, axis=2)
        precision_azimuth = precision_azimuth % 36000
        if not minimal_shape:
            precision_azimuth = np.tile(precision_azimuth.transpose((0,2,1)), (1,2,1)).transpose((0,2,1))
        precision_azimuth = precision_azimuth.reshape((precision_azimuth.shape[0], precision_azimuth.shape[1] * precision_azimuth.shape[2]))
        return precision_azimuth[0,:]
    else:
        raise NotImplementedError

def get_azimuth_of_interest(loc_raw_data, loc_packet_pos, loc_pulse_pos):
    #azimuth
    azimuths = np.zeros((DATA_BLOCKS, 1), dtype = np.int32)
    azim_data = azimuths.view(dtype = np.uint8)
    azim_data[:,0:2] = loc_raw_data[loc_packet_pos, :-6].reshape(DATA_BLOCKS, 100)[:, 2:4]
    azim_data = azim_data.reshape((DATA_BLOCKS * 4))
    azimuths = azim_data.view(dtype= np.int32)
    azimuths_gap = get_azim_gap(azimuths)
    # ### Calculate the azimutzhs for each datapoint
    #Use the following simplified array if in dual mode
    #Otherwise can still refer to it, but it's just the original array
    mode_hypothesis = loc_raw_data[0, RETURN_MODE_OFFSET]
    if mode_hypothesis == RETURN_MODE_DUAL:
        # az_simple = azimuths[:,0::2]
        az_simple = azimuths[0::2]
    else:
        az_simple = azimuths
    prec_az = get_precision_azimuth(az_simple, azimuths_gap, True, True)
    the_azimuth_rad = np.deg2rad(prec_az[loc_pulse_pos] / 100)
    return the_azimuth_rad

def make_puck_ranges_mm(channel_data):
    puck_ranges_mm = np.zeros((DATA_BLOCKS * LASERS_PER_DATA_BLOCK), dtype = np.uint32)
    puck_range_bytes = puck_ranges_mm.view(dtype = np.uint8)
    puck_range_bytes[0::4] = channel_data[0::3]
    puck_range_bytes[1::4] = channel_data[1::3]
    puck_ranges_mm *= 2
    return puck_ranges_mm

def make_puck_intensities(channel_data):
    puck_intens = np.zeros((DATA_BLOCKS * LASERS_PER_DATA_BLOCK), dtype = np.uint8)
    puck_intens[:] = channel_data[2::3]
    return puck_intens

def puck_shperical_to_cartesian(puck_range, alpha, omega, vert_corr):
    """
        input: range, alpha, omega, vert_corr
        output:
        xyz in puck coordinates
    """
    X_puck = puck_range * np.cos(omega) * np.sin(alpha)
    Y_puck = puck_range * np.cos(omega) * np.cos(alpha)
    Z_puck = puck_range * np.sin(omega)
    Z_puck += vert_corr
    return np.vstack((X_puck, Y_puck, Z_puck)).T

def puck_to_uav_coords(XYZ_puck):
    # first rotate into XYZ of the drone!
    # x_roll = -90 #degrees
    # y_pitch = 0
    # z_yaw = -90
    #rotation from puck to uav coordinates:
    R_01 = np.array([[0., 1., 0.],
                    [0., 0., 1.],
                    [1., 0., 0.]])

    XYZ_uav = np.matmul(XYZ_puck, R_01)
    return XYZ_uav

def pcap_to_uav_coord(pcap_file_in, pcap_dir_in, xyz_str):
    """
        Process a point upto the return coordinates in uav frame
        Input:
            1 pcap file, where the point of interest is roughly in the middle
            pcap directory name
            string corresponding to the point, coming from the xyz file
    """
    with open(os.path.join(pcap_dir_in, pcap_file_in), "rb") as fh:
        raw_pcap = fh.read()
        #24 bytes = global header, 16+42 bytes = packet header, 1200 bytes = vlp returns, 2 bytes = vlp factory bytes
        raw_data = np.frombuffer(raw_pcap, dtype = np.uint8)[24:].reshape((len(raw_pcap)//1264,1264))[:,16+42:]

    target_nano = (np.array([xyz_str.split()[9]])).astype(np.int64)[0]
    target_channel = int(xyz_str.split()[10])
    target_intensity = int(xyz_str.split()[5])


    # #### Take Ethernet timestamp of (1st) packet, discard sub-hour info and add replace it with that from GNSS µs timestamp
    # 
    # What happens when the capture rolls over a full hour?
    # 
    # **Need to deal with this when such data is captured!**
    # 
    # # Solution below!
    micros = np.zeros((raw_data.shape[0],), dtype = np.int64)
    micro_bytes = micros.view(dtype = np.uint8)
    micro_bytes[0::8] = raw_data[:, DATA_PACK_TIMESTAMP_OFFSET + 0]
    micro_bytes[1::8] = raw_data[:, DATA_PACK_TIMESTAMP_OFFSET + 1]
    micro_bytes[2::8] = raw_data[:, DATA_PACK_TIMESTAMP_OFFSET + 2]
    micro_bytes[3::8] = raw_data[:, DATA_PACK_TIMESTAMP_OFFSET + 3]

    # #### Another problem could be that the UDP packets are not guaranteed to arrive in order.
    # 
    # An assumption that is made for the following calculations is that this does not happen.
    # 
    # **Need to deal with this when such data is captured!**
    while (micros[1:] < micros[:-1]).sum() > 0:
        jump_position = np.where((micros[1:] < micros[:-1]))[0][0] + 1
        micros[jump_position:] += int(3.6e9)

    nanos_pulses = get_nanos_pulses(micros)

    precise_location = np.where(nanos_pulses == target_nano)
    if precise_location[0].size == 0: #wrong packet?!
        return None
    
    the_packet_pos = precise_location[0][0]
    the_pulse_pos = precise_location[1][0]
    assert target_channel == the_pulse_pos % 16, f'Target channel does not match. xyz_str: {xyz_str}. the_pulse_pos: {the_pulse_pos}'

    # the following contains only channel data (i.e. no timestamp, factory bytes or azimuth)
    channel_data = raw_data[the_packet_pos,:-6].reshape((1, DATA_BLOCKS, 100))[:,:,4:]
    # channel_data = channel_data.reshape((1, DATA_BLOCKS * LASERS_PER_DATA_BLOCK * 3))
    channel_data = channel_data.reshape((DATA_BLOCKS * LASERS_PER_DATA_BLOCK * 3))

    #puck ranges in mm
    puck_ranges = make_puck_ranges_mm(channel_data)

    #intensities as 1 byte
    puck_intens = make_puck_intensities(channel_data)


    # strongest and last ranges (meters) + intensities
    strongest_return_ranges = puck_ranges.reshape((DATA_BLOCKS, LASERS_PER_DATA_BLOCK))[1::2].flatten() / 1000
    strongest_return_intensities = puck_intens.reshape((DATA_BLOCKS, LASERS_PER_DATA_BLOCK))[1::2].flatten()
    last_return_ranges = puck_ranges.reshape((DATA_BLOCKS, LASERS_PER_DATA_BLOCK))[0::2].flatten() / 1000
    last_return_intensities = puck_intens.reshape((DATA_BLOCKS, LASERS_PER_DATA_BLOCK))[0::2].flatten()

    #range of the point of interest
    the_return_id = int(xyz_str.split()[4])
    if the_return_id==1:
        the_precise_range = strongest_return_ranges[the_pulse_pos]
        assert strongest_return_intensities[the_pulse_pos]==target_intensity,\
            f'Target intensity not matched! xyz_str: {xyz_str}. This intensity: {strongest_return_intensities[the_pulse_pos]}'
    elif the_return_id==2:
        the_precise_range = last_return_ranges[the_pulse_pos]
        assert last_return_intensities[the_pulse_pos]==target_intensity,\
            f'Target intensity not matched! xyz_str: {xyz_str}. This intensity: {last_return_intensities[the_pulse_pos]}'
    else:
        raise ValueError(f'return ID > 2! ({the_return_id}). xyz_str: {xyz_str}')
    assert the_precise_range > 0, f'Range is not larger than 0! xyz_str: {xyz_str}'

    the_azimuth = get_azimuth_of_interest(raw_data, the_packet_pos, the_pulse_pos)
    elev_angle_and_corr = elevation_and_vert_corr_by_laser_id[target_channel]
    the_elev_angle = elev_angle_and_corr[0]
    the_vert_corr = elev_angle_and_corr[1]

    puck_xyz = puck_shperical_to_cartesian(the_precise_range, the_azimuth, the_elev_angle, the_vert_corr)
    uav_xyz = puck_to_uav_coords(puck_xyz)

    return uav_xyz

def apx_df_to_interp_funcs(apx_df):
    """
        Makes 6 functions that are used to interpolate lon, lat, ele,
        rol, pitch and yaw for any time that is covered by the apx dataset.
    """
    f_lat = interp1d(apx_df.index, apx_df["lat_EPSG32632"], kind = 'cubic', fill_value="extrapolate")
    f_lon = interp1d(apx_df.index, apx_df["lon_EPSG32632"], kind = 'cubic', fill_value="extrapolate")
    f_ele = interp1d(apx_df.index, apx_df["elevation"], kind = 'cubic', fill_value="extrapolate")
    f_yaw = interp1d(apx_df.index, apx_df["heading_continuous"], kind = 'cubic', fill_value="extrapolate")
    f_rol = interp1d(apx_df.index, apx_df["roll"], kind = 'cubic', fill_value="extrapolate")
    f_pit = interp1d(apx_df.index, apx_df["pitch"], kind = 'cubic', fill_value="extrapolate")
    return f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit

def apx_funcs_to_interp_values(xyz_str, f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit):
    """
        Uses the 6 functions made above to provide lon, lat, ... for a specific timestamp
    """
    the_time = float(xyz_str.split()[7]) + 1e9
    lat = f_lat(the_time)
    lon = f_lon(the_time)
    ele = f_ele(the_time)
    yaw = -np.deg2rad(f_yaw(the_time) % 360)
    rol = -np.deg2rad(f_rol(the_time))
    pit = -np.deg2rad(f_pit(the_time))
    return lat, lon, ele, yaw, rol, pit

def yaw_correction_to_interp_func():
    """
        Makes 1 function that is used to interpolate yaw correction is present.
        'yaw_correction.csv' must be present in p_03_pcap!
    """
    if 'yaw_correction.csv' in os.listdir('p_03_pcap'):
        yaw_df = pd.read_csv(os.path.join('p_03_pcap', 'yaw_correction.csv'), index_col=0)
    else:
        return None

    f_yaw_corr = interp1d(yaw_df.index, yaw_df["smooth_yaw_err"], kind = 'cubic', fill_value="extrapolate")

    return f_yaw_corr

def yaw_corr_func_to_interp_value(xyz_str, f_yaw_corr):
    """
        Uses the function made above to provide yaw correction for a specific timestamp
    """
    if f_yaw_corr is None:
        return 0
    
    the_time = float(xyz_str.split()[7]) + 1e9
    yaw_corr = np.deg2rad((f_yaw_corr(the_time) +360) % 360)
    
    return yaw_corr

if __name__=='__main__':
    this_script = sys.argv[0]

    if len(sys.argv)<=1:
        print("ERROR\n\nPlease provide the absolute path of the campaign's top directory.\n\nEXITING")
        sys.exit(0)
    else:
        try: top_dir = sys.argv[1].rstrip(os.path.sep)
        except Exception as e: print(e); sys.exit(0)

    if sys.byteorder == 'little':
        print(f"System endianness: {sys.byteorder}.\nGood to go :)")
    else:
        print(f"System endianness: {sys.byteorder}.\nSTOP right now!")
        sys.exit(0)

    os.chdir(top_dir)
    if os.getcwd() != top_dir:
        print("Something went wrong.")
        print(f"cwd: {os.getcwd()}")
        sys.exit(0)
    
    # if refined files are available
    if xyz_dir_in_ref in os.listdir():
        xyz_dir_in = xyz_dir_in_ref
    
    if log_dir not in os.listdir():
        os.mkdir(log_dir)

    if out_dir_ascii not in os.listdir():
        os.mkdir(out_dir_ascii)

    logging.basicConfig(format='<%(asctime)s>   <%(levelname)-8s>   <%(message)s>',
                        level=logging.ERROR,
                        filename=os.path.join(top_dir, log_dir, f'{datetime.date.today().strftime("%Y.%m.%d")}_{os.path.split(this_script)[-1]}.log'),
                        datefmt='%Y-%m-%d %H:%M:%S')
    logging.info(f"{this_script} has started and received a valid top directory name")

    potential_dirs_in = len(re.findall(txt_dir_in, " ".join(os.listdir())))
    if potential_dirs_in == 0:
        print("No valid input directories found. Make sure to run script 00_join_pcap_.. first\nEXITING!")
        sys.exit(0)
    elif potential_dirs_in > 1:
        print("Multiple input directories found. Choose one:")
        valid_choices = dict()
        for i, d_name in enumerate(os.listdir()):
            if os.path.isfile(d_name):
                continue
            elif txt_dir_in not in d_name:
                continue
            else:
                print(f"{i}: {d_name}")
                valid_choices[i] = d_name

        dir_in_selected = False
        while not dir_in_selected:
            try:
                dir_in_id = int(input("Which directory do we use? Type a number and press enter.\n"))
                if dir_in_id in valid_choices.keys():
                    txt_dir_in = valid_choices[dir_in_id]
                    dir_in_selected = True
            except:
                print("Invalid choice!")
    potential_pcap_dirs_in = len(re.findall(pcap_dir_in, " ".join(os.listdir())))
    if potential_pcap_dirs_in == 0:
        print("No valid PCAP input directories found. Make sure to run the other scripts first\nEXITING!")
        sys.exit(0)
    elif potential_pcap_dirs_in > 1:
        print("Multiple input directories found. Choose one:")
        valid_choices = dict()
        for i, d_name in enumerate(os.listdir()):
            if os.path.isfile(d_name):
                continue
            elif pcap_dir_in not in d_name:
                continue
            else:
                print(f"{i}: {d_name}")
                valid_choices[i] = d_name

        dir_in_selected = False
        while not dir_in_selected:
            try:
                dir_in_id = int(input("Which directory do we use? Type a number and press enter.\n"))
                if dir_in_id in valid_choices.keys():
                    pcap_dir_in = valid_choices[dir_in_id]
                    dir_in_selected = True
            except:
                print("Invalid choice!")

    print(f"Using {txt_dir_in} and {pcap_dir_in} as input")
    logging.info(f"Using {txt_dir_in} and {pcap_dir_in} as input")

    print("Making APX dataframe...")
    csv_files = [f for f in sorted(os.listdir(txt_dir_in)) if fn_keyword in f]
    print(f"Found {len(csv_files)} CSV files.")
    big_apx_df = pd.concat((pd.read_csv(os.path.join(txt_dir_in, f), sep = ";", index_col = 0, na_values = "NAN") for f in csv_files))
    big_apx_df = big_apx_df[["lon_EPSG32632", "lat_EPSG32632", "elevation", "heading_continuous", "roll", "pitch"]]
    
    # print(big_apx_df.head())
    # somehow there might be duplicate times??
    big_apx_df = big_apx_df[~big_apx_df.index.duplicated(keep='first')]
    bad_rows = np.isnan(big_apx_df).sum(axis=1) > 0
    big_apx_df = big_apx_df[~bad_rows]

    f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit = apx_df_to_interp_funcs(big_apx_df)
    f_yaw_corr = yaw_correction_to_interp_func() # returns interpolation function or None
    if f_yaw_corr is None:
        print('NO yaw correction')
    else:
        print ('USING yaw correction')

    print(f"Done concatenating {len(csv_files)} CSV files into dataframe.")
    print("\n\n\n")
    logging.info(f"Used {csv_files} from {txt_dir_in}")



    # pcap_to_uav_coord('line_001_point_0000.pcap', pcap_dir_in, '343040.693 5709133.784 28.559   1 1 171 1 614249080.422688961 -18.780 -12.399 8 28.559')
    # curr_str = '343040.693 5709133.784 28.559   1 1 171 1 614249080.422688961 -18.780 1880422689024 8 28.559'
    # uav_coords = pcap_to_uav_coord('line_001_point_0000.pcap', pcap_dir_in, curr_str)
    # print(uav_coords)

    # lat, lon, ele, yaw, rol, pit = apx_funcs_to_interp_values(curr_str, f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit)
    # print(lat, lon, ele, yaw, rol, pit)
    # pcap_to_uav_coord('line_001_point_0001.pcap', pcap_dir_in, '343040.717 5709133.886 28.578   1 1 177 1 614249080.122606993 -18.267 1880122606904 12 28.578')

    pcap_files_to_process = sorted([fn for fn in os.listdir(pcap_dir_in) \
                            if len(fn)==24 and fn.endswith('.pcap')])
    xyz_lines_to_process = []
    xyz_files_to_process = sorted([fn for fn in os.listdir(xyz_dir_in)\
                            if len(fn)==12 and fn.endswith('.xyz')])
    for xyz_file in xyz_files_to_process:
        with open(os.path.join(xyz_dir_in, xyz_file), 'r') as fh:
            curr_lines = fh.readlines()
        xyz_lines_to_process.extend(curr_lines)

    assert len(pcap_files_to_process)==len(xyz_lines_to_process), f'nr of PCAP files and strings do not match ({len(pcap_files_to_process)} vs {len(xyz_lines_to_process)})'
    n_rows = len(pcap_files_to_process)
    n_cols = 3 + 3 + 6 + 1 #xyz pulse, xyz drone, sin/cos rotation, yaw correction
    output = np.ones((n_rows, n_cols), dtype = np.float64) * np.nan
    temp_angles = np.ones((n_rows, 3), dtype = np.float64) * np.nan
    for pcap_file, xyz_line, row_number in zip(pcap_files_to_process, xyz_lines_to_process, np.arange(n_rows)):
        print(row_number)
        uav_coords = pcap_to_uav_coord(pcap_file, pcap_dir_in, xyz_line)
        if uav_coords is not None:
            uav_coords = uav_coords.flatten()
            lat, lon, ele, yaw, rol, pit = apx_funcs_to_interp_values(xyz_line, f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit)
            output[row_number][:3] = uav_coords
            output[row_number][3:6] = lat, lon, ele
            temp_angles[row_number][:] = yaw, rol, pit
            yaw_correction = yaw_corr_func_to_interp_value(xyz_line, f_yaw_corr)
            output[row_number][12] = yaw_correction
    for i in range(temp_angles.shape[1]):
        #6 columns with xyz uav and lon, lat, elev
        col0 = 6 + i*2
        col1 = col0 + 1
        print(col0, col1, temp_angles[0,i])
        output[:,col0] = np.sin(temp_angles[:,i])
        output[:,col1] = np.cos(temp_angles[:,i])
    
    if np.isnan(output).any():
        print('WARNING bad output.')
    np.savetxt(os.path.join(out_dir_ascii, 'human_readable.txt'), output)
    np.save(os.path.join(out_dir_ascii, 'point_infos.npy'), output, False)
#!/usr/bin/env python # coding: utf-8 This script should parse the pcap and csv files and find     the required info for each return from the .xyz files.     Then save it in 1 place... # ### Function to calculate the gaps between given azimuths. Needed to interpolate azimuths that are not given. Only works for dual returns now.                  preserve_shape is relevant for dual, where the azimuths repeat.         if False: return only unique gaps.         if True: return same shape as azimuths #either of the following lines should work the same. only use one. #azimuths_gap = np.hstack((azimuths_gap.reshape((azimuths_gap.size,1)), azimuths_gap.reshape((azimuths_gap.size,1)))).flatten().reshape(azimuths.shape) preserve_shape is relevant for dual, where the azimuths repeat.         if False: return only unique gaps.         if True: return same shape as azimuths Get the nanoseconds from the top of the hour for each laser pulse. Need for boresight!         preserve_shape is relevant for dual, where the azimuths repeat.         if False: return only unique gaps.         if True: return same shape as azimuths #azimuth # ### Calculate the azimutzhs for each datapoint #Use the following simplified array if in dual mode #Otherwise can still refer to it, but it's just the original array # az_simple = azimuths[:,0::2] input: range, alpha, omega, vert_corr         output:         xyz in puck coordinates # first rotate into XYZ of the drone! # x_roll = -90 #degrees # y_pitch = 0 # z_yaw = -90 #rotation from puck to uav coordinates: Process a point upto the return coordinates in uav frame         Input:             1 pcap file, where the point of interest is roughly in the middle             pcap directory name             string corresponding to the point, coming from the xyz file #24 bytes = global header, 16+42 bytes = packet header, 1200 bytes = vlp returns, 2 bytes = vlp factory bytes # #### Take Ethernet timestamp of (1st) packet, discard sub-hour info and add replace it with that from GNSS µs timestamp # # What happens when the capture rolls over a full hour? # # **Need to deal with this when such data is captured!** # # # Solution below! # #### Another problem could be that the UDP packets are not guaranteed to arrive in order. # # An assumption that is made for the following calculations is that this does not happen. # # **Need to deal with this when such data is captured!** #wrong packet?! # the following contains only channel data (i.e. no timestamp, factory bytes or azimuth) # channel_data = channel_data.reshape((1, DATA_BLOCKS * LASERS_PER_DATA_BLOCK * 3)) #puck ranges in mm #intensities as 1 byte # strongest and last ranges (meters) + intensities #range of the point of interest Makes 6 functions that are used to interpolate lon, lat, ele,         rol, pitch and yaw for any time that is covered by the apx dataset. Uses the 6 functions made above to provide lon, lat, ... for a specific timestamp Makes 1 function that is used to interpolate yaw correction is present.         'yaw_correction.csv' must be present in p_03_pcap! Uses the function made above to provide yaw correction for a specific timestamp # if refined files are available # print(big_apx_df.head()) # somehow there might be duplicate times?? # returns interpolation function or None # pcap_to_uav_coord('line_001_point_0000.pcap', pcap_dir_in, '343040.693 5709133.784 28.559   1 1 171 1 614249080.422688961 -18.780 -12.399 8 28.559') # curr_str = '343040.693 5709133.784 28.559   1 1 171 1 614249080.422688961 -18.780 1880422689024 8 28.559' # uav_coords = pcap_to_uav_coord('line_001_point_0000.pcap', pcap_dir_in, curr_str) # print(uav_coords) # lat, lon, ele, yaw, rol, pit = apx_funcs_to_interp_values(curr_str, f_lat, f_lon, f_ele, f_yaw, f_rol, f_pit) # print(lat, lon, ele, yaw, rol, pit) # pcap_to_uav_coord('line_001_point_0001.pcap', pcap_dir_in, '343040.717 5709133.886 28.578   1 1 177 1 614249080.122606993 -18.267 1880122606904 12 28.578') #xyz pulse, xyz drone, sin/cos rotation, yaw correction #6 columns with xyz uav and lon, lat, elev