% -------------------------------------------------------------------------------
% MATLAB code for generating a visualization
% of an inverted pendulum
%
% Copyright (C) 2015 by Xuhui Feng and Yuning Jiang
% ShanghaiTech University
% -------------------------------------------------------------------------------

% -------------------------------------------------------------------------------
% Instructions:
%
% The file result.txt should contain at least three columns:
% time, position of the trolley and angle of the pendulum.
% Run the file with MATLAB in order to generate a
% video visualizing your data.
% -------------------------------------------------------------------------------


clear

% path = inputdlg('Please enter the path: ')

Data = load('result.txt');   % reads the data from a file
T = Data(:, 1);                   % time series (first column)
x = Data(:, 2);                    % position x (second column)
angle = Data(:, 3);             % angle theta (third column)


% T = [0 0.42 0.7 1];
% x = [0 0.12 0.16 0.2];
% angle = [-0.2 -0.15 -0.05 0.15];


tq = 0: 10/1000: max(T);

x = interp1(T, x, tq);
angle = interp1(T, angle, tq);

%amplify the show effects of x
x = x*3*10;

N = size(tq, 2);
M = moviein(N);

for i = 1: N
clf
%% Cord
l = 3;
theta = angle(i);

%% Trolley
TROLLEY_WIDTH = 3;
TROLLEY_HEIGHT = 1.2;
WHEEL_DIAMETER = 0.5;
t_xpos = x(i);
t_ypos = 2 + WHEEL_DIAMETER;

%% Weight
WEIGHT_DIAMETER = 1;
w_xpos = t_xpos + TROLLEY_WIDTH/2 + l * sin(theta);
w_ypos = t_ypos + TROLLEY_HEIGHT + l * cos(theta);

%% Draw
rectangle('Position',[w_xpos - WEIGHT_DIAMETER/2, w_ypos - WEIGHT_DIAMETER/2, WEIGHT_DIAMETER, WEIGHT_DIAMETER],'Curvature',[1,1],'FaceColor','r','LineStyle','none')
rectangle('Position',[t_xpos, t_ypos, TROLLEY_WIDTH, TROLLEY_HEIGHT],'FaceColor','b','LineStyle','none')
rectangle('Position',[t_xpos + WHEEL_DIAMETER, 2, WHEEL_DIAMETER, WHEEL_DIAMETER],'FaceColor','k','Curvature',[1,1],'LineStyle','none')
rectangle('Position',[t_xpos + TROLLEY_WIDTH - 2*WHEEL_DIAMETER, 2, WHEEL_DIAMETER, WHEEL_DIAMETER],'FaceColor','k','Curvature',[1,1],'LineStyle','none')
line([ t_xpos + TROLLEY_WIDTH/2 w_xpos], [t_ypos + TROLLEY_HEIGHT w_ypos],'Color','r');
line([ -3 10 ], [ 2 2 ],'Color','k');
axis([-3, 10, 0, 10]);
M(:, i) = getframe;
end

%movie(M, 1);
movie2avi(M,'simulation.avi');