%
% This is the fourth encoder script for
%
%   cubase64 by Pex 'Mahoney' Tufvesson, October 2010
%
% Run it using Matlab, you'll probably need a couple of toolboxes
% and a lot of patience.
% After running all matlab scripts, you'll find a number of
% files named "SID*.s".
% Copy these into the demo folder.
%
% Want to know more?  http://mahoney.c64.org
% Want to hear C64 a cappella?  Visa Röster at http://www.livet.se/visa
% Want a happy life? Do something _not_ requiring electricity! :-D
%

format long;
format compact;
clear;

load 'whichWaveformsAreChosen.mat' %The order of which waveforms were chosen
load 'waveformStatus.mat' %Which waveform every waveforms resembles most.
load 'SIDVoiceVolume.mat' %Play/noPlay
load 'Formant0FreqencyTD2.mat' %contains pitch in Formant0TimeDomain2
load 'waveformsNormalizedFD.mat'
load 'waveforms.mat'
load 'filteredAudioX2.mat' % contains filteredAudioX
waveformsNormalized = waveformsNormalizedFD;
baseFreq = Formant0TimeDomain2;
[originalAudio,Fs,bits]=wavread('Suzanne_Vega_-_Toms_Diner_mono');

% Noteno, 1=55 Hz=A in octave 2
noteNo=(0:60);
twelfthRootOfTwo=2 .^ (1/12); % =1.059463094;
Hz = 55 * ( twelfthRootOfTwo .^ noteNo );

% Now, we have everything to "simulate" playback.
% SIDVoiceVolume tells us when to sound, and when to be quiet.
% Every time SIDVoiceVolume == 1, increase waveformNo
% Use waveformStatus to check which waveform we shall play.
% Increase pointer into SIDVoiceVolume every 8.192ms.


Fs=44100;
% We want to detect pitch "approximately" every 8.192ms.
sliceSecondsTarget = 0.008192;
% This is due to using a 64-word buffer for
% playing samples, using a 7812.5Hz sample rate.
% So the audio can be sliced up in 8.192ms slices.
% Input audio is 44100Hz, 16-bit.
% Each slice is:
sliceLength = round(Fs * sliceSecondsTarget) %#ok<NOPTS>
% This is what we really got:
sliceSeconds = sliceLength / Fs %#ok<NOPTS>
% Calculate "how wrong" the speed will be due to rounding
% the sliceLength to an integer number of 16-bit audio samples.
timeSkew = (sliceSecondsTarget - sliceSeconds) / sliceSecondsTarget %#ok<NOPTS>

nofSlices=length(SIDVoiceVolume)

% Build a table with waveforms and SIDVoiceVolume combined, so that sliceNo
% can index it directly:
waveformIndex = 1;
sliceWavetableIndex = zeros(1,nofSlices);
for sliceNo=1:nofSlices-3,
   if (SIDVoiceVolume(sliceNo) == 1)
       waveformNoInWaveformsNormalized = waveformStatus(waveformIndex);
       % Find this waveform in whichWaveformsAreChosen, and renumber it:
       tableIndex = find(whichWaveformsAreChosen == waveformNoInWaveformsNormalized);
       sliceWavetableIndex(sliceNo) = tableIndex(1) - 1;
       waveformIndex = waveformIndex + 1;
   else
       sliceWavetableIndex(sliceNo) = 255;  %Silent waveform
%       waveformIndex = waveformIndex + 1;
   end
end

% Now, build the include file to use with all the waveforms in it:
SIDwaveformLength = floor(7812.5 / Hz(18)) %This is really 53, but to save a couple
                    %of clock cycles (2 per calculated sample), we'll choose 51 instead.
SIDwaveformLength = 51
waveformSize = size(waveformsNormalized);
nofWaveforms = waveformSize(1);
waveformSize = waveformSize(2);




SIDwaveforms = zeros(256,SIDwaveformLength);
for waveformNo = 0:255,
    waveformsNormalizedIndex = whichWaveformsAreChosen(waveformNo+1);
    if waveformsNormalizedIndex ~= 0
        for i = 0:SIDwaveformLength-1
            % Time to resample the waveforms so they fit into
            % SIDwaveformLength number of bytes.
            value = waveformsNormalized(waveformsNormalizedIndex, 1 + floor((i / SIDwaveformLength * waveformSize)));
            % Now, add some noise to it for reducing quantization noise:
%            value = value + rand(1) * 0.5 * (1 / 256);
            % And quantize it into 8 signed bits:
            value = round(value * 127);
            value = max(-128,value); % limit it
            value = min(127,value);  % limit it
            SIDwaveforms(waveformNo + 1,i + 1) = value;
        end
    end
end

% Dump the encoded SIDwaveforms into a file
% This file shall be placed at $xx00 in C64 memory
% This is the old way of dumping the waveforms in memory:
% fid = fopen('SIDwaveforms.s','w');
% fprintf(fid,'; encoded SIDwaveforms generated by cubase64_4encodeWaveforms.m\n');
% fprintf(fid,'; this table contains 256*%d = %d values\n',SIDwaveformLength,SIDwaveformLength*256);
% fprintf(fid,'; To be placed at even 256-byte boundary in C64 memory');
% fprintf(fid,'; Generated on the %s\n',date);
% fprintf(fid,'; Pex Mahoney Tufvesson, 2010\n');
% fprintf(fid,'\n');
% for i = SIDwaveformLength-1:-1:0
%     fprintf(fid,';These are byte number #%d for all the 256 waveforms:\n',i);
%     for waveformNo = 0:255,
%         fprintf(fid,'  .byte %d\n',SIDwaveforms(waveformNo+1,i+1) + 128);
%     end
% end
% fprintf(fid,'; End of generated data\n');
% fprintf(fid,'\n');
% fclose(fid);



fid = fopen('SIDwaveforms.s','w');
fprintf(fid,'; encoded SIDwaveforms generated by cubase64_4encodeWaveforms.m\n');
fprintf(fid,'; this table contains 256*%d = %d values\n',SIDwaveformLength,SIDwaveformLength*256);
fprintf(fid,'; To be placed at an even 256-byte boundary in C64 memory');
fprintf(fid,'; Generated on the %s\n',date);
fprintf(fid,'; Pex Mahoney Tufvesson, 2010\n');
fprintf(fid,'\n');
for waveformNo = 0:255,
    fprintf(fid,';This is waveform number %d, backwards:\n',waveformNo);
    fprintf(fid,'wave%dstart:\n',waveformNo);
    for i = SIDwaveformLength-1:-1:0
        fprintf(fid,'  .byte %d\n',SIDwaveforms(waveformNo+1,i+1) + 128);
    end
    fprintf(fid,'wave%dend:\n',waveformNo);
    if (waveformNo - floor(waveformNo / 5) * 5) == 4
        fprintf(fid,'  .byte 0   ;waste a byte at end of waveform %d, to make sure that waveforms never crosses a page.\n',waveformNo);
    end
end
fprintf(fid,'; End of generated data\n');
fprintf(fid,'\n');
fclose(fid);


fid = fopen('SIDwaveformPoiTable.s','w');
fprintf(fid,'; encoded SIDwaveformPointers generated by cubase64_4encodeWaveforms.m\n');
fprintf(fid,'; To be placed at $xx00 in C64 memory');
fprintf(fid,'; Generated on the %s\n',date);
fprintf(fid,'; Pex Mahoney Tufvesson, 2010\n');
fprintf(fid,'\n');
fprintf(fid,'waveLength = %d\n',SIDwaveformLength);
fprintf(fid,'SIDwaveformPoiTableMSB:\n');
for waveformNo = 0:255,
    fprintf(fid,'  .byte >wave%dstart\n',waveformNo);
end
fprintf(fid,'\nSIDwaveformPoiTableLSB:\n');
for waveformNo = 0:255,
    fprintf(fid,'  .byte <wave%dstart\n',waveformNo);
end
fprintf(fid,'; End of generated table\n');
fprintf(fid,'\n');
fclose(fid);







finished = 0;
sliceNoPrev = 1;
sliceNo = 1;
waveformIndex = 0;
SIDindex = 1;
SIDspeed = zeros(1,nofSlices * 3); %just a guess
SIDwave = zeros(1,nofSlices * 3); %just a guess
SIDwaveVolume = zeros(1,nofSlices * 3); %just a guess
audioIndex = 1;
audioIndices = zeros(1,nofSlices * 3); %just a guess
oversamplingRatio = 2;

amplitudeWindowLength = 300;
W = hanning(amplitudeWindowLength*2);

while ((finished==0) && (sliceNo < (nofSlices-1)))
    sliceNo = floor(audioIndex / sliceLength) + 1;
    sliceFrac = (audioIndex - floor(audioIndex / sliceLength)*sliceLength)/sliceLength; % 0 = spot on, 0.99="almost the next one"
    
    pitch = baseFreq(sliceNo) * (1.0-sliceFrac) + baseFreq(sliceNo+1) * sliceFrac;
    pitch = max(pitch, Hz(18));
    pitch = min(pitch, Hz(30));
    

    
    %This is also pretty bad. This chooses the waveform that we have
    %extracted with 8.192ms interval. But, now we're progressing
    %through the audio at a different rate (4-8ms), and so some
    %waves are falsely chosen as "nearest neighbour" to the
    %8ms intervals. 
    SIDwave(SIDindex) = sliceWavetableIndex(sliceNo);
    
    
    
    % ToDo: actually implement this, better, selection algorithm.
%     
%     % Instead, we should really take the actual waveform
%     %in the audio, and correlate it to all the chosen waveforms.
%     % And, we should do this even for waveforms that are declared
%     % as silent, since some non-periodic waveforms could find a
%     % "good enough" fit with some waveform.
%     % There's still a reason for _not_ including the noisy waveforms
%     % in the previous stages of the algorithm, since they probably
%     % would pollute our chosen waveforms to contain less useful
%     % waveforms.
%     
%     % Note: audioIndex points into the originalAudio, not the
%     % constantPitchAudio. We need to resample the waveform
%     % when correlating it. And, we should really use
%     % filteredAudioX instead of originalAudio, to get better
%     % correlation results.
%     waveformLength = ceil((Fs / pitch) * oversamplingRatio)
%     sliceMiddleIndex = round(audioIndex*oversamplingRatio + waveformLength/2) %#ok<NOPTS>
%     sliceStartIndex = floor(sliceMiddleIndex - waveformLength/2);
%     sliceEndIndex = floor(sliceMiddleIndex + waveformLength/2);
%     [maxValue, maxIndex] = max(filteredAudioX(sliceStartIndex:sliceEndIndex));
%     i = maxIndex + sliceStartIndex;
%     % Now find a zero crossing to the left of maxIndex:
%     while(originalAudio(i) > 0)
%         i = i - 1;
%     end
%     %We have found our waveform:
%     waveformStartIndex = i+1;
%     waveformEndIndex = i + Fs/pitch; %This is not an integer.
% 
%     
%     
%     
%     
%     
%     % Now we need to resample the waveform to make it the same
%     % length as the extracted waveforms:
%     %ToDo: Insert waveform resampling here, and correlate with all
%     % chosen waveforms.
%     
%     maxValue = max(constantPitchAudio(waveformStartIndex:waveformEndIndex));
%     minValue = min(constantPitchAudio(waveformStartIndex:waveformEndIndex));
%     extremeValue = max(maxValue, -minValue);
%     % Copy the waveform, normalized:
%     thisWaveformNormalized = constantPitchAudio(waveformStartIndex:waveformEndIndex) ./ extremeValue;
%     
    
    
    
%         score = 2000 - sum((waveformsNormalized(waveformNo,:) - waveformsNormalized(compareNo,:)).^2);
    
    
    % This is too bad. Should do something else, really...
%    SIDwaveVolume(SIDindex) = max(max(waveforms(sliceNo)),-min(waveforms(sliceNo)));

    startI=round(max(1,audioIndex-amplitudeWindowLength));
    endI = round(audioIndex+amplitudeWindowLength-1);
    SIDwaveVolumeMax = max(originalAudio(startI:endI) .* hanning(endI-startI+1));
    SIDwaveVolumeMin = min(originalAudio(startI:endI) .* hanning(endI-startI+1));
    SIDwaveVolume(SIDindex) = max(SIDwaveVolumeMax, -SIDwaveVolumeMin);

    if (SIDwave(SIDindex) == 255) % silent
        pitch=Hz(30);  % Always run silence as quick as possible
    end
    waveformIndexSpeed = pitch / Hz(18); % 1.0 = Hz(18), 2.0 = Hz(30)
    SIDspeed(SIDindex) = floor((waveformIndexSpeed - 1.0) * 256);
    SIDspeed(SIDindex) = max(0,SIDspeed(SIDindex));
    SIDspeed(SIDindex) = min(255,SIDspeed(SIDindex));

    audioIndices(SIDindex) = audioIndex; % Keep track of this since we need to encode Noisevalues with this as sync
    SIDindex = SIDindex + 1;
    
    % Now jump forward "1 period" in the audio file:
    audioIndex = audioIndex + Fs / pitch;

end

audioIndices = audioIndices(1:SIDindex);
save 'audioIndices.mat' audioIndices

SIDspeed = SIDspeed(1:SIDindex);
SIDwave = SIDwave(1:SIDindex);
SIDwaveVolume = SIDwaveVolume(1:SIDindex);

figure(50);
subplot(2,1,1);
plot(SIDwave);
title('SIDwave');
subplot(2,1,2);
plot(SIDspeed);
title('SIDspeed');

maxVol = max(SIDwaveVolume)
SIDwaveSustainLevel = zeros(1,SIDindex);

for SIDindex2=1:SIDindex
    SIDwaveVolume(SIDindex2) = SIDwaveVolume(SIDindex2) / maxVol;
    
    a = SIDwaveVolume(SIDindex2);
    b = floor(a / 0.5 * 15);
    c = max(0,b);
    d = min(15,c);
    SIDwaveSustainLevel(SIDindex2) = d;

    if (SIDwave(SIDindex2) == 255) % silent
        SIDwaveSustainLevel(SIDindex2) = 15;
    end

end

figure(60)
plot(SIDwaveVolume);
title('SIDwaveVolume');
figure(61)
plot(SIDwaveSustainLevel);
title('SIDwaveSustainLevel');

% Now smooth the Sustain Level so that it _never_ raises the
% volume unless we're playing a silent wave:
% 
% currentVol = 1;
% 
% for SIDindex2=SIDindex:-1:1
%     if (SIDwaveSustainLevel(SIDindex2) == 16)
%         currentVol = 1;
%         SIDwaveSustainLevel(SIDindex2) = 15;
%     else
%         currentVol = max(currentVol, SIDwaveSustainLevel(SIDindex2));
%         SIDwaveSustainLevel(SIDindex2) = currentVol;
%     end
% end
% 
% figure(62)
% plot(SIDwaveSustainLevel);
% title('SIDwaveSustainLevel with smoothing');






% Now it's time to run-length encode these.
% Encode them as:
% SIDwave:
% Byte even: bit7:4=SIDwaveSustainLevel + bit3:0=number of repeats
%            if value=255, then bit7:0=number of repeats, and SustainLevel = 15
% Byte odd:  value

% SIDspeed:
% If SIDwave=255, skip it. (set to 255 directly)

SIDencWaveIndex = 1;
SIDencSpeedIndex = 1;
SIDencWave = zeros(1,SIDindex); % a bad guess
SIDencSpeed = zeros(1,SIDindex); % a bad guess
index = 1;
nofSame = 0;
oldValue = SIDwave(index);
SIDindex
maxVolThisTime = 0;
previousVol = 15;

while index<SIDindex,
    if ((SIDwave(index) == oldValue) && nofSame < 15)
        nofSame = nofSame + 1;
        maxVolThisTime = max(maxVolThisTime, SIDwaveSustainLevel(index));
    else
        SIDencWave(SIDencWaveIndex) = maxVolThisTime * 16 + nofSame;
        SIDencWave(SIDencWaveIndex+1) = SIDwave(index-1);
        nofSame = 0;
        maxVolThisTime = SIDwaveSustainLevel(index);
        oldValue = SIDwave(index);
        SIDencWaveIndex = SIDencWaveIndex + 2;
    end
    index=index+1;
end
SIDencWave = SIDencWave(1:SIDencWaveIndex);
SIDencWaveIndex


% Dump the encoded SIDencWave into a file
fid = fopen('SIDencWave.s','w');
fprintf(fid,'; encoded SIDwaveTable generated by cubase64_4encodeWaveforms.m\n');
fprintf(fid,'; this table contains %d values\n',SIDencWaveIndex);
fprintf(fid,'; Generated on the %s\n',date);
fprintf(fid,'; Pex Mahoney Tufvesson, 2010\n');
fprintf(fid,'\n');
for i = 1:SIDencWaveIndex
    fprintf(fid,'  .byte %d\n',SIDencWave(i));
end
fprintf(fid,'; End of generated data\n');
fprintf(fid,'\n');
fclose(fid);







for index=1:SIDindex,
   if (SIDwave(index) ~= 255)
       SIDencSpeed(SIDencSpeedIndex) = SIDspeed(index);
       SIDencSpeedIndex = SIDencSpeedIndex + 1;
   end
end
%SIDencSpeed = SIDencSpeed(1:SIDencSpeedIndex);
SIDencSpeedIndex
SIDencSpeedIndex = SIDencSpeedIndex-2

figure(51);
plot(SIDencWave);
title('SIDencWave');
figure(52);
plot(SIDencSpeed);
title('SIDencSpeed');

% SIDencSpeed could be reasonably cheap encoded by encoding
% "differences" from -7 to +7 with one nybble.
% If nybble = -8, then grab next byte (may be split in 
% two separate nybbles), and put that into Speed value.
% First byte is the start speed.

SIDenc2Speed = zeros(1,SIDencSpeedIndex*2);
SIDenc2Speed(1) = floor(SIDencSpeed(1) / 16); % Split nybbles
SIDenc2Speed(2) = SIDencSpeed(1) - floor(SIDencSpeed(1) / 16) * 16;     % Split nybbles
oldSpeed = SIDencSpeed(1);
SIDenc2SpeedIndex = 3;

for index=1:SIDencSpeedIndex,
    if (abs(SIDencSpeed(index) - oldSpeed) > 7)
        SIDenc2Speed(SIDenc2SpeedIndex) = 8; % Use next 8 bits for speed
        SIDenc2SpeedIndex = SIDenc2SpeedIndex + 1;
        SIDenc2Speed(SIDenc2SpeedIndex) = floor(SIDencSpeed(index) / 16);
        SIDenc2SpeedIndex = SIDenc2SpeedIndex + 1;
        SIDenc2Speed(SIDenc2SpeedIndex) = SIDencSpeed(index) - floor(SIDencSpeed(index) / 16) * 16;
        SIDenc2SpeedIndex = SIDenc2SpeedIndex + 1;
        oldSpeed = SIDencSpeed(index);
    else
        SIDenc2Speed(SIDenc2SpeedIndex) = SIDencSpeed(index) - oldSpeed;
        if (SIDenc2Speed(SIDenc2SpeedIndex) < 0)
            SIDenc2Speed(SIDenc2SpeedIndex) = SIDenc2Speed(SIDenc2SpeedIndex) + 16;
        end
        SIDenc2SpeedIndex = SIDenc2SpeedIndex + 1;
        oldSpeed = SIDencSpeed(index);
    end
end

SIDenc2Speed = SIDenc2Speed(1:SIDenc2SpeedIndex+3);
SIDenc2Length = SIDenc2SpeedIndex/2

% Now, combine all gathered nybbles:
SIDenc3Speed = zeros(1,floor(SIDenc2SpeedIndex/2) + 2);
for index=0:(floor(SIDenc2SpeedIndex/2)),
    SIDenc3Speed(index+1) = SIDenc2Speed(index*2 + 1) * 16 + SIDenc2Speed(index*2+2);
end

SIDenc3SpeedLength = floor(SIDenc2SpeedIndex/2) + 1

figure(53)
plot(SIDenc3Speed);
title('SIDenc3Speed');

% Dump the encoded SIDenc3Speed into a file
fid = fopen('SIDenc3Speed.s','w');
fprintf(fid,'; encoded SIDspeed generated by cubase64_4encodeWaveforms.m\n');
fprintf(fid,'; this table contains %d values\n',SIDenc3SpeedLength);
fprintf(fid,'; Generated on the %s\n',date);
fprintf(fid,'; Pex Mahoney Tufvesson, 2010\n');
fprintf(fid,'\n');
for i = 1:SIDenc3SpeedLength
    fprintf(fid,'  .byte %d\n',SIDenc3Speed(i));
end
fprintf(fid,'; End of generated data\n');
fprintf(fid,'\n');
fclose(fid);



%The note values used through the whole song are:
% Note Name   Hz                     nofSamples@7812.5Hz
% D   Hz(18)  146.8323839587038 Hz   53.206927445904853
% E   Hz(20)  164.8137784564350 Hz   47.401983457742659
% F   Hz(21)  174.6141157165020 Hz   44.741514555925860
% F#  Hz(22) 184.9972113558173 Hz    42.230366299812502
% G#  Hz(24) 207.6523487899727 Hz    37.622979203099938
% A   Hz(25) 220 Hz                  35.511363636363598
% B   Hz(27) 246.9416506280622 Hz    31.637028343051789
% C#  Hz(29) 277.1826309768723 Hz    28.185387996594418
% D   Hz(30) 293.6647679174077 Hz    26.603463722952405





% When all waveforms are chosen, it's time to try to map _every_ single
% waveform that we have onto a linear combination of two waveforms.
% Maybe this will be with some heavy limitations due to memory
% requirements.
% And also, the number of volume levels will be only a few, probably
% only 25%, 33%, 50%, 66%, 75% and 100%.



% Let's try to create one waveform and modify this.

% The replay routine will have to take care of the pitching.
% And no pitch tables, we need to have "human" qualities of the
% pitch curve, so it needs to be imperfect.

% The mixing routine will blend "the old waveform" with a new one.
% The mixing routine will use pha to store values.
% The mixing routine cannot use pla to get values, since any
% interrupt that occurs will destroy the waveform.

% Or, the waveform could be stored, double buffered, in ZeroPage.


% Replay with no mixing routine:

% IRQ handling                    7
%         STA     SaveAnmi+1      3
%         ...a few clocks sync 0 - 4 of them
%         JMP     ZeroCode        3
%ZeroCode:
%        LDA     #$11             2
%        STA     $D404            4
%        LDA     #9               2
%        STA     $D404            4
%Poi:
%        LDA     $3000            4
%        STA     $D401            4
%        LDA     #1               2
%        STA     $D404            4
%        INC     Poi+1            5
%SaveAnmi:
%        LDA     #$A              2
%        JMP     $DD0C            3
%DD0C:   RTI                      6
%                               =55-59 clocks in all
% We have 126-55 = 67-71 clocks left per sample for the mixing routine
% A badline will take 40 of these. = 27-31 clocks left on a badline

