% Efficiency_count.m - field efficiency count
% By Viacheslav Adamchuk
% Modified: 04/10/2009

% Input data
clear all; close all; clc;
infile='Example.txt';           % Input data
outfile='Example_count.txt';    % Output data
width = 4.57;                   % Swath width, m
speed = 1.31;                   % Average operation travel speed, m/s
time = 1;                       % Log interval, s

% Definition of parameters
disp('Loading data...');
rawdata = load(infile);
maxpar=max(rawdata);            % Maximum field extend, m
xmax = ceil(maxpar(1)); 
ymax = ceil(maxpar(2));
points = size(rawdata,1);       % Number of records
gc1(1:ymax,1:xmax)=0;           % Coverage 1
gc2(1:ymax,1:xmax)=0;           % Coverage 2
gc3(1:ymax,1:xmax)=0;           % Coverage of stops
dfixed = speed*time;            % Fixed distance per time interval, m 
istep = round(5*width)*2;             
t1=0; t2=0;

% Calculations
disp('Calculations');
for point=1:points-1
disp (['Processing point ',int2str(point),' of ',int2str(points)]);
if rawdata(point,3)==time
    x1 = rawdata(point,1); y1 = rawdata(point,2); x2 = rawdata(point+1,1); y2 = rawdata(point+1,2);
    % Average X,Y
    dbp = sqrt((y2-y1)^2+(x2-x1)^2);
    x0 = (x1+x2)/2; y0 = (y1+y2)/2;
    % Angle alpha
    if x1 == x2
        if y2 > y1
            alpha = 0.5*pi;
        elseif y2 < y1
            alpha = 1.5*pi;        
        end
    elseif x1 > x2
        alpha = pi + atan((y2-y1)/(x2-x1));
    else 
        alpha = atan((y2-y1)/(x2-x1));
    end
    % IJ coordinates (variable d)
    if dbp > 0
        jstep = round (5*dbp)*2;
        for i = -istep/20+0.05:0.1:istep/20-0.05
            for j = -jstep/20+0.05:0.1:jstep/20-0.05
                t1=t1+1;
                r=sqrt(i^2+j^2);
                if i<0
                    th=pi+atan(j/i);
                else
                    th=atan(j/i);
                end
                x=round(x0+r*cos(th+alpha+pi/2));
                y=round(y0+r*sin(th+alpha+pi/2));
                if and(x>0,y>0)
                    if and(x<=xmax,y<=ymax) 
                        t2=t2+1;
                        gc1(y,x)=gc1(y,x)+1;
                    end
                end
            end
        end          
    end
    % IJ coordinates (fixed d)
    jstep = round (5*dfixed)*2;
    for i = -istep/20+0.05:0.1:istep/20-0.05
        for j = -jstep/20+0.05:0.1:jstep/20-0.05
            r=sqrt(i^2+j^2);
            if i<0
                th=pi+atan(j/i);
            else
                th=atan(j/i);
            end
            x=round(x0+r*cos(th+alpha+pi/2));
            y=round(y0+r*sin(th+alpha+pi/2));
            if and(x>0,y>0)
                if and(x<=xmax,y<=ymax)
                    gc2(y,x)=gc2(y,x)+1;
                    if dbp==0 
                        gc3(y,x)=gc3(y,x)+1;
                    end
                end
            end
        end
    end
end
end

% Data saving
disp('Saving data...');
fid=fopen(outfile,'w');
fprintf(fid,'X\tY\tCount1\tCount2\tCount3\n');
for x=1:xmax
    for y=1:ymax
        fprintf(fid,'%i\t%i\t%i\t%i\t%i\n',x,y,gc1(y,x),gc2(y,x),gc3(y,x));
    end
end
fclose(fid);
disp('Script execution is complete');