function isValid = exampleHelperCollisionChecker(obstacles,L1,L2,M,W1,W2,bounds,state)
%exampleHelperCollisionChecker Checks state validity
%
%   Verifies that states are in the xy planning limits and checks whether
%   the truck and trailer are in collision with any obstacles using
%   Oriented Bounding Boxes (OBBs).

% Copyright 2021 The MathWorks Inc.

    % Extract collision body info
    truckBody   = collisionBox(L1,W1,1e-4);
    trailerBody = collisionBox(L2,W2,1e-4);
    truckGeom = truckBody.GeometryInternal;
    trailerGeom = trailerBody.GeometryInternal;
    pObs     = zeros(3,1);
    pTruck   = zeros(3,1);
    pTrailer = zeros(3,1);
    
    % Calculate full state
    stateFull = exampleHelperExpandToFullState(state,L2,M);
    
    % Calc untransformed vertices
    truckPts0 = [0 L1 L1 0; W1/2 W1/2 -W1/2 -W1/2];
    trailerPts0 = [0 L2 L2 0; W2/2 W2/2 -W2/2 -W2/2];
    
    % Allocate output
    isValid = true(size(state,1),1);
    
    for j = 1:size(state,1)
        % Calculate vehicle poses
        truckTheta = stateFull(j,3);
        trailerTheta = stateFull(j,6);
        pTruck(1:2)   = stateFull(j,1:2) + L1/2 * [cos(truckTheta) sin(truckTheta)];
        pTrailer(1:2) = stateFull(j,4:5) + L2/2 * [cos(trailerTheta) sin(trailerTheta)];
        qTruck        = eul2quat([truckTheta 0 0]);
        qTrailer      = eul2quat([trailerTheta 0 0]);
        
        % Calculate vertices
        cTh1 = cos(truckTheta); sTh1 = sin(truckTheta);
        cTh2 = cos(trailerTheta); sTh2 = sin(trailerTheta);
        R1 = [cTh1 -sTh1; sTh1 cTh1];
        R2 = [cTh2 -sTh2; sTh2 cTh2];
        pts = [(stateFull(j,1:2)' + R1*truckPts0) (stateFull(j,4:5)' + R2*trailerPts0)];
        
        % Validate states against bounds
        isValid(j) = ...
            bounds(1,4) <= state(j,4) && state(j,4) <= bounds(2,4) && ...   % Beta-check
            all(bounds(1,1) <= pts(1,:) & pts(1,:) <= bounds(2,1)) && ...   % X-check
            all(bounds(1,2) <= pts(2,:) & pts(2,:) <= bounds(2,2));         % Y-check
        
        if isValid(j)
            % Check truck and trailer for intersection with obstacles
            for i = 1:length(obstacles)
                obs = obstacles(i);
                obstacle = collisionBox(obs.Length,obs.Width,1e-4);
                obsGeom = obstacle.GeometryInternal;
                pObs(1:2) = obs.XY;
                qObs = eul2quat([obs.Theta 0 0]);

                if coder.target('MATLAB')
                    collisionStatus = checkCG(obsGeom,pTruck,qTruck,pTrailer,qTrailer,pObs,qObs);
                else
                    collisionStatus = checkML(obsGeom,pTruck,qTruck,pTrailer,qTrailer,pObs,qObs);
                end

                if collisionStatus
                    isValid(j) = false;
                    break;
                end
            end
        end
    end
    
    function collisionStatus = checkCG(obsGeom,pTruck,qTruck,pTrailer,qTrailer,pObs,qObs)
        collisionStatus = ...
                robotics.core.internal.intersect(truckGeom, pTruck, qTruck,...
                                                 obsGeom, pObs, qObs, 0);
        collisionStatus = collisionStatus || ...
                robotics.core.internal.intersect(trailerGeom, pTrailer, qTrailer,...
                                                 obsGeom, pObs, qObs, 0);
    end

    function collisionStatus = checkML(obsGeom,pTruck,qTruck,pTrailer,qTrailer,pObs,qObs)
        collisionStatus =...
            robotics.core.internal.coder.CollisionGeometryBuildable.checkCollision(truckGeom, pTruck, qTruck,...
                                                 obsGeom, pObs, qObs, 0);
        collisionStatus = collisionStatus || ...
            robotics.core.internal.coder.CollisionGeometryBuildable.checkCollision(trailerGeom, pTrailer, qTrailer,...
                                                 obsGeom, pObs, qObs, 0);
    end
end