import numpy as np
import matplotlib.pyplot as plt
import plotting as plo
import dataAcquisitionMulti as datAc
    
    
#def readMagState(mStateF):
sstring = 'set4'    
        
leapFile = "../datasets/160217/160217_"+'set1'+"_leap"    
magFile = "../datasets/160217/160217_"+sstring+"_mag"

#caliFile = "../datasets/160217/160217_"+'cali3'
#(tC,s1c,s2c,s3c,s4c) = datAc.readMag(caliFile)

(tLeap,ind,mid,rin,pin) = datAc.readLeap(leapFile)
#(tMag,s1,s2,s3,s4) = datAc.readMag(magFile1)
(tMag,s1,s2,s3,s4) = datAc.readMag(magFile)

#plo.plotAngles(tLeap,(ind,mid,rin,pin),head='leapStates set5')

#plo.plotLeapVsMag((np.zeros((3,1)),np.zeros((3,3))),(tMag,s1,s2,s3,s4),head=sstring)
#plo.plotLeapVsMag((tLeap,ind),(tC,s1c,s2c,s3c,s4c),head="cli")
plo.plotLeapVsMag((tLeap,ind),(tMag,s1,s2,s3,s4),head=sstring)
cnt = 0
for i in range(len(t90)):
    b90_A[i] = modCA.angToB_cyl(angles90[i],fingList,sensList,jointList)              # simulating model with ad-ab    


#fi = open("../datasets/evalSets/estResults/160217_real/results.csv","a")
# all the start and end states for the corresponding datasets...
startL = [480,728,99 ,50,   180,40,120,120,120,120,140,130,220,140,110]
endL =   [529,799,185,90,   280,80,180,160,180,160,180,175,300,180,160]

for i in range(4,5,1):      
    sstring = 'set'+str(i)
    
    (tim, s1, s2, s3, s4) = datAc.readMag("../datasets/evalSets/"+sstring+"_mag")
    (tLeap, lInd, lMid, lRin, lPin) = datAc.readLeap("../datasets/evalSets/"+sstring+"_leap")
    lInd_re = sf.resampleLeap((tLeap,lInd),tim)[0]
    
    ''' fitting measurements ''' 
#    print "fitting for 90"
    start = startL[i-1]
    end = endL[i-1]
    (scale, off) = datAc.getScaleOff(b90_A[:,:3],s1[start:end]) 
    s1_fit = s1*scale+off
    
    (scale, off) = datAc.getScaleOff(b90_A[:,3:6],s2[start:end]) 
    s2_fit = s2*scale+off
    
    (scale, off) = datAc.getScaleOff(b90_A[:,6:9],s3[start:end]) 
    s3_fit = s3*scale+off
    
datAc.saveStates("../datasets/MPU/"+dayString+'_'+sstring+"cylA12.txt", estAngc_A)
print "time c_A12: ", c_A

''' 14 '''
startT = time.time()
estAngd_A = modDA.estimateSeries(s1_fit, fingList, sensList, jointList, bnds=True, met=1)
d_A = time.time()-startT
print "time d_A14: ", d_A 
datAc.saveStates("../datasets/MPU/"+dayString+'_'+sstring+"dipA44.txt", estAngd_A)

startT = time.time()
estAngc_A = modCA.estimateSeries(s1_fit, fingList, sensList, jointList, bnds=True, met=1)    
c_A = time.time()-startT
datAc.saveStates("../datasets/MPU/"+dayString+'_'+sstring+"cylA44.txt", estAngc_A)
print "time c_A14: ", c_A

    
plo.plotLeapVsMag((tim,estAngd_A),(tim,s1,s1_fit[:,:3]),head="estAngd_A vs B-field "+sstring)


(timLeap, angInd, angMid, angRin, angPin) = datAc.readLeap("../datasets/160210/"+dayString+'_'+sstring+"_leap" )
#plo.plotLeapVsMag((timLeap,angInd),(tim,estAngd_A),head="leap state vs estAngd_A "+sstring,dif=False)

#plo.plotLeapVsMag((tim,estAngd_A),(tim,estAngc_A),head="dip vs cyl ad-ab "+sstring,dif=False)
#plo.plotLeapVsMag((tim,estAngd),(tim,estAngc),head="dip vs cyl NO ad-ab "+sstring,dif=False)

plo.plotLeapVsMag((timLeap,angInd,angMid,angRin,angPin),
                  (tim,estAngd_A[:,:4],estAngd_A[:,4:8],estAngd_A[:,8:12],estAngd_A[:,12:]),head="estAngd")

plo.plotLeapVsMag((timLeap,angInd,angMid,angRin,angPin),
                  (tim,estAngc_A[:,:4],estAngc_A[:,4:8],estAngc_A[:,8:12],estAngc_A[:,12:]),head="estAngc")
""" for plotting least best result """
# sstring = "set5"
# estCA = datAc.readStateFile("../datasets/evalSets/estResults/160226_real/"+sstring+"_cylA14.txt")
# (tLeap,lInd,lMid,lRin,lPin) = datAc.readLeap("../datasets/evalSets/"+sstring+"_leap")
# (tMag,s1,s2,s3,s4) = datAc.readMag("../datasets/evalSets/"+sstring+"_mag")
# b = resampleLeap_point((tLeap,lInd), tMag)[0]
# b = b[:-1]
# tMag = tMag[:-1]
# plotDif_ind(b,estCA, tMag, 'estCA')
# plotDif(b,estCA, tMag, 'estCA')

""" for plotting 44 estimation... """
sstring = "set6"
estCA = datAc.readStateFile("../datasets/44/160210_" + sstring + "cylA44.txt")
(tLeap, lInd, lMid, lRin, lPin) = datAc.readLeap("../datasets/160210/160210_" + sstring + "_leap")
(tMag, s1, s2, s3, s4) = datAc.readMag("../datasets/160210/160210_" + sstring + "_mag")
(indRe, midRe, rinRe, pinRe) = resampleLeap_point((tLeap, lInd, lMid, lRin, lPin), tMag)

tMag = tMag[:-1]
# plotDif_ind(indRe[:-1],estCA[:,:4], tMag, 'estCA Index')
# plotDif_ind(rinRe[:-1],estCA[:,4:8], tMag, 'estCA Middle')
# plotDif_ind(midRe[:-1],estCA[:,8:12], tMag, 'estCA Ring')
# plotDif_ind(pinRe[:-1],estCA[:,12:], tMag, 'estCA Pinky')
# plotMulti([indRe[:-1],midRe[:-1]],estCA,tMag)

lIndN = np.linalg.norm(indRe, axis=1)
lMidN = np.linalg.norm(midRe, axis=1)
lRinN = np.linalg.norm(rinRe, axis=1)
lPinN = np.linalg.norm(pinRe, axis=1)
    
    cnt += 1
    
    
    
# add the DIP states
estAng_I = estAng[:,:3]
dip_I = estAng_I[:,1]*(2./3.)
estAng_I = np.insert(estAng_I,2,dip_I,1)

estAng_M = estAng[:,3:6]
dip_M = estAng_M[:,1]*(2./3)
estAng_M = np.insert(estAng_M,2,dip_M,1)

estAng_R = estAng[:,6:9]
dip_R = estAng_R[:,1]*(2./3)
estAng_R = np.insert(estAng_R,2,dip_R,1)

estAng_P = estAng[:,9:]
dip_P = estAng_P[:,1]*(2./3)
estAng_P = np.insert(estAng_P,2,dip_P,1)


    
plo.plotLeapVsMag((tim,estAng_I[1:]),(tim,s1,s1_fit[:,:3]),head="estAng vs B-field "+sString)

(timLeap, angInd, angMid, angRin, angPin) = datAc.readLeap("../datasets/160210/160210_"+sString+"_leap")
plo.plotLeapVsMag((timLeap,angInd,angMid,angRin,angPin),
                  (tim,estAng_I[1:],estAng_M[1:],estAng_R[1:],estAng_P[1:]),head="leap state vs est "+sString,dif=False)