clear;
%% Parameters

scalingfactor=1; %2.5 for logging
% gather feedback (needs more time, scalingfactor should be increased!)
feedback = false;

% motor constant
Ke = 5.497; % Vrms/1000rpm
voltage = 30; % V
maxrpm = voltage/Ke*1000;
cnt_rev=5760; % cnts/rev

% parameters from exosceleton (experimentally determined)
maxcounts = 550e3;
maxctss = maxrpm * cnt_rev / 60; % rpm in counts/s
maxAngle = 65; % deg

% trajectory from MMM database
ParseMMMXML();
KW(:,1) = RKx_joint(:,1); % time in s
KW(:,2) = RKx_joint(:,2)*(180/pi); % angle of knee joint in rad
timestamp = KW(2,1);

% linear approximation of angle to motor counts
angle2Counts =@(x) x * maxcounts/maxAngle;
atc = angle2Counts(KW(:,2)); % Angle to counts

%% Trajectory modifications

% get velocity by numerical differenciation
d_atc = diff(atc) / timestamp;

% cut off at maxctss
d_atc_cut = d_atc;
d_atc_cut_idx = abs(d_atc) > maxctss;
d_atc_cut(d_atc_cut_idx) = d_atc(d_atc_cut_idx)./abs(d_atc(d_atc_cut_idx)) * maxctss;

% filter data using a median filter
d_atc_filtered = medfilt1(d_atc,3);

% scale filtered data, if above maxctss
d_atc_filtered_scaled = d_atc_filtered;
if (max(abs(d_atc_filtered)) > maxctss)
    d_atc_filtered_scaled = d_atc_filtered/max(abs(d_atc_filtered))*maxctss;
end

% scale time
if (max(abs(atc)) > maxcounts)
    atc_scaled = atc/max(abs(atc))*maxcounts;
end
d_atc_scaled = diff(atc_scaled)/timestamp;
d_atc_scaled_time = medfilt1(d_atc_scaled,3);
t_scale = scalingfactor*max(abs(d_atc_scaled_time))/maxctss;

% create output commands q and q_dot (pos and vel)
q = cumsum(d_atc_scaled_time./t_scale).*(timestamp*t_scale);
q(q>0)=0;
q_dot = d_atc_scaled_time./t_scale;
timesteps = KW(1:end-1,1).*t_scale;

%% visualization
figure(1); clf;
plot(KW(1:end-1,1), [cumsum(d_atc), cumsum(d_atc_cut) cumsum(d_atc_filtered), cumsum(d_atc_filtered_scaled)].*timestamp)
hold all
plot( KW(1:end-1,1).*t_scale, cumsum(d_atc_scaled_time./t_scale).*(timestamp*t_scale))
xlabel('Time [s]')
ylabel('Angle Knee [motorctns]')
legend('data', 'cutoff','median filter', 'median scaled','time scaled')
title('Trajectories Position');

figure(2); clf;
plot(KW(1:end-1,1), [d_atc d_atc_cut d_atc_filtered d_atc_filtered_scaled ])
hold all
plot( KW(1:end-1,1).*t_scale, d_atc_scaled_time./t_scale)
xlabel('Time [s]')
ylabel('Angular Knee velocity [motorcnts/s]')
legend('original','cutoff', 'median filter', 'median scaled', 'time scaled')
title('Trajectories Velocity');


%% execute trajectory
signaldelay = 0.03;
sizeKW = size(KW)-1;
dt =timesteps(2); %diskrete Zeitschrittweite

% connect
delete(instrfind())
port = '/dev/ttyUSB2';
sp =serial(port);  %Seriellen Port einbinden
set(sp,'BaudRate',57600); %Baudrate auf die Platine anpassen
fopen(sp); %Port oeffnen

% start motor
cmd =['UM=5' 13 'MO=1' 13];
fprintf(sp,cmd);
BA = sp.BytesAvailable;     %Echo weglesen bis nichts mehr in den Puffer geschrieben wird
while(BA>1)
  fscanf(sp) %Speicher leeren
  BA = sp.BytesAvailable;
end

%Zeitverzoegerung nach Motorstart Abwarten
t = clock; 
while etime(clock,t) < signaldelay       
    continue
end
 
fscanf(sp);   %Echo weglesen
 
PX = zeros(size(q));
for i= 1:1:sizeKW(1)-1
     
  startTime = clock; 
  speed = abs(q_dot(i));   %Geschwindigkeit berechnen
  sc = ['SP=' num2str(round(speed))];  %Geschwindigkeitskommando als String speichern
  pc = ['PA=' num2str(round(q(i)))]; %Positionskommando als String speichern
  cmd = [pc 13 sc 13 'BG' 13]; %Beide Kommandos k�nnen gleichzeitig geschickt werden. Mit BG wird die Bewegung begonnen
  fprintf(sp, cmd);       %Kommando abschicken

  %Echo weglesen bis nichts mehr in den Puffer geschrieben wird
  BA = sp.BytesAvailable;     
  while(BA>1)
    fscanf(sp); %Speicher leeren
    BA = sp.BytesAvailable;
  end

  % Derzeitige Position auslesen
  if feedback==true
    cmd = ['PX' 13];        %Kommando um die Position auszulesen
    fprintf(sp,cmd);
    t = clock;
    BA = sp.BytesAvailable;
    while BA<=1;       %Sobald das Elmo antwortet mit dem Auslesen beginnen.
       BA = sp.BytesAvailable;
       continue
    end 
    PX(i)=fscanf(sp,'%*s %i');     %Wert auslesen
  end
  
  %Derzeitige Geschwindigkeit auslesen
  % not implemented
  
  %Zeitverzoegerung Abwarten
  while (etime(clock,startTime) < dt)       
     continue
  end
end

%Motor ausschalten
cmd =['MO=0' 13];  
fprintf(sp,cmd);

fclose(sp);  %Verbindung schlie�en
delete(sp);  %Seriellen Port loeschen

%% plot feedback
if feedback==true
  figure(3)
  plot(timesteps(1:end-1), [q(1:end-1), PX])
  legend('soll', 'ist')
  xlabel('Time [s]')
  ylabel('Angle Knee [motorctns]')
  title('Trajectory feedback');
end