## Copyright (c) 2012 Juan Pablo Carbajal
##
##    This program is free software: you can redistribute it and/or modify
##    it under the terms of the GNU General Public License as published by
##    the Free Software Foundation, either version 3 of the License, or
##    any later version.
##
##    This program is distributed in the hope that it will be useful,
##    but WITHOUT ANY WARRANTY; without even the implied warranty of
##    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
##    GNU General Public License for more details.
##
##    You should have received a copy of the GNU General Public License
##    along with this program. If not, see <http://www.gnu.org/licenses/>.

## -*- texinfo -*-
## @deftypefn {Function File} {@var{h} =} srl_plot ()
## @deftypefnx {Function File} {@var{h} =} srl_plot (@var{property},@var{value})
## Plots data read from the serial port.
##
## It takes the following properties from the command line.
## @strong{Properties}
## @table @asis
##  @item "Figure"
##  Handle to the figure to plot into. Default @code{gcf()}.
##  @item "QeueSize"
##  Size of the array to be plotted. Default 1e3.
##  @item "BufferSize"
##  Size of the array to be read form the serial port. Default 1e2.
##  @item "Baudrate"
##  Baud rate of the communication. Default 9600
##  @item "Timeout"
##  Time out for reading from the serial port. Default 1e3 ms.
##  @item "Axis"
##  What to do with the y-axis after the plot is updated. It can be a 1-by-2 array
##  [miny maxy] indicating the limits of the axis. Default @asis{"tight"}.
##  @item "PlotMethod"
##  Indicates the method used ot update the plot. Either via "source" using callback
##  or directly updating the "data" of the plot. Default @asis{"data"}.
##  @item "Deserializer"
##  Handle to a function executed exactly after the data gas been read from the serial
##  port. It must take a vector input (the data) and return an array of double.
##  Default @code{"@double"}.
## @end table
##
## @seealso{srl_read, srl_write}
## @end deftypefn

## Author: Juan Pablo Carbajal <ajuanpi+dev@gmail.com>

function h = srl_plot (varargin)

  % Parse argumnets %%%%%%%%%%%%
  parser = inputParser ();
  parser.FunctionName = "srl_plot";

  parser = addParamValue (parser, "Figure", gcf (), @isindex);
  parser = addParamValue (parser, "QeueSize", 5e2, @(x) x > 0 );
  parser = addParamValue (parser, "BufferSize", 1e2, @(x) x > 0 );
  parser = addParamValue (parser, "Baudrate", 9600, @isbaud);
  parser = addParamValue (parser, "Timeout", 1e3, @(x) x > 0 ); % in ms
  parser = addParamValue (parser, "Axis", "tight", @isaxis);
  parser = addParamValue (parser, "PlotMethod", "data");
  parser = addParamValue (parser, "Deserializer", @double, @is_function_handle);

  parser = parse (parser, varargin{:});

  fig     = parser.Results.Figure;
  qsize   = parser.Results.QeueSize;
  bsize   = parser.Results.BufferSize;
  baud    = parser.Results.Baudrate;
  timeo   = parser.Results.Timeout/1e3*10; %to ds
  axistyp = parser.Results.Axis;
  if isnumeric (axistyp)
    axistyp = [1 qsize axistyp(:)'];
  endif

  plotmet = parser.Results.PlotMethod;
  desrl   = parser.Results.Deserializer;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % Open serial port
  s = serial ();
  srl_baudrate (s,baud);
  srl_timeout (s,timeo);
  srl_flush (s);

  % Test if desrl changes sizes
  y = uint8 (randi(255,qsize,1));
  y = desrl (y);
  if any (!isfloat (y))
    error ("Octave:invalid-input-arg", ["Deserializer function handle must " ...
                                        "return double."])
  end

  % Allocate vectors
  x    = (1:qsize)';
  y    = nan (qsize,1);

  % Prepare plot
  figure(fig);
  clf;
  h  = plot (x,y,'.g');
  t_txt = @(x) sprintf("Time between updates: %.2f seconds",x);
  ht = title (t_txt(0));
  axis (axistyp);

  % Initate transmission
  srl_write (s, "\r");

  % Get data
  idx = 0;

  switch plotmet

    case "source"
      set (h,"ydatasource","y");

      while true
        tic
        [data n]    = srl_read (s, bsize);
        data        = desrl (data);
        n           = length (data);
        [y idx]     = updatedata (y,data,idx,n,qsize);

        refreshdata (fig, "caller");
        set (ht, "string", t_txt(toc));
        sleep (0.008);

      endwhile

    case "data"

      while (true)
        tic
        [data n]    = srl_read (s, bsize);
        data        = desrl (data);
        n           = length (data);
        [y idx]     = updatedata (y,data,idx,n,qsize);

        set (h, "ydata", y);
        axis (axistyp);
        set (ht, "string", t_txt(toc));
        sleep (0.008)

      endwhile

  endswitch


  % Close serial port
  srl_close (s);

endfunction

function [y idxnew scrap] = updatedata (y, data, idx, n, qsize)
  persistent qeue_full

  if isempty(qeue_full)
    % While queue not full, increase the position in the queue
    idxnew = idx + n;
  else
    % Once is full just return how much data was pushed in.
    idxnew = n;
  end

  if qeue_full
    % The qeue is full push the n entries
    scrap              = y(1:n);
    y(1:qsize-n)       = y(n+1:qsize);
    y(qsize-n+1:qsize) = data;

  elseif idxnew > qsize
    % The qeue will be full next iteration
    % insert the elements that fit, push the rest.
    in  = n - (qsize - idx);
    out = idx - in;

    scrap          = y(1:out);
    y(1:out)       = y(in+1:idx);
    y(out+1:qsize) = data;

    qeue_full = true;

  else
    % The qeue is still not full, insert elements.
    y(idx+1:idxnew) = data;
    scrap = [];

  endif

endfunction

% Validator functions
function v = isbaud (x)
  v = any(x == [0, 50, 75, 110, 134, 150, 200, 300, 600, ...
                1200, 1800, 2400, 4800, 9600 19200, 38400, ...
                57600, 115200, 230400]);
endfunction

function v = isaxis (x)
  v = ischar(x) || all(size (x) == [1 2]) || all(size (x) == [2 1]);
endfunction