%%
sampleTime = 0.05;
initPose = [path(1,:)'; 0];
tVec = 0:sampleTime:100;

% rower jednokolowy
unicycle = unicycleKinematics("VehicleInputs","VehicleSpeedHeadingRate");

% naped roznicowy
diffDrive = differentialDriveKinematics("VehicleInputs","VehicleSpeedHeadingRate");
diffDrive.WheelSpeedRange = [-10 10]*2*pi;

% rower bicykl
bicycle = bicycleKinematics("VehicleInputs","VehicleSpeedHeadingRate","MaxSteeringAngle",pi/8);

% controller
controller3 = controllerPurePursuit("Waypoints",path,"DesiredLinearVelocity",4,"MaxAngularVelocity",2*pi);
controller2 = controllerPurePursuit("Waypoints",path,"DesiredLinearVelocity",4,"MaxAngularVelocity",2*pi);
controller1 = controllerPurePursuit("Waypoints",path,"DesiredLinearVelocity",4,"MaxAngularVelocity",2*pi);

% trajektorie
[tUnicycle,unicyclePose] = ode45(@(t,y)derivative(unicycle,y,exampleHelperMobileRobotController(controller1,y,path(end,:)',1)),tVec,initPose);
[tDiffDrive,diffDrivePose] = ode45(@(t,y)derivative(diffDrive,y,exampleHelperMobileRobotController(controller2,y,path(end,:)',1)),tVec,initPose);
[tBicycle,bicyclePose] = ode45(@(t,y)derivative(bicycle,y,exampleHelperMobileRobotController(controller3,y,path(end,:)',1)),tVec,initPose);

%wyniki
unicycleTranslations = [unicyclePose(:,1:2),zeros(length(unicyclePose),1)];
unicycleRot = axang2quat([repmat([0 0 1],length(unicyclePose),1),unicyclePose(:,3)]);

diffDriveTranslations = [diffDrivePose(:,1:2),zeros(length(diffDrivePose),1)];
diffDriveRot = axang2quat([repmat([0 0 1],length(diffDrivePose),1),diffDrivePose(:,3)]);

bicycleTranslations = [bicyclePose(:,1:2) zeros(length(bicyclePose),1)];
bicycleRot = axang2quat([repmat([0 0 1],length(bicyclePose),1),bicyclePose(:,3)]);

path
show(prm)
hold on
plotTransforms(unicycleTranslations(1:10:end,:),unicycleRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","r");
plotTransforms(bicycleTranslations(1:10:end,:),bicycleRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","b");
plotTransforms(diffDriveTranslations(1:10:end,:),diffDriveRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","g");
view(0,90)