%% prepare
COM_CloseNXT all
close all
clear all
format compact


%% open NXT
h = COM_OpenNXT('bluetooth.ini', 'check');
COM_SetDefaultNXT(h);


%% settings
port = MOTOR_B;
measuringTime = 3; % seconds
%programName = 'test';
%programName = 'TestRampMotorCo';
%programName = 'ManualMotorRamp';
%programName = 'NXTG_Wait1100_M';
programName = 'NXTG_MovePwr100';
%programName = 'NXTG_MotorPwr60';


%% precall functions to fill matlab cache
tic;
tmp = toc;
tmp = NXT_GetOutputState(port);




%% initialize vars
runState    = zeros(measuringTime * 1000, 1);
mode        = runState;
regMode     = runState;
tachoCount  = runState;
tachoLimit  = runState;
power       = runState;
time        = runState;
speed       = runState;


%% start embedded program
NXT_StopProgram();
pause(1);
NXT_StartProgram(programName);
pause(1)




%% record stuff...
j = 0;
tic;
while(toc < measuringTime)
    j = j + 1;
    
    tmp = NXT_GetOutputState(port);
    
    time(j)         = toc;
    runState(j)     = tmp.RunStateByte;
    mode(j)         = tmp.Mode;
    regMode(j)      = tmp.RegModeByte;
    tachoCount(j)   = tmp.TachoCount;
    tachoLimit(j)   = tmp.TachoLimit;
    power(j)        = tmp.Power;
    
    
end%while


%% cut off 
time        = time(1:j);
runState    = runState(1:j);
mode        = mode(1:j);
regMode     = regMode(1:j);
tachoCount  = tachoCount(1:j);
tachoLimit  = tachoLimit(1:j);
power       = power(1:j);
speed       = speed(1:j);
speed1      = speed;
speed2      = speed;
speed3      = speed;
accel       = speed;

%% smooth out tachoCount
tachoCount = moving_average(tachoCount, 4);


%% calculate real speed, simple method
% central difference, only accurate to order of h (i.e. delta t)
% also very sensitive to noise
speed0 = [0; diff(tachoCount) ./ diff(time)];

%% calculate real speed
for j = 2 : length(tachoCount) - 1
    % see http://numericalmethods.eng.usf.edu/mws/gen/02dif/mws_gen_dif_ppt_discrete.pdf
    % sheet 18
    
    % 3 points for lagrangian interpolation
    t0 = time(j-1);
    t1 = time(j);
    t2 = time(j+1);
    % we evaluate at t1...
    t = t1;
    
    speed(j) =             ((2*t - (t1 + t2)) / ((t0 - t1) * (t0 - t2))) * tachoCount(j-1);
    speed(j) = speed(j) +  ((2*t - (t0 + t2)) / ((t1 - t0) * (t1 - t2))) * tachoCount(j);
    speed(j) = speed(j) +  ((2*t - (t0 + t1)) / ((t2 - t0) * (t2 - t1))) * tachoCount(j+1);
    
end%for
speed1 = speed;

%% calculate real speed, alternative method...
for j = 3 : length(tachoCount) - 2
    % use central differences aka the 5-point-stencil
    % average spacing (the best we can do)
    h = (time(j+2) - time(j-2)) / 5;
    speed2(j) = (tachoCount(j-2) - 8*tachoCount(j-1) + 8*tachoCount(j+1) - tachoCount(j+2)) / (12*h);
end%for

%% final algo, this time 9-point method...
coeffs = [3; -32; 168; -672; 0; 672; -168; 32; -3];
for j = 5 : length(tachoCount) - 4
    % use central differences aka the 9-point-stencil
    % average spacing (the best we can do)
    h = (time(j+4) - time(j-4)) / 9;
    for k = 1 : 9
        speed3(j) = speed3(j) + coeffs(k) * tachoCount(j + k-5);
    end%for
    speed3(j) = speed3(j) / (840*h);
            
end%for


% I finally decided to take speed3
n = 4;
speed = moving_average(speed3, n);


%% get acceleration
coeffs = [3; -32; 168; -672; 0; 672; -168; 32; -3];
for j = 5 : length(tachoCount) - 4
    % use central differences aka the 9-point-stencil
    % average spacing (the best we can do)
    h = (time(j+4) - time(j-4)) / 9;
    accel(j) = 0;
    for k = 1 : 9
        accel(j) = accel(j) + coeffs(k) * speed(j + k-5);
    end%for
    accel(j) = accel(j) / (840*h);
            
end%for


%% plot position, speeds


figure('Name', 'Speed vs. Time')
hold all
h1 = plot(time, tachoCount, '.-');
h2 = plot(time, power * 10, '.-');

% n = 4;
% plot(time, moving_average(speed0, n) .* 0.5, '-');
% plot(time, moving_average(speed1 , n) .* 0.5, '-');
% plot(time, moving_average(speed2, n) .* 0.5, '-');
% plot(time, moving_average(speed3, n) .* 0.5, '-');

h3 = plot(time, speed .* 0.5, '-');
h4 = plot(time, moving_average(accel, 10) .* 0.05, '-');

%plot(time, runState * 3, '-')
%plot(time, mode * 3, '-')

offset = true;
for j = 2 : length(mode)
    if mode(j) ~= mode(j-1)
        x = time(j);
        y = -350;
        offset = ~offset;
        if offset y = y + 50; end
        
        [isMOTORON isBRAKE isREGULATED] = byte2outputmode(mode(j));
        if isMOTORON;   name = 'ON '; else name = 'OFF ';  end
        if isBRAKE;     name = [name 'BRK '];     end
        if isREGULATED; name = [name 'REG ']; end
        
        c = [191/255 191/255 0];
        text(time(j), y, name, 'Color', c);
        %set(h5, 'EdgeColor', 'black')
        h5 = line([x; x], [0; y * 1.1], 'Color', c);
    end%if
end%for

offset = true;
for j = 2 : length(runState)
    if runState(j) ~= runState(j-1)
        x = time(j);
        y = -200;
        offset = ~offset;
        if offset y = y + 50; end
        
        c = [191/255 0 191/255];
        text(time(j), y, byte2runstate(runState(j)), 'Color', c);
        %set(h6, 'EdgeColor', 'black')
        h6 = line([x; x], [0; y * 1.1], 'Color', c);
    end%if
end%for



h6b = plot(time, tachoLimit, 'g--');

h7 = plot([0; measuringTime], [0; 0], '-k');
h8 = plot([0; measuringTime], [1000; 1000], '-k');



%legend('TachCount', 'Power * 10', 'Simple Diff', 'Lagrangian 3-point-poly', '5-point-stencil', '9-point-stencil', 'Start: 0', 'Target: 1000')
legend([h1 h2 h3 h4 h5 h6 h6b h7 h8], 'TachoCount', 'Power * 10', 'Speed (avg.) / 2', 'Acceleration (avg.) / 20', 'MODE', 'RUNSTATE', 'TachoLimit', 'Start: 0', 'Target: 1000')

axis([0 measuringTime -500 1100])
xlabel('Time [s]')
ylabel('TachoCount [degrees]')

text(measuringTime, -570, '(C) Institute of Imaging & Computer Vision, RWTH Aachen University', 'HorizontalAlignment', 'right');
text(measuringTime, -610, 'http://www.mindstorms.rwth-aachen.de', 'HorizontalAlignment', 'right');




%% Plot everything vs position!
figure('Name', 'Speed vs. Position')
hold all


tmp = find(tachoCount);
first = tmp(1);
[forget last] = max(tachoCount);


h1 = plot([tachoCount(first); tachoCount(last)], [0; 0], '-');

h2 = plot(tachoCount(first:last), power(first:last) * 10, '.-');

h3 = plot(tachoCount(first:last), speed(first:last) .* 0.5, '-');
h4 = plot(tachoCount(first:last), moving_average(accel(first:last), 10) .* 0.05, '-');


%plot(time, runState * 3, '-')
%plot(time, mode * 3, '-')

offset = true;
for j = 2 : length(mode)
    if mode(j) ~= mode(j-1)
        x = tachoCount(j);
        y = -350;
        offset = ~offset;
        if offset, y = y + 50; end
        
        [isMOTORON isBRAKE isREGULATED] = byte2outputmode(mode(j));
        if isMOTORON;   name = 'ON '; else name = 'OFF ';  end
        if isBRAKE;     name = [name 'BRK '];     end
        if isREGULATED; name = [name 'REG ']; end
        
        c = [191/255 191/255 0];
        text(tachoCount(j), y, name, 'Color', c);
        %set(h5, 'EdgeColor', 'black')
        h5 = line([x; x], [0; y * 1.1], 'Color', c);
    end%if
end%for

offset = true;
for j = 2 : length(runState)
    if runState(j) ~= runState(j-1)
        x = tachoCount(j);
        y = -200;
        offset = ~offset;
        if offset, y = y + 50; end
        
        c = [191/255 0 191/255];
        text(tachoCount(j), y, byte2runstate(runState(j)), 'Color', c);
        %set(h6, 'EdgeColor', 'black')
        h6 = line([x; x], [0; y * 1.1], 'Color', c);
    end%if
end%for



%legend('TachCount', 'Power * 10', 'Simple Diff', 'Lagrangian 3-point-poly', '5-point-stencil', '9-point-stencil', 'Start: 0', 'Target: 1000')
legend([h1 h2 h3 h4 h5 h6], '0-line (x-axis)', 'Power * 10', 'Speed (avg.) / 2', 'Acceleration (avg.) / 20', 'MODE', 'RUNSTATE')

axis([tachoCount(first) tachoCount(last) -500 1100])
xlabel('TachoCount [degrees]')
ylabel('Speed / Acceleration / Power')

text(tachoCount(last), -570, '(C) Institute of Imaging & Computer Vision, RWTH Aachen University', 'HorizontalAlignment', 'right');
text(tachoCount(last), -610, 'http://www.mindstorms.rwth-aachen.de', 'HorizontalAlignment', 'right');