import math
import numpy as np
import sys
sys.path.insert(0,'../functions/')
import pi_to_pi
from scipy.linalg import sqrtm
import scipy.optimize as op
from scipy.stats.distributions import chi2

class EstimatorClassFgSimOn:
      landmark_map = None

      XX= np.zeros((15,1))
      x_true= np.zeros((3,1))
      alpha = None# state of interest extraction vector
      PX= np.zeros(15)
        
      association = None # association of current features
      association_full = None # association of current features
      association_true = None # only for simulation
      association_no_zeros = None # association of associated features
      num_landmarks = None # nunber of landmarks in the map
      num_associated_lms= 0
      num_extracted_features = None
      num_of_extracted_features = None
      number_of_associated_LMs = None
        
      n_k = None # number of absolute measurements at current time
      num_faults_k = None # number of injected faults at current time

      gamma_k = None
      q_k = None
      Y_k = None
      H_k = None
      L_k = None
      Phi_k = None   # state evolution matrix
      D_bar = None # covariance increase for the state evolution
        
      T_d= 0 # detector threshold
      q_d= 0 # detector for the window of time
        
      initial_attitude = None # save initial attitude for the calibration of IM?U biases
      appearances= np.zeros((1,300)) # if there are more than 300 landmarks, something's wrong
      FoV_landmarks_at_k = None # landmarks in the field of view
      current_wp_ind= 1 # index of the sought way point
      goal_is_reached= 0
      steering_angle= 0
      lm_ind_fov = None # indexes of the landmarks in the field of view
        
      M= 0 # preceding horizon size in epochs
      x_ph = None # poses in the time window
      z_fg = None # all the msmts in the time window
      z_lidar_ph = None # lidar msmts in the ph
      z_lidar = None # current lidar msmts
      z_gyro= 0 # current gyro msmt
      z_gyro_ph = None # gyro msmts in the ph
      PX_prior = None # cov matrix of the prior
      Gamma_prior = None # information matrix of the prior
      m_M = None # number of states to estimate
      n_total = None # total numbe of msmts
      association_ph = None # associations during the ph
      odometry_k = None # odometry msmts at the current time
      odometry_ph = None # velocity and steering angle for the ph
      x_prior = None # stores x_{k-M} as a msmt for the next epoch
      n_L_k= 0 # number of associations at k
      n_L_M= 0 # number of associations in the ph
      H_k_gps = None
      H_k_lidar = None
      n_gps_k = None
      n_L_k_ph = None # number of associations in the ph
      # ----------------------------------------------
      # ----------------------------------------------
      def __init__(self,params):
         # initialize preceding horizon size
         if (params.SWITCH_FIXED_LM_SIZE_PH == 1):
             self.M= 0
         else:
             self.M= params.M
         # initialize preceding horizon size
         if (params.SWITCH_FIXED_LM_SIZE_PH == 1):
             self.M= 0
         else:
             self.M= params.M

         # initialize sizes differently for simulation
         self.XX= np.zeros((3,1))
         self.XX[params.ind_yaw]= np.deg2rad(params.initial_yaw_angle)
         self.x_true[params.ind_yaw]= np.deg2rad(params.initial_yaw_angle)
         self.PX= np.dot(np.eye(3),eps)
            
            
         # initialize to uninformative prior
         self.PX_prior= np.diag( np.dot(np.ones(params.m,1),eps) )
         # initialize covariance
         self.Gamma_prior= np.inv(self.PX_prior)
         self.x_prior= np.zeros((params.m, 1))
         # allocate memory
         self.x_ph= [None]*params.M
         self.z_lidar_ph= [None]*params.M
         self.association_ph= [None]*params.M
         self.odometry_ph= [None]*params.M
         self.z_gyro_ph= [None]*params.M
         self.n_L_k_ph= np.zeros((params.M, 1))
            
            
         # load map if exists
         if (params.SWITCH_GENERATE_RANDOM_MAP ==1): # map generated by params
            self.landmark_map= params.landmark_map
            self.num_landmarks= self.landmark_map.shape[1]
         else: # map is loaded from saved variable
             tdir = params.path+ 'landmark_map.mat'
             data = sio.loadmat(tdir)
             data = data['landmark_map']
             self.landmark_map= data
             self.num_landmarks= self.landmark_map.shape[1]
      # ----------------------------------------------
      # ----------------------------------------------
      def compute_alpha(self,params):
          self.alpha= np.array([[-math.sin( self.XX(params.ind_yaw) )]
                      [math.cos( self.XX(params.ind_yaw) )]
                       [0] ])
      # ----------------------------------------------
      # ---------------------------------------------- 
      def compute_steering(self, params):
          if (params.SWITCH_OFFLINE == 1):
              xx= self.x_true
          else:
              xx= self.XX


          # determine if current waypoint reached
          while(True):
              current_wp= params.way_points[:, self.current_wp_ind+1]
              d= np.sqrt( ( current_wp[0]-  xx[0] )**2 + ( current_wp[1] - xx[1] )**2 )
    
              # check current distance to the waypoint
              if (d < params.min_distance_to_way_point):
                  self.current_wp_ind= self.current_wp_ind + 1 # next way point
        
                  # reached final waypoint ---> flag and return
                  if (self.current_wp_ind > params.way_points.shape[1]):
                      self.goal_is_reached= 1
                      return 0
              else:
                  break


          # compute change in G to point towards current waypoint
          delta_steering= pi_to_pi.pi_to_pi(np.atan2( current_wp[1] - xx[1], current_wp[0] - xx[0] ) - xx[2])
          delta_steering= pi_to_pi.pi_to_pi(delta_steering - self.steering_angle)

          # # weighting factor for the delta steering
          # d_min= 0
          # d_max= 100
          # weight_min= 1
          # weight_max= 100
          # slope= (weight_max - weight_min) / (d_max - d_min)
          # if d <= d_min
          #     weight= weight_min
          # elseif d > d_min && d < d_max
          #     weight= weight_min + slope * (d - d_min)
          # elseif d >= d_max
          #     weight= weight_max
          # end
          # delta_steering= delta_steering / weight

          # limit rate
          max_delta= np.dot(params.max_delta_steering,params.dt_sim)
          if (np.abs(delta_steering) > max_delta):
              delta_steering= np.dot(np.sign( delta_steering ),max_delta)

          # limit angle
          self.steering_angle= pi_to_pi.pi_to_pi( self.steering_angle + delta_steering )
          if (np.abs(self.steering_angle) > params.max_steering):
              self.steering_angle= np.dot(np.sign( self.steering_angle ),params.max_steering)
      # ----------------------------------------------
      # ---------------------------------------------- 
      def from_estimator_to_vector(self, params):

          # from cells to vectors
          x= None
          for i in range(self.M,-1,1):
              x= np.array([[x],[self.x_ph[i-1]]])
          x= np.array([[x], [self.XX]])
          return x
      # ----------------------------------------------
      # ---------------------------------------------- 
      def from_vector_to_estimator(self, x, params):
          # from a vector to cells 
          for i in range( self.M,-1,1):
              # update the cell
              self.x_ph[i-1]= x[1:params.m]
    
              # update index
              inds= inds + params.m
          # current pose
          self.XX= x[x[x.shape[0]] - params.m + 1:x[x.shape[0]]]
      # ----------------------------------------------
      # ---------------------------------------------- 
      def generate_gyro_msmt(self, theta_prev, theta_next, params):
          # true gyro msmt
          self.z_gyro= np.dot(( theta_next - theta_prev ),np.inv(params.dt_sim))
          # add noise
          self.z_gyro= self.z_gyro + np.random.normal(0, params.sig_gyro_z)
          # keep in [-pi-pi]
          self.z_gyro= pi_to_pi.pi_to_pi[self.z_gyro-1]
      # ----------------------------------------------
      # ----------------------------------------------
      def get_gps_msmt(self, params):
          # simulate measurement
          z= self.x_true[0:3] + np.transpose(np.random.multivariate_normal(np.zeros((2,1)), params.R_gps_sim))
          return z
      # ----------------------------------------------
      # ----------------------------------------------
      def get_lidar_msmt(self, params):
          spsi= math.sin(self.x_true[2])
          cpsi= math.cos(self.x_true[2])
          z_lidar= None
          self.association_true= None
          self.num_faults_k= 0
          for l in range(1,self.num_landmarks+1):
              # check if the landmark is in the FoV
              dx= self.landmark_map[l-1,0] - self.x_true[0]
              if (abs(dx) > params.lidarRange):
                  continue
              dy= self.landmark_map[l-1,0] - self.x_true[1]
              if (abs(dy) > params.lidarRange):
                  continue
              if (sqrt( dx**2 + dy**2 ) > params.lidarRange):
                  continue        
              # simulate msmt with noise
              z_lm[0]=  dx*cpsi + dy*spsi + np.random.normal(0, params.sig_lidar)
              z_lm[1]= -dx*spsi + dy*cpsi + np.random.normal(0, params.sig_lidar)
        
              # add possible fault
              if (params.SWITCH_LIDAR_FAULTS == 1):
                  if (np.random.binomial(1, params.P_UA)):
                      z_lm= z_lm + random()* params.sig_lidar * 10
                      self.num_faults_k= self.num_faults_k + 1
    
              # add measurement
              z_lidar= np.array([[z_lidar], [z_lm]])

              # save the true association
              self.association_true= np.array([[self.association_true], [l]])

          # add them to the estimator class property
          self.z_lidar= z_lidar

          # if we use the NN associaiton this values get overwritten 
          self.n_L_k= self.association_true.shape[0]
          self.n_k= np.dot(self.n_L_k,params.m_F)
          return z_lidar
      # ----------------------------------------------
      # ----------------------------------------------
      def odometry_update(self, params):
          # this function updates the estimate and true state for the given odometry
          # (velocity, steering angle) controls. Note that the the estimate is
          # updated with the computed controls, i.e. the ones we want to send to the
          # system. The true state is udpated with the actual controls executed in
          # the system, which have noise.


          # velocity & steering angle
          vel= params.velocity_sim
          phi= self.steering_angle

          # if it's the offline im analysis --> compute matrices and update true x
          if (params.SWITCH_OFFLINE==1):
              # compute state evolution matrix and its noise covariance matrix
              [self.Phi_k, self.D_bar]= self.return_Phi_and_D_bar(self.x_true, vel, phi, params)
    
          # if we are online --> add noise
          else:  
              # compute state evolution matrix and its noise covariance matrix
              [self.Phi_k, self.D_bar]= self.return_Phi_and_D_bar(self.XX, vel, phi, params)
    
              # estimate state with computed controls
              self.XX= self.return_odometry_update(self.XX, np.array([[vel],[phi]]), params)
    
              # Add noise to the computed controls
              vel= vel + np.random.normal(0, params.sig_velocity_sim)
              phi= phi + np.random.normal(0, params.sig_steering_angle_sim)

          # True State
          self.x_true= self.return_odometry_update(self.x_true,np.array([[vel],[phi]]), params)

          # save the velocity and steering angle 
          self.odometry_k= np.array([[vel],[phi]])
      # ----------------------------------------------
      # ----------------------------------------------
      def optimization_fn(self, x, params):
          # previous x star
          x_prev= self.from_estimator_to_vector(params)

          # from a vector to cells
          self.from_vector_to_estimator(x, params)

          # compute delta
          delta= x - x_prev

          # initialize
          cost= 0
          inv_R_lidar= np.inv(params.R_lidar)
          sqrt_inv_R_lidar= sqrtm(inv_R_lidar)


          # cost of the prior msmt
          z= self.x_prior
          z_expected= x[0:params.m+1]
          innov= z - z_expected
          inv_PX_prior= np.inv(self.PX_prior)
          cost= cost +  np.dot(np.dot(np.transpose(innov),inv_PX_prior),innov)
          if (nargout > 1):
              sqrt_inv_PX_prior= sqrtm(inv_PX_prior)
    
              # initialize normalized Jacobian
              A= np.zeros( (self.n_total, self.m_M))
    
              # plug the prior in A
              A[ 0:params.m+1, 0:params.m+1]= sqrt_inv_PX_prior
              # initialize b and add the prior residual
              b= np.dot(np.ones(self.n_total,1),math.inf)
              b[ 0:params.m+1 ]= np.dot(sqrt_inv_PX_prior,innov)

          # pointers to the next part of A to be filled
          r_ind= params.m + 1
          c_ind= 1

          for i in range(self.M,-1,2):
    
              # ------------ gyro cost ------------
              z= self.z_gyro_ph[i-1]
              z_expected= np.dot(pi_to_pi.pi_to_pi(( self.x_ph[i-1][params.ind_yaw] - self.x_ph[i](params.ind_yaw) )),np.inv(params.dt_sim))
              innov= pi_to_pi.pi_to_pi(z - z_expected)
              cost= np.dot((cost + innov**2),np.inv(params.sig_gyro_z**2))
              # ------------------------------------
          
              # ---------- gyro submatrix ----------
              if (nargout > 1):
                  A[r_ind, c_ind : c_ind + params.m ]= np.dot(-np.inv(params.sig_gyro_z), np.dot([0,0,1/params.dt_sim]))
        
                  A[ r_ind, c_ind + params.m : c_ind + 2*params.m - 1 ]= np.dot(np.inv(params.sig_gyro_z),np.dot([0,0,1/params.dt_sim]))
                  b[ r_ind-1 ]= np.dot((innov),np.inv(params.sig_gyro_z))
              # ------------------------------------
    
    
              # update the row index to point towards the next msmt
              r_ind= r_ind + 1
    
    
              # ---------- odometry cost ----------
              z_expected= self.return_odometry_update(self.x_ph[i-1], self.odometry_ph[i-1], params) - self.x_ph[i-1]
              z_expected[params.ind_yaw-1]= pi_to_pi.pi_to_pi(z_expected(params.ind_yaw))
              innov= - z_expected
              [Phi, D_bar]= self.return_Phi_and_D_bar(self.x_ph[i-1], self.odometry_ph[i-1](0), self.odometry_ph[i-1][1], params)
              [fart,S,V]= np.linalg.svd(D_bar)
              r_S= np.linalg.rank(S)
              sqrt_inv_D_bar= np.dot(sqrtm( np.inv(S[0:r_S+1,0:r_S+1]) ),np.transpose(V[:,0:r_S+1]))
#               cost= cost + z_expected' * pinv(D_bar) * z_expected
              cost= cost + np.dot(np.dot(np.transpose(innov),np.dot(np.transpose(sqrt_inv_D_bar),sqrt_inv_D_bar)),innov)
              # ------------------------------------
    
              # ---------- odometry submatrix ----------
              if (nargout > 1):
                  A[r_ind : r_ind + r_S , c_ind : c_ind + params.m ]= sqrt_inv_D_bar * Phi
        
                  A[ r_ind : r_ind + r_S , c_ind + params.m : c_ind + 2*params.m]= -sqrt_inv_D_bar
        
                  b[ r_ind : r_ind + r_S ]= sqrt_inv_D_bar * innov
              # ------------------------------------

    
              # update the row & column indexes
              r_ind= r_ind + r_S
              c_ind= c_ind + params.m
    
    
              # ------------ lidar cost ------------
              z= self.z_lidar_ph[i-2]
              n= z.shape[0]
              n_L= n /params.m_F
              if (n > 0):
                 z_expected= self.return_expected_z_lidar(self.x_ph[i-2], self.association_ph[i-2], params)
                 innov= z - z_expected
                 kron_inv_R_lidar= np.kron( eye(n_L) , inv_R_lidar )
                 kron_sqrt_inv_R_lidar=  np.kron( eye(n_L) , sqrt_inv_R_lidar )
                 cost= cost + np.dot(np.dot(np.transpose(innov),kron_inv_R_lidar),innov)
                 # ------------------------------------
        
                 # ---------- lidar submatrix ----------
                 if (nargout > 1):
                    A_lidar= self.return_lidar_A(self.x_ph[i-2], self.association_ph[i-2], params)
                    A[ r_ind : r_ind + r_S , c_ind + params.m : c_ind + 2*params.m]= A_lidar
            
                    b[ r_ind : r_ind + r_S ]= kron_sqrt_inv_R_lidar * innov
    
              # update the row index
              r_ind= r_ind + n
          # ------------ gyro cost ------------
          z= self.z_gyro
          z_expected= ( np.dot(pi_to_pi.pi_to_pi(self.XX[params.ind_yaw-1] - self.x_ph[i-1][params.ind_yaw-1] )),np.inv(params.dt_sim))
          innov= pi_to_pi.pi_to_pi(z - z_expected)
          cost= cost + innov**2 / params.sig_gyro_z**2
          # ------------------------------------

          # ---------- gyro submatrix ----------
          if (nargout > 1):
              A[r_ind, c_ind : c_ind + params.m]= np.dot(np.inv(-params.sig_gyro_z),np.array([0,0,1/params.dt_sim]))
    
              A[r_ind, c_ind + params.m : c_ind + 2*params.m]= np.dot(np.inv(params.sig_gyro_z),np.array([0,0,1/params.dt_sim]))
              b[r_ind]= innov / params.sig_gyro_z
          # ------------------------------------


          # update the row index to point towards the next msmt
          r_ind= r_ind + 1
 
    
          # ---------- odometry cost ----------
          z_expected= self.return_odometry_update(self.x_ph[i-1], self.odometry_k, params) - self.XX
          z_expected[params.ind_yaw-1]= pi_to_pi.pi_to_pi(z_expected(params.ind_yaw))
          innov= - z_expected
          [Phi, D_bar]= self.return_Phi_and_D_bar(self.x_ph[0], self.odometry_k[0], self.odometry_k[1], params)
          [fart,S,V]= np.linalgsvd( D_bar )
          r_S= np.linalg.rank(S)
          sqrt_inv_D_bar= np.dot(sqrtm( np.inv(S[0:r_S+1,0:r_S+1]) ),np.inv(V[:,1:r_S]))
          # cost= cost + z_expected' * pinv(D_bar) * z_expected
          cost= cost + np.dot(np.dot(np.inv(innov),np.dot(np.inv(sqrt_inv_D_bar),sqrt_inv_D_bar)),innov)
          # ------------------------------------
          # ---------- odometry submatrix ----------
          if (nargout > 1):
              A[r_ind, c_ind : c_ind + params.m]= sqrt_inv_D_bar * Phi
    
              A[r_ind, c_ind + params.m : c_ind + 2*params.m]= -sqrt_inv_D_bar
    
              b[r_ind]= sqrt_inv_D_bar * innov
          # ------------------------------------
          # update the row & column indexes
          r_ind= r_ind + r_S
          c_ind= c_ind + params.m

          # ------------ lidar cost ------------
          z= np.transpose(self.z_lidar)
          z= z
          n= z.shape[0]
          n_L= n / params.m_F
          if (n > 0):
              z_expected= self.return_expected_z_lidar(self.XX, self.association, params)
              innov= z - z_expected
              kron_inv_R_lidar= np.kron( np.seye(n_L) ,inv_R_lidar )
              kron_sqrt_inv_R_lidar=  np.kron( np.eye(n_L) , sqrt_inv_R_lidar )
              cost= cost + np.dot(np.dot(np.inv(innov), kron_inv_R_lidar),innov)
              # ------------------------------------
    
              # ---------- lidar submatrix ----------
              if (nargout > 1):
                  A_lidar= self.return_lidar_A(self.XX, self.association, params)
                  A[r_ind:r_ind + n, c_ind : c_ind + params.m]= A_lidar
        
                  b[r_ind : r_ind + n]= kron_sqrt_inv_R_lidar * innov
              end
              # ------------------------------------
          if (nargout > 1):
              # return hessian
              hessian= np.dot(np.transpose(A),A)
    
              # return the gradient
              grad= np.dot(hessian,delta) - np.dot(np.transpos(A),b)

          # divide cost by 2 to make Jacobian consistent
          cost= cost / 2
          return [cost, grad, hessian, A, b]
      # ----------------------------------------------
      # ----------------------------------------------  
      def return_A(self, x, params):
          # from a vector to cells
          self.from_vector_to_estimator(x, params)

          # initialize normalized Jacobian
          A= np.zeros((self.n_total, self.m_M))

          # plug the prior in A
          A[ 0:params.m, 0:params.m ]= sqrtm( self.Gamma_prior )

          # pointers to the next part of A to be filled
          r_ind= params.m + 1
          c_ind= 1

          # build A whithen Jacobian
          for i in range( self.M , -1 , 2):
              # ---------- gyro submatrix ----------
              A[ r_ind, c_ind : c_ind + params.m - 1 ]= np.dot(np.inv(-params.sig_gyro_z),np.array([0,0,1/params.dt_sim]))
              
              A[ r_ind, c_ind + params.m : c_ind + 2*params.m ]= np.dot(np.inv(params.sig_gyro_z),np.array([0,0,1/params.dt_sim]))
              # update the row index to point towards the next msmt
              r_ind= r_ind + 1
              # ------------------------------------
    
  
    
              # ---------- odometry submatrix ----------
              [Phi, D_bar]= self.return_Phi_and_D_bar(self.x_ph[i-1], self.odometry_ph[i-1][0], self.odometry_ph[i-1](1), params)
    
              [fart,S,V]= np.linalg.svd( D_bar )
              r_S= np.linalg.rank(S)
              sqrt_inv_D_bar= np.dot(sqrtm( inv(S[0:r_S,0:r_S]) ),np.inv(V[:,0:r_S+1]))
   
              A[ r_ind : r_ind + r_S, c_ind : c_ind + params.m]=  sqrt_inv_D_bar * Phi
    
              A[ r_ind : r_ind + r_S, c_ind + params.m : c_ind + 2*params.m]= -sqrt_inv_D_bar
    
              # update the row & column indexes
              r_ind= r_ind + r_S
              c_ind= c_ind + params.m
              # ------------------------------------
              # ---------- lidar submatrix ----------
              A_lidar= self.return_lidar_A(self.x_ph[i-2], self.association_ph[i-2], params)
              n_L= self.association_ph[i-2].shape[0]
              n= np.dot(n_L * params.m_F)
              A[ r_ind : r_ind + n, c_ind : c_ind + params.m]= A_lidar
              # update the row index
              r_ind= r_ind + n
              # ------------------------------------
          # ---------- gyro submatrix ----------
          A[ r_ind, c_ind : c_ind + params.m ]= np.dot(np.inv(-params.sig_gyro_z),np.array([0,0,1/params.dt_sim]))
          A[ r_ind, c_ind + params.m : c_ind + 2*params.m  ]= np.dot(np.inv(params.sig_gyro_z),np.array([0,0,1/params.dt_sim]))
          # update the row index to point towards the next msmt
          r_ind= r_ind + 1
          # ------------------------------------
          # ---------- odometry submatrix ----------
          [Phi, D_bar]= self.return_Phi_and_D_bar(self.x_ph[i-1], self.odometry_k[0], self.odometry_k[1], params)
          [fart,S,V]= np.linalg.svd(D_bar)
          r_S= np.linalg.rank(S)
          sqrt_inv_D_bar= np.dot(sqrtm( np.inv(S[0:r_S,1:r_S]) ),np.inv(V[:,1:r_S]))

          A[ r_ind : r_ind + r_S, c_ind : c_ind + params.m]= sqrt_inv_D_bar * Phi

          A[ r_ind : r_ind + r_S, c_ind + params.m : c_ind + 2*params.m]= -sqrt_inv_D_bar

          # update the row & column indexes
          r_ind= r_ind + r_S
          c_ind= c_ind + params.m
          # ------------------------------------


          # ---------- lidar submatrix ----------
          A_lidar= self.return_lidar_A(self.XX, self.association, params)
          n_L= length(self.association)
          n= np.dot(n_L,params.m_F)
          A[ r_ind : r_ind + n, c_ind : c_ind + params.m]= A_lidar
          return A
          # ------------------------------------
      # ----------------------------------------------
      # ----------------------------------------------  
      def return_jacobian_A(self, params):


          # initialize normalized Jacobian
          self.A= np.zeros((self.n_total, self.m_M))

          # plug the prior in A
          self.A[ 0:params.m+1, 0:params.m]= sqrtm( np.inv(self.PX_prior) )

          # pointers to the next part of A to be filled
          r_ind= params.m + 1
          c_ind= 1

          # build whithen Jacobian A
          for i in range(self.M-1 , -1 , 0):
    
              # gyro msmt submatrix
              self.A[ r_ind, c_ind : c_ind + params.m]= np.dot(np.inv( -params.sig_gyro_z),np.array([0, 0, 1/params.dt_sim]))
              self.A[ r_ind, c_ind + params.m : c_ind + 2*params.m]= np.dot(np.inv( params.sig_gyro_z),np.array([0, 0, 1/params.dt_sim]))
    
              # update the row index to point towards the next msmt
              r_ind= r_ind + 1
    
    
              if (i == 0):
        
                  # plug steering angle and wheel speed model in A
                  [fart,S,V]= np.linalg.svd( estimator.D_bar )
                  r_S= np.linalg.rank(S)
                  D_bar_p= np.dot(sqrtm( np.inv(S[0:r_S,0:r_S]) ), np.transpose(V[:, 0:r_S]))
        
                  self.A[ r_ind : r_ind + r_S, c_ind : c_ind + params.m]= D_bar_p * estimator.Phi_k
        
                  self.A[ r_ind : r_ind + r_S - 1, c_ind + params.m : c_ind + 2*params.m]= -D_bar_p
        
                  # update the row & column indexes
                  r_ind= r_ind + r_S
                  c_ind= c_ind + params.m
        
                  # plug lidar model in A
                  self.A[  r_ind : r_ind + estimator.n_k ,c_ind : c_ind + params.m ]= np.dot(np.kron( np.eye( estimator.n_L_k ) , params.sqrt_inv_R_lidar ),estimator.H_k)
        
                  # record lidar msmt indieces in A
                  self.abs_msmt_ind= np.array([ self.abs_msmt_ind,reshape[ r_ind : r_ind + estimator.n_k - 1, params.m_F , [] ] ])
        
        
              else:
        
        
                  # plug steering angle and wheel speed model in A
                  [fart,S,V]= svd( self.D_bar_ph[i+1] )
                  r_S= np.linalg.rank(S)
                  D_bar_ph_p= np.dot(sqrtm( np.inv(S[0:r_S,0:r_S]) ),np.transpose(V[:,1:r_S]))
        
                  self.A[ r_ind : r_ind + r_S , c_ind : c_ind + params.m ]= D_bar_ph_p * self.Phi_ph[i]
                  self.A[ r_ind : r_ind + r_S , c_ind + params.m : c_ind + 2*params.m]= -D_bar_ph_p
        
                  # update the row & column indexes
                  r_ind= r_ind + r_S
                  c_ind= c_ind + params.m
        
                  # lidar Jacobian part
                  n_L_i= self.n_ph[i-1] / params.m_F
                  self.A[ r_ind : r_ind + self.n_ph( i ),c_ind : c_ind + params.m]= np.dot(np.kron( eye(n_L_i) , params.sqrt_inv_R_lidar ),self.H_p[i-1])
        
                  # record lidar msmt indieces in A
                  self.abs_msmt_ind= self.abs_msmt_ind= np.array([ self.abs_msmt_ind,reshape[ r_ind : r_ind + estimator.n_k - 1, params.m_F , [] ] ])                
                 # update the row index
                  r_ind= r_ind + self.n_ph[i-1]
                  return A
      # ----------------------------------------------
      # ----------------------------------------------  
      def return_lidar_A(self, x, association, params):
          # this function computes one part of the jacobian for the lidar msmts at
          # the corresponding time where estimate x and association occur


          # number of landmarks and msmts at this time
          n_L= association.shape[0]
          n= np.dot(n_L,params.m_F)

          # needed parameters
          spsi= math.sin(x[ params.ind_yaw-1 ])
          cpsi= math.cos(x[ params.ind_yaw-1 ])

          # initialize
          A= inf * ones( n , params.m )
          for i  in range(1,n_L):
              # Indexes
              inds= 2*i + np.array([-1,0])
    
              dx= self.landmark_map[association[i-1], 1] - x[0]
              dy= self.landmark_map[association[i-1], 2] - x[1]
    
              # Jacobian -- H
              A[inds,2]= np.array([[-cpsi], [spsi]])
              A[inds,3]= np.array([[-spsi],[ -cpsi]])
              A[inds,params.ind_yaw]= np.array([[-dx * spsi + dy * cpsi],
                               -[dx * cpsi - dy * spsi]])
                                         
              # whiten Jacobian
              A[inds, :]= np.dot(params.sqrt_inv_R_lidar,A[inds, :])
              return A
      # ----------------------------------------------
      # ---------------------------------------------- 
      def update_preceding_horizon(self, params):

          # update odometry msmts in the ph
          self.odometry_ph= [math.inf, self.odometry_k, self.odometry_ph[2:self.M]]

          # update odometry msmts in the ph
          self.z_gyro_ph= [math.inf, self.z_gyro, self.z_gyro_ph[2:self.M]]

          # update lidar msmts in the ph
          z_lidar= np.transpose(self.z_lidar)
          self.z_lidar_ph= [z_lidar, self.z_lidar_ph[0:self.M]]

          # update the previous poses
          self.x_ph= [self.XX, self.x_ph[0:self.M]]

          # update the associations in the ph
          self.association_ph= [self.association, self.association_ph[0:self.M]]

          # update the number of associations in the ph
          self.n_L_k_ph= np.array([ [self.n_L_k], [self.n_L_k_ph[0:self.M ]] ])
      # ----------------------------------------------
      # ---------------------------------------------- 
      def return_odometry_update(self, x, u, params):
          # u: [velocity steering angle]

          vel= u[0]
          phi= u[1]

          # True State
          x= math.array([[x[0] + vel * params.dt_sim * math.cos(phi + x[2])],
          [x[1] + vel * params.dt_sim * math.sin(phi + x[2])],
          [pi_to_pi.pi_to_pi( x[2] + vel * params.dt_sim * math.sin(phi) / params.wheelbase_sim)] ])
          return x
      # ----------------------------------------------
      # ---------------------------------------------- 
      def  return_Phi_and_D_bar(self, x, vel, phi, params):
           # this function computes the state evolution matrix and its noise at the
           # corresponding time where the estimate is x and the odometry inputs are
           # vel and phi
           # compute variables
           s= math.sin(phi + x[params.ind_yaw-1])
           c= cos(phi + x[params.ind_yaw-1])
           vts= np.dot(np.dot(vel,params.dt_sim ),s)
           vtc= np.dot(np.dot(vel,params.dt_sim),c)  
     
           # state evolution model jacobian
           Phi=np.array( [[1,0,-vts],
                 [0,1,vtc],
                 [0,0,1]])

           # controls jacobian (only steering angle and wheel velocity)
           Gu=np.array( [[params.dt_sim * c,                       -vts],
                [params.dt_sim * s,                        vtc],
                [params.dt_sim * sin(phi)/params.wheelbase_sim, np.dot(np.dot(np.dot(vel,params.dt_sim),math.cos(phi)),np.inv(params.wheelbase_sim))]])

           # projection of controls uncertainty on the state (only steering angle and wheel velocity)
           D_bar= np.dot(np.dot(Gu,params.W_odometry_sim),np.transpose(Gu))

           return [Phi, D_bar]
      # ----------------------------------------------
      # ---------------------------------------------- 
      def solve(self, counters, params):

          # number of associated landmarks at k
          self.n_L_k= self.association.shape[0]


          # compute M
          if (params.SWITCH_FIXED_LM_SIZE_PH==1):
       
              # initialize number of associations in the ph
              self.n_L_M= self.n_L_k

              is_enough_landmarks= 0
              for M in range(1,self.n_L_k_ph.shape[0]):
                  self.n_L_M= self.n_L_M + self.n_L_k_ph[M-1]
        
                  # if the preceding horizon is long enough --> stop
                  if (self.n_L_M >= params.min_n_L_M): 
                      is_enough_landmarks= 1
                      M= M + 1
                      break

    
              # check that there are enough associations
              if (is_enough_landmarks == 1 and self.x_ph.shape[0] >= M and not isempty(self.x_ph[M-1])):
                  self.M= M
              else: # increase ph
                  self.M= self.M + 1
                  return
    
          else:
              # number of associations in the ph
              self.n_L_M= self.n_L_k + sum( self.n_L_k_ph[1:self.M] )
    
              # check that there are enough epochs
              if (counters.k_lidar <= self.M):
                 return


          # total number of states to estimate
          self.m_M= np.dot((self.M + 1),params.m)

          # total number of measurements (relativ + absolute + prior)
          self.n_total= self.m_M + np.dot(self.n_L_M,params.m_F)

          # create optimization function 
          fun= self.optimization_fn(x, params)

          # from cells to vectors
          x_star= self.from_estimator_to_vector(params)

          # saves the prior separately
          self.x_prior= self.x_ph[self.M-1]

          # solve the problem
          [residual, grad, A, b]= self.optimization_fn(x_star, params)

          [x_star, self.q_d,fart,fart1,fart2,hessian] = op.fminimize(fun, x_star, params.optimoptions)

          # multiply by two so that it fits the non-central chi-squared
          self.q_d= self.q_d * 2

          # compute detector threshold
          self.T_d= chi2.ppf( 1- params.continuity_requirement, self.n_total - self.m_M )

          # debuggin points to check jacobian
          # [residual, grad, A, b]= self.optimization_fn_fg(x_star, params)
          # dif= inv(A'*A) - inv(hessian)
          # dif( abs(dif) < 5 )= 0
          # diag(dif)

          # from a vector to cells 
          from_vector_to_estimator(self, x_star, params)

          # store the prior, x_(k-M+1), as a future msmt
          # hessian= A' * A

          self.PX= np.inv( hessian )
          self.PX_prior= self.PX[ params.m + 1 : 2 * params.m+1 , params.m + 1 : 2 * params.m+1 ]
          self.PX= self.PX[ self.PX[self.PX.shape[0]] - params.m + 1 : self.PX[self.PX.shape[0]] , self.PX[self.PX.shape[0]] - params.m + 1 : self.PX[self.PX.shape[0]]  ]
          # self.Gamma_prior= inv( PX(params.m+1:2*params.m, params.m+1:2*params.m) )
