function output = callsnopt(model)

% Build trees etc. This common format is used for fmincon, snopt and ipopt
% interfaces
model = yalmip2nonlinearsolver(model);

if ~model.derivative_available
    disp('Derivate-free call to snopt not yet implemented')
    error('Derivate-free call to snopt not yet implemented')
end

% Figure out which variables are artificially introduced to normalize
% arguments in callback operators to simplify chain rules etc. We can do
% our function evaluations and gradient computations in our lifted world,
% but only expose the model in the original variables to the nonlinear
% solver.
% model = compressLifted(model);

if model.options.savedebug
    save snoptdebug model
end

showprogress('Calling SNOPT',model.options.showprogress);

x0 = model.x0;
solveopt = 1;
xlow = model.lb;
xupp = model.ub;
xmul   = zeros(length(xupp),1);
xstate = zeros(length(xupp),1);

Fupp = [ inf;
         repmat(0,length(model.bnonlinineq),1);
         repmat(0,nnz(model.K.q),1);
         repmat(0,length(model.bnonlineq),1);
         repmat(0,length(model.b),1);
         repmat(0,length(model.beq),1)];

Flow = [ -inf;
         repmat(-inf,length(model.bnonlinineq),1);
         repmat(-inf,nnz(model.K.q),1);
         repmat(0,length(model.bnonlineq),1);
         repmat(-inf,length(model.b),1);
         repmat(0,length(model.beq),1)];

Fmul   = zeros(length(Fupp),1);
Fstate = zeros(length(Fupp),1);
ObjAdd = 0;
ObjRow = 1;
A = [];
iAfun = [];
jAvar = [];
% Sparsity pattern of jacobian
if ~isempty(Fupp)
    G = jacobiansparsityfromnonlinear(model);
else
    G = [];
end
% Add a row for objective. No sparsity declared
G = [ones(1,size(G,2));G];
[iGfun,jGvar] = find(G);
model.sparsityElements = find(G);

usrf = 'snopt_callback';
snopt_callback([],model);

global latest_xevaled
global latest_x_xevaled
latest_xevaled = [];
latest_x_xevaled = [];

solvertime = tic;
if strcmpi(model.solver.version,'cmex')
    % Some old interface? Keep for safety
    if model.options.verbose == 0
        snscreen('off')
    else
        snscreen('on');
    end
    snseti('Minimize',1)
    [xout,F,xmul,Fmul,inform, xstate, Fstate, ns, ninf, sinf, mincw, miniw, minrw] = snoptcmex( solveopt, x0, xlow, xupp, xmul, xstate, Flow, Fupp, Fmul, Fstate,ObjAdd, ObjRow, A, iAfun(:), jAvar(:),iGfun(:), jGvar(:), usrf );
else
    Astruct.A = A;
    Astruct.row = iAfun;
    Astruct.col = jAvar;
    if model.options.verbose == 0
       model.options.snopt.screen = 'off';
    end
    [xout,F,inform,xmul,Fmul] = snopt(x0, xlow, xupp, xmul, xstate,Flow, Fupp, Fmul, Fstate, usrf,ObjAdd, ObjRow,A,G,model.options.snopt);
end

solvertime = toc(solvertime);

lambda = Fmul(2:end);

if ~isempty(xout) && ~isempty(model.lift);
    x = zeros(length(model.linearindicies),1);
    x(model.lift.linearIndex) = xout;
    x(model.lift.liftedIndex) = model.lift.T*xout + model.lift.d;
    x = RecoverNonlinearSolverSolution(model,x);
else
    x = RecoverNonlinearSolverSolution(model,xout);
end

problem = 0;

% Internal format for duals
D_struc = [];

% Check, currently not exhaustive...
switch inform
    case {1}
        problem = 0;
    case {11,12,13,14,40,43,91} % 1 is sent when I test
        problem = 1;
    case 21
        problem = 2;
    case {31,32}
        problem = 3;
    case {2,3,33,41,42,43,44}
        problem = 4;
    case {71,72,73,74}
        problem = 16;
    otherwise
        problem = 11;
end

% Save all data sent to solver?
if model.options.savesolverinput
    solverinput.model = model;
else
    solverinput = [];
end

% Save all data from the solver?
if model.options.savesolveroutput
    solveroutput.x = x;
    solveroutput.F = F;
    solveroutput.xmul = xmul;
    solveroutput.inform=inform;
    solveroutput.Fstate=Fstate;
    solveroutput.ns = ns;
    solveroutput.ninf = ninf;
    solveroutput.sinf = sinf;
    solveroutput.mincw = mincw;
    solveroutput.miniw = miniw;
    solveroutput.miniw = miniw;

else
    solveroutput = [];
end

% Standard interface
output = createOutputStructure(x,D_struc,[],problem,'SNOPT',solverinput,solveroutput,solvertime);


% WriteSu : writes seismic data in the Seismic Unix format
%
% EX
% WriteSu('datacube.su',data,'dt',.004,'Inline3D',Inline,'Crossline3D',Crossline,'cdpX',X,'cdpY',Y);
%
% to use a specific SEG revision use :
%
% to use a specific Data Sampling Format use :
% WriteSu('test.su',seisdata,'dsf',1); % IBM FLAOTING POINT
%
% WriteSu('test.su',seisdata,'dsf');
%

%
% (C) 2001-2004, Thomas Mejer Hansen, thomas@cultpenguin.com
%
%    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 2 of the License, or
%    (at your option) 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, write to the Free Software
%    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
%
function WriteSu(filename,data,varargin);

[ns,ntraces]=size(data);

if exist('dt')~=1
  dt=0.004;
  SegymatVerbose([mfilename,' - No dt set. Using dt=',num2str(dt)])
end

for i=1:2:length(varargin)
  var=varargin{i};
  val=varargin{i+1};
  eval([var,'=[',num2str(val),'];']);
end

SegyHeader.SegyFormatRevisionNumber=256;
SegyHeader.DataSampleFormat=5;
SegyHeader.Rev=GetSegyHeaderBasics;

% OPEN SEGY FILE HANDLE
segyid = fopen(filename,'w');
% segyid = fopen(filename,'w','b'); % BIG ENDIAN

SegyTraceHeader=InitSegyTraceHeader(ns,dt*1e+6);
for i=1:ntraces;
    if (i/100)==round(i/100),SegymatVerbose(['writing trace ',num2str(i),' of ',num2str(ntraces)],1),end
    % Basic TraceHeader information
    %SegyTraceHeader.ns=ns;
    %SegyTraceHeader.dt=dt.*1e+6;
    % Update TraceHeader information if available
    if exist('cdpX')==1,SegyTraceHeader.cdpX = cdpX(i);end
    if exist('offset')==1,SegyTraceHeader.offset = offset(i);end
    if exist('cdpY')==1,SegyTraceHeader.cdpY = cdpY(i);end
    if exist('Inline3D')==1,SegyTraceHeader.Inline3D = Inline3D(i);end
    if exist('Crossline3D')==1,SegyTraceHeader.Crossline3D=Crossline3D(i);end
    % Write the Trace
    PutSegyTrace(segyid,data(:,i),SegyTraceHeader,SegyHeader);
end

% CLOSE SEGY FILE HANDLE

fclose(segyid);


function [im_th,TOAref,trgt,ijdim_ref,ul,bbox,zen,azi,zc,B1Satu,B2Satu,B3Satu,resolu]=nd2toarbt(path_top,filename)
% convert DNs to TOA ref and BT
% Revisions:
% Add Landsat 9 (Shi 2/17/2022)
% Cannot use the GRIDobj to read our band, because it may result in Nan data.
% (Shi 2/26/2018)
% Added a target image based on blue band, which will be helpfull to
% resample and reproject the auxiliary data.  (Shi 2/16/2018)
% Use REF vs. DN instead of RAD vs. DN (Zhe 06/20/2013)
% Combined the Earth-Sun distance table into the function (Zhe 04/09/2013)
% Process Landsat 8 DN values (Zhe 04/04/2013)
% Proces the new metadata for Landsat TM/ETM+ images (Zhe 09/28/2012)
% Fixed bugs caused by earth-sun distance table (Zhe 01/15/2012)
%
% [im_th,TOAref,ijdim_ref,ul,zen,azi,zc,B1Satu,B2Satu,B3Satu,resolu]=nd2toarbt(filename)
% Where:
% Inputs:
% filename='L*MTL.txt';
% Outputs:
% 1) im_th = Brightness Temperature (BT)
% 2) TOAref = Top Of Atmoshpere (TOA) reflectance
% 3) trgt = Target Grid Object
% 4) ijdim = [nrows,ncols]; % dimension of optical bands
% 5) ul = [upperleft_mapx upperleft_mapy];
% 6) zen = solar zenith angle (degrees);
% 7) azi = solar azimuth angle (degrees);
% 8) zc = Zone Number
% 9,10,11) Saturation (true) in the Visible bands
% 12) resolution of Fmask results

% % % fprintf('Read in header information & TIF images\n');
[Lmax,Lmin,Qcalmax,Qcalmin,Refmax,Refmin,ijdim_ref,ijdim_thm,reso_ref,...
    reso_thm,ul,bbox,zen,azi,zc,Lnum,doy]=lndhdrread(path_top,filename);

% earth-sun distance see G. Chander et al. RSE 113 (2009) 893-903
Tab_ES_Dist = [
    1 0.98331
    2 0.98330
    3 0.98330
    4 0.98330
    5 0.98330
    6 0.98332
    7 0.98333
    8 0.98335
    9 0.98338
    10 0.98341
    11 0.98345
    12 0.98349
    13 0.98354
    14 0.98359
    15 0.98365
    16 0.98371
    17 0.98378
    18 0.98385
    19 0.98393
    20 0.98401
    21 0.98410
    22 0.98419
    23 0.98428
    24 0.98439
    25 0.98449
    26 0.98460
    27 0.98472
    28 0.98484
    29 0.98496
    30 0.98509
    31 0.98523
    32 0.98536
    33 0.98551
    34 0.98565
    35 0.98580
    36 0.98596
    37 0.98612
    38 0.98628
    39 0.98645
    40 0.98662
    41 0.98680
    42 0.98698
    43 0.98717
    44 0.98735
    45 0.98755
    46 0.98774
    47 0.98794
    48 0.98814
    49 0.98835
    50 0.98856
    51 0.98877
    52 0.98899
    53 0.98921
    54 0.98944
    55 0.98966
    56 0.98989
    57 0.99012
    58 0.99036
    59 0.99060
    60 0.99084
    61 0.99108
    62 0.99133
    63 0.99158
    64 0.99183
    65 0.99208
    66 0.99234
    67 0.99260
    68 0.99286
    69 0.99312
    70 0.99339
    71 0.99365
    72 0.99392
    73 0.99419
    74 0.99446
    75 0.99474
    76 0.99501
    77 0.99529
    78 0.99556
    79 0.99584
    80 0.99612
    81 0.99640
    82 0.99669
    83 0.99697
    84 0.99725
    85 0.99754
    86 0.99782
    87 0.99811
    88 0.99840
    89 0.99868
    90 0.99897
    91 0.99926
    92 0.99954
    93 0.99983
    94 1.00012
    95 1.00041
    96 1.00069
    97 1.00098
    98 1.00127
    99 1.00155
    100 1.00184
    101 1.00212
    102 1.00240
    103 1.00269
    104 1.00297
    105 1.00325
    106 1.00353
    107 1.00381
    108 1.00409
    109 1.00437
    110 1.00464
    111 1.00492
    112 1.00519
    113 1.00546
    114 1.00573
    115 1.00600
    116 1.00626
    117 1.00653
    118 1.00679
    119 1.00705
    120 1.00731
    121 1.00756
    122 1.00781
    123 1.00806
    124 1.00831
    125 1.00856
    126 1.00880
    127 1.00904
    128 1.00928
    129 1.00952
    130 1.00975
    131 1.00998
    132 1.01020
    133 1.01043
    134 1.01065
    135 1.01087
    136 1.01108
    137 1.01129
    138 1.01150
    139 1.01170
    140 1.01191
    141 1.01210
    142 1.01230
    143 1.01249
    144 1.01267
    145 1.01286
    146 1.01304
    147 1.01321
    148 1.01338
    149 1.01355
    150 1.01371
    151 1.01387
    152 1.01403
    153 1.01418
    154 1.01433
    155 1.01447
    156 1.01461
    157 1.01475
    158 1.01488
    159 1.01500
    160 1.01513
    161 1.01524
    162 1.01536
    163 1.01547
    164 1.01557
    165 1.01567
    166 1.01577
    167 1.01586
    168 1.01595
    169 1.01603
    170 1.01610
    171 1.01618
    172 1.01625
    173 1.01631
    174 1.01637
    175 1.01642
    176 1.01647
    177 1.01652
    178 1.01656
    179 1.01659
    180 1.01662
    181 1.01665
    182 1.01667
    183 1.01668
    184 1.01670
    185 1.01670
    186 1.01670
    187 1.01670
    188 1.01669
    189 1.01668
    190 1.01666
    191 1.01664
    192 1.01661
    193 1.01658
    194 1.01655
    195 1.01650
    196 1.01646
    197 1.01641
    198 1.01635
    199 1.01629
    200 1.01623
    201 1.01616
    202 1.01609
    203 1.01601
    204 1.01592
    205 1.01584
    206 1.01575
    207 1.01565
    208 1.01555
    209 1.01544
    210 1.01533
    211 1.01522
    212 1.01510
    213 1.01497
    214 1.01485
    215 1.01471
    216 1.01458
    217 1.01444
    218 1.01429
    219 1.01414
    220 1.01399
    221 1.01383
    222 1.01367
    223 1.01351
    224 1.01334
    225 1.01317
    226 1.01299
    227 1.01281
    228 1.01263
    229 1.01244
    230 1.01225
    231 1.01205
    232 1.01186
    233 1.01165
    234 1.01145
    235 1.01124
    236 1.01103
    237 1.01081
    238 1.01060
    239 1.01037
    240 1.01015
    241 1.00992
    242 1.00969
    243 1.00946
    244 1.00922
    245 1.00898
    246 1.00874
    247 1.00850
    248 1.00825
    249 1.00800
    250 1.00775
    251 1.00750
    252 1.00724
    253 1.00698
    254 1.00672
    255 1.00646
    256 1.00620
    257 1.00593
    258 1.00566
    259 1.00539
    260 1.00512
    261 1.00485
    262 1.00457
    263 1.00430
    264 1.00402
    265 1.00374
    266 1.00346
    267 1.00318
    268 1.00290
    269 1.00262
    270 1.00234
    271 1.00205
    272 1.00177
    273 1.00148
    274 1.00119
    275 1.00091
    276 1.00062
    277 1.00033
    278 1.00005
    279 0.99976
    280 0.99947
    281 0.99918
    282 0.99890
    283 0.99861
    284 0.99832
    285 0.99804
    286 0.99775
    287 0.99747
    288 0.99718
    289 0.99690
    290 0.99662
    291 0.99634
    292 0.99605
    293 0.99577
    294 0.99550
    295 0.99522
    296 0.99494
    297 0.99467
    298 0.99440
    299 0.99412
    300 0.99385
    301 0.99359
    302 0.99332
    303 0.99306
    304 0.99279
    305 0.99253
    306 0.99228
    307 0.99202
    308 0.99177
    309 0.99152
    310 0.99127
    311 0.99102
    312 0.99078
    313 0.99054
    314 0.99030
    315 0.99007
    316 0.98983
    317 0.98961
    318 0.98938
    319 0.98916
    320 0.98894
    321 0.98872
    322 0.98851
    323 0.98830
    324 0.98809
    325 0.98789
    326 0.98769
    327 0.98750
    328 0.98731
    329 0.98712
    330 0.98694
    331 0.98676
    332 0.98658
    333 0.98641
    334 0.98624
    335 0.98608
    336 0.98592
    337 0.98577
    338 0.98562
    339 0.98547
    340 0.98533
    341 0.98519
    342 0.98506
    343 0.98493
    344 0.98481
    345 0.98469
    346 0.98457
    347 0.98446
    348 0.98436
    349 0.98426
    350 0.98416
    351 0.98407
    352 0.98399
    353 0.98391
    354 0.98383
    355 0.98376
    356 0.98370
    357 0.98363
    358 0.98358
    359 0.98353
    360 0.98348
    361 0.98344
    362 0.98340
    363 0.98337
    364 0.98335
    365 0.98333
    366 0.98331];

if Lnum >= 4 && Lnum <= 7
    % LPGS Upper lef corner alignment (see Landsat handbook for detail)
    ul(1)=ul(1)-15;
    ul(2)=ul(2)+15;
    resolu=[reso_ref,reso_ref];
    % Read in all bands
    % Band1
    n_B1=dir(fullfile(path_top,'L*B1*'));
    if isempty(n_B1)
        n_B1=dir(fullfile(path_top,'L*b1*'));
    end
    im_B1=single(imread(fullfile(path_top,n_B1.name)));
    % Band2
    % Also used to be target or referenced image when resampling and reprojecting to the image extent and resolution.
    n_B2=dir(fullfile(path_top,'L*B2*'));
    if isempty(n_B2)
        n_B2=dir(fullfile(path_top,'L*b2*'));
    end
    im_B2=single(imread(fullfile(path_top,n_B2.name)));
    trgt = GRIDobj(fullfile(path_top,n_B2.name));
%     im_B2=single(trgt.Z);
    trgt.Z=[];% remove non-used values to save memory.

    % Band3
    n_B3=dir(fullfile(path_top,'L*B3*'));
    if isempty(n_B3)
        n_B3=dir(fullfile(path_top,'L*b3*'));
    end
    im_B3=single(imread(fullfile(path_top,n_B3.name)));
    % Band4
    n_B4=dir(fullfile(path_top,'L*B4*'));
    if isempty(n_B4)
        n_B4=dir(fullfile(path_top,'L*b4*'));
    end
    im_B4=single(imread(fullfile(path_top,n_B4.name)));
    % Band5
    n_B5=dir(fullfile(path_top,'L*B5*'));
    if isempty(n_B5)
        n_B5=dir(fullfile(path_top,'L*b5*'));
    end
    im_B5=single(imread(fullfile(path_top,n_B5.name)));
    % Band6
    if Lnum==7
        n_B6=dir(fullfile(path_top,'L*B6*1*'));
        if isempty(n_B6)
            n_B6=dir(fullfile(path_top,'L*b6*1*'));
        end
    else
        n_B6=dir(fullfile(path_top,'L*B6*'));
        if isempty(n_B6)
            n_B6=dir(fullfile(path_top,'L*b6*'));
        end
    end
    im_th=single(imread(fullfile(path_top,n_B6.name)));
    % check to see whether need to resample thermal band
    if reso_ref~=reso_thm
        % resmaple thermal band
        im_th=pixel2pixv([ul(2),ul(1)],[ul(2),ul(1)],...
            resolu,[reso_thm,reso_thm],...
            im_th,[ijdim_ref(2),ijdim_ref(1)],[ijdim_thm(2),ijdim_thm(1)]);
    end
    % Band7
    n_B7=dir(fullfile(path_top,'L*B7*'));
    if isempty(n_B7)
        n_B7=dir(fullfile(path_top,'L*b7*'));
    end
    im_B7=single(imread(fullfile(path_top,n_B7.name)));
    % only processing pixesl where all bands have values (id_mssing)
    id_missing=im_B1==0|im_B2==0|im_B3==0|im_B4==0|im_B5==0|im_th==0|im_B7==0;
    % find pixels that are saturated in the visible bands
    B1Satu=im_B1==255;
    B2Satu=im_B2==255;
    B3Satu=im_B3==255;

    % ND to radiance first
% % %     fprintf('From DNs to TOA ref & BT\n');
    im_B1=((Lmax(1)-Lmin(1))/(Qcalmax(1)-Qcalmin(1)))*(im_B1-Qcalmin(1))+Lmin(1);
    im_B2=((Lmax(2)-Lmin(2))/(Qcalmax(2)-Qcalmin(2)))*(im_B2-Qcalmin(2))+Lmin(2);
    im_B3=((Lmax(3)-Lmin(3))/(Qcalmax(3)-Qcalmin(3)))*(im_B3-Qcalmin(3))+Lmin(3);
    im_B4=((Lmax(4)-Lmin(4))/(Qcalmax(4)-Qcalmin(4)))*(im_B4-Qcalmin(4))+Lmin(4);
    im_B5=((Lmax(5)-Lmin(5))/(Qcalmax(5)-Qcalmin(5)))*(im_B5-Qcalmin(5))+Lmin(5);
    im_th=((Lmax(6)-Lmin(6))/(Qcalmax(6)-Qcalmin(6)))*(im_th-Qcalmin(6))+Lmin(6);
    im_B7=((Lmax(7)-Lmin(7))/(Qcalmax(7)-Qcalmin(7)))*(im_B7-Qcalmin(7))+Lmin(7);

    % Landsat 4, 5, and 7 solar spectral irradiance
    % see G. Chander et al. RSE 113 (2009) 893-903
    esun_L7=[1997.000, 1812.000, 1533.000, 1039.000, 230.800, -1.0, 84.90];
    esun_L5=[1983.0, 1796.0, 1536.0, 1031.0, 220.0, -1.0, 83.44];
    esun_L4=[1983.0, 1795.0, 1539.0, 1028.0, 219.8, -1.0, 83.49];

    if Lnum==7
        ESUN=esun_L7;
    elseif Lnum==5
        ESUN=esun_L5;
    elseif Lnum==4
        ESUN=esun_L4;
    end

    % earth-sun distance see G. Chander et al. RSE 113 (2009) 893-903
    dsun_doy = Tab_ES_Dist(doy,2);

    % compute TOA reflectances
    % converted from degrees to radiance
    s_zen=deg2rad(zen);
    im_B1=10^4*pi*im_B1*dsun_doy^2/(ESUN(1)*cos(s_zen));
    im_B2=10^4*pi*im_B2*dsun_doy^2/(ESUN(2)*cos(s_zen));
    im_B3=10^4*pi*im_B3*dsun_doy^2/(ESUN(3)*cos(s_zen));
    im_B4=10^4*pi*im_B4*dsun_doy^2/(ESUN(4)*cos(s_zen));
    im_B5=10^4*pi*im_B5*dsun_doy^2/(ESUN(5)*cos(s_zen));
    im_B7=10^4*pi*im_B7*dsun_doy^2/(ESUN(7)*cos(s_zen));

    % convert Band6 from radiance to BT
    % fprintf('From Band 6 Radiance to Brightness Temperature\n');
    % see G. Chander et al. RSE 113 (2009) 893-903
    K1_L4=  671.62;
    K2_L4= 1284.30;
    K1_L5=  607.76;
    K2_L5= 1260.56;
    K1_L7 =  666.09;
    K2_L7 = 1282.71;

    if Lnum==7
        K1=K1_L7;
        K2=K2_L7;
    elseif Lnum==5
        K1=K1_L5;
        K2=K2_L5;
    elseif Lnum==4
        K1=K1_L4;
        K2=K2_L4;
    end

    im_th=K2./log((K1./im_th)+1);
    % convert from Kelvin to Celcius with 0.01 scale_facor
    im_th=100*(im_th-273.15);
    % get data ready for Fmask
    im_B1(id_missing)=-9999;
    im_B2(id_missing)=-9999;
    im_B3(id_missing)=-9999;
    im_B4(id_missing)=-9999;
    im_B5(id_missing)=-9999;
    im_th(id_missing)=-9999;
    im_B7(id_missing)=-9999;
    clear id_missing;
    TOAref=zeros(ijdim_ref(1),ijdim_ref(2),6,'single');% Band1,2,3,4,5,&7
    TOAref(:,:,1)=im_B1;clear im_B1;
    TOAref(:,:,2)=im_B2;clear im_B2;
    TOAref(:,:,3)=im_B3;clear im_B3;
    TOAref(:,:,4)=im_B4;clear im_B4;
    TOAref(:,:,5)=im_B5;clear im_B5;
    TOAref(:,:,6)=im_B7;clear im_B7;

elseif Lnum == 8 || Lnum == 9
    % LPGS Upper lef corner alignment (see Landsat handbook for detail)
    ul(1)=ul(1)-15;
    ul(2)=ul(2)+15;
    resolu=[reso_ref,reso_ref];
    % Read in all bands
    % Band2
    n_B2=dir(fullfile(path_top,'L*B2*'));
    if isempty(n_B2)
        n_B2=dir(fullfile(path_top,'L*b2*'));
    end
    im_B2=single(imread(fullfile(path_top,n_B2(1).name))); clear n_B2;
    % Band3
    % Also used to be target or referenced image when resampling and reprojecting to the image extent and resolution.
    n_B3=dir(fullfile(path_top,'L*B3*'));
    if isempty(n_B3)
        n_B3=dir(fullfile(path_top,'L*b3*'));
    end
    im_B3=single(imread(fullfile(path_top,n_B3(1).name)));
    trgt = GRIDobj(fullfile(path_top,n_B3.name)); clear n_B3;
%     im_B3=single(trgt.Z);
    trgt.Z=[];% remove non-used values to save memory.

    % Band4
    n_B4=dir(fullfile(path_top,'L*B4*'));
    if isempty(n_B4)
        n_B4=dir(fullfile(path_top,'L*b4*'));
    end
    im_B4=single(imread(fullfile(path_top,n_B4(1).name))); clear n_B4;
    % Band5
    n_B5=dir(fullfile(path_top,'L*B5*'));
    if isempty(n_B5)
        n_B5=dir(fullfile(path_top,'L*b5*'));
    end
    im_B5=single(imread(fullfile(path_top,n_B5(1).name))); clear n_B5;
    % Band6
    n_B6=dir(fullfile(path_top,'L*B6*'));
    if isempty(n_B6)
        n_B6=dir(fullfile(path_top,'L*b6*'));
    end
    im_B6=single(imread(fullfile(path_top,n_B6(1).name))); clear n_B6;
    % Band7
    n_B7=dir(fullfile(path_top,'L*B7*'));
    if isempty(n_B7)
        n_B7=dir(fullfile(path_top,'L*b7*'));
    end
    im_B7=single(imread(fullfile(path_top,n_B7(1).name))); clear n_B7;
    % Band9
    n_B9=dir(fullfile(path_top,'L*B9.*'));
    if isempty(n_B9)
        n_B9=dir(fullfile(path_top,'L*b9.*'));
    end
    im_B9=single(imread(fullfile(path_top,n_B9(1).name))); clear n_B9;
    % Band10
    n_B10=dir(fullfile(path_top,'L*B10*'));
    if isempty(n_B10)
        n_B10=dir(fullfile(path_top,'L*b10*'));
    end
    im_B10=single(imread(fullfile(path_top,n_B10(1).name))); clear n_B10 path_top;

    % check to see whether need to resample thermal band
    if reso_ref~=reso_thm
        % resmaple thermal band
        im_B10=pixel2pixv([ul(2),ul(1)],[ul(2),ul(1)],...
            resolu,[reso_thm,reso_thm],...
            im_B10,[ijdim_ref(2),ijdim_ref(1)],[ijdim_thm(2),ijdim_thm(1)]);
    end

    % only processing pixesl where all bands have values (id_mssing)
    id_missing=im_B2==0|im_B3==0|im_B4==0|im_B5==0|im_B6==0|im_B7==0|im_B9==0|im_B10==0;
    % find pixels that are saturated in the visible bands
    B1Satu=im_B2==65535;
    B2Satu=im_B3==65535;
    B3Satu=im_B4==65535;

    % ND to TOA reflectance with 0.0001 scale_facor
% % %     fprintf('From DNs to TOA ref & BT\n');
    im_B2=((Refmax(1)-Refmin(1))/(Qcalmax(1)-Qcalmin(1)))*(im_B2-Qcalmin(1))+Refmin(1);
    im_B3=((Refmax(2)-Refmin(2))/(Qcalmax(2)-Qcalmin(2)))*(im_B3-Qcalmin(2))+Refmin(2);
    im_B4=((Refmax(3)-Refmin(3))/(Qcalmax(3)-Qcalmin(3)))*(im_B4-Qcalmin(3))+Refmin(3);
    im_B5=((Refmax(4)-Refmin(4))/(Qcalmax(4)-Qcalmin(4)))*(im_B5-Qcalmin(4))+Refmin(4);
    im_B6=((Refmax(5)-Refmin(5))/(Qcalmax(5)-Qcalmin(5)))*(im_B6-Qcalmin(5))+Refmin(5);
    im_B7=((Refmax(6)-Refmin(6))/(Qcalmax(6)-Qcalmin(6)))*(im_B7-Qcalmin(6))+Refmin(6);
    im_B9=((Refmax(7)-Refmin(7))/(Qcalmax(7)-Qcalmin(7)))*(im_B9-Qcalmin(7))+Refmin(7);
    im_B10=((Lmax(8)-Lmin(8))/(Qcalmax(8)-Qcalmin(8)))*(im_B10-Qcalmin(8))+Lmin(8);

    % compute TOA reflectances
    % with a correction for the sun angle
    s_zen=deg2rad(zen);
    im_B2=10^4*im_B2/cos(s_zen);
    im_B3=10^4*im_B3/cos(s_zen);
    im_B4=10^4*im_B4/cos(s_zen);
    im_B5=10^4*im_B5/cos(s_zen);
    im_B6=10^4*im_B6/cos(s_zen);
    im_B7=10^4*im_B7/cos(s_zen);
    im_B9=10^4*im_B9/cos(s_zen);

    % convert Band6 from radiance to BT
    % fprintf('From Band 6 Radiance to Brightness Temperature\n');
    K1_B10 =  774.89;
    K2_B10 = 1321.08;

    im_B10=K2_B10./log((K1_B10./im_B10)+1);

    % convert from Kelvin to Celcius with 0.01 scale_facor
    im_B10=100*(im_B10-273.15);

    % get data ready for Fmask
    im_B2(id_missing)=-9999;
    im_B3(id_missing)=-9999;
    im_B4(id_missing)=-9999;
    im_B5(id_missing)=-9999;
    im_B6(id_missing)=-9999;
    im_B7(id_missing)=-9999;
    im_B9(id_missing)=-9999;
    im_B10(id_missing) = -9999;
    clear id_missing; % empty memory

    TOAref=zeros(ijdim_ref(1),ijdim_ref(2),7,'single');% Band 2,3,4,5,6,7,& 9
    TOAref(:,:,1)=im_B2; clear im_B2;
    TOAref(:,:,2)=im_B3; clear im_B3;
    TOAref(:,:,3)=im_B4; clear im_B4;
    TOAref(:,:,4)=im_B5; clear im_B5;
    TOAref(:,:,5)=im_B6; clear im_B6;
    TOAref(:,:,6)=im_B7; clear im_B7;
    TOAref(:,:,7)=im_B9; clear im_B9;

    im_th=zeros(ijdim_ref(1),ijdim_ref(2),'single');% Band 10
    im_th(:,:) = im_B10; clear im_B10;
else
    fprintf('This sensor is not Landsat 4, 5, 7, or 8!\n');
    return;
end

end


function vt_video_main(visualDataFile, varargin)
%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     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
%     (at your option) 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/>.

    %% initial some variables
    % define a variable of visual template
    global VT;

    % define the number of visual templates
    global NUM_VT;
    NUM_VT = 1;

    % define a variable of previous vt id
    global PREV_VT_ID;
    PREV_VT_ID = 1;

    % define history of visual template
    global VT_HISTORY;
    global VT_HISTORY_FIRST;
    global VT_HISTORY_OLD;

    VT_HISTORY = [0];

    % for showing the comparing result between current imgs with templates
    global MIN_DIFF_CURR_IMG_VTS;
    MIN_DIFF_CURR_IMG_VTS = [0];

    global DIFFS_ALL_IMGS_VTS;
    DIFFS_ALL_IMGS_VTS = [0];

    % define a variable for showing sub vt imgage
    global SUB_VT_IMG;

    %% Process the parameters
    % parser some pameters

    % define the threshold for vt matching
    % This threshold determines whether a new view template is generated
    global VT_MATCH_THRESHOLD;

    % define the x, y range of visual template img
    global VT_IMG_CROP_Y_RANGE; % Row range
    global VT_IMG_CROP_X_RANGE; % Column range

    % Define resized range of visual template img for reducing resolution
    global VT_IMG_RESIZE_X_RANGE;
    global VT_IMG_RESIZE_Y_RANGE;

    % define x, y shift range for template matching
    % This determines how many +- pixels (therefore rotation) will be tested for match
    global VT_IMG_X_SHIFT;
    global VT_IMG_Y_SHIFT;

    % define decay of vt for updating vts
    % VT_GLOBAL_DECAY is subtracted for all the view templates at each time step
    % VT_ACTIVE_DECAY is added the best matching view template
    global VT_GLOBAL_DECAY;
    global VT_ACTIVE_DECAY;

     % define the patch size in x, y for patch normalisation
    global PATCH_SIZE_Y_K;
    global PATCH_SIZE_X_K;
    global BLOCK_READ;

    for i=1:(nargin-1)
        if ischar(varargin{i})
            switch varargin{i}
                case 'VT_MATCH_THRESHOLD', VT_MATCH_THRESHOLD = varargin{i+1};

                case 'VT_IMG_CROP_Y_RANGE', VT_IMG_CROP_Y_RANGE = varargin{i+1};
                case 'VT_IMG_CROP_X_RANGE', VT_IMG_CROP_X_RANGE = varargin{i+1};

                case 'VT_IMG_RESIZE_X_RANGE', VT_IMG_RESIZE_X_RANGE = varargin{i+1};
                case 'VT_IMG_RESIZE_Y_RANGE', VT_IMG_RESIZE_Y_RANGE = varargin{i+1};

                case 'VT_IMG_X_SHIFT', VT_IMG_X_SHIFT = varargin{i+1};
                case 'VT_IMG_Y_SHIFT', VT_IMG_Y_SHIFT = varargin{i+1};

                case 'VT_GLOBAL_DECAY', VT_GLOBAL_DECAY = varargin{i+1};
                case 'VT_ACTIVE_DECAY', VT_ACTIVE_DECAY = varargin{i+1};

                case 'PATCH_SIZE_Y_K', PATCH_SIZE_Y_K = varargin{i+1};
                case 'PATCH_SIZE_X_K', PATCH_SIZE_X_K = varargin{i+1};

                case 'BLOCK_READ', BLOCK_READ = varargin{i+1};
            end
        end
    end

    % define half offset for reversed matching
    global VT_IMG_HALF_OFFSET;
    VT_IMG_HALF_OFFSET = [0 floor(size(VT_IMG_CROP_X_RANGE) / 2)];

    gc_x = 1;
    gc_y = 1;
    gc_z = 1;

    hdc_yaw = 0;
    hdc_pitch = 0;

    VT(1).id = 1;
    VT(1).template(1) = 1;
    VT(1).decay = 0.7;
    VT(1).gc_x = gc_x;
    VT(1).gc_y = gc_y;
    VT(1).gc_z = gc_z;
    VT(1).hdc_yaw = hdc_yaw;
    VT(1).hdc_pitch = hdc_pitch;
    VT(1).first = 1;           % don't want to inject energy as the vt is been created
    VT(1).numExp = 1;
    VT(1).exps(1).id = 1;

    VT(1).template = zeros(size(VT_IMG_CROP_Y_RANGE, 2), size(VT_IMG_CROP_X_RANGE, 2));

    %% read imgs as VT input

    % specify the movie and the frames to read
    [videoInfo, iStartFrame, totalFramesNum] = get_video_data_info(visualDataFile);

    for curFrame = 1 : totalFramesNum

        % read the visual data in blocks
        if (mod(curFrame, BLOCK_READ) == 1)
            [curVideoBlock] = read_video_block(videoInfo,curFrame, iStartFrame, totalFramesNum, BLOCK_READ);
        end

        % visual templates and visual odometry uses intensity so convert to grayscale
        [curImg] = get_current_frame_image(curVideoBlock, curFrame, BLOCK_READ);

        curGrayImg = rgb2gray(curImg);
        rawImg = im2double(curGrayImg);

        % get the most active view template
        [vt_id] = visual_template(rawImg, gc_x, gc_y,gc_z, hdc_yaw, hdc_pitch);

        % render the raw img
        subplot(3, 3, 1, 'replace');
        imshow(curGrayImg, []);
        title('Raw img');
        axis on

        % render the history of visual templates
        subplot(3, 3, [2 3 5 6], 'replace');
        plot(VT_HISTORY, '.r');
        title('Frame vs View Template');
        axis on

        % render the resized image of visual templates
        subplot(3, 3, 4, 'replace');
        imshow(SUB_VT_IMG, []);
        title('Resized img');
        axis on

        % render the history of visual templates
        subplot(3, 3, 7, 'replace');
        plot(MIN_DIFF_CURR_IMG_VTS, '.');
        title('Min SAD with each template');
        axis on

        % render the history of visual templates
        subplot(3, 3, [8 9], 'replace');
        plot(DIFFS_ALL_IMGS_VTS, '.');
        title('Min SAD of each current img');
        axis on

        DIFFS_ALL_IMGS_VTS = vt_id;

        drawnow;
    end

end


%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by: Giulio Tagliaferro
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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
%   (at your option) 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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

function time = getFileStTime(filename)
% Return the start time of the file using the standard naming convention

[~,name, ext] = fileparts(filename);
if strcmpi(ext,'.${YY}p') || ((strcmpi(ext,'.${YY}[n|N]') || ~isempty(regexpi(ext,'\.\d\d[n|N]'))) && isempty(strfind(name, 'CGIM')))  || strcmpi(ext,'.${YY}l') || ~isempty(regexpi(ext,'\.\d\d[p|P]')) || ~isempty(regexp(ext,'\.\d\d[l|L]', 'once')) %#ok<STREMP>
    % name should be : cccwwwwd
    % note: nav file rinex does not have first epoch at the beginning
    % it is too expesivo to look all the file
    if length(name) >= 8
        week = str2double(name(4:7));
        dow = str2double(name(8));
        if ~isnan(week) && ~isnan(dow)
            time = GPS_Time.fromWeekDow(week, dow);
        else
            time = [];
        end
    else
        time = [];
    end

elseif isempty(strfind(lower(ext),lower('eph'))) || isempty(strfind(lower(ext),lower('sp3')))
    % read first 50 lines SP3 headers is 24 (allowing some space for more line comment)
    fid = fopen(filename,'rt');
    if fid > 0
        txt = fread(fid,61*50,'*char')';
        fclose(fid);
        if isempty(txt)
            Core.getLogger.addWarning(sprintf('"%s" is empty ', filename));
            time = [];
        else
            % get new line separators
            nl = regexp(txt, '\n')';
            if nl(end) <  numel(txt)
                nl = [nl; numel(txt)];
            end
            lim = [[1; nl(1 : end - 1) + 1] (nl - 1)];
            lim = [lim lim(:,2) - lim(:,1)];
            if lim(end,3) < 3
                lim(end,:) = [];
            end
            % find time line
            idx_epoch = find(txt(lim(:,1)) == '*');
            txt = txt(lim(idx_epoch):end);
            if ~isempty(idx_epoch)
                time = GPS_Time([str2num(txt(4:7)) str2num(txt(9:10)) str2num(txt(12:13)) str2num(txt(15:16)) str2num(txt(18:19)) str2num(txt(21:31))]);
            else
                time = [];
            end
        end
    else
        time = [];
    end
elseif isempty(strfind(lower(ext),lower('clk')))
    fid = fopen(filename,'rt');
    tline = fgetl(fid);
    i = 0;
    eoh_found = false;
    while ischar(tline) && i < 300 && eoh_found
        eoh_found = strfind(tline,'END OF HEADER');
        tline = fgetl(fid);
        i = i + 1;
    end
    fclose(fid);
    if eoh_found
        tline = fgetl(fid);
        if ischar(tline) && length(tline) >  36
            time = GPS_Time([str2num(tline(9:12)) str2num(tline(14:15)) str2num(tline(17:18)) str2num(tline(20:21)) str2num(tline(24:25)) str2num(tline(26:34))]);
        else
            time = [];
        end
    else
        time = [];
    end

else
    time = [];
end
end


function planC = importVarianFDF_3D(dirT,optFile)
% function planC = importVarianFDF_3D(dirT,optFile)
% Input: directory containing multi-frame FDF file and CERRoptions
% file or optS structure
% Output: CERR plan (planC)
% Example:
% dirT = 'C:\Projects\FDF reader\Test_data_Joel\3D-Large';
% optFile = 'CERROptions.m';
% planC = importVarianFDF_3D(dirT,optFile);
%
% APA, 05/15/2007
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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
%     (at your option) any later version.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

% Check the format of optFile.
if ~isstruct(optFile)
    optS = opts4Exe(optFile);
else
    optS = optFile;
end

planC = initializeCERR;
indexS = planC{end};
planC{indexS.CERROptions} = optS;
planC{indexS.indexS}     = indexS;

pathT=[dirT,'/'];

%%%% Import Transverse slices and interpolate

% 1> Read [X,Y,Z,f] from transverse slices

%%%%% Extract Data for Transverse Section
contents = dir(fullfile(pathT,'*.fdf'));
[header, data] = readVarianFDF_3D(pathT, contents(1).name);
for i=1:size(data,3)
    H{i}=header; DATA{i}=data(:,:,i);
    %%% Extract info from header
    fbr1=find(header=='{');
    fbr2=find(header=='}');
    MatrixDim(i,:)=str2num(header(fbr1(1)+1:fbr2(1)-1));
    span(i,:)=str2num(header(fbr1(4)+1:fbr2(4)-1));
    offset(i,:)=str2num(header(fbr1(5)+1:fbr2(5)-1));
    sliceCenter(i,:)=str2num(header(fbr1(8)+1:fbr2(8)-1));
    xC(i)=sliceCenter(i,1); yC(i)=sliceCenter(i,2);
    %zC(i)=sliceCenter(i,3);
    roi(i,:)=str2num(header(fbr1(9)+1:fbr2(9)-1));
    orientation(i,:)=str2num(header(fbr1(10)+1:fbr2(10)-1));
end
zC = linspace(0,span(1,3),MatrixDim(1,3));
sizeOfDimension1=MatrixDim(1,1);
sizeOfDimension2=MatrixDim(1,2);
sizeOfDimension1U=sizeOfDimension1; sizeOfDimension2U=sizeOfDimension2;
sizeDim1 = sizeOfDimension1-1;
sizeDim2 = sizeOfDimension2-1;
xSpan=span(1,1); ySpan=span(1,2);
xOffset=sliceCenter(1,1); yOffset=sliceCenter(1,2);
xOffsetU=xOffset; yOffsetU=yOffset;
grid2Units=xSpan/sizeDim2;
grid1Units=ySpan/sizeDim1;
voxelThickness=zC(2)-zC(1);

% xVals = xOffset - (sizeDim2*grid2Units)/2 : grid2Units : xOffset + (sizeDim2*grid2Units)/2;
% yVals = yOffset - (sizeDim1*grid1Units)/2 : grid1Units : yOffset + (sizeDim1*grid1Units)/2;
% zVals = zC;
% [Xt,Yt] = meshgrid(xVals,yVals);
% XXt=Xt(:); YYt=Yt(:);
% XTrans=[]; DD=[]; DDt=[];
% for i=1:length(zVals)
%     XTrans=[XTrans;
%         XXt, YYt, zVals(i)*XXt.^0];
%         DD1=DATA{i};
%     DD=[DD;
%         DD1(:)];
%     DDt=[DDt;
%         DD1(:)];
% end
%
% %% Reshape Data
% DDT3=reshape(DDt,[sizeOfDimension1,sizeOfDimension2,length(zVals)]);

DDT3 = data;

for i=1:length(zC)
    scanInfo(i).grid1Units=grid1Units;
    scanInfo(i).grid2Units=grid2Units;
    scanInfo(i).sizeOfDimension1=sizeOfDimension1U;
    scanInfo(i).sizeOfDimension2=sizeOfDimension2U;
    scanInfo(i).xOffset=xOffsetU;
    scanInfo(i).yOffset=yOffsetU;
    scanInfo(i).voxelThickness=voxelThickness;
    scanInfo(i).zValue=zC(i);
    scanInfo(i).CTOffset=1000;
    scanInfo(i).sliceThickness=[];
    scanInfo(i).imageType = 'MR Scan';
end
planC{3}(1).scanInfo = scanInfo;

minScanArray = min(DDT3(:));
maxScanArray = max(DDT3(:));
scanArray = (DDT3-minScanArray)/maxScanArray*4095;
planC{3}.scanArray = scanArray;
planC{3}.scanType = 'MRI';
planC{3}.scanUID = createUID('scan');
planC = setUniformizedData(planC,optS);

return


classdef TuneAGC < adi.common.DebugAttribute & adi.common.RegisterReadWrite
    properties (Nontunable, Hidden)
        CustomAGC = 0;

        AttackDelay = 1;
        PeakOverloadWaitTime = 10;
        AGCLockLevel = 10;
        DecStepSizeFullTableCase3 = 3;
        ADCLargeOverloadThresh = 58;
        ADCSmallOverloadThresh = 47;
        DecStepSizeFullTableCase2 = 3;
        DecStepSizeFullTableCase1 = 3;
        LargeLMTOverloadThresh = 35;
        SmallLMTOverloadThresh = 25;
        SettlingDelay = 3;
        EnergyLostThresh = 3;
        LowPowerThresh = 15;
        IncrementGainStep
        FAGCLockLevelGainIncreaseUpperLimit  = 7;
        FAGCLPThreshIncrementTime = 3;
        DecPowMeasurementDuration = 16;
    end

    properties (Constant, Hidden, Access = private)
        % Register addresses in hexadecimal
        AttackDelay_Reg = '022';
        PeakOverloadWaitTime_Reg = '0FE';
        AGCLockLevel_Reg = '101';
        DecStepSizeFullTableCase3_Reg = '103';
        ADCSmallOverloadThresh_Reg = '104';
        ADCLargeOverloadThresh_Reg = '105';
        DecStepSizeFullTableCase2_Reg = '106';
        DecStepSizeFullTableCase1_Reg = '106';
        LargeLMTOverloadThresh_Reg = '108';
        SmallLMTOverloadThresh_Reg = '107';
        SettlingDelay_Reg = '111';
        EnergyLostThresh_Reg = '112';
        LowPowerThresh_Reg = '114';
        IncrementGainStep_Reg = '117';
        FAGCLockLevelGainIncreaseUpperLimit_Reg = '118';
        FAGCLPThreshIncrementTime_Reg = '11B';
        DecPowMeasurementDuration_Reg = '15C';

        % Register mask in binary
        AttackDelay_Mask = '11000000';
        PeakOverloadWaitTime_Mask = '11100000';
        AGCLockLevel_Mask = '10000000';
        DecStepSizeFullTableCase3_Mask = '11100011';
        DecStepSizeFullTableCase2_Mask = '10001111';
        DecStepSizeFullTableCase1_Mask = '11110000';
        LargeLMTOverloadThresh_Mask = '11000000';
        SmallLMTOverloadThresh_Mask = '11000000';
        SettlingDelay_Mask = '11100000';
        EnergyLostThresh_Mask = '11000000';
        LowPowerThresh_Mask = '10000000';
        IncrementGainStep_Mask = '00011111';
        FAGCLockLevelGainIncreaseUpperLimit_Mask = '11000000';
        DecPowMeasurementDuration_Mask = '11110000';

        % Bit-shifts to be applied
        DecStepSizeFullTableCase3_BitShift = 2;
        DecStepSizeFullTableCase2_BitShift = 4;
        IncrementGainStep_BitShift = 5;
    end

    methods
        function set.AttackDelay(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',63}, ...
                '', 'AttackDelay');
            obj.AttackDelay = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.AttackDelay_Reg, obj.AttackDelay_Mask);
            end
        end
        function set.PeakOverloadWaitTime(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',31}, ...
                '', 'PeakOverloadWaitTime');
            obj.PeakOverloadWaitTime = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.PeakOverloadWaitTime_Reg, obj.PeakOverloadWaitTime_Mask);
            end
        end
        function set.AGCLockLevel(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',127}, ...
                '', 'AGCLockLevel');
            obj.AGCLockLevel = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.AGCLockLevel_Reg, obj.AGCLockLevel_Mask);
            end
        end
        function set.DecStepSizeFullTableCase3(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',7}, ...
                '', 'DecStepSizeFullTableCase3');
            obj.DecStepSizeFullTableCase3 = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.DecStepSizeFullTableCase3_Reg, obj.DecStepSizeFullTableCase3_Mask, obj.DecStepSizeFullTableCase3_BitShift);
            end
        end
        function set.ADCLargeOverloadThresh(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',255}, ...
                '', 'ADCLargeOverloadThresh');
            obj.ADCLargeOverloadThresh = value;
            if obj.ConnectedToDevice
                obj.setDebugAttributeLongLong('adi,gc-adc-large-overload-thresh',value);
            end
        end
        function set.ADCSmallOverloadThresh(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',obj.ADCLargeOverloadThresh}, ...
                '', 'ADCSmallOverloadThresh');
            obj.ADCSmallOverloadThresh = value;
            if obj.ConnectedToDevice
                obj.setDebugAttributeLongLong('adi,gc-adc-small-overload-thresh',value);
            end
        end
        function set.DecStepSizeFullTableCase2(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',7}, ...
                '', 'DecStepSizeFullTableCase2');
            obj.DecStepSizeFullTableCase2 = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.DecStepSizeFullTableCase2_Reg, obj.DecStepSizeFullTableCase2_Mask, obj.DecStepSizeFullTableCase2_BitShift);
            end
        end
        function set.DecStepSizeFullTableCase1(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',15}, ...
                '', 'DecStepSizeFullTableCase1');
            obj.DecStepSizeFullTableCase1 = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.DecStepSizeFullTableCase1_Reg, obj.DecStepSizeFullTableCase1_Mask);
            end
        end
        function set.LargeLMTOverloadThresh(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',63}, ...
                '', 'LargeLMTOverloadThresh');
            obj.LargeLMTOverloadThresh = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.LargeLMTOverloadThresh_Reg, obj.LargeLMTOverloadThresh_Mask);
            end
        end
        function set.SmallLMTOverloadThresh(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',obj.LargeLMTOverloadThresh}, ...
                '', 'SmallLMTOverloadThresh');
            obj.SmallLMTOverloadThresh = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.SmallLMTOverloadThresh_Reg, obj.SmallLMTOverloadThresh_Mask);
            end
        end
        function set.SettlingDelay(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',31}, ...
                '', 'SettlingDelay');
            obj.SettlingDelay = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.SettlingDelay_Reg, obj.SettlingDelay_Mask);
            end
        end
        function set.EnergyLostThresh(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',63}, ...
                '', 'SettlingDelay');
            obj.EnergyLostThresh = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.EnergyLostThresh_Reg, obj.EnergyLostThresh_Mask);
            end
        end
        function set.LowPowerThresh(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',63}, ...
                '', 'LowPowerThresh');
            obj.LowPowerThresh = value;
            if obj.ConnectedToDevice
                obj.setDebugAttributeLongLong('adi,gc-low-power-thresh',value);
            end
        end
        function set.IncrementGainStep(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',7}, ...
                '', 'IncrementGainStep');
            obj.IncrementGainStep = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.IncrementGainStep_Reg, obj.IncrementGainStep_Mask, obj.IncrementGainStep_BitShift);
            end
        end
        function set.FAGCLockLevelGainIncreaseUpperLimit(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',63}, ...
                '', 'FAGCLockLevelGainIncreaseUpperLimit');
            obj.FAGCLockLevelGainIncreaseUpperLimit = value;
            if obj.ConnectedToDevice
                obj.setDebugAttributeLongLong('adi,fagc-lock-level-gain-increase-upper-limit',value);
            end
        end
        function set.FAGCLPThreshIncrementTime(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',255}, ...
                '', 'FAGCLPThreshIncrementTime');
            obj.FAGCLPThreshIncrementTime = value;
            if obj.ConnectedToDevice
                obj.setDebugAttributeLongLong('adi,fagc-lp-thresh-increment-time',value);
            end
        end
        function set.DecPowMeasurementDuration(obj, value)
            validateattributes( value, { 'double','single', 'uint32' }, ...
                { 'real', 'nonnegative','scalar', 'finite', 'nonnan', 'nonempty','integer','>=',0,'<=',15}, ...
                '', 'DecPowMeasurementDuration');
            obj.DecPowMeasurementDuration = value;
            if obj.ConnectedToDevice
                obj.setRegister(value, obj.DecPowMeasurementDuration_Reg, obj.DecPowMeasurementDuration_Mask);
            end
        end
        function WriteDebugAttributes(obj)
            if obj.ConnectedToDevice
                obj.setDebugAttributeLongLong('adi,gc-adc-large-overload-thresh',obj.ADCLargeOverloadThresh);
                obj.setDebugAttributeLongLong('adi,gc-adc-small-overload-thresh',obj.ADCSmallOverloadThresh);
                obj.setDebugAttributeLongLong('adi,gc-low-power-thresh',obj.LowPowerThresh);
                obj.setDebugAttributeLongLong('adi,fagc-lock-level-gain-increase-upper-limit',obj.FAGCLockLevelGainIncreaseUpperLimit);
                obj.setDebugAttributeLongLong('adi,fagc-lp-thresh-increment-time',obj.FAGCLPThreshIncrementTime);
            end
        end
        function WriteToRegisters(obj)
            if obj.ConnectedToDevice
                obj.setRegister(obj.AttackDelay, obj.AttackDelay_Reg, obj.AttackDelay_Mask);
                obj.setRegister(obj.PeakOverloadWaitTime, obj.PeakOverloadWaitTime_Reg, obj.PeakOverloadWaitTime_Mask);
                obj.setRegister(obj.AGCLockLevel, obj.AGCLockLevel_Reg, obj.AGCLockLevel_Mask);
                obj.setRegister(obj.DecStepSizeFullTableCase3, obj.DecStepSizeFullTableCase3_Reg, obj.DecStepSizeFullTableCase3_Mask, obj.DecStepSizeFullTableCase3_BitShift);
                obj.setRegister(obj.DecStepSizeFullTableCase2, obj.DecStepSizeFullTableCase2_Reg, obj.DecStepSizeFullTableCase2_Mask, obj.DecStepSizeFullTableCase2_BitShift);
                obj.setRegister(obj.DecStepSizeFullTableCase1, obj.DecStepSizeFullTableCase1_Reg, obj.DecStepSizeFullTableCase1_Mask);
                obj.setRegister(obj.LargeLMTOverloadThresh, obj.LargeLMTOverloadThresh_Reg, obj.LargeLMTOverloadThresh_Mask);
                obj.setRegister(obj.SmallLMTOverloadThresh, obj.SmallLMTOverloadThresh_Reg, obj.SmallLMTOverloadThresh_Mask);
                obj.setRegister(obj.SettlingDelay, obj.SettlingDelay_Reg, obj.SettlingDelay_Mask);
                obj.setRegister(obj.EnergyLostThresh, obj.EnergyLostThresh_Reg, obj.EnergyLostThresh_Mask);
                obj.setRegister(obj.IncrementGainStep, obj.IncrementGainStep_Reg, obj.IncrementGainStep_Mask, obj.IncrementGainStep_BitShift);
                obj.setRegister(obj.DecPowMeasurementDuration, obj.DecPowMeasurementDuration_Reg, obj.DecPowMeasurementDuration_Mask);
            end
        end
        function value = ReadFromRegister(obj, prop_name)
            if obj.ConnectedToDevice
                switch prop_name
                    case 'AttackDelay'
                        value = obj.getRegister(obj.AttackDelay_Reg, obj.AttackDelay_Mask);
                    case 'PeakOverloadWaitTime'
                        value = obj.getRegister(obj.PeakOverloadWaitTime_Reg, obj.PeakOverloadWaitTime_Mask);
                    case 'AGCLockLevel'
                        value = obj.getRegister(obj.AGCLockLevel_Reg, obj.AGCLockLevel_Mask);
                    case 'DecStepSizeFullTableCase3'
                        value = obj.getRegister(obj.DecStepSizeFullTableCase3_Reg, obj.DecStepSizeFullTableCase3_Mask, obj.DecStepSizeFullTableCase3_BitShift);
                    case 'ADCSmallOverloadThresh'
                        value = obj.getRegister(obj.ADCSmallOverloadThresh_Reg);
                    case 'ADCLargeOverloadThresh'
                        value = obj.getRegister(obj.ADCLargeOverloadThresh_Reg);
                    case 'DecStepSizeFullTableCase2'
                        value = obj.getRegister(obj.DecStepSizeFullTableCase2_Reg, obj.DecStepSizeFullTableCase2_Mask, obj.DecStepSizeFullTableCase2_BitShift);
                    case 'DecStepSizeFullTableCase1'
                        value = obj.getRegister(obj.DecStepSizeFullTableCase1_Reg, obj.DecStepSizeFullTableCase1_Mask);
                    case 'LargeLMTOverloadThresh'
                        value = obj.getRegister(obj.LargeLMTOverloadThresh_Reg, obj.LargeLMTOverloadThresh_Mask);
                    case 'SmallLMTOverloadThresh'
                        value = obj.getRegister(obj.SmallLMTOverloadThresh_Reg, obj.SmallLMTOverloadThresh_Mask);
                    case 'SettlingDelay'
                        value = obj.getRegister(obj.SettlingDelay_Reg, obj.SettlingDelay_Mask);
                    case 'EnergyLostThresh'
                        value = obj.getRegister(obj.EnergyLostThresh_Reg, obj.EnergyLostThresh_Mask);
                    case 'LowPowerThresh'
                        value = obj.getRegister(obj.LowPowerThresh_Reg, obj.LowPowerThresh_Mask);
                    case 'IncrementGainStep'
                        value = obj.getRegister(obj.IncrementGainStep_Reg, obj.IncrementGainStep_Mask, obj.IncrementGainStep_BitShift);
                    case 'FAGCLockLevelGainIncreaseUpperLimit'
                        value = obj.getRegister(obj.FAGCLockLevelGainIncreaseUpperLimit_Reg, obj.FAGCLockLevelGainIncreaseUpperLimit_Mask);
                    case 'FAGCLPThreshIncrementTime'
                        value = obj.getRegister(obj.FAGCLPThreshIncrementTime_Reg);
                    case 'DecPowMeasurementDuration'
                        value = obj.getRegister(obj.DecPowMeasurementDuration_Reg, obj.DecPowMeasurementDuration_Mask);
                    otherwise
                        error('Attempted to read unknown property %s\n', prop_name);
                end
            end
        end
    end
end


function eval(obj, inputs, derOutputs, stopLayer)
%EVAL Evaluate the DAGNN
%   EVAL(obj, inputs) evaluates the DaG for the specified input
%   values. `inputs` is a cell array of the type `{'inputName',
%   inputValue, ...}`. This call results in a forward pass through the
%   graph, computing the values of the output variables. These can
%   then be accessed using the `obj.vars(outputIndex)` property of the
%   DaG object. The index of an output can be obtained using the
%   `obj.getOutputIndex(outputName)` call.
%
%   EVAL(obj, inputs, derOutputs) evaluates the DaG forward and then
%   backward, performing backpropagation. Similar to `inputs`,
%   `derOutputs` is a cell array of the type {'outputName',
%   outputDerValue, ...} of output derivatives.
%
%   # Understanding backpropagation
%
%   Only those outputs for which an `outputDerValue` which is
%   non-empty are involved in backpropagation, while the others are
%   ignored. This is useful to attach to the graph auxiliary layers to
%   compute errors or other statistics, without however involving them
%   in backpropagation.
%
%   Usually one starts backpropagation from scalar outptus,
%   corresponding to loss functions. In this case `outputDerValue` can
%   be interpreted as the weight of that output and is usually set to
%   one. For example: `{'objective', 1}` backpropagates from the
%   `'objective'` output variable with a weight of 1.
%
%   However, in some cases the DaG may contain more than one such
%   node, for example because one has more than one loss function.  In
%   this case `{'objective1', w1, 'objective2', w2, ...}` allows to
%   balance the different objectives.
%
%   Finally, one can backpropagate from outputs that are *not*
%   scalars. While this is unusual, it is possible by specifying a
%   value of `outputDerValue` that has the same dimensionality as the
%   output; in this case, this value is used as a matrix of weights,
%   or projection.
%
%   # Factors affecting evaluation
%
%   There are several factors affecting evaluation:
%
%   * The *evaluation mode* can be either `normal` or `test`. Layers
%     may behave differently depending on the mode. For example,
%     dropout becomes a pass-through layer in test mode and batch
%     normalization use fixed moments (this usually improves the test
%     performance significantly).
%
%   * By default, the DaG aggressively conserves memory. This is
%     particularly important on the GPU, where memory is
%     scarce. However, this also means that the values of most
%     variables and of their derivatives are dropped during the
%     computation. For debugging purposes, it may be interesting to
%     observe these variables; in this case you can set the
%     `obj.conserveMemory` property of the DaG to `false`. It is also
%     possible to preserve individual variables by setting the
%     property `obj.vars(v).precious` to `true`.

% Copyright (C) 2015 Karel Lenc and Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

obj.computingDerivative = nargin > 2 && ~isempty(derOutputs) ;

if ~iscell(inputs), error('INPUTS is not a cell array.') ; end
if obj.computingDerivative && ~iscell(derOutputs), error('DEROUTPUTS is not a cell array.') ; end

if nargin < 4
  stopLayer = [];
end
% -------------------------------------------------------------------------
% Forward pass
% -------------------------------------------------------------------------

% set the input values
v = obj.getVarIndex(inputs(1:2:end)) ;
if any(isnan(v))
  broken = find(isnan(v)) ;
%   error('No variable of name ''%s'' could be found in the DAG.', inputs{2*broken(1)-1}) ;
end

for i = 1:2:numel(inputs)
  v = obj.getVarIndex(inputs{i}) ;
  if ~isnan(v)
      switch obj.device
        case 'cpu', obj.vars(v).value = gather(inputs{i+1}) ;
        case 'gpu', obj.vars(v).value = gpuArray(inputs{i+1}) ;
      end
  end
end

inputs = [] ;
obj.numPendingVarRefs = [obj.vars.fanout] ;
for l = obj.executionOrder
  time = tic ;
  obj.layers(l).block.forwardAdvanced(obj.layers(l)) ;
  obj.layers(l).forwardTime = toc(time) ;

  if stopLayer == l
    break;
  end
end

% -------------------------------------------------------------------------
% Backward pass
% -------------------------------------------------------------------------

if ~obj.computingDerivative, return ; end

% conserve memory
if obj.backpropDepth
for l = obj.executionOrder
  [obj.vars(obj.layers(l).inputIndexes).value] = deal([]) ;
  if strcmp(obj.layers(l).name, obj.backpropDepth)
    break;
  end
end
end

v = obj.getVarIndex(derOutputs(1:2:end)) ;
[obj.vars(v).der] = deal(derOutputs{2:2:end}) ;
derOutputs = [] ;

obj.numPendingVarRefs = zeros(1, numel(obj.vars)) ;
obj.numPendingParamRefs = zeros(1, numel(obj.params)) ;
for l = fliplr(obj.executionOrder)
  time = tic ;
  if strcmp(obj.layers(l).name, obj.backpropDepth)
    [obj.vars.value] = deal([]) ;
    [obj.vars.der] = deal([]) ;
    break;
  end
  obj.layers(l).block.backwardAdvanced(obj.layers(l)) ;
  obj.layers(l).backwardTime = toc(time) ;
end


function varargout = subsref(varargin)
%SUBSREF (overloaded)

% Stupid first slice call (supported by MATLAB)
% x = sdpvar(2);x(1,:,:)
Y = varargin{2};
if length(Y)==1
    if  length(Y.subs) > 2 && isequal(Y.type,'()') && ~( isa(Y(1).subs{1},'constraint') || isa(Y(1).subs{1},'lmi') )
        i = 3;
        ok = 1;
        while ok && (i <= length(Y.subs))
            ok = ok && (isequal(Y.subs{i},1) || isequal(Y.subs{i},':'));
            i = i + 1;
        end
        if ok
            Y.subs = {Y.subs{1:2}};
        else
            error('??? Index exceeds matrix dimensions.');
        end
    end
end

X = varargin{1};

try
    switch Y(1).type
        case '()'
            if  isa(Y(1).subs{1},'constraint') || isa(Y(1).subs{1},'lmi')
                z = Y(1).subs{1};
                for i = 1:length(Y(1).subs)
                    z = [z, Y(1).subs{i}];
                end
                % z == 0 => b+Ax == 0 => x = -A\b
                z = sdpvar(z);
                B = getbase(z);
                A = B(:,2:end);
                b = B(:,1);
                x0 = -A\b;
                x = recover(getvariables(z));
                varargout{1} = replace(X,x,x0);
                return
            end
            % Check for simple cases to speed things up (yes, ugly but we all want speed don't we!)
            switch size(Y(1).subs,2)
                case 1
                    y = subsref1d(X,Y(1).subs{1},Y);
                case 2
                    y = subsref2d(X,Y.subs{1},Y(1).subs{2},Y);
                otherwise
                    if all( [Y(1).subs{3:end}]==1)
                        y = subsref2d(X,Y.subs{1},Y(1).subs{2},Y);
                    else
                        error('Indexation error.');
                    end
            end
        case '{}'
            varargout{nargout} = [];

            % it could be the case that we have an extended variable
            % This is a bit tricky, so we do the best we can; assume that
            % we want to replace the internal argument wih the new
            % expression
            OldArgument = recover(depends(X));
            vars = getvariables(X);
            mpt_solution = 1;
            if all(ismembc(vars,yalmip('extvariables')))
                for i = 1:length(X)
                    nonlinearModel = yalmip('extstruct',vars);
                    if isequal(nonlinearModel{1}.fcn,'pwa_yalmip') | isequal(nonlinearModel{1}.fcn,'pwq_yalmip')
                    else
                        mpt_solution = 0;
                    end
                end
                if mpt_solution
                    assign(nonlinearModel{1}.arg{2},Y(1).subs{:});
                    XX = value(X);
                    varargout{1} = value(X);
                    return
                end
            end
            vars = getvariables(X);
            if (length(vars) == 1) & ismembc(vars,yalmip('extvariables'))
                nonlinearModel = yalmip('extstruct',vars);
                OldArgument = [];
                for i = 1:length(nonlinearModel.arg)
                    if isa(nonlinearModel.arg{i},'sdpvar')
                        OldArgument = [OldArgument;  nonlinearModel.arg{i}];
                    end
                end
                if isnumeric([Y.subs{:}])
                    assign(reshape(OldArgument,[],1),reshape([Y(1).subs{:}],[],1));
                    varargout{1} = value(X);
                    return
                end
            end
            y = replace(X,OldArgument,[Y(1).subs{:}]);
            if isnumeric(y)
                varargout{1} = y;
                return
            end

        case '.'
            switch Y(1).subs
                case {'minimize','maximize'}
                    options = [];
                    constraints = [];
                    objective = varargin{1};
                    opsargs = {};
                    if length(Y)==2
                        if isequal(Y(2).type,'()')
                            for i = 1:length(Y(2).subs)
                                switch class(Y(2).subs{i})
                                    case {'lmi','constraint'}
                                        constraints = [constraints, Y(2).subs{i}];
                                    case 'struct'
                                        options = Y(2).subs{i};
                                    case {'double','char','gem','sgem'}
                                        opsargs{end+1} = Y(2).subs{i};
                                    otherwise
                                        error('Argument to minimize should be constraints or options');
                                end
                            end
                        else
                            error(['What do you mean with ' Y(2).type '?']);
                        end
                    end
                    if length(opsargs)>0
                        if isempty(options)
                           options = sdpsettings(opsargs{:});
                        else
                            options = sdpsettings(options,opsargs{:});
                        end
                    end
                    if isequal(Y(1).subs,'minimize')
                        sol = solvesdp(constraints,objective,options);
                    else
                        sol = solvesdp(constraints,-objective,options);
                    end
                    varargout{1} = varargin{1};
                    varargout{2} = sol;
                    return
                case 'derivative'
                    try
                        m = model(varargin{1});
                        varargout{1} = m{1}.derivative;
                    catch
                        varargout{1} = 1;
                    end
                    return
                otherwise
                    error(['Indexation  ''' Y.type Y.subs ''' not supported']) ;
            end
        otherwise
            error(['Indexation with ''' Y.type ''' not supported']) ;
    end
catch
    error(lasterr)
end
if isempty(y.lmi_variables)
    y = full(reshape(y.basis(:,1),y.dim(1),y.dim(2)));
else
    % Reset info about conic terms
    y.conicinfo = [0 0];
    y.extra.createTime = definecreationtime;
end
varargout{1} = y;

function X = subsref1d(X,ind1,Y)

% Get old and new size
n = X.dim(1);
m = X.dim(2);

% Convert to linear indecicies
if islogical(ind1)
    ind1 = double(find(ind1));
elseif ischar(ind1)
    X.dim(1) = n*m;
    X.dim(2) = 1;
    return;
elseif ~isnumeric(ind1)
    X = milpsubsref(X,Y);
    return
end

% Detect X(scalar)
if length(ind1) == 1 & ind1 <= n*m

    Z = X.basis(ind1,:);
    nnew = 1;
    mnew = 1;

else

    % What would the size be for a double
    dummy = reshape(X.basis(:,1),n,m);
    dummy = dummy(ind1);
    nnew = size(dummy,1);
    mnew = size(dummy,2);
    [nx,mx] = size(X.basis);

    if length(ind1) > 1
        try
            % row-based subsref for sparse objects can be very slow and
            % take a lot of memory, transposing a big sparse array as in
            %   Z = X.basis.';
            %   Z = Z(:,ind1);
            %   Z = Z.';
            % can also be quite slow for very large matrices, so we use
            % some custom code
            [ix,jx,sx] = find(X.basis);
            if length(ix) == length(unique(ix))
                % We never have two elements on the same row of X.basis
                [isNnz,loc] = ismember(ind1(:),ix);
                sel = loc(isNnz);
                ix = find(isNnz);
                jx = jx(sel);
                sx = sx(sel);
                Z = sparse(ix,jx,sx,numel(ind1),mx);
            else
                % We can have seveal elements on a given row of X.basis
                if numel(ind1) == length(unique(ind1))
                    % Every row of X.basis is selected at most once
                    [keep,loc] = ismember(ix,ind1(:));
                    ix = loc(keep);
                    jx = jx(keep);
                    sx = sx(keep);
                    Z = sparse(ix,jx,sx,numel(ind1),mx);
                else
                    % Complicated case, we use more generica code
                    Z = sparse(1:numel(ind1),ind1,1,numel(ind1),size(X,1))*X.basis;
                end
            end

            if false
                % For checking purpose
                Z0 = X.basis.';
                Z0 = Z0(:,ind1);
                Z0 = Z0.';
                assert(isequal(Z, Z0));
            end
        catch
            Z = X.basis(ind1,:);
        end
    else
        Z = X.basis(ind1,:);
    end
end

% Find non-zero basematrices
nzZ = find(any(Z(:,2:end),1));
if ~isempty(nzZ)
    X.dim(1) = nnew;
    X.dim(2) = mnew;
    X.lmi_variables =  X.lmi_variables(nzZ);
    X.basis = Z(:,[1 1+nzZ]);
else
    bas = reshape(X.basis(:,1),n,m);
    X.dim(1) = nnew;
    X.dim(2) = mnew;
    X.lmi_variables = [];
    X.basis = reshape(bas(ind1),nnew*mnew,1);
end

function X = subsref2d(X,ind1,ind2,Y)

if isnumeric(ind1)
elseif ischar(ind1)
    ind1 = 1:X.dim(1);
elseif islogical(ind1)
    ind1 = double(find(ind1));
elseif ~isnumeric(ind1)
    X = milpsubsref(X,Y);
    return
end
if isnumeric(ind2)
elseif ischar(ind2)
    ind2 = 1:X.dim(2);
elseif islogical(ind2)
    ind2 = double(find(ind2));
elseif  ~isnumeric(ind2)
    X = milpsubsref(X,Y);
    return
end

n = X.dim(1);
m = X.dim(2);
lind2 = length(ind2);
lind1 = length(ind1);
if lind2 == 1
    ind1_ext = ind1(:);
else
    ind1_ext = kron(ones(lind2,1),ind1(:));
end
if lind1 == 1
    ind2_ext = ind2(:);
else
    ind2_ext = kron(ind2(:),ones(lind1,1));
end

if any(ind1 > n) || any(ind2 > m)
    error('Index exceeds matrix dimensions.');
end

if lind1==1 && lind2==1
    if isequal(X.conicinfo,[-1 0])
        X.basis = [0 1];
        X.lmi_variables = X.lmi_variables(1)+ind1+(ind2-1)*n-1;
        X.dim = [1 1];
        X.conicinfo = [0 0];
        return
    end
end

if prod(size(ind1_ext))==0 | prod(size(ind2_ext))==0
    linear_index = [];
else
    % Speed-up for some bizarre code with loads of indexing of vector
    if m==1 & ind2_ext==1
        linear_index = ind1_ext;
    elseif length(ind2_ext)==1 && length(ind1_ext)==1
        linear_index = ind1_ext + (ind2_ext-1)*n;
    else
        linear_index = sub2ind([n m],ind1_ext,ind2_ext);
    end
end
nnew = length(ind1);
mnew = length(ind2);

% Put all matrices in vectors and extract sub matrix
Z = X.basis(linear_index,:);
% Find non-zero basematrices
%nzZ = find(any(Z(:,2:end),1));
nzZ = find(any(Z,1))-1;
if numel(nzZ)>0
    if nzZ(1)==0
    nzZ = nzZ(2:end);
    end
end
if ~isempty(nzZ)
    X.dim(1) = nnew;
    X.dim(2) = mnew;
    X.lmi_variables =  X.lmi_variables(nzZ);
    X.basis = Z(:,[1 1+nzZ]);
else
    bas = reshape(X.basis(:,1),n,m);
    X.dim(1) = nnew;
    X.dim(2) = mnew;
    X.lmi_variables = [];
    X.basis = reshape(bas(linear_index),nnew*mnew,1);
end


addpath(genpath('D:\GitHub\KiloSort2')) % path to kilosort folder
addpath('D:\GitHub\npy-matlab')

pathToYourConfigFile = 'D:\GitHub\KiloSort2\configFiles'; % take from Github folder and put it somewhere else (together with the master_file)
run(fullfile(pathToYourConfigFile, 'configFileBench384.m'))

% common options for every probe
ops.chanMap     = 'D:\GitHub\KiloSort2\configFiles\neuropixPhase3A_kilosortChanMap.mat';
ops.trange      = [0 Inf]; % TIME RANGE IN SECONDS TO PROCESS

 % these settings overwrite any settings from config
ops.Th       = [10 10];     % threshold on projections (like in Kilosort1)
ops.lam      = 40^2;   % weighting on the amplitude penalty (like in Kilosort1, but it has to be much larger)

ops.ThS      = [8 8];  % lower bound on acceptable single spike quality
ops.momentum = [20 400]; % number of samples to average over
ops.minFR    = 1/50; % minimum spike rate (Hz)

ops.sigmaMask  = 30;

ops.Nfilt       = 1024; % max number of clusters
ops.nfullpasses = 3; % how many forward backward passes to do
ops.nPCs        = 3; % how many PCs to project the spikes into

ops.NchanTOT = 385;
ops.useRAM = 0;
ops.ccsplit = .99;

% rootZ = 'D:\DATA\ALLEN\mouse366119\probeC_2018-03-02_15-18-32_SN619041624\experiment1\recording1\continuous\Neuropix-120.0\';
% rootZ = 'H:\DATA\Spikes\Robbins\ZNP1';
rootZ = 'H:\DATA\Spikes\WillAllen';
rootH = 'H:\DATA\Spikes\temp\';

fs = dir(fullfile(rootZ, '*.bin'));

fname = fs(1).name;

ops.fbinary     = fullfile(rootZ,  fname);
ops.fproc       = fullfile(rootH, 'temp_wh2.dat'); % residual from RAM of preprocessed data

% preprocess data
rez = preprocessDataSub(ops);

rez = clusterSingleBatches(rez);

% figure(191);
% imagesc(rez.ccb(rez.iorig, rez.iorig), [20 100])
%%
learnAndSolve8
% rez = learnAndSolve8(rez);

rez2    = splitAllClusters(rez);

rezToPhy(rez2, rootZ);


classdef (Abstract, Hidden = true) Base < adi.common.Attribute & ...
        adi.common.DebugAttribute & adi.common.RxTx & ...
        matlabshared.libiio.base & matlab.system.mixin.CustomIcon

    %adi.AD9361.Base Class
    %   This class contains shared parameters and methods between TX and RX
    %   classes
    properties (Nontunable)
        %SamplesPerFrame Samples Per Frame
        %   Number of samples per frame, specified as an even positive
        %   integer from 2 to 16,777,216. Using values less than 3660 can
        %   yield poor performance.
        SamplesPerFrame = 2^15;
    end

    properties (Nontunable, Logical)
        %EnableCustomFilter Enable Custom Filter
        %   Enable use of custom filter file to set SamplingRate,
        %   RFBandwidth, and FIR in datapaths
        EnableCustomFilter = false;
    end

    properties (Nontunable)
        %CustomFilterFileName Custom Filter File Name
        %   Path to custom filter file created from filter wizard
        CustomFilterFileName = '';
    end

    properties (Abstract)
        %CenterFrequency Center Frequency
        %   RF center frequency, specified in Hz as a scalar. The
        %   default is 2.4e9.  This property is tunable.
        CenterFrequency
        %SamplingRate Sampling Rate
        %   Baseband sampling rate in Hz, specified as a scalar
        %   from 65105 to 61.44e6 samples per second.
        SamplingRate
        %RFBandwidth RF Bandwidth
        %   RF Bandwidth of front-end analog filter in Hz, specified as a
        %   scalar from 200 kHz to 56 MHz.
        RFBandwidth
    end

    properties(Nontunable, Hidden)
        Timeout = Inf;
        kernelBuffersCount = 2;
        dataTypeStr = 'int16';
        phyDevName = 'ad9361-phy';
        iioDevPHY
    end

    properties (Hidden, Constant)
        ComplexData = true;
    end

    methods
        %% Constructor
        function obj = Base(varargin)
            coder.allowpcode('plain');
            obj = obj@matlabshared.libiio.base(varargin{:});
        end
        % Destructor
        function delete(obj)
            teardownLibad9361(obj);
            delete@adi.common.RxTx(obj);
        end
        % Check SamplesPerFrame
        function set.SamplesPerFrame(obj, value)
            validateattributes( value, { 'double','single' }, ...
                { 'real', 'positive','scalar', 'finite', 'nonnan', 'nonempty','integer','>',0,'<=',2^20}, ...
                '', 'SamplesPerFrame');
            obj.SamplesPerFrame = value;
        end
        % Check EnableCustomFilter
        function set.EnableCustomFilter(obj, value)
            validateattributes( value, { 'logical' }, ...
                { }, ...
                '', 'EnableCustomFilter');
            obj.EnableCustomFilter = value;
        end
        % Check CustomFilterFileName
        function set.CustomFilterFileName(obj, value)
            validateattributes( value, { 'char' }, ...
                { }, ...
                '', 'CustomFilterFileName');
            obj.CustomFilterFileName = value;
            if obj.EnableCustomFilter && obj.ConnectedToDevice %#ok<MCSUP>
                writeFilterFile(obj);
            end
        end
    end

    %% API Functions
    methods (Hidden, Access = protected)

        function icon = getIconImpl(obj)
            icon = sprintf(['AD9361 ',obj.Type]);
        end

        function setupLibad9361(obj)
            libName = 'libad9361';
            ad9361wrapperh = 'ad9361-wrapper.h';
            ad9361h = 'ad9361.h';
            fp = fileparts(which(ad9361h));
            loadlibraryArgs = {ad9361wrapperh,'includepath',fp,'addheader',ad9361h};
            if ~libisloaded(libName)
                msgID = 'MATLAB:loadlibrary:StructTypeExists';
                warnStruct = warning('off',msgID);
                [~, ~] = loadlibrary(libName, loadlibraryArgs{:});
                warning(warnStruct);
            end
            obj.iioDevPHY = calllib('libiio', 'iio_context_find_device',obj.iioCtx,'ad9361-phy');
        end

        function teardownLibad9361(~)
            libName = 'libad9361';
            if libisloaded(libName)
                unloadlibrary(libName);
            end
        end

        function writeFilterFile(obj)
            fir_data_file = obj.CustomFilterFileName;
            fir_data_str = fileread(fir_data_file);
            obj.setAttributeRAW('voltage0','filter_fir_en','0',false);
            obj.setAttributeRAW('voltage0','filter_fir_en','0',true);
            obj.setDeviceAttributeRAW('filter_fir_config',fir_data_str);
            obj.setAttributeRAW('voltage0','filter_fir_en','1',true);
            obj.setAttributeRAW('voltage0','filter_fir_en','1',false);
        end

    end

    %% External Dependency Methods
    methods (Hidden, Static)

        function tf = isSupportedContext(bldCfg)
            tf = matlabshared.libiio.ExternalDependency.isSupportedContext(bldCfg);
        end

        function updateBuildInfo(buildInfo, bldCfg)
            % Call the matlabshared.libiio.method first
            matlabshared.libiio.ExternalDependency.updateBuildInfo(buildInfo, bldCfg);
        end

        function bName = getDescriptiveName(~)
            bName = 'AD9361';
        end

    end
end


% IN ORDER TO SIMULATE THE PROGRAM:
%   A) FIRST, LOAD A ROBOT
%       robot = load_robot('abb','irb140');
%       OR
%       robot = load_robot('abb','irb52');
%       or any other robot, the target points may not be reachable,
%       depending on the links' lengths.
%
%   B) NEXT, LOAD SOME EQUIPMENT.
%       robot.equipment = load_robot('equipment','tables/table_two_areas');
%
%
%
% The example presents the robot achieving different positions and orientations
% In addition, the conf values, change accordingly.

function test_configurations2

global  TD_tool0 robot

%definition of the end effector.
TD_tool0=[1,[[0,0,0],[1,0,0,0]],[0,[0,0,0],[1,0,0,0],0,0,0]];

main
end

function main
global  TD_tool0 robot

MoveJ([[217.4,1.57,1072.81]/1000,[0.972982,-0.01947,0.224767,-0.049073],[0,1,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],'vmax','z50',TD_tool0);
MoveJ( [[86.96,1.57,1072.8]/1000,[0.972992,-0.01944,0.224739,-0.049009],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],'vmax','z50',TD_tool0);
MoveJ( [[28.15,1.55,1072.8]/1000,[0.973021,-0.019611,0.224611,-0.048949],[1,-2,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],'vmax','z50',TD_tool0);
MoveJ( [[14.42,1.55,1072.8]/1000,[0.973015,-0.019655,0.224611,-0.049041],[1,-2,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],'vmax','z50',TD_tool0);
MoveJ( [[0.11,1.57,1072.82]/1000,[0.973031,-0.019796,0.224522,-0.049091],[1,-2,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],'vmax','z50',TD_tool0);
MoveJ( [[-12.28,1.55,1072.81]/1000,[0.973022,-0.019733,0.224521,-0.049298],[1,-2,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],'vmax','z50',TD_tool0);
MoveJ( [[-98.02,1.54,1072.82]/1000,[0.973023,-0.019651,0.22448,-0.049486],[1,-2,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],'vmax','z50',TD_tool0);
MoveJ( [[-231.93,1.54,1072.81]/1000,[0.973047,-0.019566,0.224335,-0.049716],[1,-2,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],'vmax','z50',TD_tool0);

end


function transducer = makeTransducer(kgrid, transducer_properties)
%MAKETRANSDUCER Create k-Wave ultrasound transducer.
%
% DESCRIPTION:
%       makeTransducer creates an object of the kWaveTransducer class which
%       can be substituted for the source or sensor inputs when using
%       kspaceFirstOrder3D.
%
%       Note: This function will not work with older versions of MATLAB in
%       which custom class definitions are not supported.
%
% USAGE:
%       transducer = makeTransducer(kgrid, settings)
%
% INPUTS:
%
%       kgrid           - k-Wave grid structure returned by makeGrid
%       settings        - input structure used to define the properties of
%                         the transducer (see below)
%
%       The following parameters can be appended as fields to the settings
%       input structure. These parameters are fixed when the transducer is
%       initialised (they cannot be changed without re-creating the
%       transducer). All the settings are optional and are given their
%       default values if not defined.
%
%       number_elements - total number of transducer elements (default =
%                         128)
%       element_width   - width of each element in grid points
%                         (default = 1)
%       element_length  - length of each element in grid points
%                         (default = 10)
%       element_spacing - spacing (kerf width) between the transducer
%                         elements in grid points (default = 0)
%       position        - position of the corner of the transducer within
%                         the grid in grid points (default = [1, 1, 1])
%       radius          - radius of curvature of the transducer [m]
%                         (currently only inf is supported) (default = inf)
%       input_signal    - signal used to drive the ultrasound transducer
%                         (where the sampling rate is defined by kgrid.dt
%                         or kgrid.t_array) (default = [])
%
%       The following parameters can also be appended as fields to the
%       settings input structure, and can additionally be modified after
%       the transducer has been initialised.
%
%       active_elements - transducer elements that are currently active
%                         elements (default = all elements)
%       beamforming_delay_offset
%                       - beamforming delay offset (used to force
%                         beamforming delays to be positive) (default =
%                         'auto')
%       elevation_focus_distance
%                       - focus depth in the elevation direction [m]
%                         (default = inf)
%       focus_distance  - focus distance used to calculate beamforming
%                         delays [m] (default = inf)
%       receive_apodization
%                       - receive apodization; can be set to any of the
%                         window shapes supported by getWin, or given as a
%                         vector the same length as number_elements
%                         (default = 'Rectangular')
%       sound_speed     - sound speed used to calculate beamforming delays
%                         [m/s] (default = 1540)
%       steering_angle  - steering angle used to calculate beamforming
%                         delays [deg] (default = 0)
%       transmit_apodization
%                       - transmit apodization; can be set to any of the
%                         window shapes supported by getWin, or given as a
%                         vector the same length as number_elements
%                         (default = 'Rectangular')
%
% OUTPUTS:
%
%       transducer      - kWaveTransducer object which can be used to
%                         replace the source or sensor inputs of
%                         kspaceFirstOrder3D
%
%       In addition to the input parameters given above (which are also
%       accessible after the transducer has been created), the
%       kWaveTransducer object has a number dependent properties and
%       methods.
%
%       input_signal    - user defined input signal appended and prepended
%                         with additional zeros depending on the values of
%                         focus_distance, elevation_focus_distance, and
%                         steering_angle
%       mask            - binary mask of the active transducer elements
%       number_active_elements
%                       - current number of active transducer elements
%       transducer_width
%                       - total width of the transducer in grid points
%
%       active_elements_mask
%                       - return a binary mask of the active transducer
%                         elements (identical to mask)
%       all_elements_mask
%                       - return a binary mask of all the transducer
%                         elements (both active and inactive)
%       beamforming_delays
%                       - return a vector of the beam forming delays (in
%                         units of time samples) for each active element
%                         based on the focus and steering angle settings
%       combine_sensor_data(sensor_data)
%                       - combine the sensor data returned by
%                         kspaceFirstOrder3D-OMP and
%                         kspaceFirstOrder3D-CUDA to give a single time
%                         series per active transducer element (anlogous to
%                         the output from the MATLAB code), rather than a
%                         time series per grid point
%       delay_mask      - return a mask of the active transducer elements,
%                         where the mask values contain the beamforming
%                         delays (an integer input can also be given to
%                         control the beamforming delays used, where 1:
%                         both delays, 2: elevation only, 3: azimuth only)
%       elevation_beamforming_delays - return a vector of the elevation
%                         beam forming delays (in units of time samples)
%                         for each active element based on the elevation
%                         focus setting
%       get_receive_apodization
%                       - return the receive apodization
%       get_transmit_apodization
%                       - return the transmit apodization
%       indexed_active_elements_mask
%                       - return a mask of the active transducer elements,
%                         where the mask values indicate which transducer
%                         element each grid point corresponds to
%       indexed_elements_mask
%                       - return a mask of all the transducer elements
%                         (both active and inactive), where the mask values
%                         indicate which transducer element each grid point
%                         corresponds to
%       plot            - plot the transducer using voxelPlot
%       properties      - print a list of the transducer properties to the
%                         command line
%       scan_line(sensor_data)
%                       - combine the input sensor data using the current
%                         apodization and beamforming setting to generate a
%                         single scan line
%       transmit_apodization_mask
%                       - return a mask of the active transducer elements,
%                         where the mask values contain the apodization
%                         weights
%
% ABOUT:
%       author          - Bradley Treeby
%       date            - 28th July 2011
%       last update     - 4th November 2013
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also makeGrid, kspaceFirstOrder3D, kWaveTransducer

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave 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 Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

try
    % create a new instance of the kWaveTransducer class
    transducer = kWaveTransducer(kgrid, transducer_properties);
catch ME
    % if user defined classes aren't supported, throw an error
    if strcmp(ME.identifier, 'MATLAB:UndefinedFunction')
        error('The transducer cannot be created because user defined classes are not supported in your version of MATLAB. To use this functionality, please try using a newer MATLAB version.');
    end
    rethrow(ME);
end


% Valid ending arguments:
% - SequenceNumber (0 - 65535)
% - StateInfo0 (1 byte)
% - StateInfo1 (1 byte)
% - SerialNumber (14-16 characters)
% - Longitude (double)
% - Latitude (double)
% - Height (-32768 to 32767)
% - Altitude (-32768 to 32767)
% - VelocityNorth (-32768 to 32767)
% - VelocityEast (-32768 to 32767)
% - VelocityUp (-32768 to 32767)
% - Yaw (-32768 to 32767)
% - PhoneAppGPSTime (milliseconds since Epoch)
% - PhoneAppLatitude (double)
% - PhoneAppLongitude (double)
% - HomeLatitude (double)
% - HomeLongitude (double)
% - ProductType (1 byte)
% - UUID (19 bytes)

function [frame] = create_frame_bytes(varargin)
%     MIN_SAMPLE_RATE = 15.36e6;
%
%     assert(sample_rate >= MIN_SAMPLE_RATE, "Sample rate must be at least 15.36 MSPS");
%     assert(round(sample_rate / MIN_SAMPLE_RATE) * MIN_SAMPLE_RATE == sample_rate, ...
%         "Sample rate must be multiple of %d", MIN_SAMPLE_RATE);

    assert(isempty(varargin) || mod(length(varargin), 2) == 0, ...
        "Variable argument list must have even number of elements");

    sequence_number = 0;
    state_info = [0, 0];
    serial_number = '0123456789abcd';
    longitude = 0.0;
    latitude = 0.0;
    height = 0;
    altitude = 0;
    velocity_north = 0;
    velocity_east = 0;
    velocity_up = 0;
    yaw = 0;
    phone_app_gps_time = 0;
    phone_app_latitude = 0.0;
    phone_app_longitude = 0.0;
    home_latitude = 0.0;
    home_longitude = 0.0;
    product_type = 0;
    uuid = zeros(1, 19);

    for idx=1:2:length(varargin)
        key = varargin{idx};
        val = varargin{idx + 1};

        switch(key)
            case 'SequenceNumber'
                assert(isnumeric(val), 'Sequence number must be an integer');
                assert(val >= -32768 && val <= 32767, 'Invalid sequence number');

                sequence_number = uint16(val);
            case 'StateInfo0'
                assert(isinteger(val), 'StateInfo0 must be an integer');
                assert(val >= 0 && val <= 255, 'Invalid StateInfo0 value');

                state_info(1) = uint8(val);
            case 'StateInfo1'
                assert(isinteger(val), 'StateInfo1 must be an integer');
                assert(val >= 0 && val <= 255, 'Invalid StateInfo1 value');

                state_info(2) = uint8(val);
            case 'SerialNumber'
                assert(isstring(val) || ischar(val), 'SerialNumber must be a string')
                assert(length(val) >= 14 && length(val) <= 16, ...
                    'SerialNumber length %d is not valid.  Must be between 14 and 16 chars', length(val))

                serial_number = val;
            case 'Longitude'
                assert(isnumeric(val), 'Longitude must be a number')
                assert(abs(val) <= 180, 'Longitude must be between -180 and 180')

                longitude = double(val);
            case 'Latitude'
                assert(isnumeric(val), 'Latitude must be a number')
                assert(abs(val) <= 90, 'Latitude must be between -90 and 90')

                latitude = double(val);
            case 'Height'
                assert(isinteger(val), 'Height must be an integer')
                assert(val >= -32768 && val <= 32767, 'Height must be between -32768 and 32767')

                height = int16(val);
            case 'Altitude'
                assert(isinteger(val), 'Altitude must be an integer')
                assert(val >= -32768 && val <= 32767, 'Altitude must be between -32768 and 32767')

                altitude = int16(val);
            case 'VelocityNorth'
                assert(isinteger(val), 'VelocityNorth must be an integer')
                assert(val >= -32768 && val <= 32767, 'VelocityNorth must be between -32768 and 32767')

                velocity_north = int16(val);
            case 'VelocityEast'
                assert(isinteger(val), 'VelocityEast must be an integer')
                assert(val >= -32768 && val <= 32767, 'VelocityEast must be between -32768 and 32767')

                velocity_east = int16(val);
            case 'VelocityUp'
                assert(isinteger(val), 'VelocityUp must be an integer')
                assert(val >= -32768 && val <= 32767, 'VelocityUp must be between -32768 and 32767')

                velocity_up = int16(val);
            case 'Yaw'
                assert(isinteger(val), 'Yaw must be an integer')
                assert(val >= -32768 && val <= 32767, 'Yaw must be between -32768 and 32767')

                yaw = int16(val);
            case 'PhoneAppGPSTime'
                assert(isinteger(val), 'PhoneAppGPSTime must be an integer')
                assert(val >= 0 && val <= (2^64)-1, 'PhoneAppGPSTime must be between 0 and (2^64)-1')

                phone_app_gps_time = uint64(val);
            case 'PhoneAppLatitude'
                assert(isnumeric(val), 'PhoneAppLatitude must be a number')
                assert(abs(val) <= 90, 'PhoneAppLatitude must be between -90 and 90')

                phone_app_latitude = double(val);
            case 'PhoneAppLongitude'
                assert(isnumeric(val), 'PhoneAppLongitude must be a number')
                assert(abs(val) <= 180, 'PhoneAppLongitude must be between -180 and 180')

                phone_app_longitude = double(val);
            case 'HomeLatitude'
                assert(isnumeric(val), 'HomeLatitude must be a number')
                assert(abs(val) <= 90, 'HomeLatitude must be between -90 and 90')

                home_latitude = double(val);
            case 'HomeLongitude'
                assert(isnumeric(val), 'HomeLongitude must be a number')
                assert(abs(val) <= 180, 'HomeLongitude must be between -180 and 180')

                home_longitude = double(val);
            case 'ProductType'
                assert(isinteger(val), 'ProductType must be an integer')
                assert(val >= 0 && val <= 255, 'ProductType must be between 0 and 255')

                product_type = uint8(val);
            case 'UUID'
                assert(isstring(val) || ischar(val), 'UUID must be a string')
                assert(length(val) == 19, 'UUID must be 19 characters long (THIS IS TEMPORARY!!)')

                uuid = val;
            otherwise
                error('Invalid key "%s"', key);
        end
    end

    coord_adj = 10000000 / 57.2957795785523;

    % Pack the fields into bytes
    sequence_number_bytes = to_bytes(sequence_number, 2);
    state_info_bytes = state_info;
    serial_number_bytes = [uint8(serial_number) zeros(1, 16 - length(serial_number))];
    longitude_bytes = to_bytes(int32(round(longitude * coord_adj)), 4);
    latitude_bytes = to_bytes(int32(round(latitude * coord_adj)), 4);
    height_bytes = to_bytes(height, 2);
    altitude_bytes = to_bytes(altitude, 2);
    velocity_north_bytes = to_bytes(velocity_north, 2);
    velocity_east_bytes = to_bytes(velocity_east, 2);
    velocity_up_bytes = to_bytes(velocity_up, 2);
    yaw_bytes = to_bytes(yaw, 2);
    phone_app_gps_time_bytes = to_bytes(phone_app_gps_time, 8);
    phone_app_latitude_bytes = to_bytes(int32(round(phone_app_latitude * coord_adj)), 4);
    phone_app_longitude_bytes = to_bytes(int32(round(phone_app_longitude * coord_adj)), 4);
    home_latitude_bytes = to_bytes(int32(round(home_latitude * coord_adj)), 4);
    home_longitude_bytes = to_bytes(int32(round(home_longitude * coord_adj)), 4);
    product_type_bytes = product_type;
    uuid_bytes = uint8(uuid);

    version = uint8(2);
    message_type = uint8(16);
    uuid_len = uint8(length(uuid_bytes));

    frame = [ ...
        message_type, version, sequence_number_bytes, state_info_bytes, serial_number_bytes, longitude_bytes, ...
        latitude_bytes, height_bytes, altitude_bytes, velocity_north_bytes, velocity_east_bytes, velocity_up_bytes, ...
        yaw_bytes, phone_app_gps_time_bytes, phone_app_latitude_bytes, phone_app_longitude_bytes, home_longitude_bytes, ...
        home_latitude_bytes, product_type_bytes, uuid_len, uuid_bytes, uint8(0) ...
    ];

    frame = [uint8(length(frame)) frame];

    crc_bytes = to_bytes(calculate_crc(frame), 2);
    frame = [frame, crc_bytes];
end