classdef Robot < handle
  properties
    dt;
    i = 0;
    fig;
    pdf;
    robot;
    est;
    door_y_scale = 0.5;
    locations = [-2 -1.9024085006623 -1.8071593902147 -1.71422899328104 -1.62359363448517 -1.53522963845092 -1.44911332980213 -1.36522103316266 -1.28352907315634 -1.20401377440701 -1.12665146153853 -1.05141845917472 -0.978291091939431 -0.907245684456508 -0.838258561349793 -0.771306047243127 -0.706364466760352 -0.643410144525311 -0.582419405161847 -0.523368573293801 -0.466233973545016 -0.410991930539335 -0.3576187689006 -0.306090813252652 -0.256384388219336 -0.208475818424492 -0.162341428491964 -0.117957543045593 -0.0753004867092233 -0.0343465841066959 0.00492784013814695 0.0425464614014626 0.0785329550594085 0.112910996488142 0.145704261063822 0.176936424162604 0.206631161160647 0.234812147434109 0.261503058359146 0.286727569311918 0.310509355668581 0.332872092805292 0.35383945609821 0.373435120923492 0.391682762657296 0.408606056675779 0.424228678355099 0.438574303071414 0.451666606200881 0.463529263119657 0.474185949203902 0.483660339829771 0.491976110373423 0.499156936211016 0.505226492718705 0.510208455272651 0.51412649924901 0.517004300023939 0.518865532973597 0.519733873474141 0.519632996901728 0.518586578632517 0.516618294042665 0.513751818508329 0.510010827405667 0.505418996110836 0.499999999999995 0.493777514449302 0.486775214834912 0.479016776532985 0.470525874919676 0.461326185371147 0.451441383263552 0.440895143973049 0.429711142875797 0.417913055347953 0.405524556765673 0.392569322505118 0.379071027942443 0.365053348453806 0.350539959415365 0.335554536203277 0.320120754193702 0.304262288762794 0.288002815286714 0.271366009141618 0.254375545703662 0.237055100349005 0.219428348453807 0.201518965394223 0.18335062654641 0.164947007286528 0.146331782990731 0.127528629035182 0.108561220796034 0.0894532336494471 0.070228342971578 0.0509102241385846 0.0315225525266216 0.0120890035118513 -0.00736674752957045 -0.0268210252214867 -0.0462501541877396 -0.065630459052171 -0.0849382644386243 -0.104149894970941 -0.123241675272963 -0.142189929968533 -0.160970983681496 -0.179561161035689 -0.197936786654961 -0.216074185163148 -0.233949681184097 -0.251539599341646 -0.268820264259644 -0.285768000561927 -0.30235913287234 -0.318569985814726 -0.334376884012926 -0.349756152090785 -0.36468411467214 -0.379137096380838 -0.39309142184072 -0.406523415675631 -0.419409402509409 -0.431725706965896 -0.443448653668939 -0.454554567242379 -0.465019772310057 -0.474820593495813 -0.483933355423496 -0.492334382716942 -0.499999999999996 -0.506906531896503 -0.513030303030299 -0.518347638025233 -0.522834861505144 -0.526468298093872 -0.529224272415265 -0.531079109093161 -0.532009132751406 -0.53199066801384 -0.531000039504305 -0.529013571846644 -0.526007589664701 -0.521958417582314 -0.516842380223333 -0.510635802211592 -0.503315008170937 -0.494856322725212 -0.485236070498257 -0.474430576113915 -0.46241616419603 -0.44916915936844 -0.434665886254993 -0.41888266947953 -0.401795833665889 -0.383381703437918 -0.363616603419456 -0.342476858234346 -0.319938792506429 -0.295978730859552 -0.270572997917553 -0.243697918304276 -0.215329816643562 -0.185445017559257 -0.1540198456752 -0.121030625615233 -0.0864536820032029 -0.0502653394629451 -0.0124419226183079 0.0270402439068693 0.0682048354887437 0.11107552750347 0.15567599532721 0.202029914336119 0.250160959906357 0.300092807414077 0.35184913223544 0.405453609746607 0.460929915323727 0.518301724342966 0.577592712180475 0.638826554212412 0.702026925814942 0.767217502364217 0.834421959236394 0.903663971807633 0.974967215454091 1.04835536555193 1.12385209747729 1.20148108660635 1.28126600831526 1.36323053798017 1.44739835097725 1.53379312268265 1.62243852847253 1.71335824372305 1.80657594381037 1.90211530411063 2];
    dx = [0.0975914993376978 0.0952491104475999 0.092930396933659 0.0906353587958761 0.0883639960342508 0.0861163086487831 0.0838922966394731 0.0816919600063204 0.0795152987493255 0.0773623128684884 0.0752330023638088 0.0731273672352868 0.0710454074829223 0.0689871231067154 0.0669525141066662 0.0649415804827749 0.0629543222350408 0.0609907393634643 0.0590508318680458 0.0571345997487849 0.055242043005681 0.0533731616387354 0.0515279556479473 0.0497064250333163 0.0479085697948436 0.0461343899325284 0.0443838854463705 0.0426570563363702 0.0409539026025274 0.0392744242448428 0.0376186212633156 0.0359864936579459 0.0343780414287336 0.0327932645756794 0.0312321630987826 0.0296947369980431 0.0281809862734614 0.0266909109250375 0.0252245109527718 0.0237817863566629 0.0223627371367114 0.0209673632929177 0.0195956648252817 0.018247641733804 0.0169232940184831 0.0156226216793205 0.0143456247163147 0.0130923031294667 0.0118626569187765 0.0106566860842445 0.00947439062586897 0.00831577054365207 0.0071808258375925 0.00606955650768937 0.00498196255394623 0.00391804397635864 0.00287780077492927 0.00186123294965768 0.000868340500544296 -0.000100876572412645 -0.00104641826921137 -0.00196828458985232 -0.00286647553433594 -0.00374099110266179 -0.00459183129483076 -0.00541899611084107 -0.0062224855506936 -0.0070022996143897 -0.00775843830192668 -0.00849090161330901 -0.00919968954852957 -0.00988480210759501 -0.0105462392905022 -0.0111840010972521 -0.0117980875278443 -0.0123884985822795 -0.0129552342605552 -0.0134982945626754 -0.0140176794886364 -0.014513389038441 -0.0149854232120883 -0.0154337820095751 -0.0158584654309077 -0.0162594734760799 -0.0166368061450965 -0.0169904634379554 -0.0173204453546569 -0.0176267518951985 -0.017909383059584 -0.0181683388478127 -0.0184036192598822 -0.0186152242957967 -0.0188031539555498 -0.0189674082391473 -0.0191079871465871 -0.0192248906778691 -0.0193181188329934 -0.019387671611963 -0.0194335490147703 -0.0194557510414217 -0.0194542776919162 -0.0194291289662529 -0.0193803048644314 -0.0193078053864533 -0.0192116305323167 -0.0190917803020216 -0.0189482546955702 -0.0187810537129633 -0.0185901773541932 -0.0183756256192713 -0.0181373985081876 -0.0178754960209486 -0.0175899181575494 -0.0172806649179973 -0.0169477363022832 -0.0165911323104135 -0.0162108529423861 -0.0158068981981998 -0.0153792680778584 -0.0149279625813552 -0.0144529817086985 -0.013954325459882 -0.0134319938349103 -0.0128859868337785 -0.0123163044564871 -0.0117229467030424 -0.0111059135734402 -0.0104652050676781 -0.00980082118575587 -0.00911276192768362 -0.00840102729344561 -0.00766561728305359 -0.00690653189650736 -0.00612377113379559 -0.00531733499493403 -0.0044872234799116 -0.00363343658872806 -0.00275597432139252 -0.0018548366778961 -0.00093002365824546 1.84647375662816e-05 0.000990628509534686 0.00198646765766131 0.00300598218194281 0.0040491720823872 0.00511603735898114 0.00620657801174085 0.00732079404065544 0.00845868544572492 0.00962025222695462 0.0108054943843421 0.0120144119178847 0.0132470048275899 0.0145032731134473 0.0157832167754628 0.0170868358136409 0.0184141302279712 0.0197651000184622 0.0211397451851101 0.0225380657279168 0.0239600616468771 0.0254057329419988 0.0268750796132773 0.0283681016607142 0.029884799084305 0.0314251718840568 0.0329892200599664 0.0345769436120305 0.0361883425402578 0.0378234168446372 0.0394821665251772 0.0411645915818744 0.0428706920147266 0.0446004678237393 0.0463539190089097 0.0481310455702375 0.0499318475077204 0.0517563248213633 0.053604477511167 0.0554763055771197 0.0573718090192386 0.059290987837509 0.0612338420319372 0.0632003716025298 0.0651905765492757 0.0672044568721768 0.0692420125712387 0.071303243646458 0.0733881500978351 0.0754967319253632 0.077628989129062 0.0797849217089088 0.0819645296649125 0.084167812997078 0.0863947717054043 0.0886454057898778 0.0909197152505192 0.0932177000873131 0.0955393603002626 0.0978846958893755];
    data_length = 200;
    door_locations = [-1.5 -0.5 1.5];
    detected = [false false false false false true false false false false false false false false false false false false false true true false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false true true true true true true true true true true true true true true true true true true true true true true true true true true false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false false true false false false false];
    doors = [-1.65 -1.65 -1.35 -1.35 NaN -1.3725 -1.37600933335322 -1.384895277335 -1.395 -1.40159538931179 -1.40159538931179 -1.395 -1.384895277335 -1.37600933335322 -1.3725 NaN -0.65 -0.65 -0.35 -0.35 NaN -0.3725 -0.376009333353215 -0.384895277334996 -0.395 -0.401595389311789 -0.401595389311789 -0.395 -0.384895277334996 -0.376009333353215 -0.3725 NaN 1.35 1.35 1.65 1.65 NaN 1.6275 1.62399066664678 1.615104722665 1.605 1.59840461068821 1.59840461068821 1.605 1.615104722665 1.62399066664678 1.6275 NaN;0 0.6 0.6 0 NaN 0.3 0.309641814145298 0.314772116295183 0.312990381056767 0.305130302149885 0.294869697850115 0.287009618943233 0.285227883704817 0.290358185854702 0.3 NaN 0 0.6 0.6 0 NaN 0.3 0.309641814145298 0.314772116295183 0.312990381056767 0.305130302149885 0.294869697850115 0.287009618943233 0.285227883704817 0.290358185854702 0.3 NaN 0 0.6 0.6 0 NaN 0.3 0.309641814145298 0.314772116295183 0.312990381056767 0.305130302149885 0.294869697850115 0.287009618943233 0.285227883704817 0.290358185854702 0.3 NaN];
    limits = [-2 2];
  end
  methods
    function obj = Robot(dt)
      obj.dt = dt;
      obj.fig = figure();
      hold on
      xlim(obj.limits)
      ylim([0, 0.4])
      plot(obj.doors(1,:), obj.door_y_scale * obj.doors(2,:), 'k')
      pbaspect([4 1 1])
      obj.pdf = plot(nan, nan, 'r.');
      obj.robot = plot([obj.locations(1)], [0.02], 's', 'MarkerSize', 10, 'MarkerFaceColor', 'blue', 'MarkerEdgeColor', 'blue');
      obj.est = plot([nan], [0.02], 's', 'MarkerSize', 10, 'MarkerFaceColor', 'green', 'MarkerEdgeColor', 'green');
    end
    function [detected, dx] = step(obj, x, w, est)
      obj.pdf.YData = w;
      obj.pdf.XData = x;
      obj.robot.XData = [obj.locations(obj.i + 1)];
      obj.est.XData = [est];
      obj.i = obj.i + 1;
      if obj.i <= length(obj.detected)
        detected = obj.detected(obj.i);
        dx = obj.dx(obj.i);
      else
        detected = false;
        dx = 0;
      end
      pause(obj.dt)
    end
    function save(obj, format)
        exportgraphics(obj.fig, "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
    function p = prob(obj, x, measurement_sigma)
        p = zeros(size(x));
        for location = obj.door_locations
            p = p + 1 / (measurement_sigma * sqrt(2 * pi)) * exp(-(x - location).^2 / (2 * measurement_sigma^2));
        end
        p = p / 3;
    end
    function [xnew, wnew] = resample(~, x, w)
        xnew = zeros(size(x));
        wnew = zeros(size(w));
        for i = 1:length(w)
            val = sum(w) * rand();
            ind = sum(cumsum(w) < val) + 1;
            xnew(:,i) = x(:,ind);
            wnew(i) = w(ind);
        end
    end
  end
end