clc
close all
startplot
delete(instrfind)

s = serial('COM5','BAUD',19200);                      % Create serial object (PORT Dependent)
fopen(s)                                              % Open the serial port for r/w

% Send initial control signal u = zeros;
my_acc = zeros(1,100);                                % prepare 30 int16 byte array
u = zeros(1,100);
ubefore = u;

%%%%%% ALTER U FOR TX %%%%%%%%  THIS Removes bytes 128-151 as they are
%%%%%% reserved unicode characters.  This looses 5 bits of resolution and
%%%%%% is an area for improvement
u = round(u * 225/256);
for j = 1:numel(u)
    if u(j) > 127 
        u(j) = u(j) + 32; 
    end
    if u(j) > 255
        u(j) = 255;
    end
    if u(j) <= 0
        u(j) = 1;
    end
end

%%%%%% TRANSMIT Initial guess to PIC %%%%%%%%
for i = 1:numel(u)
    fprintf(s, '%s', char(u(i)));
    %fprintf(s, '%x', 80);
    pause(0.01)
end


% Main loop, this processes the continuous operation of the program
while(true)
    % Read acc data from PIC
    for i = 1:100
        in = fscanf(s);                               % Read Data back from PIC from Serial port
        comma = find(in == ',');                      % Process data in form xxx,xxx (look for commas)
        first_num = in(1:comma);
        second_num = in(comma+1:end);
        my_acc(i) = str2double(second_num);
    end
    
    my_acc = smooth(my_acc);                          % Call smoothing function.  Peforms sliding window n = 3
    
    % Convert acceleration data to G's
    scale = 2;                                        % 2G scale
    my_acc = my_acc ./ (2^10-1);                      % normalize from 0 to 1
    my_acc = my_acc - 0.5;                            % subtract to center at 0
    my_acc = my_acc * 2 * scale;                      % multiply by scaling Mode

    %Center Data to 0 (remove offset of accelerometer)
    for i=1:100
        my_acc = my_acc - sum(my_acc)/numel(my_acc);
    end

    % Call Learning function
    u = learn(my_acc, ubefore);                       % Call the learning algorithm
    ubefore = u;                                      % Save previous control signal into ubefore
    u = (u + 3)*255/6;                                % Convert from +/- 3V to 0-255 for the PIC

    %%%%%% ALTER U FOR TX %%%%%%%%  THIS Removes bytes 128-151 as they are
    %%%%%% reserved unicode characters.  This looses 5 bits of resolution and
    %%%%%% is an area for improvement
        u = round(u * 225/256);
    for j = 1:numel(u)
        if u(j) > 127
            u(j) = u(j) + 32;
        end
        if u(j) > 255
            u(j) = 255;
        end
        if u(j) <= 0
            u(j) = 1;
        end
    end

    %%%%%% TRANSMIT Control voltage to PIC %%%%%%%%
    y = [u u];
    u = y(50:149);
    for i = 1:numel(u)
        fprintf(s, '%s', char(u(i)));
        pause(0.01)
    end
end

fclose(s);                                  % Close the serial port
delete(s);                                  % And Destory the PORT