%This Scripts decode the DCA data capture send over RDIF
% MDIF Chirp data and sideband data
clear all;
close all;
rawData.binDirName = ['C:/Users/A0492772/Documents/Matlab/'];
files = dir([rawData.binDirName 'mdif_capture_sdk_5_2_Raw_0.bin']); 

n_line=9;
fid=fopen('MotionDetect_mdif.cfg');
tline = textscan(fid, '%s', 1, 'Headerlines', n_line-1, 'Delimiter' ,'');
e=split(tline{1},' ');
N_samples = str2double(e(5));
tline = textscan(fid, '%s', 1, 'Headerlines', 2, 'Delimiter' ,'');
e=split(tline{1},' ');
N_chirps=str2double(e(2));
N_brusts=str2double(e(5));
N_frames=str2double(e(7));
N_bits=12;
N_rx=3;
side_band_EN=1;
chirp1 = N_chirps*N_brusts;

frameSizeSamples = (N_samples* N_rx+12)*chirp1;

%z=(N_samples+12)*chirps*N_rx;
z=files(:).bytes/2; %Total_samples
x=floor(z/731);
y=z-3*x;
max_frame= floor(y/frameSizeSamples);

%max_frame=64;
%N_samples=256;
%N_chirps=2;
%N_brusts=64;
%N_frames=4;

if (N_frames==0)
    N_frames = max_frame-1;
end
fclose(fid);

chirps = N_frames*N_chirps*N_brusts;
total_samples_per_rx = N_samples * chirps;
total_samples = total_samples_per_rx * N_rx;
total_samples_per_chrip = N_samples * N_rx;

%fid=fopen('mdif_capture_sdk_5_2_Raw_1.bin','rb');
fid=fopen(files.name,'rb');
%input0=fread(fid, (z+3*x+3), '*ubit16');
input0=fread(fid, z, '*ubit16');
fclose(fid);

%input0 = input0(4:(z+3*x+3));
input0 = input0(4:z);
for m = 1:(x-4)
  input1((1+728*(m-1)):(728*m))= input0((1+(731*(m-1))):((731*m)-3));
end

t=N_samples*chirp1*N_rx;

Frame_data = zeros(N_frames, (t+12*chirp1));
chrip_data = zeros(N_frames, chirp1, (N_samples*N_rx));
sideband_data = zeros(chirps, N_bits);
SideBand_data = zeros((chirps-1), N_bits);
SideBand_data_hex = zeros((chirps-1), N_bits);
rx_val = zeros(1, total_samples);

%%%%%%%%%%%%%%%%%%%%Decoding%%%%%%%%%%%%%%%%%%%%%
for k = 1:N_frames
  Frame_data(k,:) = input1((1+(t+(12*chirp1))*(k-1)):((t+(12*chirp1))*k));
  for l = 1:chirp1
    if (side_band_EN==1)
      chrip_data(k,l,:)=Frame_data(k,(1+(l-1)*(N_samples*N_rx+12)):(l*(N_samples*N_rx)+12*(l-1)));
    else
      chrip_data(k,l,:)=Frame_data(k,(1+(l-1)*(N_samples*N_rx)):(l*N_samples*N_rx));
    end
  end
end

%%%%%%%%%%%%%%%%%%%%Sideband data decode%%%%%%%%%%%%%%%%%%%%
for k = 1:chirps
    sideband_data(k,:)=input1((1+(k*N_samples*N_rx)+(12*(k-1))):(k*(N_samples*N_rx+12)));
end
  
  for s = 1:chirps
    input1_2bytes = sideband_data(s,:);
    count1 = 0;
    for i =1:4:N_bits
      temp_array(1) = input1_2bytes(i);
      temp_array(2) = input1_2bytes(i+1);
      temp_array(3) = input1_2bytes(i+2);
      temp_array(4) = input1_2bytes(i+3);
      for j = N_bits:-1:1
        temp0 = bitand(temp_array(1), 2^(j-1)) / 2^(j-1);
        temp1 = bitand(temp_array(2), 2^(j-1)) / 2^(j-1);
        temp2 = bitand(temp_array(3), 2^(j-1)) / 2^(j-1);
        temp3 = bitand(temp_array(4), 2^(j-1)) / 2^(j-1);
        
        temp_val = ((temp3 * 8) + (temp2 * 4) + (temp1 * 2) + temp0);
        count1 = count1 + 1;
        
        rx_val(count1) = temp_val;
      end
    end
    p=1;
    for i= 1:3: (N_bits * 3)
      temp_rx_val = (rx_val(i+2) * 256) + (rx_val(i+1) * 16) + rx_val(i);
      SideBand_data(s,p)= temp_rx_val;
      p=p+1;
    end
 %   SideBand_data_hex(s,:)=dec2hex(SideBand_data(s,:));    
  end

%%%%%%%%%%%%%%%%%%%%%%USER_INTERFACE%%%%%%%%%%%%%%%%%
fprintf('Bin file has %i Frame\n', N_frames);
f = input("Plz Enter Frame number : ");
fprintf('CFG has %i burst\n', N_brusts);
b = input("Enter Burst number : ");
fprintf('CFG has %i chirp\n', N_chirps);
c = input("Enter Chirp number : ");
r = input("Device has 3 Rx,enter the Rx : ");


%%%%%%%%%%%%%%%%%%sideband_check%%%%%%%%%%%%%%%%%%%%%%
key=(f-1)*N_brusts*N_chirps + (b-1)*N_chirps + c;
if (SideBand_data(key,1)==(f-1) && SideBand_data(key,2)==(b-1) && SideBand_data(key,3)==(c-1))
  k=(b-1)*N_chirps+c;
  Chrip_data_out=chirp_data_decode(chrip_data(f,k,:),r);
  Chrip_data_out=bitand(int16(Chrip_data_out), 4095); %0xfff
  disp(dec2hex(Chrip_data_out));
  plot(abs(fft(Chrip_data_out)));

  fprintf("Done:Chrip_data_out\n");
 else 
   fprintf("Sideband data is not correct or input provided is wrong\n");
end


%%%%%%%%%%%%%%%%%%%%%chirp data decode%%%%%%%%%%%%%%%%%%%%
function [s] = chirp_data_decode(input_2bytes,r)
N_samples=256;
N_rx=3;
N_bits=12;
N_frames=4;
N_chirps=2;
N_brusts=64;
chirps = N_frames*N_chirps*N_brusts;
total_samples_per_rx = N_samples * chirps;
total_samples = total_samples_per_rx * N_rx;
total_samples_per_chrip = N_samples * N_rx;

count = 0;
rx_count = 0;
max_val = 2 ^ N_bits;
half_max_val = (max_val/2) - 1;

rx_val = zeros(1,total_samples);
rx_data = zeros(N_rx, N_samples);
temp_array = zeros(1,4);
for i = 1:4:total_samples_per_chrip 
%   input_2bytes=fread(fid, 4, '*ubit16'); 
    temp_array(1) = input_2bytes(i);
    temp_array(2) = input_2bytes(i+1);
    temp_array(3) = input_2bytes(i+2);
    temp_array(4) = input_2bytes(i+3);
    for j = N_bits:-1:1
       temp0 = (bitand(temp_array(1), 2^(j-1)) / 2^(j-1));
       temp1 = (bitand(temp_array(2), 2^(j-1)) / 2^(j-1));
       temp2 = (bitand(temp_array(3), 2^(j-1)) / 2^(j-1));
       temp3 = (bitand(temp_array(4), 2^(j-1)) / 2^(j-1));
       temp_val = (temp3 * 8) + (temp2 * 4) + (temp1 * 2) + temp0;
       count = count + 1;
       rx_val(count) = temp_val ;
    end       
end

for i = 1:3:(total_samples_per_chrip * 3) 
    temp_rx_val = (rx_val(i+2) * 256) + (rx_val(i+1) * 16) + rx_val(i);
    rx_count = rx_count + 1;    
    row_num = mod(rx_count,3);    

    if (row_num == 0)
      row_num = 3;
    end
    col_num = ceil(rx_count / 3);
    rx_data(row_num, col_num) = temp_rx_val;

end    
rx1 = rx_data(1,:);
rx2 = rx_data(2,:);
rx3 = rx_data(3,:);
for i =1:N_samples
    if (rx1(i)> half_max_val)
      rx1(i) = rx1(i) - max_val;
    end
    
    if (rx2(i) > half_max_val)
      rx2(i) = rx2(i) - max_val;
    end
    
    if (rx3(i) > half_max_val)
      rx3(i) = rx3(i) - max_val;
    end
end
  if (r==1)
  s = rx1;
  elseif (r==2)
  s = rx2;
  else  
      s = rx3;

  end
end


