function [err, err_grad] = vonMisesError(X, Y, model, lamb, priorCoef)
% [err, err_grad] = vonMisesError(X, Y, model, lamb, priorCoef)
%
% This function implements MAP using Gaussian noise
%
%
% 
% Input:  
% -- Y					N X 1 vector (counts; N = number of trials OR directions)
% -- X					N X 1 vector (angles)
% -- model			4 X 1 VM parameters
% -- lambda			the effect of the prior-model
%
%
% Output:
%         1. error
%         2. The gradient = partial derivatives of err at point model, 
%             ith component of g is the partial derivative of err 
%             with respect to the ith component of model.
%         3. The Hessian matrix = 
%             second partial derivatives matrix of err at point model.

% The error is defined as: sum(diff^2) - log(posterior)
% The function is used by the optimization algorithm (fminsearch/fmincon)
% Following bishop's book page 29 (Eq 1.63) 

  % Init
  VM = fVonMises(model,X);
  diff = VM-Y;
  if any(isnan(diff)), error('Unexpected NaN values'); end
  
  % The Error is defined as err-=log(posterior)
  kp = model(3);
	prior_kappa = exp(-priorCoef * kp);
  err = sum(diff.^2) - lamb*log(prior_kappa);  
  
  if nargout > 1  
    % The gradient of the likelihood is: 2*(VM-Y)*VM_Grad    
    % gradVonMises(model,X) returns a matrix size( length(x), length(model) )
    gradVM = gradVonMises(model, X); % (size n_samples x 4)
    err_grad = 2 * diff' * gradVM;
    if lamb>0
			err_grad = err_grad + lamb*priorCoef;
    end
  end
  
  if nargout > 2
    error('hessian is not implemented');
    [hessVM,a] = hessVonMises(model,X); %#ok
    hessVM = nansum(hessVM,3); %?
    err_hess = 2*sum(gradVM.^2+diff .* hessVM);
  end
end

function yhat = fVonMises(coefs,X)
  base = coefs(1); 
  m = coefs(2);
  kp = coefs(3);
  mu = coefs(4);
  yhat = base + m.*exp(kp.*cos(X-mu));
end

function grad = gradVonMises(model,X)
  m=model(2); kp=model(3); mu=model(4);
  grad = zeros(length(X),4);
  grad(:,1) = 1;
  grad(:,2) = exp(kp*cos(X-mu));
  grad(:,3) = m*cos(X-mu).*exp(kp*cos(X-mu));
  grad(:,4) = m*sin(X-mu)*kp.*exp(kp*cos(X-mu));
end

function hess = hessVonMises(model,X)
  %bs = model(1); 
  m=model(2); kp=model(3); mu=model(4);
  
  hess = zeros(4,4,length(X));
  %column and row 1 are base's second partial derivatives which are 0
  hess(2,2,:) = 0;                                                  %dm *dm
  hess(2,3,:) = cos(X-mu).*exp(kp*cos(X-mu));                       %dkp*dm
  hess(3,2,:) = hess(2,3);
  hess(2,4,:) = kp*sin(X-mu).*exp(kp*cos(X-mu));                    %dmu*dm
  hess(4,2,:) = hess(2,4);
  hess(3,3,:) = m*(cos(X-mu).^2).*exp(kp*cos(X-mu));               %dkp*dkp
  hess(3,4,:) = m*sin(X-mu).*exp(kp*cos(X-mu)).*(1+kp*cos(X-mu));  %dmu*dkp
  hess(4,3,:) = hess(3,4);
  hess(4,4,:) = m*kp*exp(kp*cos(X-mu)).*(kp*(sin(X-mu).^2)-cos(X-mu));%dmu*dmu
end

