import numpy as np
import math
import scipy.io as sio
import sys
sys.path.insert(0,'../functions/')
import pi_to_pi
class EstimatorClassFgSimOff:
      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_lidar_H_k(self, params, FG, epoch): 
          # this funcion builds the Jacobian H of LMs msmts for the factor graphs...
          # case without actual mesaurements.

          spsi= math.sin(self.XX[9])
          cpsi= math.cos(self.XX[9])

          # landmarks in the field of view (saved from the online run)
          self.lm_ind_fov= FG.associations[epoch]

          # number of extracted landmarks (more specifically features)
          self.n_L_k= FG.associations[epoch].shape[0]

          # number of expected measurements
          self.n_k = np.dot(self.n_L_k,params.m_F)

          # build Jacobian
          self.H_k_lidar= np.zeros( (self.n_k , params.m) )
          for i in range(1,self.n_L_k):
              # Indexes
              indz= 2*i + np.array([-1,0])
              dx= self.landmark_map[self.lm_ind_fov[i], 1] - self.XX[0]
              dy= self.landmark_map[self.lm_ind_fov[i], 2] - self.XX[1]
    
              # Jacobian -- H
              self.H_k_lidar[indz,0]= np.array([[-cpsi], [spsi]])
              self.H_k_lidar[indz,1]= np.array([[-spsi], [-cpsi]])
              self.H_k_lidar[indz,params.ind_yaw]= np.concatenate((np.dot(-dx,spsi) + np.dot(dy,cpsi),np.dot(-dx,cpsi) - np.dot(dy,spsi)),axis = 1)               

      # ----------------------------------------------
      # ---------------------------------------------- 
      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 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 return_odometry_update(self, x, u, params):
          # u: [velocity steering angle]

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

          # True State
          x= [[x[0] + np.dot(np.dot(vel,params.dt_sim),math.cos(phi + x[2]))],
              [x[1] + np.dot(np.dot(vel,params.dt_sim),math.sin(phi + x[2]))],
              [pi_to_pi.pi_to_pi(x[2] + np.dot(np.dot(np.dot(vel,params.dt_sim),math.sin(phi))),np.inv( 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 compute_alpha(self,params):
          self.alpha= np.array([[-math.sin( self.XX(params.ind_yaw) )]
                      [math.cos( self.XX(params.ind_yaw) )]
                       [0] ])

