import matplotlib.pyplot as plt
#import csv
import pandas as pd

import numpy as np


import argparse

def mapRange(value, inMin, inMax, outMin, outMax,constrain=False):
    if constrain:
        return max(outMin, min(outMax, outMin + (((value - inMin) / (inMax - inMin)) * (outMax - outMin))))
    else:
        return outMin + (((value - inMin) / (inMax - inMin)) * (outMax - outMin))


parser = argparse.ArgumentParser(description='Analyzes fixed csv logs from bobbycar')
parser.add_argument('-i', '--input', type=argparse.FileType('r'), required=True, help="input csv log file")
args = parser.parse_args()

df = pd.read_csv(args.input.name)

x = df['timestamp']
x = [i-x[0] for i in x] #offset time by starttime



def template():
    scattersize=1
    scatteralpha=0.1


    fig, ax1 = plt.subplots()

    ax2 = ax1.twinx()


    #plt.scatter(x,df['rpm_FrontL'], s=scattersize, alpha=scatteralpha, label="rpm_FrontL")
    ax1.plot(x,np.array(df['temp_Front']), c='#0000ff', alpha=0.5, label="temp_Front")
    ax1.plot(x,np.array(df['temp_Rear']), c='#ff0000', alpha=0.5, label="temp_Rear")
    ax1.plot(x,np.array(df['temp_Air']), c='#00ff00', alpha=0.5, label="temp_Air")
    #ax2.plot(x,np.array(df['throttle']), c='g', alpha=0.5, label="throttle")


    #confidence
    #confidence_FrontL=[abs(x-df['cmd_FrontL'][i-5 if i>4 else 0])+abs(x-df['cmd_FrontL'][i-10 if i>9 else 0])+abs(x-df['cmd_FrontL'][i-20 if i>19 else 0]) for i,x in enumerate(df['cmd_FrontL'])]
    #confidence_FrontL=[mapRange(x,0,50,1.0,0.0,True) for x in confidence_FrontL]

    
    #ax1.scatter(df['cmd_FrontL'],df['rpm_FrontL'], s=scattersize, alpha=scatteralpha, label="FrontL")
    #ax1.scatter(df['cmd_FrontR'],df['rpm_FrontR'], s=scattersize, alpha=scatteralpha, label="FrontR")
    #ax1.scatter(df['cmd_RearL'],df['rpm_RearL'], s=scattersize, alpha=scatteralpha, label="RearL")
    #ax1.scatter(df['cmd_RearR'],df['rpm_RearR'], s=scattersize, alpha=scatteralpha, label="RearR")

    ax1.set_xlabel('timestamp')
    #plt.ylabel('data')
    ax1.set_ylabel('first axis')
    ax2.set_ylabel('second axis')
    #plt.title('')
    ax1.legend(loc='upper left')
    ax2.legend(loc='upper right')
    plt.show()


def rpmDifference():
    scattersize=1
    scatteralpha=0.1


    fig, ax1 = plt.subplots()

    #confidence_FrontL=[abs(x-df['cmd_FrontL'][i-5 if i>4 else 0])+abs(x-df['cmd_FrontL'][i-10 if i>9 else 0])+abs(x-df['cmd_FrontL'][i-20 if i>19 else 0]) for i,x in enumerate(df['cmd_FrontL'])]
    frontRPMdiff=np.convolve(np.array(df['rpm_FrontL'])-np.array(df['rpm_FrontR']), np.ones(10)/10, mode='same')
    rearRPMdiff=np.convolve(np.array(df['rpm_RearL'])-np.array(df['rpm_RearR']), np.ones(10)/10, mode='same')
    
    ax1.plot(x,frontRPMdiff, c='#0000ff', alpha=0.5, label="rpm Difference Front")
    ax1.plot(x,rearRPMdiff, c='#00ff00', alpha=0.5, label="rpm Difference Rear")
   
    ax1.set_xlabel('timestamp')
    ax1.set_ylabel('rpm Difference')
    ax1.legend(loc='upper left')
    plt.show()


def plot_rpmVsCurrent():
    scattersize=2
    scatteralpha=0.5

    #confidence
    #confidence_FrontL=[abs(x-df['cmd_FrontL'][i-5 if i>4 else 0])+abs(x-df['cmd_FrontL'][i-10 if i>9 else 0])+abs(x-df['cmd_FrontL'][i-20 if i>19 else 0]) for i,x in enumerate(df['cmd_FrontL'])]
    #confidence_FrontL=[mapRange(x,0,50,1.0,0.0,True) for x in confidence_FrontL]



    fig, ax1 = plt.subplots()
    ax1.scatter(df['rpm_FrontL'],df['current_FrontL'], s=5, alpha=scatteralpha, label="FrontL")
    ax1.scatter(df['rpm_FrontR'],df['current_FrontR'], s=5, alpha=scatteralpha, label="FrontR")
    #ax1.scatter(df['cmd_FrontR'],df['rpm_FrontR'], s=scattersize, alpha=scatteralpha, label="FrontR")
    #ax1.scatter(df['cmd_RearL'],df['rpm_RearL'], s=scattersize, alpha=scatteralpha, label="RearL")
    #ax1.scatter(df['cmd_RearR'],df['rpm_RearR'], s=scattersize, alpha=scatteralpha, label="RearR")

    ax1.set_xlabel('rpm')
    ax1.set_ylabel('current (A)')
    plt.title('rpm vs current')
    ax1.legend(loc='upper left')
    plt.show()


#plot_rpmVsCurrent()
rpmDifference()



exit()