function output = additive_synth(sdiffile, fs, synthType, stream, ...
                                 whichframes, maxindex)
% additive_synth: Output time-domain synthesized sound from an 
%                 SDIF file containing a sinusoidal track model
%
% by Matt Wright, 3/23/4 (CCRMA and CNMAT)
% see http://www-ccrma.stanford.edu/~matt/jos/Additive_Synthesis_Sinusoidal_Peak.html
%
% sdiffile: path of SDIF file on disk
% fs: sample rate of output
% synthType:  How does synthesizer handle phase and interpolation?  
%  Current options:
%  'ignore_phase' (default) - linear freq/amp; ignore all but initial phase
%  'db_ignore_phase' - same as 'ignore_phase', but linear amp interp. on dB scale
%  'cubic_freq+phase' - Cubic polynomial interpolation to match freq and phase
%  'cubic_freq+phase_cos' - Same thing, but with phases interpreted for cosine instead of sine
%  'stairstep' - No interpolation at all; freq+amp are step functions
%  'stairstep+phase' - same as 'stairstep' but phase also resets to new value
%  'just_error_check' - No synthesis; just do error checking on SDIF file
%  'naive' - No phase accumulation
% stream: (optional) stream number to synthesize.  (Omit = use first stream in file.)
% whichframes: (optional) list of frame numbers of the frames in the requested stream
%              to be synthesized. (Omit = all.)
% maxindex: (optional) Maximum partial index number  (default 100000)


% The columns of SDIF 1TRC and 1HRM data:
%   1. Index number
%   2. Frequency (Hz)
%   3. Amplitude (linear)
%   4. Phase (radians)
% INDEX = 1;  Defined below
FREQ = 2;
AMP = 3;
PHASE = 4;

% This synth has two parts: 
%
% 1) a main outer loop that reads the SDIF file, checks for lots of
% kinds of error conditions, selects the desired frames, 
% matches partials across frames to handle births and deaths, etc.
%
% 2) A collection of different synthesis inner loops that handle phase
% and frequency interpolation in different ways.
%
% The two communicate via an "additive synthesis to-do list" whose
% rows are all the partials that appear in a given segment of the output
% file and whose columns have these nice self-documenting names:
global INDEX OLDFREQ NEWFREQ OLDAMP NEWAMP OLDPHASE NEWPHASE UNKNOWN_PHASE
INDEX = 1;
OLDFREQ = 2;
NEWFREQ = 3;
OLDAMP = 4;
NEWAMP = 5;
OLDPHASE = 6;
NEWPHASE = 7;

% We'll use this value to indicate a phase that is unknown, "don't care", etc:
UNKNOWN_PHASE = -inf;

% They also share these two arrays (by reference) thanks to their being globals:
global output phases;
% output - full array of time-domain output samples
% phases - "current" phase of every partial



% Argument checking:
sdifexist(sdiffile);
T = 1/fs;

if (nargin < 3)
   synthType = 'ignore_phase';
end

if (nargin < 4)
   usefirststream = 1;
else
   usefirststream = 0;
   if (size(stream) == [1 1])
      %% OK
   else
      error('Stream argument must be a single streamID');
   end
end

if (nargin < 5)
    whichframes = 0;
end

if (nargin < 6)
   maxindex = 100000;
end


%% Read the SDIF file
[ data, header, frameTypes, matrixTypes ] = loadsdiffile(sdiffile);


times = header(:,1);


%% Which stream do we want to synthesize?
if (usefirststream) 
   % Get streamID of first 1TRC or 1HRM frame in the file
   found = 0;
   for i = 1:length(frameTypes)
       type = char(frameTypes(i,:));
       if ((type == '1TRC') | (type == '1HRM'))
	  stream = header(i, 2);
	  found = 1;
	  break;
       end
   end

   if (~found)
       error('No streams found of type 1TRC or 1HRM!');
   end
end

%% Get just the frames of the desired stream
StreamIDsOfAllFrames = header(:,2);
allFramesOfThisStream = find(StreamIDsOfAllFrames == stream);


%% Of the frames in this stream, which ones shall we synthesize?
if whichframes == 0
   % No subset specified, so use them all
   frames = allFramesOfThisStream;
else
   if min(whichframes) < 1
      error(['all frames in whichframes argument must be >= 1']);
   end
   if max(whichframes) > length(allFramesOfThisStream)
      error(['Stream has only ' num2str(length(allFramesOfThisStream)) ...
             ' frames; cannot synthesize frame ' num2str(max(whichframes))]);
   end

   frames = allFramesOfThisStream(whichframes);
end


%% Make sure all frames to be synthesized have the correct frame and matrix types
correctType = char(frameTypes(frames(1)));
if ((correctType ~= '1TRC') & (correctType ~= '1HRM'))
   error(['I only know how to synthesize 1TRC and 1HRM types, not ' correctType]);
end
for f=frames
    if char(frameTypes(f)) ~= correctType
       error(['Frame ' num2str(f) ' has frameType ' char(frameTypes(f)) ...
              'but this stream should be frame type ' correctType]);
    end

    if char(matrixTypes(f)) ~= correctType
       error(['Matrix in frame ' num2str(f) ' has matrixType ' ...
	      char(matrixTypes(f)) 'but this stream should be matrix type ' ...
              correctType]);
    end
end


%% Get rid of "hanging" partials (anything with nonzero amplitude in the last frame)

lastFrameNumber = frames(length(frames));
lastFrameTime = header(lastFrameNumber, 1);
lastmatrix = data{lastFrameNumber};

if (lastmatrix(:,AMP) == 0)
   % Last matrix of this stream has all amplitudes zero, so its time is the
   % total duration of the output
   output_duration = lastFrameTime;
else
   % Last matrix has at least one nonzero amplitude, so use extra time
   % (equal to the time between the last two frames) to ramp everything down to zero.

   penultimateFrameNumber = frames(length(frames)-1);
   penultimateFrameTime = header(penultimateFrameNumber, 1);
   lastInterFrameTime = lastFrameTime - penultimateFrameTime;

   newLastFrameTime = lastFrameTime + lastInterFrameTime;
   header = [header ; newLastFrameTime , stream];

   % The new last frame will be the same as the old last frame...
   newLastFrame = data{lastFrameNumber};
   % ...except that all amplitudes will be zero:
   newLastFrame(:,AMP) = zeros(size(newLastFrame(:,AMP)));
   newLastFrame;
   newLastFrameNumber = 1+ size(data,1);   % This was size(data,2) with sdif matlab 0.2 ???
   data{newLastFrameNumber} = newLastFrame;
   frames = [frames; newLastFrameNumber];

   output_duration = newLastFrameTime;
end


% Just the data we want to synthesize:
nframes = length(frames);
synthdata = data(frames);
synthTimesInSeconds = header(frames,1);
size_synthTimesInSeconds = size(synthTimesInSeconds);
synthtimes = round(fs * synthTimesInSeconds);


% Check for too-high frequencies (above Nyquist rate), too-high
% partial index numbers (above maxindex), and nonincreasing frame times

lasttime = -inf;

for f = 1:nframes
    time = synthTimesInSeconds(f);
    this = synthdata{f};

    if (time <= lasttime)
       error(['Nonincreasing frame times ' num2str(lasttime) ', ' ...
              num2str(time) ' in ' sdiffile ' stream ' num2str(stream)]);
    end

    for i = 1:size(this,1);
	index = this(i, INDEX);
        freq = this(i, FREQ);

	if (index > maxindex)
	   error(['Index ' num2str(index) ' exceeds maxindex of ' ...
	          num2str(maxindex) '. (' sdiffile ...
		  ', stream ' num2str(stream) ...
		  ', time ' num2str(time) ')']);
        end

	if (index < 0)
	   error(['Negative index ' num2str(index) ' not allowed. (' ...
	          sdiffile ', stream ' num2str(stream) ...
		  ', time ' num2str(time) ')']);
        end

	if(freq > fs/2)
	    warning('additive_synth:aliasing', ...
	            ['frequency ' num2str(freq) ...
		     ' exceeds Nyquist rate for fs ' num2str(fs) ...
		     ' (index ' num2str(index) ', time ' num2str(time) ')']);
        end
    end

    lasttime = time;

end



% Allocate output buffer:
numSamples = ceil(output_duration * fs);
output = zeros(1, numSamples);

% Where we keep track of running phase:
% We'll index this with the SDIF index number plus 1
phases = zeros(1, maxindex+1) + UNKNOWN_PHASE;

prev = synthdata{1};
for f = 2:nframes
    fromSample = synthtimes(f-1);
    toSample = synthtimes(f);
    disp(['Frame ' num2str(f) ' of ' sdiffile ' stream ' num2str(stream) ...
          ' (' char(synthType) ') samples ' num2str(fromSample) ':' num2str(toSample)]);
    this = synthdata{f};

    % The "additive synthesis to-do list": see comment near top of file
    todo = [];

    for p = 1:size(prev,1)
	% loop through partials of previous frame, looking
	% for continued partials or deaths
	index = prev(p, INDEX);

	newlocation = find(this(:,INDEX)==index);

	if (newlocation)
	   % found a match
	   newToDoItem = [index, prev(p, FREQ), this(newlocation, FREQ), ...
			  prev(p, AMP), this(newlocation, AMP), ...
			  prev(p, PHASE), this(newlocation, PHASE)];

	else
	   % No match, so let it decay (and don't constrain final phase)
	   newToDoItem =[index, prev(p, FREQ), prev(p,FREQ), ...
	                prev(p, AMP), 0, prev(p, PHASE), UNKNOWN_PHASE];
	end

	todo = [todo ; newToDoItem];
    end

    for p = 1:size(this,1)
	% loop through partials of current frame, looking
	% for births
	index = this(p,INDEX);

	oldlocation = find(prev(:,INDEX)==index);

	if (oldlocation)
	   % match was already recorded above
	else
	   % Not found, so this is a birth.
           if (this(p,AMP) == 0)
              % A "proper" birth; the partial starts at zero amplitude.
              % So do nothing; everything will be right for the next
	      % synthesis frame
	   else 
	      % Technically this shouldn't happen, but we'll fade
	      % in the partial over this frame with constant frequency.
	      % "Reverse-engineer" correct starting phase so that it will
	      % reach the stated value at the right time;
	      phaseChange=this(p,FREQ) * 2 * pi * ((toSample-fromSample)*T);
	      todo = [todo ; index, this(p,FREQ), this(p,FREQ), ...
			     0, this(p,AMP), ...
			     this(p,PHASE)-phaseChange, this(p, PHASE)];
           end
	end
    end



    if isempty(todo) 
       % Nothing to do, so don't bother calling synth
       % disp(['empty to-do list for frame ' num2str(f)]);
    else
        % Add one to all index numbers so that 0 (legal in SDIF) will work
	todo(:,1) = 1 + todo(:,1);

        % Dispatch to the appropriate synth
	if strcmp (synthType, 'ignore_phase')
           ignore_phase_synth(fromSample, toSample, todo, T);
	elseif strcmp (synthType, 'db_ignore_phase')
           db_ignore_phase_synth(fromSample, toSample, todo, T);	   
	elseif strcmp (synthType, 'cubic_freq+phase')
           cubicphase_synth(fromSample, toSample, todo, T, 'sine');
	elseif strcmp (synthType, 'cubic_freq+phase_cos')
           cubicphase_synth(fromSample, toSample, todo, T, 'cosine');
	elseif strcmp (synthType, 'stairstep')
           stairstep_synth(fromSample, toSample, todo, T)
	elseif strcmp (synthType, 'stairstep+phase')
           stairstep_phase_synth(fromSample, toSample, todo, T);	   
        elseif strcmp (synthType, 'just_error_check')
           errorcheck_synth(fromSample, toSample, todo, T);
        elseif strcmp (synthType, 'naive')
           ignore_phase_synth_naive(fromSample, toSample, todo, T);
        else
           error(['Unrecognized synth type: ', synthType] );
        end
    end


    for i = [fromSample+1:toSample]
        if (isnan(output(i)))
             warning(['Synthesizer just output a NaN for sample ' num2str(i)]);
             pause
        end
    end

    prev = this;
end
return


function errorcheck_synth(fromSample, toSample, todo, T)
    disp(['ErrorCheck Synth samples [' num2str(fromSample) ':' ...
          num2str(toSample) '], T=' num2str(T) ', 1/T=' num2str(1/T)]);
    todo
return


function ignore_phase_synth_naive(fromSample, toSample, todo, T)
    % How you might think it would make sense to do synthesis
    % Ignore all stated phases except when a partial is first seen

    global INDEX OLDFREQ NEWFREQ OLDAMP NEWAMP OLDPHASE NEWPHASE UNKNOWN_PHASE;
    global output phases;

    R = toSample - fromSample;

    for j = 1:size(todo,1)
 	index = todo(j, INDEX);
	oldf =  todo(j, OLDFREQ) * 2 * pi;
	newf =  todo(j, NEWFREQ) * 2 * pi;
	olda =  todo(j, OLDAMP);
	newa =  todo(j, NEWAMP);

	if(phases(index) == UNKNOWN_PHASE)
	    phases(index) = todo(j, OLDPHASE);
	end

	% Additive synthesis inner loop:
        f = oldf;
	df = (newf-oldf) / R;
        a = olda;
	da = (newa-olda) / R;
        t = 0;

	for i = fromSample+1:toSample
	    output(i) = output(i) + (a * sin(f * t + phases(index)));
            f = f + df;
	    a = a + da;
            t = t + T;
        end
    end
return




function ignore_phase_synth(fromSample, toSample, todo, T)
    % Linearly interpolate frequency and amplitude between frames;
    % Ignore all stated phases except when a partial is first seen

    global INDEX OLDFREQ NEWFREQ OLDAMP NEWAMP OLDPHASE NEWPHASE UNKNOWN_PHASE;
    global output phases;

    R = toSample - fromSample;

    for j = 1:size(todo,1)
 	index = todo(j, INDEX);
	oldf =  todo(j, OLDFREQ) * 2 * pi;
	newf =  todo(j, NEWFREQ) * 2 * pi;
	olda =  todo(j, OLDAMP);
	newa =  todo(j, NEWAMP);

	if(phases(index) == UNKNOWN_PHASE)
	    phases(index) = todo(j, OLDPHASE);
	end

	% Additive synthesis inner loop:
	dp = oldf * T;
	ddp = (newf-oldf) * T / R;

        a = olda;
	da = (newa-olda) / R;

	for i = fromSample+1:toSample
	    output(i) = output(i) + (a * sin(phases(index)));
	    phases(index) = phases(index) + dp;
	    dp = dp + ddp;
	    a = a + da;
        end
    end
return



function linear = dbtolin(db)
     linear = 10.^(db * 0.05);
return

function dbout = mydb(linear)
  if (linear<1.0e-6)
     dbout = -120;
  else
     dbout = db(linear);
  end
return


function db_ignore_phase_synth(fromSample, toSample, todo, T)
    % Linearly interpolate frequency between frames.
    % Ignore all stated phases except when a partial is first seen.
    % Linearly interpolate amplitude on a log scale
    % This should be made more efficient by reworking not to have to call
    %     dbtolin in the inner loop

    global INDEX OLDFREQ NEWFREQ OLDAMP NEWAMP OLDPHASE NEWPHASE UNKNOWN_PHASE;
    global output phases;

    R = toSample - fromSample;

    for j = 1:size(todo,1)
 	index = todo(j, INDEX);
	oldf =  todo(j, OLDFREQ) * 2 * pi;
	newf =  todo(j, NEWFREQ) * 2 * pi;
	olda_linear =  todo(j, OLDAMP);
	newa_linear =  todo(j, NEWAMP);

	olda_db = mydb(olda_linear);
	newa_db = mydb(newa_linear);

	if(phases(index) == UNKNOWN_PHASE)
	    phases(index) = todo(j, OLDPHASE);
	end

	% Additive synthesis inner loop:
	dp = oldf * T;
	ddp = (newf-oldf) * T / R;

        a_db = olda_db;
	da_db = (newa_db-olda_db) / R;

	for i = fromSample+1:toSample
	    output(i) = output(i) + (dbtolin(a_db) * sin(phases(index)));
	    phases(index) = phases(index) + dp;
	    dp = dp + ddp;
	    a_db = a_db + da_db;
        end
    end

return


function cubicphase_synth(fromSample, toSample, todo, T, sine_or_cosine)
    % Linearly interpolate amplitude between frames.
    % Make phase be a cubic (spline) so we can match starting
    % and ending phase and frequency.

    global INDEX OLDFREQ NEWFREQ OLDAMP NEWAMP OLDPHASE NEWPHASE UNKNOWN_PHASE;
    global output phases;

    R = toSample - fromSample;
    time = R*T;
    worst = 0.01;

    for j = 1:size(todo,1)
 	index = todo(j, INDEX);
	oldf =  todo(j, OLDFREQ) * 2 * pi;
	newf =  todo(j, NEWFREQ) * 2 * pi;
	olda =  todo(j, OLDAMP);
	newa =  todo(j, NEWAMP);
	newp =  todo(j, NEWPHASE);

	if(phases(index) == UNKNOWN_PHASE)
	    % Haven't seen this partial before, so don't worry about unwrapping
	    phases(index) = todo(j, OLDPHASE);
	end

	oldphase = phases(index);
	newphase = unwrap_phase(oldphase, oldf, newf, newp, time);

        

        % Remember unwrapped final phase for next time:
        phases(index) = newphase;

	% Find the cubic polynomial.  Domain is seconds, over the sample indices of this frame
	polynomial = cubic_from_endpoints(1*T, oldphase, oldf, R*T, newphase, newf);
	phase = polyval(polynomial, [1:R]*T);


        effectiveavgfreq = (newphase-oldphase)/time;
        avgfreq = (oldf+newf)/2;

	off = abs(effectiveavgfreq - avgfreq) / avgfreq;

	if (off > worst)
	   off
	   worst=off;
	   index
	   effectiveavgfreq
	   avgfreq
	   plot_phase(phase, oldf, newf)
	   pause
        end



        if (strcmp(sine_or_cosine, 'cosine'))
             phase = phase + pi/2;
        elseif (strcmp(sine_or_cosine,'sine'))
             % Phase is correct
        else
             error(['Unrecognized sine_or_cosine parameter: ' sine_or_cosine]);
        end


	% plot_phase(phase)

	% Additive synthesis inner loop:
        a = olda;
	da = (newa-olda) / R;

	for i = 1:R
	    output(fromSample+i) = output(fromSample+i) + (a * sin(phase(i)));
	    a = a + da;
        end
    end
return

function plot_phase(phase, oldf, newf)
        figure(1)
	subplot(311);
        plot(phase); xlabel('Time'); ylabel('Phase');

	subplot(312);
	f = [diff(phase) newf/44100] ./ (1/44100);
	plot(f);  xlabel('Time'); ylabel('radian freq.  red=oldf, green=newf');
	hold on;
	plot([1 length(f)], [oldf oldf], 'r');
	plot([1 length(f)], [newf newf], 'g--');
	hold off;
	
	subplot(313);
	plot(sin(phase)); xlabel('Time'); ylabel('sin(phase)');
	hold on;
	phase_oldf = (oldf/44100)*[0:length(f)-1] + phase(1);
	plot(sin(phase_oldf), 'r');
	phase_newf = (newf/44100)*[0:length(f)-1] + phase(1);
	plot(sin(phase_newf), '--g');
        hold off;

	pause;

return


function stairstep_synth(fromSample, toSample, todo, T)
    % No interpolation; just hold freq and amplitude constant across
    % each synthesis frame.
    % Ignore all stated phases except when a partial is first seen

    global INDEX OLDFREQ NEWFREQ OLDAMP NEWAMP OLDPHASE NEWPHASE UNKNOWN_PHASE;
    global output phases;

    for j = 1:size(todo,1)
 	index = todo(j, INDEX);
	f =  todo(j, OLDFREQ) * 2 * pi;
	a =  todo(j, OLDAMP);

	if(phases(index) == UNKNOWN_PHASE)
	    phases(index) = todo(j, OLDPHASE);
	end

	% Additive synthesis inner loop:
	dp = f * T;

	for i = fromSample+1:toSample
	    output(i) = output(i) + (a * sin(phases(index)));
	    phases(index) = phases(index) + dp;
        end
    end
return


function stairstep_phase_synth(fromSample, toSample, todo, T)
    % No interpolation; just hold freq and amplitude constant across
    % each synthesis frame.
    % Reset phase (totally discontinuously) to given value at each synth frame

    global INDEX OLDFREQ NEWFREQ OLDAMP NEWAMP OLDPHASE NEWPHASE UNKNOWN_PHASE;
    global output phases;

    for j = 1:size(todo,1)
	f =  todo(j, OLDFREQ) * 2 * pi;
	a =  todo(j, OLDAMP);
	p =  todo(j, OLDPHASE);

	% Additive synthesis inner loop:
	dp = f * T;

	for i = fromSample+1:toSample
	    output(i) = output(i) + (a * sin(p));
	    p = p + dp;
        end
    end
return
