%batchFileNames = ["vodka.log"];
    %"evens_med_moving.log" "dualMusic.log" ...
    %"evens_quiet_moving.log" "locating2.log" "odds_medium_moving.log" ...
    %"synthLeak_offcntr.log"];
%%
%[~,numBatchFiles] = size(batchFileNames);
%for batchFileNumber = 1:numBatchFiles
%    fprintf("Staring processing on %s\n",batchFileNames(batchFileNumber));
%    doProcOn(batchFileNames(batchFileNumber));
%end

close all
%%
bytesPerLine = 25;
numMics = 96;
fileString ="Imaging_20k_11x1_2.log"; 
useExisting = true(1);
doSaveMat = true(1);
filePath = "D:\College\cmu\capst\BeamFormingCode\Post_imaging\V001\";
fileSaveSubPath = "rendered_images\";
limitLines = -1;%4850000;
startAtReading = -1;
maxFileLines = 4850000;

doImaging = false(1);
fois = [500 1000 2000 3000 6000];%[4000 6000 8000 10000];
[~,numFois] = size(fois);
chunkSize = 50000;
imgXRes = 455;
imgYRes = 256;
imgYFOV = 60;
imgXFOV = 60;
spacingX = 1;
spacingY = 1;
elementsX = 12;
elementsY = 8;


convToWav = false(1);
beamCenterX = 30;
beamCenterY = -4;

doBarker = true(1);
barkF = 20000;
barkL = 11;
barkN = 1;


Fs = 1000000;
rdgsPerPack = 200;
c = 1125*12;
PI = 3.14159265358979323;

%%
micOrderLut = zeros(1,numMics);
for i=1:12
    for j=1:8
        micOrderLut((i-1)*8+j) = (i*8)-(j-1);
    end
end

%%
loopOnFile = false(1); %if set, breaks up a large input logfile into
  % pieces and processes those one at a time.
if(limitLines < 0)
    s = dir(filePath + fileString);
    fileSizeBytes = s.bytes;
    fileSizeLines = fileSizeBytes / bytesPerLine;
    if(fileSizeLines > 4850000)
        loopOnFile = true(1);
    end
else
    if(limitLines > 4850000)
        loopOnFile = true(1);
    end
end
%
if(limitLines > 0)
    fileSizeLines = limitLines;
else
    fileSizeLines = fileSizeBytes / bytesPerLine;
end
trueFileSizeLines = fileSizeLines;%we'll trick the parser by setting this
   % lower, so create a backup for this "level"
actualImageIndex = 1;%incremented for every image written, not reset in the loop
for fileSubIndex = 1:ceil(trueFileSizeLines/maxFileLines)
    fprintf("Doing stuff, I guess\n");
    %if (fileSubIndex > trueFileSizeLines/maxFileLines)
    %    fileSizeLines = trueFileSizeLines - (fileSubIndex*maxFileLines);
    %else
        fileSizeLines = maxFileLines;
    %end
    
    
    splitFileString = strsplit(fileString,'.');
    matNamePrefix = splitFileString(1);
    matFileName = sprintf("%s%s%04d.mat",filePath,matNamePrefix,fileSubIndex);
    if(~isfile(matFileName) && useExisting)
        useExisting = false(1);
        fprintf("Set vars to use existing mat file but not found!\n");
    end
    if useExisting
        fprintf("Loading .mat file ... ");
        load(matFileName,'fileLines');
        fprintf("Done\n");
    else
        if(fileSubIndex == 1)
            fid = fopen(filePath + fileString,'rt');
        end

        if false(1) %startAtReading > 0)
            lineIndex = 1;
            fprintf("Scanning forward in file ... ");
            while ~feof(fid) && lineIndex < startAtReading
                line = fgetl(fid);
                if line(1) ~= ':'
                    lineIndex = lineIndex + 1;
                end
            end
            fprintf("Done\n");
        end
        prevTimeStamp = -1;
        prevLength = -1;
        numReadings = 0;

        micInts = [0 0 0];
        micBit = 0;
        %%
        lineIndex = 1;
        fileLines = zeros(96,fileSizeLines,'single');
        lastError = "None";
        while ~feof(fid) && lineIndex <= fileSizeLines
            line = fgetl(fid);
            if line(1) == ':'
                headerData = sscanf(line(4:12),'%X');
                switch (line(2))
                    case 't'
                        if (prevTimeStamp ~= -1 && prevLength ~= -1)
                            if(headerData ~= (prevLength/rdgsPerPack) + prevTimeStamp && headerData ~= 0)
                                lastError = sprintf("Dropped %d packets @ %d", ...
                                    (headerData -((prevLength/rdgsPerPack) + ...
                                    prevTimeStamp)),headerData);
                            end
                        end
                        prevTimeStamp = headerData;
                        %if (mod(headerData,3) == 0)
                        fprintf("load %s : %f %% done\n",fileString,fix(100*lineIndex / fileSizeLines));
                        %end
                    case 'l'
                        numReadings = numReadings + headerData;
                        prevLength = headerData;
                    case 'v'
                    case 'e'
                        if headerData ~= 0
                            lastError = sprintf("Log error!: %X",headerData);
                        end
                    otherwise
                        fprintf("load %s : %f %% done\n",fileString,fix(100*lineIndex / fileSizeLines));
                end
            else
                micInts(1) = sscanf(line(1:8  ),'%x');
                micInts(2) = sscanf(line(9:16 ),'%x');
                micInts(3) = sscanf(line(17:24),'%x');
                for i = 1:32
                    fileLines(micOrderLut(i   ),lineIndex) = single(bitget(micInts(1),33-i)*2-1);
                    fileLines(micOrderLut(i+32),lineIndex) = single(bitget(micInts(2),33-i)*2-1);
                    fileLines(micOrderLut(i+64),lineIndex) = single(bitget(micInts(3),33-i)*2-1);
                end
                lineIndex = lineIndex + 1;
            end
        end
        
        if doSaveMat
            fprintf("Saving data to \n%s\n",matFileName);
            save(matFileName,'fileLines','-v7.3');
            fprintf("Done!\n");
        end
    end
    %

    %%
    %%%%%%%%% displays a basic audio FFT of one of the mics %%%%%%%%%%%%%%%%%%
    %wav = fileLines(8,:);%zeros(1,100000);
    %size(wav)
    %fftAudio(100000,[1000 15000],wav);
    %%
    fprintf("Finished loading!\n");
    %[~,fileSizeLines] = size(fileLines);
    numOfImages = fix(fileSizeLines/chunkSize);
    images = zeros(numOfImages,numFois,imgYRes,imgXRes);
    mapArray = [ ...
      74,  72,  73,  75,  82,  80,  81,  83,  90,  88,  89,  91 ; ...
      26,  24,  25,  27,  34,  32,  33,  35,  42,  40,  41,  43 ; ...
      76,  78,  79,  77,  84,  86,  87,  85,  92,  94,  95,  93 ; ...
      28,  30,  31,  29,  36,  38,  39,  37,  44,  46,  47,  45 ; ...
      50,  48,  49,  51,  58,  56,  57,  59,  66,  64,  65,  67 ; ...
      2,   0,   1,   3,   10,  8,   9,   11,  18,  16,  17,  19 ; ...
      52,  54,  55,  53,  60,  62,  63,  61,  68,  70,  71,  69 ; ...
      4,  6,  7,  5,  12,  14,  15,  13,  20,  22,  23,  21  ];
    deMapArray = zeros(numMics,2);
    for n = 0:numMics-1
        [y,x] = find(mapArray==n);
        deMapArray(n+1,1) = ((x-1) * spacingX)-( elementsX * spacingX / 2.0);
        deMapArray(n+1,2) = ((y-1) * spacingY)-( elementsY * spacingY / 2.0);
    end
    if doImaging
        for imageNumber = 1:numOfImages
            fprintf("starting SINCOS on %d ... ",imageNumber);
            sumRe = zeros(numMics,numFois);
            sumIm = zeros(numMics,numFois);
            lineSubSet = fileLines(:,(imageNumber-1)*chunkSize+1:imageNumber*chunkSize);
            for n = 1:numMics
                for fi = 1:numFois
                    factor = fois(fi) * 2 * PI / Fs;
                    for i = 1:chunkSize
                        sumRe(n,fi) = sumRe(n,fi) + (lineSubSet(n,i)*cos(factor*i));
                        sumIm(n,fi) = sumIm(n,fi) + (lineSubSet(n,i)*sin(factor*i));
                    end
                    %scale up to "normalize" but don't allow very small vectors to
                    %get big, limit to about 10* normal size
                    ampl = 0.1 + cmag(sumRe(n,fi),sumIm(n,fi));
                    sumRe(n,fi) = sumRe(n,fi) / ampl;
                    sumIm(n,fi) = sumIm(n,fi) / ampl;
                end
            end
            fprintf("Done\n");
            %% actual phase delay beamforming
            fprintf("Generating images ... ");
            %imgXRes = 64;
            %imgYRes = 32;
            %imgYFOV = 60;
            %imgXFOV = 60;
            for x = 1:imgXRes
                for y = 1:imgYRes
                    pixel = x+(y*imgXRes); %get pixel index
                    thetax = (x / imgXRes) * imgXFOV - (imgXFOV/2);
                    thetay = (y / imgYRes) * imgYFOV - (imgYFOV/2);
                    thetax = thetax * PI / 180.0;
                    thetay = thetay * PI / 180.0;
                    si1x = -2*PI*sin(thetax) / c;
                    si1y = 2*PI*sin(thetay) / c;
                    for fi = 1:numFois
                        foi = fois(fi); 
                        si2x = si1x * foi;
                        si2y = si1y * foi;
                        sumr = 0;
                        sumi = 0;
                        for n = 1:numMics
                            xPos = deMapArray(n,1);
                            yPos = deMapArray(n,2);
                            six = xPos * si2x;
                            siy = yPos * si2y;
                            re = cos(six+siy); %real and imag parts of phase delay vector
                            im = sin(six+siy);
                            mre = sumRe(n,fi);
                            mim = sumIm(n,fi);
                            sumr = sumr + (mre*re) - (mim*im);
                            sumi = sumi + (mre*im) + (mim*re);
                        end
                        images(imageNumber,fi,y,x)= magSq(sumr,sumi);
                    end
                end
            end
            fprintf("Done\n");
        end
        %prodOfImgs = squeeze(images(1,:,:) .* images(2,:,:));
        %image(prodOfImgs,'CDataMapping','scaled');
        %daspect([1 1 1]);
        %ttl = sprintf("Frq = %d",fois(1));
        %title(ttl);
        %%
        fprintf("Saving images to file\n");

        %just figures out the new directory and filenames and stuff, makes a folder
        %to hold all the new images
        splitFileString = strsplit(fileString,'.');
        imgNamePrefix = splitFileString(1);
        newFolderName = sprintf("images_%s",imgNamePrefix);
        mkdir (filePath+fileSaveSubPath, newFolderName);
        newPath = filePath + fileSaveSubPath + newFolderName + "\";
        rgbi = zeros(imgYRes,imgXRes,3);
        %% 
        close all
        fig = figure;
        setColorMap();
        prodOfImgs = zeros(numOfImages,imgYRes,imgXRes);
        for imNum = 1:numOfImages
            prodOfImgs(imNum,:,:) = squeeze(images(imNum,1,:,:));
            for fi = 2:numFois
                prodOfImgs(imNum,:,:) = squeeze(prodOfImgs(imNum,:,:)) .* squeeze(images(imNum,fi,:,:));
            end
        end
        absMaxProd = max(max(max(prodOfImgs)));
        for imNum = 1:numOfImages
            subplottight(2,1,1);
            %prodMax = max(max(prodOfImgs));
            %prodRange = [1e9 (prodMax+1e9) ];
            %if( prodMax < 1e9)
            %    prodRange = [3e8 2e9];
            %end
            prodRMax = max([max(max(squeeze(prodOfImgs(imNum,:,:)))) absMaxProd*0.1]);
            %prodRMax = max(max(squeeze(prodOfImgs(imNum,:,:))));
            prodRange = [0 prodRMax];
            imagesc(squeeze(prodOfImgs(imNum,:,:)),prodRange);
            %%
            maxColor = max(max(max(squeeze(images(imNum,numFois-2:numFois,:,:)))));
            maxColor = max([maxColor 250]);

            rgbi(:,:,3) = squeeze(images(imNum,numFois,:,:)) / maxColor;
            if(numFois > 1)
                rgbi(:,:,2) = squeeze(images(imNum,numFois-1,:,:)) / maxColor;
            else
                rgbi(:,:,2) = rgbi(:,:,3);%zeros(imgYRes,imgXRes);
            end
            if(numFois > 2)
                rgbi(:,:,1) = squeeze(images(imNum,numFois-2,:,:)) / maxColor;
            else
                rgbi(:,:,1) = rgbi(:,:,2);
            end
            rgbi = rgbi .* rgbi;
            subplottight(2,1,2);
            image(rgbi);
            %subplottight(2,2,2);
            %imagesc(squeeze(images(imNum,2,:,:)),'CDataMapping','scaled');
            %subplottight(2,2,3);
            %imagesc(squeeze(images(imNum,3,:,:)),'CDataMapping','scaled');
            %subplottight(2,2,4);
            %imagesc(squeeze(images(imNum,4,:,:)),[]);

            %convert matlab frame to image and save
            %%
            frame = getframe(fig);
            outputImage = frame2im(frame);
            fileName = sprintf("%s%04d.jpg",imgNamePrefix,actualImageIndex);
            actualImageIndex = actualImageIndex+1;
            imwrite(outputImage,newPath+fileName);
            fprintf("Finished %s\n",fileName);

        end
    end
    if convToWav
        %%
        %deMapArray = zeros(mic num,[x, y]);
        dasArray = zeros(fix(0.015*fileSizeLines),32,32);
        dasImage = zeros(32,32);
        L = 128;
        fc = 0.015;
        n =(1:L);
        filter = 2*fc*sinc(2*fc*(n-L/2));
        waves = zeros(2,fix(fileSizeLines/10));
        %%
        for beamCenterX = -30:2:30
            for beamCenterY = -30:2:30                
                pixelX = fix((beamCenterX+30)/2)+1;
                pixelY = fix((beamCenterY+30)/2)+1;
                fprintf("Solving %d %d\n",pixelX,pixelY);
                thetax = - beamCenterX * 2 * PI / 180;
                thetay =   beamCenterY * 2 * PI / 180;
                timeDif = zeros(1,numMics);
                %filtered = zeros(numMics,dsampleSize);
                %L = 128;
                %fc = 0.015;
                %n =(1:L);
                %filter = 2*fc*sinc(2*fc*(n-L/2));

                for n = 1:numMics
                    xPos = deMapArray(n,1);
                    yPos = deMapArray(n,2);
                    timeDif(n) = round(Fs*(sin(thetax)*xPos + sin(thetay)*yPos) / c);
                    %filtered(n) = conv(fileLines(n,:),filter,'same');
                    %fprintf("Filtered %d\n",n);
                end
                %timeDif = zeros(1,numMics);
                dasPDM = zeros(1,fileSizeLines,'single');
                for n = 1:numMics
                    if timeDif(n) == 0
                        micstart = 1;
                        sumstart = 1;
                        micend = fileSizeLines;
                        sumend = fileSizeLines;
                    elseif timeDif(n) < 0
                       micstart = fix((-timeDif(n))+1);
                       sumstart = 1;
                       micend = fileSizeLines;
                       sumend = fix(fileSizeLines+timeDif(n));
                   else
                       micstart = 1;
                       sumstart = fix(timeDif(n)+1);
                       micend = fix(fileSizeLines-timeDif(n));
                       sumend = fileSizeLines;
                   end
                   dasPDM(sumstart:sumend) = dasPDM(sumstart:sumend) + ...
                       fileLines(n,micstart : micend);
                   %if mod(n,10) == 0
                   %    fprintf("Finished adding %d\n",n);
                   %end
                end

                %wave = dasPDM;
                %fprintf("Filtering ... ");
                %wave = conv(dasPDM,filter,'same');
                %fprintf("Done\n");
                %fftAudio(fileSizeLines,[200 20000],dasPDM);
                %audioF = wave(1,1:10:fileSizeLines);
                %audioF = audioF - mean(audioF);
                %sound(audioF,100000);
                fftData = fftAudio(fileSizeLines,[100 20000],dasPDM);
                dasArray(:,pixelY,pixelX) = fftData(1:fix(0.015*fileSizeLines));
                dasImage(pixelY,pixelX) = mean(squeeze(dasArray(1000:fix(0.015*fileSizeLines),pixelY,pixelX)));
                if(pixelY == 17 && pixelX == 19 || true(1))
                    fprintf("Filtering ... ");
                    wave = conv(dasPDM,filter,'same');
                    fprintf("Done\n");
                    %fftAudio(fileSizeLines,[200 20000],dasPDM);
                    audioF = wave(1,1:10:fileSizeLines);
                    audioF = audioF - mean(audioF);
                    waves(1,:) = audioF;
                elseif(pixelY == 17 && pixelX == 5)
                    fprintf("Filtering ... ");
                    wave = conv(dasPDM,filter,'same');
                    fprintf("Done\n");
                    %fftAudio(fileSizeLines,[200 20000],dasPDM);
                    audioF = wave(1,1:10:fileSizeLines);
                    audioF = audioF - mean(audioF);
                    waves(2,:) = audioF;
                end
            end
        end
        imagesc(dasImage);
        
    end
    if doBarker
        %%
        corSig = single(Barker(barkF,Fs,barkN,barkL));
        %%
        [~,sCor] = size(corSig);
        postCorSize = 1400000 + sCor-1;
        cmic = zeros(numMics,postCorSize,'single');
        %%
        singWavCos = single(cos(0:2*PI*barkF/Fs:2*PI));
        singWavSin = single(sin(0:2*PI*barkF/Fs:2*PI));
        [~,singSize] = size(singWavSin);
        %%
        %micCorData = zeros(numMics,fix(postCorSize/singSize),'single');
        for n = 1:numMics
            fprintf("Mic %d Correl\n",n);
            cmic(n,:) = conv(corSig,single(squeeze(fileLines(n,1:1400000))));
        end
        %%
        firstSpike = zeros(1,numMics);
        fprintf("Finding Spikes\n");
        for n=1:numMics
            maxVal = max(abs(cmic(n,:)));
            firstSpike(1,n) = find(abs(cmic(n,:))> maxVal/2 , 1);
        end
        pulseBegin = min(firstSpike(1,:));
        maxDist = fix(Fs * 25*12 / c);
        pulseEnd = pulseBegin + maxDist;
        pulseLength = 1+pulseEnd-pulseBegin;
        if pulseEnd > postCorSize
            pulseEnd = postCorSize;
            fprintf("Only found one pulse, & not enough time\n");
        end 
        micCorData = zeros(numMics,pulseLength,'single');
        for n = 1:numMics
            fprintf("Mic %d DeMod\n",n);
            for i = pulseBegin:1:(pulseEnd-singSize)
                cosSum = sum(cmic(n,i:i+singSize-1) .* singWavCos);
                sinSum = sum(cmic(n,i:i+singSize-1) .* singWavSin);
                micCorData(n,1+i-pulseBegin) = cmag(cosSum,sinSum);
            end
        end
        %% At this point, micCorData has a positive-only value for 
        %how much signal was rxed at that time
        fprintf("BF and Image Saving\n");

        %just figures out the new directory and filenames and stuff, makes a folder
        %to hold all the new images
        splitFileString = strsplit(fileString,'.');
        imgNamePrefix = splitFileString(1);
        newFolderName = sprintf("images_%s",imgNamePrefix);
        mkdir (filePath+fileSaveSubPath, newFolderName);
        newPath = filePath + fileSaveSubPath + newFolderName + "\";
        actualImageIndex = 1;
        close all
        fig = figure;
        setColorMap();
        
        % does actual beamforming and image gen
        readingsPerFrame = 10;
        imgXRes = 256;
        imgYRes = 32;
        %imgYFOV = 60;
        %imgXFOV = 60;
        
        % microphone outputs
        micSumDirect = zeros(1,pulseLength);
        for n=1:numMics
            micCorData(n,:) = micCorData(n,:) ./ max(squeeze(micCorData(n,:)));
            micSumDirect(1,:) = micCorData(n,:);
        end
        micSumDirectMax = max(max(micSumDirect));
        %
        cImage = zeros(fix(pulseLength/readingsPerFrame),32);
        for frameInd = 1:1:fix(pulseLength/readingsPerFrame)
            startingTime = fix(frameInd * readingsPerFrame);
            fprintf("Solving %d\n",frameInd);
            if 0 > 1
                cImage = zeros(8,12);
                for n=1:numMics
                    xPos = deMapArray(n,1);
                    yPos = deMapArray(n,2);
                    cImage(yPos+5,xPos+7) = micCorData(n,startingTime);
                end
            end
            if 1 < 0
                cImage = zeros(32,32);
                for beamCenterX = -30:2:30
                    for beamCenterY = -30:2:30                
                        pixelX = fix((beamCenterX+30)/2)+1;
                        pixelY = fix((beamCenterY+30)/2)+1;
                        thetax = - beamCenterX * 2 * PI / 180;
                        thetay =   beamCenterY * 2 * PI / 180;
                        timeDif = zeros(1,numMics);
                        tdSum = ones(1,readingsPerFrame);
                        for n = 1:numMics
                            xPos = deMapArray(n,1);
                            yPos = deMapArray(n,2);
                            timeDif(n) = round(Fs*(sin(thetax)*xPos + sin(thetay)*yPos) / c);
                            micstart = timeDif(n)+startingTime;
                            micend = timeDif(n)+startingTime+readingsPerFrame-1;
                            sumstart = 1;
                            sumend = readingsPerFrame;
                            if micstart < 1
                                sumstart = sumstart + 1 - micstart;
                                micstart = 1;
                            end
                            if micend > pulseLength
                               sumend = readingsPerFrame - (micend-pulseLength);
                               micend = pulseLength;
                            end
                            tdSum(sumstart:sumend) = tdSum(sumstart:sumend) .* ...
                                (micCorData(n,micstart:micend) + 0.75);
                        end
                        cImage(pixelY,pixelX) = sum(squeeze(tdSum));
                    end
                end 
            end
            if 1>0
                %cImage = zeros(32,pulseLength);
                for x = 1:imgXRes
                    beamCenterY = 0;
                    %pixelX = fix((beamCenterX+30)/2)+1;
                    thetax = (x / imgXRes) * imgXFOV - (imgXFOV/2);
                    thetax = -thetax * PI / 180;
                    %pixelY = fix((beamCenterY+30)/2)+1;
                    %thetax = - beamCenterX * 2 * PI / 180;
                    thetay =   beamCenterY * 2 * PI / 180;
                    timeDif = zeros(1,numMics);
                    tdSum = ones(1,readingsPerFrame);
                    for n = 1:numMics
                        xPos = deMapArray(n,1);
                        yPos = deMapArray(n,2);
                        timeDif(n) = round(Fs*(sin(thetax)*xPos + sin(thetay)*yPos) / c);
                        micstart = timeDif(n)+startingTime;
                        micend = timeDif(n)+startingTime+readingsPerFrame-1;
                        sumstart = 1;
                        sumend = readingsPerFrame;
                        if micstart < 1
                            sumstart = sumstart + 1 - micstart;
                            micstart = 1;
                        end
                        if micend > pulseLength
                           sumend = readingsPerFrame - (micend-pulseLength);
                           micend = pulseLength;
                        end
                        tdSum(sumstart:sumend) = tdSum(sumstart:sumend) .* ...
                            (micCorData(n,micstart:micend) + 0.75);
                    end
                    cImage(frameInd,x) = sum(squeeze(tdSum));
                end
                %cImage(frameInd,:) = cImage(frameInd,:) ./ max(max(cImage(frameInd,:)));
            end
            if 1<0
                subplottight(2,1,1);
                imageMax = max(max(cImage));
                prodRange = [0 imageMax];
                imagesc(cImage,prodRange);

                subplottight(2,1,2);
                secPlot = zeros(1,pulseLength);
                secPlot(startingTime) = micSumDirectMax;
                plot(micSumDirect(1,:));
                hold on
                plot(secPlot);
                hold off
                frame = getframe(fig);
                outputImage = frame2im(frame);
                fileName = sprintf("%s%04d.jpg",imgNamePrefix,actualImageIndex);
                actualImageIndex = actualImageIndex+1;
                imwrite(outputImage,newPath+fileName);
                fprintf("Finished %s\n",fileName);
            end
        end
        cartImage = zeros(1024,1024);
        fprintf("Converting to polar\n")
        for th=1:imgXRes
            for ro=1:fix(pulseLength/readingsPerFrame)
                thetax = (th / imgXRes) * imgXFOV - (imgXFOV/2);
                angle = thetax * PI / 180;
                %angle = (((th-1)*2)-30)*PI/180;
                dist = ro / 2.5;
                [x,y] = pol2cart(angle,dist);
                cartImage(ceil(y+512),ceil(x)) = (cImage(ro,th)*cImage(ro,th))/200000;
            end
        end
        setColorMap();
        subplottight(1,1,1);
        imageMax = max(max(cartImage));
        prodRange = [0 20000];
        imagesc(cartImage,prodRange);
    end
end
if 1 < 0
    %%
    fftSize = fix(0.015*fileSizeLines);
    rgbFilter = zeros(3,fftSize,'single');
    windowSize = fix(fftSize/2);
    hamWindow = hamming(windowSize);
    rgbFilter(1,1:windowSize) = hamWindow*0.5; % red (include end peak for purple)
    rgbFilter(1,1+fftSize-fix(windowSize/2):fftSize) = hamWindow(1:fix(windowSize/2)) * 0.2;
    rgbFilter(2,fix(windowSize/2)+1:fix(windowSize/2)+windowSize) = hamWindow; % green
    rgbFilter(3,1+fftSize-windowSize:fftSize) = hamWindow; % blue
    dasImage = zeros(31,31,3);
    for px = 1:31
        for py = 1:31
            dasImage(py,px,1) = sum(dasArray(:,py,px) .* rgbFilter(1,:)');
            dasImage(py,px,2) = sum(dasArray(:,py,px) .* rgbFilter(2,:)');
            dasImage(py,px,3) = sum(dasArray(:,py,px) .* rgbFilter(3,:)');
        end
    end
    peak = max(max(max(dasImage)));
    dasImage = (dasImage .* dasImage) / (peak * peak);
    image(dasImage);
end
if ~useExisting
    fclose(fid);
end

if 1 < 0
    %%
    bFs = 100*1000;
    bark = Barker(20000,bFs,1,13);
    barkSig = zeros(1,bFs/10);
    [~,bsiz] = size(bark);
    barkSig(1,fix(bFs/20)+1:fix(bFs/20)+bsiz) = flip(bark);
    audiowrite('Barker_20KHz_13x1.wav',barkSig,bFs);
    
    bark = Barker(20000,bFs,1,11);
    barkSig = zeros(1,bFs/10);
    [~,bsiz] = size(bark);
    barkSig(1,fix(bFs/20)+1:fix(bFs/20)+bsiz) = flip(bark);
    audiowrite('Barker_20KHz_11x1.wav',barkSig,bFs);
    
    bark = Barker(20000,bFs,1,5);
    barkSig = zeros(1,bFs/10);
    [~,bsiz] = size(bark);
    barkSig(1,fix(bFs/20)+1:fix(bFs/20)+bsiz) = flip(bark);
    audiowrite('Barker_20KHz_5x1.wav',barkSig,bFs);
    
    bark = Barker(20000,bFs,2,11);
    barkSig = zeros(1,bFs/10);
    [~,bsiz] = size(bark);
    barkSig(1,fix(bFs/20)+1:fix(bFs/20)+bsiz) = flip(bark);
    audiowrite('Barker_20KHz_11x2.wav',barkSig,bFs);
    
    bark = Barker(20000,bFs,2,5);
    barkSig = zeros(1,bFs/10);
    [~,bsiz] = size(bark);
    barkSig(1,fix(bFs/20)+1:fix(bFs/20)+bsiz) = flip(bark);
    audiowrite('Barker_20KHz_5x2.wav',barkSig,bFs);
    
    bark = Barker(10000,bFs,1,13);
    barkSig = zeros(1,bFs/10);
    [~,bsiz] = size(bark);
    barkSig(1,fix(bFs/20)+1:fix(bFs/20)+bsiz) = flip(bark);
    audiowrite('Barker_10KHz_13x1.wav',barkSig,bFs);
    
    bark = Barker(10000,bFs,1,7);
    barkSig = zeros(1,bFs/10);
    [~,bsiz] = size(bark);
    barkSig(1,fix(bFs/20)+1:fix(bFs/20)+bsiz) = flip(bark);
    audiowrite('Barker_10KHz_7x1.wav',barkSig,bFs);
    
    bark = Barker(15000,bFs,1,11);
    barkSig = zeros(1,bFs/10);
    [~,bsiz] = size(bark);
    barkSig(1,fix(bFs/20)+1:fix(bFs/20)+bsiz) = flip(bark);
    audiowrite('Barker_15KHz_11x1.wav',barkSig,bFs);
    %%
end

function h = subplottight(n,m,i)
    [c,r] = ind2sub([m n], i);
    ax = subplot('Position', [(c-1)/m, 1-(r)/n-0.05, 1/m, 1/n]);
    if(nargout > 0)
      h = ax;
    end
end

function r = magSq(x,y)
    r = x*x+y*y;
end
function r = cmag(x,y)
    r = sqrt(x*x+y*y);
end
%Fq is frequency of carrier, Fs is sample rate
%cPC is oscillations per segment of the code
%num is the barker code size (only certain numbers allowed!)
function bark = Barker(Fq,Fs,cPC,num)
    switch num
        case 2
            code = [1 -1];
        case 3
            code = [1 1 -1];
        case 4 
            code = [1 1 -1 1];
        case 5
            code = [1 1 1 -1 1];
        case 7
            code = [1 1 1 -1 -1 1 -1];
        case 11
            code = [1 1 1 -1 -1 -1 1 -1 -1 1 -1];
        case 13 
            code = [1 1 1 1 1 -1 -1 1 1 -1 1 -1 1];
        otherwise
            error("Illegal size barker code!")
    end
    N = fix(Fs*num*cPC/(Fq));
    bark = zeros(1,N);
    factor = 2*3.14159265358979*Fq/Fs;
    for i=1:N
        n = fix((i*num)/N)+1;
        if n > num
            n = num;
        end
        bark(1,i) = sin(factor*i)*code(n);
    end
end
%% Simple power FFT
function P1 = fftAudio(L,band,wav)
    Fs = 1000000;            % Sampling frequency                    
    %L =  100000;             % Length of signal
    %band = [1000 15000];
    bandIndex = [0 0];
    bandIndex(1) = fix(L * band(1) / Fs);
    bandIndex(2) = fix(L * band(2) / Fs);
    f = Fs*(0:(L/2))/L;
    Y = fft(wav(1,:));
    P2 = abs(Y/L);
    P1 = P2(1:L/2+1);
    P1(2:end-1) = 2*P1(2:end-1);
    if(bandIndex(1) == 0)
        bandIndex(1) = 1;
    end
    P1 = P1(bandIndex(1):bandIndex(2));
    f = f(bandIndex(1):bandIndex(2));
    plot(f,P1) 
    title('Single-Sided Amplitude Spectrum of S(t)')
    xlabel('f (Hz)')
    ylabel('|P1(f)|')
end

function map=setColorMap()
map = [  0         0    0.5156
         0         0    0.5313
         0         0    0.5469
         0         0    0.5625
         0         0    0.5781
         0         0    0.5938
         0         0    0.6094
         0         0    0.6250
         0         0    0.6406
         0         0    0.6563
         0         0    0.6719
         0         0    0.6875
         0         0    0.7031
         0         0    0.7188
         0         0    0.7344
         0         0    0.7500
         0         0    0.7656
         0         0    0.7813
         0         0    0.7969
         0         0    0.8125
         0         0    0.8281
         0         0    0.8438
         0         0    0.8594
         0         0    0.8750
         0         0    0.8906
         0         0    0.9063
         0         0    0.9219
         0         0    0.9375
         0         0    0.9531
         0         0    0.9688
         0         0    0.9844
         0         0    1.0000
         0    0.0156    1.0000
         0    0.0313    1.0000
         0    0.0469    1.0000
         0    0.0625    1.0000
         0    0.0781    1.0000
         0    0.0938    1.0000
         0    0.1094    1.0000
         0    0.1250    1.0000
         0    0.1406    1.0000
         0    0.1563    1.0000
         0    0.1719    1.0000
         0    0.1875    1.0000
         0    0.2031    1.0000
         0    0.2188    1.0000
         0    0.2344    1.0000
         0    0.2500    1.0000
         0    0.2656    1.0000
         0    0.2813    1.0000
         0    0.2969    1.0000
         0    0.3125    1.0000
         0    0.3281    1.0000
         0    0.3438    1.0000
         0    0.3594    1.0000
         0    0.3750    1.0000
         0    0.3906    1.0000
         0    0.4063    1.0000
         0    0.4219    1.0000
         0    0.4375    1.0000
         0    0.4531    1.0000
         0    0.4688    1.0000
         0    0.4844    1.0000
         0    0.5000    1.0000
         0    0.5156    1.0000
         0    0.5313    1.0000
         0    0.5469    1.0000
         0    0.5625    1.0000
         0    0.5781    1.0000
         0    0.5938    1.0000
         0    0.6094    1.0000
         0    0.6250    1.0000
         0    0.6406    1.0000
         0    0.6563    1.0000
         0    0.6719    1.0000
         0    0.6875    1.0000
         0    0.7031    1.0000
         0    0.7188    1.0000
         0    0.7344    1.0000
         0    0.7500    1.0000
         0    0.7656    1.0000
         0    0.7813    1.0000
         0    0.7969    1.0000
         0    0.8125    1.0000
         0    0.8281    1.0000
         0    0.8438    1.0000
         0    0.8594    1.0000
         0    0.8750    1.0000
         0    0.8906    1.0000
         0    0.9063    1.0000
         0    0.9219    1.0000
         0    0.9375    1.0000
         0    0.9531    1.0000
         0    0.9688    1.0000
         0    0.9844    1.0000
         0    1.0000    1.0000
    0.0156    1.0000    0.9844
    0.0313    1.0000    0.9688
    0.0469    1.0000    0.9531
    0.0625    1.0000    0.9375
    0.0781    1.0000    0.9219
    0.0938    1.0000    0.9063
    0.1094    1.0000    0.8906
    0.1250    1.0000    0.8750
    0.1406    1.0000    0.8594
    0.1563    1.0000    0.8438
    0.1719    1.0000    0.8281
    0.1875    1.0000    0.8125
    0.2031    1.0000    0.7969
    0.2188    1.0000    0.7813
    0.2344    1.0000    0.7656
    0.2500    1.0000    0.7500
    0.2656    1.0000    0.7344
    0.2813    1.0000    0.7188
    0.2969    1.0000    0.7031
    0.3125    1.0000    0.6875
    0.3281    1.0000    0.6719
    0.3438    1.0000    0.6563
    0.3594    1.0000    0.6406
    0.3750    1.0000    0.6250
    0.3906    1.0000    0.6094
    0.4063    1.0000    0.5938
    0.4219    1.0000    0.5781
    0.4375    1.0000    0.5625
    0.4531    1.0000    0.5469
    0.4688    1.0000    0.5313
    0.4844    1.0000    0.5156
    0.5000    1.0000    0.5000
    0.5156    1.0000    0.4844
    0.5313    1.0000    0.4688
    0.5469    1.0000    0.4531
    0.5625    1.0000    0.4375
    0.5781    1.0000    0.4219
    0.5938    1.0000    0.4063
    0.6094    1.0000    0.3906
    0.6250    1.0000    0.3750
    0.6406    1.0000    0.3594
    0.6563    1.0000    0.3438
    0.6719    1.0000    0.3281
    0.6875    1.0000    0.3125
    0.7031    1.0000    0.2969
    0.7188    1.0000    0.2813
    0.7344    1.0000    0.2656
    0.7500    1.0000    0.2500
    0.7656    1.0000    0.2344
    0.7813    1.0000    0.2188
    0.7969    1.0000    0.2031
    0.8125    1.0000    0.1875
    0.8281    1.0000    0.1719
    0.8438    1.0000    0.1563
    0.8594    1.0000    0.1406
    0.8750    1.0000    0.1250
    0.8906    1.0000    0.1094
    0.9063    1.0000    0.0938
    0.9219    1.0000    0.0781
    0.9375    1.0000    0.0625
    0.9531    1.0000    0.0469
    0.9688    1.0000    0.0313
    0.9844    1.0000    0.0156
    1.0000    1.0000         0
    1.0000    0.9844         0
    1.0000    0.9688         0
    1.0000    0.9531         0
    1.0000    0.9375         0
    1.0000    0.9219         0
    1.0000    0.9063         0
    1.0000    0.8906         0
    1.0000    0.8750         0
    1.0000    0.8594         0
    1.0000    0.8438         0
    1.0000    0.8281         0
    1.0000    0.8125         0
    1.0000    0.7969         0
    1.0000    0.7813         0
    1.0000    0.7656         0
    1.0000    0.7500         0
    1.0000    0.7344         0
    1.0000    0.7188         0
    1.0000    0.7031         0
    1.0000    0.6875         0
    1.0000    0.6719         0
    1.0000    0.6563         0
    1.0000    0.6406         0
    1.0000    0.6250         0
    1.0000    0.6094         0
    1.0000    0.5938         0
    1.0000    0.5781         0
    1.0000    0.5625         0
    1.0000    0.5469         0
    1.0000    0.5313         0
    1.0000    0.5156         0
    1.0000    0.5000         0
    1.0000    0.4844         0
    1.0000    0.4688         0
    1.0000    0.4531         0
    1.0000    0.4375         0
    1.0000    0.4219         0
    1.0000    0.4063         0
    1.0000    0.3906         0
    1.0000    0.3750         0
    1.0000    0.3594         0
    1.0000    0.3438         0
    1.0000    0.3281         0
    1.0000    0.3125         0
    1.0000    0.2969         0
    1.0000    0.2813         0
    1.0000    0.2656         0
    1.0000    0.2500         0
    1.0000    0.2344         0
    1.0000    0.2188         0
    1.0000    0.2031         0
    1.0000    0.1875         0
    1.0000    0.1719         0
    1.0000    0.1563         0
    1.0000    0.1406         0
    1.0000    0.1250         0
    1.0000    0.1094         0
    1.0000    0.0938         0
    1.0000    0.0781         0
    1.0000    0.0625         0
    1.0000    0.0469         0
    1.0000    0.0313         0
    1.0000    0.0156         0
    1.0000         0         0];
    for i=1:30
        map(i,3) = i/30;
    end
    colormap(map);
end

