classdef FollowGap < Navigation

    properties(Access=protected)
        sensor
        vehicle
        occ_map
        rturn
    end

    methods

        function ftg = FollowGap(world, vehicle, sensor, rturn)

            % invoke the superclass constructor
            ftg = ftg@Navigation(world);
            ftg.vehicle = vehicle;
            ftg.sensor = sensor;
            ftg.occ_map = OccMap2(sensor, vehicle);
            ftg.rturn = rturn;

        end


        function plan(ftg, goal)
            ftg.goal = goal;
        end

        function navigate_init(ftg, robot)

        end
        
        
        function n = next(ftg, robot)
            
            n = [];
            % these are coordinates (x,y)
            n = ftg.vehicle.step();
            

        end
        
        function readings = get_readings(ftg)
            readings = ftg.sensor.all_readings();

        end
        

        function [readings, gaps] = get_gaps(ftg)
            readings = ftg.sensor.all_readings();
            limits = ftg.sensor.theta_range;
            [readings, gaps] = get_polar_gaps(ftg, readings, limits);
        end
        
        function gap_center = gap_center_angle(ftg)
           [readings, gaps] = get_gaps(ftg);
           gap_size = gaps(:,2) - gaps(:,1);
           [max_gap_size, max_gap_index] = max(gap_size); 
           
           left_border = [ftg.sensor.r_range(2), ftg.sensor.theta_range(1)];
           right_border = [ftg.sensor.r_range(2), ftg.sensor.theta_range(2)];
           
           obstacles = [left_border; readings; right_border];
           
           left_obstacle = obstacles(max_gap_index,:);
           right_obstacle = obstacles(max_gap_index+1,:);
           d_obl = left_obstacle(1);
           ang_obl = abs(left_obstacle(2));
           
           d_obr = right_obstacle(1);
           ang_obr = abs(right_obstacle(2));
           
           gap_center = acos((d_obl + d_obr*cos(ang_obl + ang_obr))/(sqrt(d_obl*d_obl + d_obr*d_obr + 2*d_obl*d_obr*cos(ang_obl+ang_obr)))) - ang_obl;
        end
        
        function final_angle = get_final_angle(ftg, goal_angle, gap_coeff)
           [readings, gaps] = get_gaps(ftg);
           
           if ~isempty(gaps)
               gap_size = gaps(:,2) - gaps(:,1);
               [max_gap_size, max_gap_index] = max(gap_size); 

               d_obl = gaps(max_gap_index,4);
               ang_obl = gaps(max_gap_index,2);

               d_obr = gaps(max_gap_index,3);
               ang_obr = -gaps(max_gap_index,1);


               gap_center_angle = acos((d_obr + d_obl*cos(ang_obr + ang_obl))/(sqrt(d_obr^2 + d_obl^2 + 2*d_obr*d_obl*cos(ang_obr+ang_obl)))) - ang_obr;

               if ~isempty(readings)
                    d_min = min(readings(:,1));
                   % d_min2 = min(gaps(:,[3,4]))

                    final_angle = ((gap_coeff * gap_center_angle/d_min) + goal_angle)/(gap_coeff/d_min + 1);
               else
                   d_min = nan;
                   final_angle = goal_angle;
               end


           else
               final_angle = pi

           end
           
        end

       
      
       function [readings, gaps] = get_polar_gaps(ftg, readings, limits)
            
            readings = sortrows(readings,2);
            
            dTangent = sqrt(readings(:,1).^2 - ftg.vehicle.radius^2);
            
            obstacle_boundaries = zeros(size(readings));
            
            %Left boundaries:
            obstacle_boundaries(:,1) = readings(:,2) + atan((ftg.vehicle.radius) ./ dTangent);
            %Right boundaries:
            obstacle_boundaries(:,2) = readings(:,2) - atan((ftg.vehicle.radius) ./ dTangent);
            
            
            
            if(~isempty(dTangent))
                phifov = limits - [obstacle_boundaries(1,2), obstacle_boundaries(end,1)];
                dfov = abs([dTangent(1),dTangent(end)].*sin(phifov));
                
                dnhol = sqrt(ftg.rturn^2+readings(:,1).^2 - 2*ftg.rturn*readings(:,1).*cos(pi/2 - abs(readings(:,2)))) - ftg.rturn - ftg.vehicle.radius;
                
                phinhol = pi/2 + abs(readings(:,2)) - asin(ftg.rturn*sin(pi/2- abs(readings(:,2)))./(ftg.rturn + dnhol + ftg.vehicle.radius));
  
                alpha = phinhol/2;

                
                
                dnhol_borders = zeros(1,2);
                phinhol_ind = zeros(1,2);
                
                rightvals = dnhol(readings(:,2)<=0);
                leftvals = dnhol(readings(:,2)>=0);
                
                if ~isempty(rightvals)
                    [dnhol_borders(1),phinhol_ind(1)] = min(dnhol(readings(:,2)<=0));
                    if dnhol_borders(1) < dfov(1)
                        limits(1) = max(-1*alpha(phinhol_ind(1)),limits(1));
                    end
                end
                
                if ~isempty(leftvals)
                    [dnhol_borders(2),phinhol_ind(2)] = min(dnhol(readings(:,2)>=0));
                    if dnhol_borders(2) < dfov(2)
                        limits(2) = min(alpha(phinhol_ind(2)),limits(2));
                    end
                end
                
            end
            
            
            gaps = [];
            i = 0;
            right_edge = limits(1);
            right_dist = ftg.sensor.r_range(2);
            reached_end = false;
            
            while reached_end==false
                i = i+1;
                k = find(obstacle_boundaries(:,2) < right_edge & obstacle_boundaries(:,1) > right_edge);
                while(k)
                    [right_edge, right_ind] = max(obstacle_boundaries(k,1));
                    right_dist = readings(k(right_ind),1);
                    k = find(obstacle_boundaries(:,2) < right_edge & obstacle_boundaries(:,1) > right_edge);
                end
                
                if right_edge < limits(2)
                    k = find(obstacle_boundaries(:,2) > right_edge);
                    if k
                        [left_edge, min_ind] = min(obstacle_boundaries(k,2));
                        new_right_edge = obstacle_boundaries(k(min_ind),1);
                        left_dist = readings(k(min_ind),1);
                        new_right_dist = readings(k(min_ind),1);
                    else
                        left_edge = limits(2);
                        left_dist = ftg.sensor.r_range(2);
                        new_right_edge=nan;
                        reached_end = true;
                        new_right_dist = nan;
                    end

                    %%%%%For some reason, these are giving me imaginary
                    %%%%%distances. The question is why it is getting so
                    %%%%%close to the obstacles in the first place
                    right_dist = sqrt(right_dist^2 - (ftg.vehicle.radius)^2);
                    left_dist = sqrt(left_dist^2 - (ftg.vehicle.radius)^2);
                    
                    %right_dist = real(right_distb);
                    %left_dist = real(left_distb);
                    
                    gaps = [gaps; [right_edge, left_edge, right_dist, left_dist]];
                    right_edge = new_right_edge;
                    right_dist = new_right_dist;
                else
                    reached_end = true;
                end
            end

       end
       

    end % methods
end % classdef