import GPIBPrologix
import bme280
import smbus2
from requests import post
import time
from plotly.subplots import make_subplots
import plotly.graph_objects as go
import pandas as pd
import os
import shutil
import csv
basePath = os.getcwd()

## Amount of INL averages
nrepeats = 2
## Initialize GPIB adapter
GPIB = GPIBPrologix.ResourceManager("/dev/ttyACM0")
# Connect equipment
inst2 = GPIB.open_resource(30) # 4805
inst3 = GPIB.open_resource(22) # 3458A
inst4 = GPIB.open_resource(16) # R6581T
try:
    os.remove("data_post.csv")
except:
    print('no datafile found to remove')
try:
    os.remove("data.csv")
except:
    print('no datafile found to remove')

# Initialize BME280 temperature/humidity sensor
bus = smbus2.SMBus(1)
## Configure equipment
# BME280 temperature/humidity sensor
calibration_params = bme280.load_calibration_params(bus, 0x76)
# Datron 4805 Calibrator
inst2.query("F0=")       #DCV
inst2.query("R6=")       #10/100K Range
inst2.write("G0=") #Local guard
inst2.write("S0=") #Local sense
inst2.query("M-11.5=")  #6.9mV Out, sanity check
inst2.query("O1=")       #OutputON
#Setup 3458A
#inst3.write("PRESET NORM")
inst3.write("MEM OFF")
inst3.write("OFORMAT ASCII")
inst3.write("END ALWAYS")
inst3.write("TARM HOLD")
inst3.write("TRIG AUTO")
inst3.write("DCV 10")
inst3.write("NRDGS 1,AUTO")
inst3.write("NPLC 100")
inst3.write("NDIG 9")
inst3.write("AZERO ON")
# Advantest R6581T
inst4.write(":BEEP:STAT OFF")
inst4.write(":CONF:VOLT:DC")
inst4.write(":SENS:VOLT:DC:RANG 10")
inst4.write(":SENS:VOLT:DC:NPLC 100")
inst4.write(":SENS:VOLT:DC:DIG MAX")
inst4.write(":ZERO:AUTO ON")

# save original INL constants
with open(basePath+"/original_hosei.txt", 'a') as f:
    f.write(inst4.query("CAL:INT:DCV:HOSEI?"))
# write Advantest correction to zero, in theory not needed as at the end coefficients get summed.
inst4.write("CAL:EXT:EEPROM:PROTECTION 1")
for i in range(15):
    print("CAL:INT:DCV:HOSEI {},{}".format(i,0))
    inst4.write("CAL:INT:DCV:HOSEI {},{}".format(i,0))
inst4.write("CAL:INT:DCV:HOSEI {},{}".format(15,1))
print("CAL:INT:DCV:HOSEI {},{}".format(15,1))

# Measurement functions
def readValue(instObj,handle):
    out = ""
    if handle == "3458A":
        instObj.write("TARM SGL,1")
        for i in range(10):
            out = instObj.read()
            if out:
                break
    elif handle == "6581T":
        out = instObj.query("FETch?")
    return out
def setValue(instObj, inputVar):
    instObj.query("M"+str(inputVar)+"=")
    time.sleep(40)
    return inputVar
def getEnvironment(instObj, i2cbus):
    value = instObj.sample(i2cbus, 0x76, calibration_params)
    return value
#inst3.write("BEEP")
print('3458A reading: ',readValue(inst3,"3458A"))
#inst4.write("BEEP")
print('R6581T reading: ',readValue(inst4,"6581T"))

## Have gitpython pull in the repository
import csv  
import time
import datetime
import numpy as np
import pandas as pd
from math import sin
## Create logfile
with open(basePath+"/data.csv", 'a') as f:
    writer = csv.writer(f)
    writer.writerow(["DateTime","SetVolts","RefVolts","DutVolts","Env Pressure","Env Temperature", "Env Humidity"])
print(basePath+"/data.csv")

cntr = 0
d     = datetime.datetime.now()
dlast = datetime.datetime.now()
## Collect data
time.sleep(3)
for n in range(nrepeats):
    sweep = np.append(np.linspace(-10,10,int(10/2*2)+1), np.linspace(-0.1,-0.02,int((0.1-0.02)/0.02)+1))
    np.random.shuffle(sweep)
    for x in sweep:
        cntr = cntr + 1
        try:
            #Set volt and let accimatize
            setPoint = setValue(inst2,round(x,5))
            timebetween = (d-dlast) * (len(sweep)*nrepeats-cntr)
            dlast = d
            print(str(cntr)+'/'+str(len(sweep)*nrepeats)+' Estimated Time Left: '+str(timebetween))
            for i in range(3):
                try:
                    #Get DUT value
                    readoutRef = readValue(inst3,"3458A")
                    readoutDut = readValue(inst4,"6581T")
                    #Get envirnmental values
                    data = getEnvironment(bme280, bus)
                    #Write to file
                    d = datetime.datetime.now()
                    dx = d - datetime.timedelta(microseconds=d.microsecond)
                    fields=[dx.strftime("%d-%m-%y %H:%M:%S"),setPoint,float(readoutRef),float(readoutDut),round(data.pressure,2),round(data.temperature,2),round(data.humidity,2)]
                    print(fields)
                    with open(basePath+"/data.csv", 'a') as f:
                        writer = csv.writer(f)
                        writer.writerow(fields)
                except Exception as e: 
                    time.sleep(15)
                    i = i-1
        except Exception as e: 
            print(e)
            time.sleep(1)

#  initialize the image to plot as correction goes
fig = make_subplots(rows=3, cols=1)

## Calculate high order polynomial
df = pd.read_csv(basePath+"/data.csv")
df = df.groupby(["SetVolts"], as_index=False).mean(numeric_only=True)
tmpa = df.loc[df['SetVolts'] == 0]["RefVolts"].iloc[0]
tmpb = df.loc[df['SetVolts'] == 0]["DutVolts"].iloc[0]
#  compensate for offset error around 0 -> this is cal error instead of INL
df["RefVolts"] = df["RefVolts"] - tmpa
df["DutVolts"] = df["DutVolts"] - tmpb
#df["OptVolts"] = df["OptVolts"] - df.loc[df['SetVolts'] == 0]["OptVolts"].iloc[0]

#  get 1st order to 10V -> 10V gain error, this is cal error instead of INL, -10 could be due to INL.
#  this is heavily dependant on cal procedure, say 7V is the reference calibration point
alphaRef = df.loc[df['SetVolts'] == 10]["RefVolts"].iloc[0]/10
alphaDut = df.loc[df['SetVolts'] == 10]["DutVolts"].iloc[0]/10
#alphaOpt = df.loc[df['SetVolts'] == 10]["OptVolts"].iloc[0]/10
#  remove 1st order to get the INL
df["RefVolts"] = df["RefVolts"] - alphaRef*df['SetVolts']
df["DutVolts"] = df["DutVolts"] - alphaDut*df['SetVolts']
#df["OptVolts"] = df["OptVolts"] - alphaOpt*df['SetVolts']
df["DutToRefVolts"] = df['DutVolts'] - df["RefVolts"]
#  plot INL of calibrator
fig.add_trace(go.Scatter(x=df['SetVolts'], y=df["RefVolts"],mode='lines+markers',name='Original REF INL'),row=1, col=1)
fig.add_trace(go.Scatter(x=df['SetVolts'], y=df["DutVolts"],mode='lines+markers',name='Original DUT INL'),row=1, col=1)
#fig.add_trace(go.Scatter(x=df['SetVolts'], y=df["OptVolts"],mode='lines+markers',name='Original OPT INL'),row=1, col=1)
fig.add_trace(go.Scatter(x=df['SetVolts'], y=df["DutToRefVolts"],mode='lines+markers',name='Original DUT vs REF'),row=2, col=1)
## Make plot and save
fig.write_html('inl_evaluation_plots.html')
fig.write_image('inl_evaluation_plots.png',width=720, height=1280)

#  generate correction parameters
#  default correction assumed unless minMaxEqual is set, this will equalize error above and under ideal correction
#  folding the correction down and up to have a better fit, be sure that measurements were extremely stable to use this
#  this block output is to be specifically tailored towards your application, currently R6581T INL adjustment against 3458A
minMaxEqual = 0
correctionfactors = [None] * 16
if minMaxEqual:
    # not implemented as of yet
    print("not implemented")
else:
    # postive part of INL
    correctionfactors[0]  = (df.loc[df['SetVolts'] ==     0]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==     2]["DutToRefVolts"].iloc[0])/2
    correctionfactors[1]  = (df.loc[df['SetVolts'] ==     2]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==     4]["DutToRefVolts"].iloc[0])/2
    correctionfactors[2]  = (df.loc[df['SetVolts'] ==     4]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==     6]["DutToRefVolts"].iloc[0])/2
    correctionfactors[3]  = (df.loc[df['SetVolts'] ==     6]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==     8]["DutToRefVolts"].iloc[0])/2
    correctionfactors[4]  = (df.loc[df['SetVolts'] ==     8]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==    10]["DutToRefVolts"].iloc[0])/2
    # negative part of INL
    minusRangeScaling = df.loc[df['SetVolts'] ==   -10]["DutToRefVolts"].iloc[0]/10
    df["DutToRefVolts"] = df["DutToRefVolts"]-minusRangeScaling*-df["SetVolts"]
    correctionfactors[5]  = (df.loc[df['SetVolts'] == -0.02]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==     0]["DutToRefVolts"].iloc[0])/0.02
    correctionfactors[6]  = (df.loc[df['SetVolts'] == -0.04]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] == -0.02]["DutToRefVolts"].iloc[0])/0.02
    correctionfactors[7]  = (df.loc[df['SetVolts'] == -0.06]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] == -0.04]["DutToRefVolts"].iloc[0])/0.02
    correctionfactors[8]  = (df.loc[df['SetVolts'] == -0.08]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] == -0.06]["DutToRefVolts"].iloc[0])/0.02
    correctionfactors[9]  = (df.loc[df['SetVolts'] ==  -0.1]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] == -0.08]["DutToRefVolts"].iloc[0])/0.02
    correctionfactors[10] = (df.loc[df['SetVolts'] ==    -2]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==  -0.1]["DutToRefVolts"].iloc[0])/1.9
    correctionfactors[11] = (df.loc[df['SetVolts'] ==    -4]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==    -2]["DutToRefVolts"].iloc[0])/2
    correctionfactors[12] = (df.loc[df['SetVolts'] ==    -6]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==    -4]["DutToRefVolts"].iloc[0])/2
    correctionfactors[13] = (df.loc[df['SetVolts'] ==    -8]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==    -6]["DutToRefVolts"].iloc[0])/2
    correctionfactors[14] = (df.loc[df['SetVolts'] ==   -10]["DutToRefVolts"].iloc[0] - df.loc[df['SetVolts'] ==    -8]["DutToRefVolts"].iloc[0])/2
    # negative 10v voltage reversal alpha error for faster measurements
    correctionfactors[15] = round(1+minusRangeScaling,8)

## Read the HOSEI parameters and save the original constants
## add constants together in case of nonzero sweep constants
inst4.write("CAL:EXT:EEPROM:PROTECTION 1")
print(inst4.query("CAL:INT:DCV:HOSEI?"))
tmp = inst4.query("CAL:INT:DCV:HOSEI?")
tmp = tmp.replace(',',' ').split()[1::2]
tmp = [float(i) for i in tmp[:-2]]
correctionfactors = [round(tmp[i]+correctionfactors[i],8) for i in range(0,len(correctionfactors)-1)]
correctionfactors.append(1+minusRangeScaling+1-tmp[15])
for idx in  range(0,len(correctionfactors)):
    print("CAL:INT:DCV:HOSEI {},{}".format(idx,correctionfactors[idx]))
    inst4.write("CAL:INT:DCV:HOSEI {},{}".format(idx,correctionfactors[idx]))
with open(basePath+"/new_hosei.txt", 'a') as f:
    f.write(inst4.query("CAL:INT:DCV:HOSEI?"))
print(inst4.query("CAL:INT:DCV:HOSEI?"))
inst4.write("CAL:EXT:EEPROM:PROTECTION 0")

## Create logfile
with open(basePath+"/data_post.csv", 'a') as f:
    writer = csv.writer(f)
    writer.writerow(["DateTime","SetVolts","RefVolts","DutVolts","Env Pressure","Env Temperature", "Env Humidity"])
print(basePath+"/data_post.csv")

cntr = 0
d     = datetime.datetime.now()
dlast = datetime.datetime.now()
## Collect data
time.sleep(3)
for n in range(nrepeats):
    sweep = np.append(np.linspace(-10,10,int(10/2*2)+1), np.linspace(-0.1,-0.02,int((0.1-0.02)/0.02)+1))
    np.random.shuffle(sweep)
    for x in sweep:
        cntr = cntr + 1
        try:
            #Set volt and let accimatize
            setPoint = setValue(inst2,round(x,5))
            timebetween = (d-dlast) * (len(sweep)*nrepeats-cntr)
            dlast = d
            print(str(cntr)+'/'+str(len(sweep)*nrepeats)+' Estimated Time Left: '+str(timebetween))
            for i in range(3):
                try:
                    #Get DUT value
                    readoutRef = readValue(inst3,"3458A")
                    readoutDut = readValue(inst4,"6581T")
                    #Get envirnmental values
                    data = getEnvironment(bme280, bus)
                    #Write to file
                    d = datetime.datetime.now()
                    dx = d - datetime.timedelta(microseconds=d.microsecond)
                    fields=[dx.strftime("%d-%m-%y %H:%M:%S"),setPoint,float(readoutRef),float(readoutDut),round(data.pressure,2),round(data.temperature,2),round(data.humidity,2)]
                    print(fields)
                    with open(basePath+"/data_post.csv", 'a') as f:
                        writer = csv.writer(f)
                        writer.writerow(fields)
                except Exception as e: 
                    time.sleep(15)
                    i = i-1
        except Exception as e: 
            print(e)
            time.sleep(1)

# Calculate high order polynomial
df = pd.read_csv(basePath+"/data_post.csv")
df = df.groupby(["SetVolts"], as_index=False).mean(numeric_only=True)

#  compensate for offset error around 0 -> this is cal error instead of INL
df["RefVolts"] = df["RefVolts"] - df.loc[df['SetVolts'] == 0]["RefVolts"].iloc[0]
df["DutVolts"] = df["DutVolts"] - df.loc[df['SetVolts'] == 0]["DutVolts"].iloc[0]
#df["OptVolts"] = df["OptVolts"] - df.loc[df['SetVolts'] == 0]["OptVolts"].iloc[0]

#  get 1st order to 10V -> 10V gain error, this is cal error instead of INL, -10 could be due to INL.
#  this is heavily dependant on cal procedure, say 7V is the reference calibration point
alphaRef = df.loc[df['SetVolts'] == 10]["RefVolts"].iloc[0]/10
alphaDut = df.loc[df['SetVolts'] == 10]["DutVolts"].iloc[0]/10
#alphaOpt = df.loc[df['SetVolts'] == 10]["OptVolts"].iloc[0]/10
#  remove 1st order to get the INL
df["RefVolts"] = df["RefVolts"] - alphaRef*df['SetVolts']
df["DutVolts"] = df["DutVolts"] - alphaDut*df['SetVolts']
#df["OptVolts"] = df["OptVolts"] - alphaOpt*df['SetVolts']
df["DutToRefVolts"] = df['DutVolts'] - df["RefVolts"]
#  plot INL of calibrator
fig.add_trace(go.Scatter(x=df['SetVolts'], y=df["RefVolts"],mode='lines+markers',name='Post REF INL'),row=3, col=1)
fig.add_trace(go.Scatter(x=df['SetVolts'], y=df["DutVolts"],mode='lines+markers',name='Post DUT INL'),row=3, col=1)
#fig.add_trace(go.Scatter(x=df['SetVolts'], y=df["OptVolts"],mode='lines+markers',name='Original OPT INL'),row=1, col=1)
fig.add_trace(go.Scatter(x=df['SetVolts'], y=df["DutToRefVolts"],mode='lines+markers',name='Post DUT vs REF'),row=2, col=1)
## Make plot and save
fig.write_html('inl_evaluation_plots.html')
fig.write_image('inl_evaluation_plots.png',width=720, height=1280)