Dynamics_Model.m

classdef Dynamics_Model < matlab.mixin.Copyable %handle
    properties
        num_dim_Y
                
        interpolant_XY
        
        sigma_estimation
        sigma_test
        alpha_initial
        initial_estimate_X
        
        X
        T
        Y
        %previous_N_fixed
        %N_fixed
        %nb_times_fixed
        
        current_alpha
        %alpha_variable
        %confidence
        window_size
        
        display_screen
        marginal_X_Yi
        limits_Yi
    end
    methods
        function obj = Dynamics_Model(filename_interpolant, display_screen)
            obj.display_screen = display_screen;
            if obj.display_screen ~= -1
                figure(display_screen)
                clf(display_screen)
            end
            
            structure = load(filename_interpolant);

            obj.interpolant_XY = structure.interpolant_XY;
            
            obj.num_dim_Y = size(structure.interpolant_Y.GridVectors,2);
            
            N = size(structure.interpolant_Y.GridVectors{1}, 2);
            for i=1:obj.num_dim_Y
                obj.limits_Yi(i,1) = structure.interpolant_Y.GridVectors{i}(1);
                obj.limits_Yi(i,2) = structure.interpolant_Y.GridVectors{i}(N);
            end
            
            for i=1:obj.num_dim_Y
                obj.marginal_X_Yi(:,:,i) = flipdim(structure.P_Yi_knowing_X(:,:,i),1);
            end
        end
        function initialise(obj, initial_X, alpha_ini, sigma_estimation, sigma_test)
            obj.T = [];
            obj.X = [];
            obj.Y = [];
            %obj.N_fixed = 0;
            obj.current_alpha = alpha_ini;
            %obj.alpha_variable = [];
            obj.window_size = [];
            %obj.confidence = [];
            %obj.nb_times_fixed = [];
            
            obj.initial_estimate_X = initial_X;
            obj.alpha_initial = alpha_ini;
            obj.sigma_estimation = sigma_estimation;
            obj.sigma_test = sigma_test;
        end
        function process_new_frame(obj, Yt, Tt)
            obj.T = [obj.T; Tt];
            obj.Y = [obj.Y; Yt(:,1:obj.num_dim_Y)];
            frame = size(obj.T,1);
            
            % estimate alpha and adjust the window size
            alpha = obj.estimate_alpha();
            sprintf('estimated alpha_variable: %f', alpha);
            obj.current_alpha = alpha;
            obj.window_size = [obj.window_size, frame];
            
            % estimate X inside the window (and update N_fixed)
            %obj.previous_N_fixed = obj.N_fixed;
            obj.estimate_X();
            %sprintf('"normalised" log likelihood of X (or confidence): %f', obj.confidence(frame));
            if obj.display_screen ~= -1
                obj.plot_estimated_X();
            end
            
            % save the value of alpha for the frames that have converged
            %if obj.previous_N_fixed < obj.N_fixed
            %    for i=obj.previous_N_fixed+1:obj.N_fixed
            %        obj.alpha_variable(i) = alpha;
            %    end
            %end
        end
        function alpha = estimate_alpha(obj)
            N = size(obj.X,1);
            
            if N < 3
                alpha = obj.alpha_initial;
                return
            end
            
            first_index = 1;
            nb_points = N;
            
            if nb_points < 3 % we need at least 2 intervals to compute the mean
                alpha = obj.alpha_initial;
            else
                X_2_to_N = obj.X(first_index+1:N);
                X_1_to_Nminus1 = obj.X(first_index:N-1);
                T_2_to_N = obj.T(first_index+1:N);
                T_1_to_Nminus1 = obj.T(first_index:N-1);

                delta_X = X_2_to_N - X_1_to_Nminus1;
                delta_T = T_2_to_N - T_1_to_Nminus1;

                factors = delta_X ./ delta_T;

                alpha = mean(factors);

                if alpha <= 0.85 * obj.alpha_initial; %0.75 %0.015  0.75%0.85%0.9
                    alpha = 0.85 * obj.alpha_initial; %0.75%0.015;%0.85%0.9
                end
                if alpha >= 1.15 * obj.alpha_initial; %1.25%0.015  1.25%1.15%1.1
                    alpha = 1.15 * obj.alpha_initial; %1.25%0.015;%1.15%1.1
                end
%                     if alpha <= 0.015
%                         alpha = 0.015;
%                     end
                    
%                     if alpha > 0.025
%                         alpha = 0.025;
%                     end
            end
        end
        function estimate_X(obj)
            %function to be minimised over new_X, with extra parameters Y, alpha and T
            f = @(variable_X)(-obj.logP_x_knowing_y_paquet(variable_X,obj.current_alpha,obj.sigma_estimation));

            N = size(obj.X,1);

            if N == 0
                X_ini = obj.initial_estimate_X;
            else
                X_ini = [obj.X; obj.X(N) + obj.current_alpha * (obj.T(N+1) - obj.T(N))];
            end

            lb = repmat(0,1,length(X_ini));
            ub = repmat(1,1,length(X_ini));
            A = [];
            b = [];
            Aeq = [];
            beq = [];
            [new_X_variable,loglikelihood] = fmincon(f,X_ini,A,b,Aeq,beq,lb,ub);
            %obj.confidence = [obj.confidence, -loglikelihood / size(new_X_variable,1)];
            
            obj.X = new_X_variable;
        end
        function spread_X(obj)
            N = size(obj.X,1);
            
            if N ==1
                obj.X = 0;
                %obj.confidence = 1;
                obj.current_alpha = obj.alpha_initial;
            
                obj.plot_estimated_X()
                obj.compute_loglikelihood_dynamics(1:N)
                return
            elseif N == 2
                obj.X = [0 1];
                %obj.confidence = [0 1];
                obj.current_alpha = 1;
            
                obj.plot_estimated_X()
                obj.compute_loglikelihood_dynamics(1:N)
                return
            end
            
            %%% estimation initiale de X
            X_tmp = 0:1/(N-1):1;
            
            %%% calcul du alpha correspondant
            X_2_to_N = X_tmp(2:N)';
            X_1_to_Nminus1 = X_tmp(1:N-1)';
            T_2_to_N = obj.T(2:N);
            T_1_to_Nminus1 = obj.T(1:N-1);

            delta_X = X_2_to_N - X_1_to_Nminus1;
            delta_T = T_2_to_N - T_1_to_Nminus1;

            factors = delta_X ./ delta_T;
            alpha = mean(factors);
            

            %function to be minimised over new_X, with extra parameters Y, alpha and T
            f = @(variable_X)(-obj.logP_x_knowing_y_spread(variable_X,alpha,obj.sigma_estimation));

            %%% estimation de X
            X_ini = X_tmp(2:end-1)';
            lb = repmat(0,1,length(X_ini));
            ub = repmat(1,1,length(X_ini));
            A = [];
            b = [];
            Aeq = [];
            beq = [];
            [new_X,loglikelihood] = fmincon(f,X_ini,A,b,Aeq,beq,lb,ub);
            
            new_X = [0; obj.normalise_X(new_X); 1];
            obj.X = new_X;
            %obj.confidence = [1, -loglikelihood / size(new_X,1), 1];
            
            
            %%% calcul du alpha correspondant aux X estimés
            X_2_to_N = obj.X(2:N);
            X_1_to_Nminus1 = obj.X(1:N-1);
            T_2_to_N = obj.T(2:N);
            T_1_to_Nminus1 = obj.T(1:N-1);

            delta_X = X_2_to_N - X_1_to_Nminus1;
            delta_T = T_2_to_N - T_1_to_Nminus1;

            factors = delta_X ./ delta_T;
            obj.current_alpha = mean(factors);
            
            
            if obj.current_alpha <= 0.85 * obj.alpha_initial; %0.75 %0.015  0.75%0.85%0.9
                obj.current_alpha = 0.85 * obj.alpha_initial; %0.75%0.015;%0.85%0.9
            end
            if obj.current_alpha >= 1.15 * obj.alpha_initial; %1.25%0.015  1.25%1.15%1.1
                obj.current_alpha = 1.15 * obj.alpha_initial; %1.25%0.015;%1.15%1.1
            end
            
            
            obj.plot_estimated_X()
            obj.compute_loglikelihood_dynamics(1:N)
        end
        function loglikelihood = logP_x_knowing_y_paquet(obj, variable_X, alpha, sigma)
            N_variable = size(variable_X,1);

            % if N == 1, we maximise P_cond_y(Y(1),X(1)) * P_x_Markov_simple(X(1),X(0)) avec X(0) virtuel
            % if N == 2, we maximise P_cond_y(Y(2),X(2)) * P_x_Markov_simple(X(2),X(1)) * P_cond_y(Y(1),X(1)) with P_x_Markov_simple(X(2),X(1)) = 0 if delta_X is negative, and uniform proba otherwise

            loglikelihood = obj.logP_cond_y(obj.Y(1,:),variable_X(1));
            loglikelihood = loglikelihood + obj.logP_x_Markov(variable_X(1), variable_X(1)-alpha, 2, 1, alpha, sigma);

            if N_variable > 1
                Y_2_to_N = obj.Y(2:N_variable,:);
                X_2_to_N = variable_X(2:N_variable);
                X_1_to_Nminus1 = variable_X(1:N_variable-1);
                T_2_to_N = obj.T(2:N_variable);
                T_1_to_Nminus1 = obj.T(1:N_variable-1);

                ll_proba_cond_y = obj.logP_cond_y(Y_2_to_N, X_2_to_N);
                ll_proba_x_Markov = obj.logP_x_Markov(X_2_to_N,X_1_to_Nminus1,T_2_to_N,T_1_to_Nminus1,alpha,sigma);
                vect_tmp = ll_proba_cond_y + ll_proba_x_Markov;

                loglikelihood = loglikelihood + sum(vect_tmp);
            end
        end
        function loglikelihood = logP_x_knowing_y_spread(obj, variable_X, alpha, sigma)
            N_variable = size(variable_X,1);

            % we maximise P_cond_y(Y(1),0) * P_x_Markov_simple(X(2),0) * P_cond_y(Y(i),X(i)) * P_x_Markov_simple(X(i+1),X(i)) * P_x_Markov_simple(1,X(N-1)) * P_cond_y(Y(N),1)

            loglikelihood = obj.logP_cond_y(obj.Y(1,:),0) + obj.logP_x_Markov(variable_X(1),0,obj.T(2),obj.T(1),alpha,sigma);
            loglikelihood = loglikelihood + sum(obj.logP_cond_y(obj.Y(2:end-1,:), variable_X));
            loglikelihood = loglikelihood + obj.logP_x_Markov(1, variable_X(end),obj.T(end),obj.T(end-1),alpha,sigma);
            loglikelihood = loglikelihood + obj.logP_cond_y(obj.Y(end,:),1);
            
            if N_variable > 1
                ll_proba_x_Markov = obj.logP_x_Markov(variable_X(2:end),variable_X(1:end-1),obj.T(3:end-1),obj.T(2:end-2),alpha,sigma);
                loglikelihood = loglikelihood + sum(ll_proba_x_Markov);
            end
        end
        function loglikelihood = logP_cond_y(obj, Yi, Xi)
            s = size(Yi,1);
            comp_small = repmat(obj.limits_Yi(:,1)',s,1);
            comp_big = repmat(obj.limits_Yi(:,2)',s,1);
            
            too_small = Yi < comp_small;
            too_big = Yi > comp_big;
            
            Yi(too_small) = comp_small(too_small);
            Yi(too_big) = comp_big(too_big);
            
            xy = [obj.normalise_X(Xi) Yi];
            
            if isempty(xy)
                loglikelihood = [];
                return;
            end
            
            loglikelihood = log(obj.interpolant_XY(xy));
        end
        function loglikelihood = logP_x_Markov(obj, Xi, Xim, Ti, Tim, alpha, sigma)
            
            difference = (Xi - Xim) - alpha .* (Ti - Tim);

            % penalise negative and null dX
            indexes = Xi - Xim < 0;
            if length(alpha) > 1
                difference(indexes) = 5 * (Xi(indexes) - Xim(indexes)) - alpha(indexes) .* (Ti(indexes) - Tim(indexes));
            else
                difference(indexes) = 5 * (Xi(indexes) - Xim(indexes)) - alpha .* (Ti(indexes) - Tim(indexes));
            end

            if isempty(difference)
                loglikelihood = -100;
            else
                loglikelihood = log(normpdf(difference,0,sigma));

                index2 = loglikelihood < -100;%a été supprimé
                loglikelihood(index2) = -100;%a été supprimé
            end
        end
                
        function llh_dynamics = compute_loglikelihood_dynamics(obj, frames)
            %llh_dynamics(frames <= 1) = nan;
            
            indexes = frames > 1 & frames <= length(obj.T);
            
            alpha = obj.current_alpha;
            
            terme1 = obj.logP_cond_y(obj.Y(frames(indexes),:), obj.X(frames(indexes)));
            terme2 = obj.logP_x_Markov(obj.X(frames(indexes)),obj.X(frames(indexes)-1),obj.T(frames(indexes)),obj.T(frames(indexes)-1),alpha,obj.sigma_test);
            llh_dynamics(indexes) = terme1 + terme2;
            
            indexes = frames <= 1
            frames(indexes)
            terme1 = obj.logP_cond_y(obj.Y(frames(indexes),:), obj.X(frames(indexes)));
            %llh_dynamics(frames <= 1) = terme1 + obj.logP_x_Markov(obj.alpha_initial, 0, 2, 1, obj.alpha_initial, obj.sigma_test);
            llh_dynamics(frames <= 1) = terme1 + obj.logP_x_Markov(obj.X(1), obj.X(1)-alpha, 2, 1, alpha, obj.sigma_test);
        end
        function plot_estimated_X(obj)
            if obj.display_screen ~= -1
                N = size(obj.X,1);
                start_frame = max(N - 50, 1);
                
                figure(obj.display_screen)
                
                
                for i=1:obj.num_dim_Y
                    subplot(obj.num_dim_Y,1,i)
                    hold off
                    imagesc([0 1],[obj.limits_Yi(i,1) obj.limits_Yi(i,2)],obj.marginal_X_Yi(:,:,i),'CDataMapping','scaled')
                    set(gca,'YDir','normal')
                    title(['Likelihood of (X,Y_' i ') to be normal (color plot) and estimated (X,Y_' i ') (black crosses)'])
                    xlabel('Percentage of movement completion X')
                    ylabel(['Manifold coordinate Y_' i])
                    hold on
                    plot(obj.normalise_X(obj.X(start_frame:N)),obj.Y(start_frame:N,i),'kx')
                    
                end
                %pour faire des gifs
                %saveas(obj.display_screen, strcat('estimatedX_frame', int2str(N), '.png'))
            end

        end

        function [im]=final_plot_estimated_X(obj)
            if obj.display_screen ~= -1
                N = size(obj.X,1);
                start_frame = max(N - 50, 1);
                
                figure(obj.display_screen)
                
                
                for i=1:obj.num_dim_Y
                    subplot(obj.num_dim_Y,1,i)
                    hold off
                    im=imagesc([0 1],[obj.limits_Yi(i,1) obj.limits_Yi(i,2)],obj.marginal_X_Yi(:,:,i),'CDataMapping','scaled')
                    %set(gca,'YDir','normal')
                    hold on
                    plot(obj.normalise_X(obj.X(start_frame:N)),obj.Y(start_frame:N,i),'kx')
                    axis off
                    
                end
                %pour faire des gifs
                %saveas(obj.display_screen, strcat('estimatedX_frame', int2str(N), '.png'))
            end
        end

        function X = normalise_X(obj, X)
            X(X<0) = 0;
            X(X>1) = 1;
        end
        function periodic = is_periodic(obj)
            periodic = 0;
        end
    end
    
    %methods (Abstract)
    %    X = normalise_X(obj, X)
    %    periodic = is_periodic(obj)
    %end
end