%function RefineAUV()
%
%   Estimate thew Action Unit Parameters 
%
% Copyright 2010, Jixu Chen - Rensselaer Polytechnic Institute 
% All Rights Reserved Permission to use, copy, modify, and distribute this
% software and its documentation for any non-commercial purpose is hereby
% granted without fee, provided that the above copyright notice appear in
% all copies and that both that copyright notice and this permission
% notice appear in supporting documentation, and that the name of the
% author not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
% THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
% INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR ANY
% PARTICULAR PURPOSE. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
% SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
% RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF
% CONTRACT, NEGLIGENCE OR OTHER TORTUOUS ACTION, ARISING OUT OF OR IN
% CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
%
% Written By:   Jixu Chen (chenj4@rpi.edu)
% Last Updated: 04/29/2008

function RefineAUV(vecfile, in_shapefile, data, startframe, endframe, out_aufile,disp)
    % Frist colume : the tracked points, 
    % Second colume: corresponding CANDIDE-3 points
    Map=[2,49;
        3,50;
        4,53;
        5,56;
        6,54;
        7,55;
        9,76;
        10,64;
        11,66;
        12,86;
        13,16;
        14,17;
        15,20;
        16,23;
        17,21;
        18,22;
        20,75;
        21,31;
        22,33;
        23,85;
        24,7;
        25,8;
        26,48;
        27,15;]

Vec = load(vecfile);
% Edge = load('Data\edges');
% %    =================== Display CANDIDE-3 Vecs and Edges =============
% for i=1:length(Vec)
%     plot(Vec(i,1),Vec(i,2),'.r'); hold on;
%     text(Vec(i,1)+0.01,Vec(i,2)+0.01,num2str(i-1),'FontSize',10);
% end
% for i=1:length(Edge)
%     plot(Vec(Edge(i,:)+1,1),Vec(Edge(i,:)+1,2),'-'); hold on;
% end

% ========= Load Shape Unit Vectors 
shape = LoadShape(); 

% ========= Load Shape Parameters 
temp = dlmread(in_shapefile)
SUVPar = temp(2:12);
Vec = AddUV(Vec, shape, SUVPar)

% ========= Load Animation Unit Vectors 
AUV = LoadAUV();
AUVPar = [];
for i=startframe:endframe        
    % ========== Load Image & Tracked Points
    trackpt = reshape(data(i*2-1,2:57),2,[]);
    trackpt(2,:) = 256 - trackpt(2,:);  %for image coordinate (origin in upperleft)
    %===================  Least Square Fitting =============
    A =[];b=[];
    for i=1:length(Map)
        Matrix =[];
        Matrix(:,1)= Vec(Map(i,2)+1,1:2)';
        for j=1:6
            [tf, index] = ismember(Map(i,2),AUV{j}(:,1));
            if (index>0)
                Matrix = [Matrix, AUV{j}(index,2:3)']; %adding this Unit Vector
            else
                Matrix = [Matrix,zeros(2,1)];
            end
        end
        Matrix =[Matrix,eye(2)];
        
        A=[A;Matrix];
        b=[b;trackpt(:,Map(i,1)+1)];
    end
    x=A\b;
    
    scale = x(1);
    tx = x(8); ty = x(9);
    x(2:9) = x(2:9)/scale;
    AUVPar =[AUVPar; x']; % 
    
    if (disp)
        figure(3)
        Vec_new = AddUV(Vec, AUV, AUVPar(end,2:7));
        Vec_new(:,1:2) = Vec_new(:,1:2)*scale + repmat([tx,ty],size(Vec,1),1);
        %Vec_new = Vec*scale
        plot(Vec_new(Map(:,2)+1,1),Vec_new(Map(:,2)+1,2),'*r'); hold on;
        plot(trackpt(1,:),trackpt(2,:),'ob'); hold off;
        xlim([0,256]);ylim([0,256]); axis equal;
        drawnow;
    end
end

dlmwrite(out_aufile, AUVPar, 'precision', '%.6f', 'delimiter', '\t','newline', 'pc')