#!/usr/bin/env python3

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

import viewdat_cno_lib as vdl

import gutil
import pos_lib

def plot_heading(df, png_base_name=None):
    fig, ax = plt.subplots(2, 1, figsize=(10, 4), constrained_layout=True)
    #df.plot(x='time', y=['yaw', 'course_over_ground', 'heading_north'])
    ax[0].set_title(png_base_name)
    #ax[0].plot(df.ms, df.yaw, 'bx', 'linewidth', 1.0)
    ax[0].plot(df.ms, df.yaw, 'r.', 'linewidth', 0.25)
    ax[0].legend(['yaw'])
    ax[0].set_xlim([df.ms.min(), df.ms.max()])
    #ax[0].set_ylim([260, 290])
    ax[0].set_ylabel('Deg')

    #ax[1].plot(df.time, df.delta, 'g+')
    #ax[1].plot(df.time, df.heading_north - df.heading_north.mean(), 'bx')
    yaw_mean = df.yaw.mean()
    ax[1].plot(df.ms, df.yaw - yaw_mean, 'b+', label='yaw - mean')
    ax[1].plot(df.ms, np.sqrt(df.yaw_var), 'g.', label='+ sqrt(yaw var)')
    ax[1].plot(df.ms, -np.sqrt(df.yaw_var), 'g.', label='- sqrt(yaw var)')
    x = [df.ms.iloc[0], df.ms.iloc[-1]]
    yaw_std = df.yaw.std()
    ax[1].plot(x, [yaw_std, yaw_std],
            'c-', label='+ std(yaw)')
    ax[1].plot(x, [-yaw_std, -yaw_std],
            'c-', label='- std(yaw)')
    ax[1].legend()
    ax[1].set_xlim([df.ms.min(), df.ms.max()])
    #ax[1].set_ylim([-0.25, 0.25])
    ax[1].set_ylabel('Deg')

    fname = png_base_name + '_heading.png'
    print(f"Saving {fname}")
    plt.savefig(fname)
    plt.close(fig)


def main():
    df_gsof = pd.read_csv('outputs/gsof3_heading.csv')
    df_gsof['time'] = df_gsof.ms / 1000.0
    #df_nmea = pd.read_csv('outputs/nmea3.txt_nmea_heading.csv')
    df_t04 = pd.read_csv('outputs/P3-09_heading.csv')

    fig, ax= plt.subplots(2, 1)
    ylim = [240, 280]

    gsof_x = [df_gsof.time.iloc[0],df_gsof.time.iloc[-1]]
    gsof_mean = [df_gsof.yaw.mean(),df_gsof.yaw.mean()]
    ax[0].plot(df_gsof.time, df_gsof.yaw, 'b+')
    ax[0].plot(gsof_x, gsof_mean, 'r-', linewidth=0.5, label='mean')
    ax[0].set_ylim(ylim)

    t04_x = [df_t04.adj_time.iloc[0],df_t04.adj_time.iloc[-1]]
    t04_mean = [df_t04.heading.mean(),df_t04.heading.mean()]
    ax[1].plot(df_t04.adj_time, df_t04.heading, 'b+')
    ax[1].plot(t04_x, t04_mean, 'r-', linewidth=0.5, label='mean')
    ax[1].set_ylim(ylim)

    #ax[2].plot(df_nmea.time +111600, df_nmea.yaw, 'b+')
    plt.show()



if __name__ == '__main__':
    main()