function dist = checkingLine(GapValue, L1, L2, n, j, points, threshold)
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function dist = checkingLine(GapValue, L1, L2, n, j, points)
%
% Task: Computes the distance between two points in the q1 q2 joint space
%       excepts when they can not be linked because it would collide a known obstacle in the X Y cartesian space
%       The 'colliding' (or not) is verified by sampling the line in q1 q2 joint space and computing the X Y corresponding parameters
%       for each of the sampled points on the line, as we know the obstacles position in the X Y cartesian space only
%
% Inputs:
%-GapValue: the distance between the q1 values of each sampled points along the line
%-L1: length of the first link of the 2d planar 2 link robot arm
%-L2: length of the second link of the 2d planar 2 link robot arm
%-n: index of the first given point in the 'points' matrix
%-j: index of the second given point in the 'points' matrix
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
%         the 1st column describing the 1st (start) point
%
% Outputs:
%	-nbNodes: number of nodes of the graph excluding the starting and goal points
%	-obstacle: a matrix containing the distance between connected nodes 
%		(NaN refers to not connected nodes)
%		The matrix has a size of (nbNodes+2)x(nbNodes+2)
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
%         the 1st column describing the 1st (start) point
%	
%		
%		
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  q1 = points(1,j);
  q2 = points(2,j);
  q1_ = points(1,n);
  q2_ = points(2,n);
  
  dist = 0; % 0 value, if not modified, will represent that we didn't note about an invalid link between the n and j points
  if n==j
    dist = NaN;
  endif
  if q1 != q1_
          
      %defining Q2 as a function of Q1
      A = (q2_- q2)/(q1_ - q1);
      B = q2 - A * q1;
      Q2 = @(Q1) A*Q1+B;
      
      %getting the sampling direction
      if q1 > q1_
        gap = -GapValue;
      else
        gap = GapValue;
      endif
      
      
      %getting the sample points
      for g=q1:gap:q1_
        Q1test = g;
        Q2test = Q2(g);
        
        %Is each sampled point from the line colliding with an obstacle in X Y cartesian space
        
        %[Xtest, Ytest]=MyFK(L1,L2,Q1test,Q2test); remove the % on that line and add it on the next ones for faster computation
        jTee=dh2ForwardKinematics([Q1test; Q2test], [0; 0], [L1; L2], [0; 0], 1);
        Xtest = jTee(1,4);
        Ytest = jTee(2,4);
        % if a sampled point is colliding and we haven't already noted it, we do
        if dist !=NaN && (Ytest >= L1 - threshold || Ytest <= -L1 + threshold || (Xtest>=-L2 - threshold && Xtest<=L2 - threshold && Ytest>=-L2 - threshold && Ytest<=L2 + threshold))
          % In order to avoid trajectories hitting the obstacle due to some rounding problems
          % we defined the obstacle area using a threshold
          dist = NaN;
        endif
        
      endfor
      if dist ==0 && n!=j % if we didn't note nothing about the link between n and j, we compute the distance
          dist = sqrt( (points(1,j)-points(1,n))^2 + (points(2,j)-points(2,n))^2);
      endif
    endif
      
endfunction