%% GNSS script - Zephyr
% ref  = reference, measured with encoder and UTC time
% gnss = tested system

%% Setup
% Set manually, default: 0
% 0 - Do all below
% 1 - Clear, load data, basic plot
% 2 - Crop timeframes, synchronize time (default = 0)
% 3 - Synchronize datapoints (default = 1)
% 4 - Compute initial yaw and circle middle point
% 5 - Analyze
Step =5;

%% Main 

%% Step 1 - Clear, load data, plot time and heading
if (Step == 0)||(Step == 1)
    % Clear    
    clc
    close all
    clearvars -except Step
    R = 1.025;
    disp('Workspace cleared. Loading files...')
    
    % File paths
    gnssPath = 'gnss.csv';
    refPath = 'refZephyr.txt';
    
    % Load data
    dev = loadGPScsv(gnssPath);
    ref = readlog_enc(refPath,'sp-cepl');
    disp('Data loaded.')
    
    % Filter 50, 
    temp = ref.sub_id;
    ref = structfun(@(x) ( x(temp<50) ), ref,'UniformOutput', 0);
    
    % Days in gps week
    days = floor(dev.t_gps(1)/86400);

    % Time in day (week-days)
    dev.Time = dev.t_gps - 86400*days;

    % Encoder time [s]
    ref.Time = (ref.t_utc + ref.sub_id/50);
    ref.t = ref.Time;
    
    % Plot timelines
    figure()
    subplot(2,1,1)
    plot(ref.Time,'o')
    hold on
    plot(dev.Time,'o')
    title('Timeframes')
    xlabel('Samples [-]')
    ylabel('Time [s]')
    legend('Reference time','GNSS time')
    grid on

    % Plot heading
    subplot(2,1,2)
    plot(ref.Time,(2*pi-ref.ticks/20000*2*pi)+2*pi)
    hold on
    plot(dev.Time, dev.yaw)
    title('Raw heading')   
    xlabel('Time [s]')
    ylabel('Heading [rad]')
    legend('Reference + 2pi offset','GNSS yaw') 
    grid on
    figure()
    
    plot((2*pi-ref.ticks/20000*2*pi)+2*pi)
    hold on
    plot(dev.yaw)
    title('Raw heading')   
    xlabel('Samples [-]')
    ylabel('Heading [rad]')
    legend('Reference + 2pi offset','GNSS yaw') 
    grid on
    if (Step == 1) 
        return
    end
end

%% Step 2 - Crop timeframes, synchronize GNSS and UTC
if (Step == 0)||(Step == 2)
    % Crop options
    timeCropChoice = input('Crop timelines automatically (0) or pick segment manually(1)?:');
    if 1 == timeCropChoice
        disp(['Available timeframe - reference: ', num2str(ref.Time(1)),' - ',num2str(ref.Time(end))])
        disp(['Available timeframe - gnss: ', num2str(dev.Time(1)),' - ',num2str(dev.Time(end))])
        StartTime = input('Pick start time [s]:');
        EndTime = input('Pick end time [s]:');
        % TODO check
        
    else
        disp('Cropping automatically:')
        % Crop start - which timeline starts later
        if dev.Time(1)<ref.Time(1)
            StartTime = ref.Time(1);
            disp('Reference starts later.')
        else
            StartTime = dev.Time(1);
            disp('GNSS starts later.')
        end
        disp(['Start time: ', num2str(StartTime)])
        
        % Crop end - which timeline ends first
        if dev.Time(end)<ref.Time(end)
            EndTime = dev.Time(end);
            disp('GNSS ends first.')
        else
            EndTime = ref.Time(end);
            disp('Reference ends first.')
        end
        disp(['End time: ', num2str(EndTime)])
    end
    
    disp('Cropping datasets...')
    UTCoffset = -18;
    dev = structfun(@(x) ( x(dev.Time>StartTime & dev.Time<EndTime) ), dev,'UniformOutput', 0);
    ref = structfun(@(x) ( x(ref.Time>(StartTime+UTCoffset) & ref.Time<(EndTime+UTCoffset)) ), ref,'UniformOutput', 0);
    dev.t_gps = dev.t_gps + UTCoffset;
    
    % Days in gps week
    days = floor(dev.t_gps(1)/86400);

    % Time in day (week-days)
    dev.Time = dev.t_gps - 86400*days;

    % Encoder time [s]
    ref.Time = (ref.t_utc + ref.sub_id/50);
    ref.Heading = 2*pi-ref.ticks/20000*2*pi;
    % Plot timelines
    disp('Plotting cropped timeframe and heading: ')
    figure()
    subplot(2,1,1)
    plot(ref.Time,'o')
    hold on
    plot(dev.Time,'o')
    title('Cropped timeframes')
    xlabel('Samples [-]')
    ylabel('Time [s]')
    legend('Reference time','GNSS time')
    grid on

    % Plot heading
    subplot(2,1,2)
    plot(ref.Time,(2*pi-ref.ticks/20000*2*pi)+2*pi)
    hold on
    plot(dev.Time, dev.yaw)
    title('Cropped raw heading')
    xlabel('Time [s]')
    ylabel('Heading [rad]') 
    grid on
    plot(dev.Time,dev.flags_pos)
    plot(ref.Time, ref.setpoint/10000)
    legend('Reference + 2pi offset','GNSS yaw','flags\_pos','motor setpoint')
    
    if Step == 2 
        return
    end
end



%% Step 3 - Synchronize datapoints
if (Step == 0)||(Step == 3)
    
    % Extract common time
    gnssT = round(dev.Time,2);
    refT = round(ref.Time,2);

    % Plot extracted timelines
    figure()
    plot(gnssT)
    hold on
    plot(refT)
    legend('gnss time','ref time')

    % Check length, if not equal, some samples are bad
    if not(all(size(gnssT) == size(refT)))

        datasetChoice = input('Datasets are not equal in length! Pick common points(0) or interpolate(1)?:');
        % Interpolate
        if 1 == datasetChoice
            disp('Data will be interpolated.')
            %vq = interp1(x,v,xq);
            [ref.TimeUnique, ref.Tindex] = unique(ref.Time);
            ref.Heading = interp1(ref.TimeUnique, ref.Heading(ref.Tindex), dev.Time);
            ref.mySetpoint = interp1(ref.TimeUnique, ref.setpoint(ref.Tindex), dev.Time);
            ref.Time = dev.Time;
            commonRef = ref.Heading;
            commonYaw = dev.yaw;
            commonT = dev.Time;
            
        % Pick common points
        else
            disp('Will use common points only.')
            tindex = 0;
            
            % Indexes for structure crop
            gnssSize = size(dev.t_gps);
            indexGNSS = zeros(gnssSize(1),1);
            refSize = size(refT);
            indexRef = zeros(refSize(1),1);
            
            for ind = 1:refSize(1)
                tempTime = refT(ind);
                % Check match
                if any(gnssT==tempTime)
                    tindex = tindex + 1;
                    
                    indexRef(ind,1) = 1;
                    tempIndex = gnssT==tempTime;
                    if sum(tempIndex)>1
                        disp('Double time!')
                        for temptempInd = 1:gnssSize(1)
                            aaaaaa = temptempInd;
                            if(tempIndex(aaaaaa) == 1)
                                tempIndex(aaaaaa) = 0;
                                break
                            end

                        end
                    end
                    indexGNSS(tempIndex) = 1; 
                    
                    commonT(tindex) = tempTime;
                    commonRef(tindex) = ref.Heading(ind);
                    
                    size(dev.yaw(gnssT==tempTime));
                    cropTemp = dev.yaw(gnssT==tempTime);
                    commonYaw(tindex) = cropTemp(1,1);
                end
            end
            commonRef = commonRef';
            commonYaw = commonYaw';
            commonT = commonT';
            disp('Cropping datasets...')
   
            dev = structfun(@(x) ( x(logical(indexGNSS)) ), dev,'UniformOutput', 0);
            ref = structfun(@(x) ( x(logical(indexRef)) ), ref,'UniformOutput', 0);
        end

    else
        commonRef = ref.Heading;
        commonYaw = dev.yaw;
        commonT = refT;
    end
    disp('Synchronization done.')
    if Step == 3 
        return
    end
end

%% Step 4 - Compute initial yaw and circle middle point
if (Step == 0)||(Step == 4)

    %% Compute initial yaw
    % plot samples
    figure()
    plot(ref.Heading)
    hold on
    plot(dev.yaw)
    title('Heading with offset')
    xlabel('Samples [-]')
    ylabel('Heading [rad]')
    legend('Reference heading','GNSS yaw') 
    grid on

    % 1-500
    headingOffsetStart = input('Insert start of heading synchronization (samples, must be >0)(8000): ');
    if headingOffsetStart < 1
        headingOffsetStart = 1;
    end
    headingOffsetEnd = input('Insert end of heading synchronization (samples)(18000): ');
    initYaw = nanmean(dev.yaw(headingOffsetStart:headingOffsetEnd));
    initEnc = nanmean(ref.Heading(headingOffsetStart:headingOffsetEnd));
    %initYaw = sum(commonYaw(1:1000))/1000;
    %initEnc = sum(commonRef(1:1000))/1000;

    commonRef = ref.Heading + (initYaw - initEnc);% TODO
    nanmean(commonRef)
    commonRef(commonRef>(2*pi)) = commonRef(commonRef>(2*pi))-(2*pi);
    nanmean(commonRef)
    commonRef(commonRef<0) = commonRef(commonRef<0)+(2*pi);
    nanmean(commonRef)

    figure()
    plot(commonT,commonYaw.*(360/2/pi))
    hold on
    plot(commonT,commonRef.*(360/2/pi))
    title('Heading with corrected offset')
    xlabel('Time [s]')
    ylabel('Heading [rad]')
    legend('GNSS yaw','Ref heading')

    figure()
    plot(commonT,commonYaw)
    hold on
    plot(commonT,commonRef+2*pi)
    title('Heading with corrected offset and ref with offset')
    xlabel('Time [s]')
    ylabel('Heading [rad]')
    legend('GNSS yaw','Ref heading')
    
    
    uwRef = unwrap(commonRef);
    uwGNSS = unwrap(commonYaw);

    sizeEnc = size(uwRef);

    figure()
    plot(commonT, uwRef.*(360/2/pi))
    hold on
    plot(commonT, uwGNSS.*(360/2/pi))
    title('Rozbalene natoceni, crop, synch')
    xlabel('Time [s]')
    ylabel('Heading [rad]')
    legend('Ref','GNSS')
    
    %% Urceni pocatecni polohy a smeru
    format long

    %initLat = dev.lat(1:500);
    %initLong = dev.lon(1:500);
    
    % Local coordinates
    X = dev.n;
    Y = dev.e;
    
    % Compute circle using R and heading
    % 1000 - 3000
    centerStart = input('Insert start of center computation (samples, must be >0)(20000): ');
    if centerStart < 1
        centerStart = 1;
    end
    centerEnd = input('Insert end of center computation (samples)(45000): ');
    initX = X(centerStart:centerEnd);
    initY = Y(centerStart:centerEnd);
    initYaw2 = dev.yaw(centerStart:centerEnd);

    % vectX = initX + cos(initYaw);
    % vectY = initY + sin(initYaw);

    encX = -R*cos(1*initYaw2) + initX;
    encY = -R*sin(1*initYaw2) + initY;

    SX = mean(encX);
    SY = mean(encY);

    disp(['Center of circle computed with heading using radius of ',num2str(R),' m: X = ',num2str(SX),' m, Y = ',num2str(SY), ' m.'])
    
    % Compute center using fit
    XY(:,1)=dev.n(centerStart:centerEnd);
    XY(:,2)=dev.e(centerStart:centerEnd);

    CirclePar = CircleFitByPratt(XY);
    disp(['Center of circle computed by fitting coordinates: X = ',num2str(CirclePar(1)),' m, Y = ',num2str(CirclePar(2)), ' m, R = ',num2str(CirclePar(3)),' m.'])

    figure()
    plot(X,Y)
    hold on
    plot(encX,encY)
    plot(initX,initY)
    plot(SX,SY,'ro')
    plot(CirclePar(1),CirclePar(2),'b+')
    title('Center of circle in local coordinates')
    xlabel('Local x [m]')
    ylabel('Local y [m]')
    legend('all gnss datapoints','middle','calculated points','S computed with heading', 'S computed by fit')
    axis equal
    
    if Step == 4 
        return
    end
end

%% Step 5 - Analyze
if (Step == 0)||(Step == 5)
     TimePoints = [47030, 47090;... % 0
                   47110, 47170;... % 0.8
                   47190, 47250;... % 1.6
                   47270, 47330;... % 2.4
                   47350, 47410;... % 3.2
                   47430, 47490;... % 4
                   47510, 47570;... % 4.8
                   47590, 47650;... % 5.6
                   47670, 47730;... % 6.4
                   47750, 47810;... % 5.6
                   47830, 47890;... % 4.8
                   47910, 47970;... % 4
                   47990, 48050;... % 3.2
                   48070, 48130;... % 2.4
                   48150, 48210;... % 1.6
                   48230, 48290;... % 0.8
                   48310, 48370]    %0
    pointsSize = size(TimePoints);
    for ind = 1:pointsSize(1)

    T1S = TimePoints(ind,1);
    T1E = TimePoints(ind,2);
    
    tmpIndex = (dev.Time>T1S & dev.Time<T1E);
    tmpTime = dev.Time(tmpIndex);
    
    tmpFlags = dev.flags_pos(tmpIndex);

    tmpTimeDiff= diff(tmpTime);
    
    mode1 = sum(tmpFlags==1);
    mode3 = sum(tmpFlags==3);
    mode7 = sum(tmpFlags==7);
    
    intervalT = T1E - T1S;
    
    F1Ind = tmpFlags==1;
    F3Ind = tmpFlags==3;
    F7Ind = tmpFlags==7;
    
    T_mode1 = sum(tmpTimeDiff.*F1Ind(1:end-1));
    T_mode3 = sum(tmpTimeDiff.*F3Ind(1:end-1));
    T_mode7 = sum(tmpTimeDiff.*F7Ind(1:end-1));
    
    timeSum = T_mode1 + T_mode3 + T_mode7;
    
    TP_mode1 = T_mode1*100/timeSum;
    TP_mode3 = T_mode3*100/timeSum;
    TP_mode7 = T_mode7*100/timeSum;
    
    TP_sum = TP_mode1 + TP_mode3 + TP_mode7;

    %% heading unwrap
    tmpRefHeading = commonRef(tmpIndex);
    tmpDevHeading = commonYaw(tmpIndex);
    tmpuwRef = unwrap(tmpRefHeading);
    tmpuwGNSS = unwrap(tmpDevHeading);
    tmpDelta = tmpuwRef - tmpuwGNSS;

    %% heading delta
    tmpdeltaYaw = tmpDevHeading - tmpRefHeading;
    tmpdeltaYaw(tmpdeltaYaw>3.14) = NaN;
    tmpdeltaYaw(tmpdeltaYaw<(-3.14)) = NaN;
    
    meanHeadingErr = nanmean(abs(tmpdeltaYaw.*(360/2/pi)));
    stdHeadingErr = nanstd(abs(tmpdeltaYaw.*(360/2/pi)));
    
    %% Abs chyba    
    aX = dev.n(tmpIndex);
    aY = dev.e(tmpIndex);

    %% Chyba absolutni polohy
    SSX = CirclePar(1);
    SSY = CirclePar(2);
    RR = CirclePar(3);
    
    mEncX = SSX + RR*cos(commonRef(tmpIndex));
    mEncY = SSY + RR*sin(commonRef(tmpIndex));

    correctX = mEncX;
    correctY = mEncY;

    % Absolutni chyba
    absChyba = sqrt((aX-correctX).^2 + (aY-correctY).^2);

    meanAbsErr = nanmean(absChyba);
    stdAbsErr = nanstd(absChyba);
    
    %%misc
    modus = mode(ref.mySetpoint(tmpIndex));
    %% Results
    results(:,ind) = [T1S;T1E;modus;...
        intervalT; timeSum; TP_sum;...
        T_mode1; T_mode3; T_mode7;...
        TP_mode1; TP_mode3; TP_mode7;...
        meanHeadingErr;stdHeadingErr;...
        meanAbsErr;stdAbsErr];
    
    figure()
    numOfPlots = 6;
    subplot(numOfPlots,1,1)
    plot(tmpTime, tmpRefHeading)
    ylabel('RefHeading')
    hold on
    
    subplot(numOfPlots,1,2)
    plot(tmpTime, tmpDevHeading)
    ylabel('DevHeading')
    
    subplot(numOfPlots,1,3)
    plot(tmpTime, tmpFlags)
    ylabel('Flags')
    
    subplot(numOfPlots,1,4)
    plot(tmpTime, tmpdeltaYaw)
    ylabel('deltaHeading')
    
    subplot(numOfPlots,1,5)
    plot(tmpTime, ref.mySetpoint(tmpIndex))
    ylabel('setpoint')
    
    subplot(numOfPlots,1,6)
    plot(tmpTime, absChyba)
    ylabel('position error')
    end
    results
    
    if Step == 5
        return
    end
end