classdef Robot < handle
    properties
        i = 0;
        dt;
        vel = 0.1;
        r = 0.8;
        center = [0; 1.1]
        path_line;
        goal_line;
        robot_line;
        figure;
    end
    methods
        function obj = Robot(dt)
            obj.dt = dt;
            obj.figure = figure();
            hold on
            xlim([-1.5, 1.5])
            ylim([-1, 2])
        end
        function set_center(obj, x, y)
            obj.center = [x; y];
        end
        function th = theta(obj)
            th = obj.i * obj.dt * obj.vel / obj.r;
        end
        function p = goal(obj)
            p = obj.center + obj.r * [cos(obj.theta()); sin(obj.theta())];
        end
        function plot(obj, q)
            g = obj.goal();
            p = obj.fwd_kin(q);
            if isempty(obj.path_line)
                figure(obj.figure);
                obj.goal_line = plot(g(1), g(2), 'g');
                obj.path_line = plot(p(1,end), p(2,end), 'b');
                obj.robot_line = plot(p(1,:), p(2,:), 'r');
            else
                obj.goal_line.XData = cat(2, obj.goal_line.XData, g(1));
                obj.goal_line.YData = cat(2, obj.goal_line.YData, g(2));
                obj.robot_line.XData = p(1,:);
                obj.robot_line.YData = p(2,:);
                obj.path_line.XData = cat(2, obj.path_line.XData, p(1,end));
                obj.path_line.YData = cat(2, obj.path_line.YData, p(2,end));
            end
        end
        function p = fwd_kin(obj, q)
           p = [
               0, -sin(q(1)), - sin(q(1)) - (3*cos(q(1))*sin(q(2)))/4 - (3*cos(q(2))*sin(q(1)))/4, - sin(q(1)) - (3*cos(q(1))*sin(q(2)))/4 - (3*cos(q(2))*sin(q(1)))/4 - (cos(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))))/2 - (sin(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))))/2;
               0, cos(q(1)), cos(q(1)) + (3*cos(q(1))*cos(q(2)))/4 - (3*sin(q(1))*sin(q(2)))/4,   cos(q(1)) + (3*cos(q(1))*cos(q(2)))/4 - (3*sin(q(1))*sin(q(2)))/4 + (cos(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))))/2 - (sin(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))))/2
               ];
        end
        function p = pose(obj, q)
            f = obj.fwd_kin(q);
            p = f(:,end);
        end
        function J = jacobian(obj, q)
            J = [(3*sin(q(1))*sin(q(2)))/4 - (3*cos(q(1))*cos(q(2)))/4 - cos(q(1)) - (cos(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))))/2 + (sin(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))))/2,   (3*sin(q(1))*sin(q(2)))/4 - (3*cos(q(1))*cos(q(2)))/4 - (cos(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))))/2 + (sin(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))))/2,   (sin(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))))/2 - (cos(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))))/2;
                - sin(q(1)) - (3*cos(q(1))*sin(q(2)))/4 - (3*cos(q(2))*sin(q(1)))/4 - (cos(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))))/2 - (sin(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))))/2, - (3*cos(q(1))*sin(q(2)))/4 - (3*cos(q(2))*sin(q(1)))/4 - (cos(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))))/2 - (sin(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))))/2, - (cos(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))))/2 - (sin(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))))/2];
        end
        function inc(obj, sleep)
            obj.i = obj.i + 1;
            pause(sleep)
        end
        function len = length(obj)
            len = ceil(2 * pi * obj.r / (obj.vel * obj.dt) + 1);
        end
        function comp = complete(obj)
            comp = obj.theta() > 2 * pi;
        end
        function save(obj, format)
            exportgraphics(obj.figure, "step " + obj.i + "." + format);
        end
        function save_pdf(obj)
            obj.save("pdf")
        end
        function save_png(obj)
            obj.save("png")
        end
        function save_jpg(obj)
            obj.save("jpg")
        end
    end
end