%Evaluate cost function for closest representative search given coefficients
%function ft=essential_distMinAnglePair_ft(t,m1,p1,c1,m2,p2,c2)
%Evaluates the cost function used by essential_distMinAnglePair to find the
%closest representative in the equivalence class of a QREM
%If m2,p2,c2 are omitted or empty, get value of a single term
function ft=essential_distMinAnglePair_ft(t,m1,p1,c1,m2,p2,c2)
flagSingleTerm=false;
if ~exist('m2','var') || isempty(m2)
    flagSingleTerm=true;
end

if flagSingleTerm
    ft=acos((m1*sin(t+p1)+c1-1)/2)^2;
else
    ft=acos((m1*sin(t+p1)+c1-1)/2)^2+acos((m2*sin(t+p2)+c2-1)/2)^2;
end