clc
clear all
close all

%% Basic setup
Perf_Tune=[329.6 246.9 196 146.8 110 82.4];                                 %E A D G B e, [Hz]
Tolerance=0.2;                                                              %[Hz]

%% First inputs
Detune=-15+(30).*rand(1,1);
String=round(1+(6-1).*rand(1,1));
Freq_in=Perf_Tune(String)+Detune;

%% Motor setting
Min_step=360/(200*16);

%% Decision Algorithm

%% String recognition 
Diff=Perf_Tune-Freq_in;
Diff_abs=abs(Diff);
Diff_min=min(Diff_abs);
Target=find((Diff_abs==Diff_min));



%% Control algorithm
Linear_const=0.072;
Steps=0;

while Diff_abs(Target)>Tolerance
     disp("Input #"+Steps+"=  "+Freq_in+" Hz")
     disp("Difference #"+Steps+"= "+Diff_abs(Target)+" Hz")
     fi_target=(-(Freq_in-Perf_Tune(Target))/Linear_const)*(0.2);
     M_steps=round(fi_target/Min_step);
     New_freq=Linear_const*(M_steps*Min_step)+Perf_Tune(Target);
     disp("New #"+Steps+"= "+New_freq+" Hz")
     Diff_abs(Target)=abs(Perf_Tune(Target)-New_freq);
     Freq_in=New_freq;
     Steps=Steps+1;
     disp("---------------------")
     if Steps>30
         clc
         disp("ERROR - Too many steps")
         break
     end
end
disp("Target = "+Perf_Tune(Target)+" Hz")
disp("Steps = "+Steps)
