clc;clear all;close all;
warning('off')
%[x,fs,nbits]=wavread('tang1.wav');         %打开录好的语音信号文件。
global fs;
[x,fs] = audioread('tang1.wav');            %打开录好的语音信号文件。
%info = audioinfo('tang1.wav');
%nbits = info.BitsPerSample;
x = x / max(abs(x));                        %幅度归一化到[-1,1]

%参数设置
kl = round(1 / 500 * fs);                   %500Hz
kr = round(1 / 80 * fs);                    %80Hz
N = 3 * kr;                                 %帧长
inc = round(fs / 100);                      %帧移步长10ms

%语音波形图
subplot(3, 1, 1);
plot(x);
axis([1 length(x) -1 1])                    %限制x轴与y轴的范围。
xlabel('帧数');
ylabel('Speech');
legend('FrameLen = 552');

%自相关函数构成基音周期候选点集
subplot(3, 1, 2);
A = enframe(x, N, inc);
R = zeros(size(A));
l = zeros(1, size(R, 1));
f = zeros(1, size(R, 1));
Can = zeros(1, 10);
acCan = zeros(1, 10);
CostF = zeros(1, 10);
it = 0;
for n = 1:size(R, 1)
    R(n, :) = autocorr(A(n, :), N - 1);
    Can_ = Can;
    CostF_ = CostF;
    [acCan,Can] = findpeaks(R(n, kl:kr), 'MinPeakHeight', R(n, 1) * 0.25, 'MinPeakProminence', 0.9);
    Can = Can + kl - 1;
    sz = size(Can, 2);
    if sz ~= 0
        it = it + 1;
        if it == 1
            CostF = dist(acCan);
        else
            CostF = zeros(1, sz);
            Path = zeros(1, sz);
            CostT = diff(Can, Can_);
            for j = 1:sz
                [CostF(j), Path(j)] = min(CostF_ + CostT(j, :));
                CostF = CostF + dist(acCan);
            end
        end
        [~, l(n)] = min(CostF);
        ff = f0(Can);
        plot(n, ff, '.');
        hold on;
        f(n) = ff(l(n));
    else
        it = 0;
    end
end

subplot(3, 1, 3);
f = medfilt1(f, 5);
stem(f, 'MarkerSize',3);
xlabel('帧数(n)');
ylabel('频率(Hz)');

function f = f0(Can)
    global fs;
    f = fs ./ Can;
end

function dis = dist(ac)
    dis = -log(ac);
end

function df = diff(Can1, Can2)
    n = size(Can1, 2);
    m = size(Can2, 2);
    df = zeros(n, m);
    for i = 1:n
        for j = 1:m
            df(i, j) = abs(f0(Can1(i)) - f0(Can2(j)));
        end
    end
end