content: # -*- coding: utf-8 -*-
  """
  Created on Tue Sep 12 18:47:50 2017
  
  @author: adelpret
  """
  from scipy import signal
  import numpy as np
  from scipy import ndimage
  import matplotlib.pyplot as plt
  from identification_utils import solve1stOrderLeastSquare
  
  
  def identify_motor_acc(dt, dq, ddq, current, tau, Kt_p, Kv_p, ZERO_VELOCITY_THRESHOLD_SMALL, 
                         ZERO_JERK_THRESHOLD, SHOW_THRESHOLD_EFFECT):
      #Filter current*****************************************************
      win = signal.hann(10)
      filtered_current = signal.convolve(current, win, mode='same') / sum(win)
      current = filtered_current
      
      # Mask valid data***************************************************
      #~ # remove high jerk
      dddq = np.gradient(ddq,1)/dt
      maskConstAcc = (abs (dddq)<ZERO_JERK_THRESHOLD)
      #~ # erode to get only steady phases where acceleration is constant 
      maskConstAcc=ndimage.morphology.binary_erosion(maskConstAcc,None,100)
      maskPosVel=(dq> ZERO_VELOCITY_THRESHOLD_SMALL)
      maskNegVel=(dq<-ZERO_VELOCITY_THRESHOLD_SMALL)
      maskConstPosAcc=np.logical_and( maskConstAcc ,maskPosVel )
      maskConstNegAcc=np.logical_and( maskConstAcc ,maskNegVel )
      
      if SHOW_THRESHOLD_EFFECT :
          plt.figure()
          plt.plot(ddq); plt.ylabel('ddq')
          ddq_const=ddq.copy()
          ddq_const[np.logical_not(maskConstAcc)]=np.nan
          plt.plot(ddq_const); plt.ylabel('ddq_const')
          plt.show()
  
      #~ y              = a. x   +  b
      #~ i-Kt.tau-Kv.dq = Ka.ddq +  Kf
      #~ 
      # Identification ***************************************************
      y = current-Kt_p*tau - Kv_p*dq
      y[maskConstPosAcc] = current[maskConstPosAcc]-Kt_p*tau[maskConstPosAcc] - Kv_p*dq[maskConstPosAcc]
      y[maskConstNegAcc] = current[maskConstNegAcc]-Kt_p*tau[maskConstNegAcc] - Kv_p*dq[maskConstNegAcc]
      y_label = r'$i(t)-{K_t}{\tau(t)}-{K_v}{\dot{q}(t)}$'
      x = ddq
      x_label = r'$\ddot{q}(t)$'
      (Kap,Kfp)=solve1stOrderLeastSquare(x[maskConstPosAcc],y[maskConstPosAcc])
      (Kan,b)=solve1stOrderLeastSquare(x[maskConstNegAcc],y[maskConstNegAcc])
      Kfn=-b
      
      # Plot *************************************************************
      plt.figure()    
      plt.axhline(0, color='black',lw=1)
      plt.axvline(0, color='black',lw=1)
      plt.plot(x     ,y     ,'.' ,lw=3,markersize=1,c='0.5');  
      plt.plot(x[maskConstPosAcc],y[maskConstPosAcc],'rx',lw=3,markersize=1); 
      plt.plot(x[maskConstNegAcc],y[maskConstNegAcc],'bx',lw=3,markersize=1); 
      
      #plot identified lin model
      plt.plot([min(x),max(x)],[Kap*min(x)+Kfp ,Kap*max(x)+Kfp],'g:',lw=3)
      plt.plot([min(x),max(x)],[Kan*min(x)-Kfn ,Kan*max(x)-Kfn],'g:',lw=3)
      plt.ylabel(y_label)
      plt.xlabel(x_label)
      plt.show()
      
      return (Kap, Kan, Kfp, Kfn)