#!/usr/bin/env python3

import pandas as pd
import numpy as np
import os
import argparse
import sys
import subprocess
import csv
import matplotlib.pyplot as plt

# For some gneric utiltities: json_read, json_write, timer
import montera.gutil as gutil
import montera.pos_lib as pos_lib
pos_lib.ant2ap_mode = 'single_file'


reader = csv.DictReader(open('center.csv'))
center = next(reader) 
c_lat = float(center['lat'])
c_lon = float(center['lon'])
c_hgt = float(center['hgt'])

df_ew = pd.read_csv('truth_240217.csv')
row_east = df_ew.iloc[0]
row_west = df_ew.iloc[1]

folders = [
    #'240216_first_pos_not_used',
    '240217_first_pos',
    '240218_first_pos',
    '240219_swap_pos',
    '240220_swap_pos',
    #'240222_first_pos_N_not_used',
    '240223_first_pos_N',
    #'240224_first_pos_N_include_shift',
    '240225_first_pos_N_after_shift',
    #'240226_first_pos_N_not_used',
    '240305_swap_pos_N',
    '240306_swap_pos_N',
    '240307_swap_pos_N',
    ]
os.makedirs('figures', exist_ok=True)

window_size = 200

for folder in folders:
    df = pd.read_feather(os.path.join(folder,'gen_data', 'position', 'BLDG2BD',
                                      'BLDG2BD_ant0_local.feather'
                                      ))

    df = df.drop(['RefLat', 'RefLon', 'RefHgt',
                  'MSigEN', 'MSigEU', 'MSigNU',
                  'MHDOP', 'MVDOP', 'MTDOP', 'MRMS', 'MUnitSTD',
                  'Measting', 'Mnorthing', 'Muping', 'Mhdist_from_mean',
                  'dist',
                  'VSigEN', 'VSigEU', 'VSigNU',
                  'VHDOP', 'VVDOP', 'VTDOP', 'VRMS', 'VUnitSTD',
                  'VRefLat', 'VRefLon', 'VRefHgt',
                  ],
                 axis='columns')

    #df['Mdist'] = pos_lib.dist_m(c_lat, c_lon, df['MLat'], df['MLon'])
    df['Measting'], df['Mnorthing'], df['Mup'] =  pos_lib.llh2enu(
                            np.array([c_lat, c_lon, c_hgt]),
                            np.array(df[['MLat', 'MLon', 'MHgt']]),
                            is_rad=False,
                            is_ref_rad=False,
                            )
    df['Veasting'], df['Vnorthing'], df['Vup'] =  pos_lib.llh2enu(
                            np.array([c_lat, c_lon, c_hgt]),
                            np.array(df[['VLat', 'VLon', 'VHgt']]),
                            is_rad=False,
                            is_ref_rad=False,
                            )
    df['MEmean'] = df.Measting.rolling(window_size).mean()
    df['VEmean'] = df.Veasting.rolling(window_size).mean()
    df['MUmean'] = df.Mup.rolling(window_size).mean()
    df['VUmean'] = df.Vup.rolling(window_size).mean()
    #df['MNmean'] = df.Mnorthing.rolling(window_size).mean()

    #csv_fname = os.path.join('figures', folder + '.csv')
    #print(f'Saving {csv_fname}')
    #df.to_csv(csv_fname, index=False)


    m = np.max(np.array([df.Measting.abs().max(), df.Mnorthing.abs().max(),
                         df.Veasting.abs().max(), df.Vnorthing.abs().max(),
                         ]))

    # ----------------------------------------
    fig, ax = plt.subplots(1, 1, figsize=[12,5])
    # ----------------------------------------
    df.plot('Measting', 'Mnorthing',
            ax=ax,
            marker='+', color='b',
            linewidth=0,
            label='Ant 0')
    df.plot('Veasting', 'Vnorthing',
            ax=ax,
            marker='x', color='r',
            linewidth=0,
            label='Ant 1')
    plt.legend()
    plt.xlim([-m, m])
    plt.ylim([-0.1, 0.1])
    plt.xlabel('Easting (m)')
    plt.ylabel('Northing (m)')
    plt.title('CSNMR-2963 Z3R Swap Pos ' + folder)
    fig_fname = os.path.join('figures', folder + '_c_ne.png')
    print(f"saving {fig_fname}")
    plt.savefig(fig_fname)
    plt.close(fig)


    # ----------------------------------------
    fig, axes = plt.subplots(2, 1, figsize=[12,5])
    # ----------------------------------------
    axes[0].plot(df.adjTime, df.Measting, 'bx', label='M Easting')
    axes[0].plot(df.adjTime, df.MEmean, 'c-',
                 label=f'M Easting Mean window={window_size}')
    axes[0].set_xlabel('Time (s)')
    axes[0].set_ylabel('Primary Easting (m)')
    axes[0].legend()

    axes[1].plot(df.adjTime, df.Veasting, 'rx', label='V Easting')
    axes[1].plot(df.adjTime, df.VEmean, 'm-',
                 label=f'V Easting Mean window={window_size}')
    axes[1].set_ylabel('Secondary Easting (m)')
    axes[1].legend()

    fig_fname = os.path.join('figures', folder + '_c_te.png')
    print(f"saving {fig_fname}")
    plt.savefig(fig_fname)
    plt.close(fig)

    # ----------------------------------------
    fig, axes = plt.subplots(2, 1, figsize=[12,5])
    # ----------------------------------------
    axes[0].plot(df.adjTime, df.Mup, 'bx', label='M dUp')
    axes[0].plot(df.adjTime, df.MUmean, 'c-',
                 label=f'M dUp Mean window={window_size}')
    axes[0].set_xlabel('Time (s)')
    axes[0].set_ylabel('Primary Easting (m)')
    axes[0].set_ylim([-0.04, 0.07])
    axes[0].legend()

    axes[1].plot(df.adjTime, df.Vup, 'rx', label='V dUp')
    axes[1].plot(df.adjTime, df.VUmean, 'm-',
                 label=f'V dUp Mean window={window_size}')
    axes[1].set_ylabel('Secondary Easting (m)')
    axes[1].set_ylim([-0.04, 0.07])
    axes[1].legend()

    fig_fname = os.path.join('figures', folder + '_c_tu.png')
    print(f"saving {fig_fname}")
    plt.savefig(fig_fname)
    plt.close(fig)


    """
    df['EMeasting'], df['EMnorthing'], df['EMup'] =  pos_lib.llh2enu(
                            np.array([row_east.lat, row_east.lon, row_east.hgt]),
                            np.array(df[['MLat', 'MLon', 'MHgt']]),
                            is_rad=False,
                            is_ref_rad=False,
                            )
    df['EVeasting'], df['EVnorthing'], df['EVup'] =  pos_lib.llh2enu(
                            np.array([row_east.lat, row_east.lon, row_east.hgt]),
                            np.array(df[['VLat', 'VLon', 'VHgt']]),
                            is_rad=False,
                            is_ref_rad=False,
                            )
    df['WMeasting'], df['WMnorthing'], df['WMup'] =  pos_lib.llh2enu(
                            np.array([row_west.lat, row_west.lon, row_west.hgt]),
                            np.array(df[['MLat', 'MLon', 'MHgt']]),
                            is_rad=False,
                            is_ref_rad=False,
                            )
    df['WVeasting'], df['WVnorthing'], df['WVup'] =  pos_lib.llh2enu(
                            np.array([row_west.lat, row_west.lon, row_west.hgt]),
                            np.array(df[['VLat', 'VLon', 'VHgt']]),
                            is_rad=False,
                            is_ref_rad=False,
                            )
    """
#if __name__ == '__main__':
#    main()