% Copyright (C) 2016-2021 by Paul Gaborit
%
% This file may be distributed and/or modified
%
%   1. under the LaTeX Project Public License and/or
%
%   2. under the GNU Public License.

\usetikzlibrary{calc}

\RequirePackage{etoolbox}
\RequirePackage{etextools}
\RequirePackage{xfp}

\newbool{dubinspathreverse}

\def\ifpgfmathcond#1{%
  \pgfmathparse{(#1)?1:0}%
  \ifnumequal{\pgfmathresult}{1}%
}

\def\anglebetween#1#2#3{%macro, s, t
  \path let
  \p{s}=(#2), \p{t}=(#3),
  \n{angle}={atan2(\y{t}-\y{s},\x{t}-\x{s})}
  in \pgfextra {
    \pgfmathsetmacro#1{\n{angle}}
    \aftergroup@def#1
  };
}

\def\distancebetween#1#2#3{%macro, s, t
  \path let
  \p{s}=(#2), \p{t}=(#3),
  %\n{dist}={veclen(\x{t}-\x{s},\y{t}-\y{s}))}
  \n{dist}={\fpeval{sqrt((\x{t}-\x{s})*(\x{t}-\x{s})+(\y{t}-\y{s})*(\y{t}-\y{s}))}}
  in \pgfextra {
    %\typeout{n{dist}:\n{dist}}
    \pgfmathsetmacro#1{\n{dist}}
    \aftergroup@def#1
  };
}

\newcommand\dubinspath@rsr{% s, sa, t, ta, as, len, at, r
  let
  \p{tr}=([shift={(\dbp@angb-90:\dbp@radius pt)}]\dbp@b),
  \n1={\dbp@anga+90},
  \n2={\dbp@angb+90},
  \n3={\n2+\dbp@lastangle}
  in
  arc(\n1:\n1-\dbp@firstangle:\dbp@rradius pt)
  -- ([shift={(\p{tr})}]\n3:\dbp@rradius pt)
  arc(\n3:\n2:\dbp@rradius pt)
}

\newcommand\dubinspath@lsl{% s, sa, t, ta, as, len, at, r
  let
  \p{tl}=([shift={(\dbp@angb+90:\dbp@radius pt)}]\dbp@b),
  \n1={\dbp@anga-90},\n2={\n1+\dbp@firstangle},
  \n3={\dbp@angb-90},\n4={\n3-\dbp@lastangle}
  in
  arc(\n1:\n2:\dbp@lradius pt)
  -- ([shift={(\p{tl})}]\n4:\dbp@lradius pt)
  arc(\n4:\n3:\dbp@lradius pt)
}

\newcommand\dubinspath@rsl{% s, sa, t, ta, as, len, at, r
  let
  \p{tl}=([shift={(\dbp@angb+90:\dbp@radius pt)}]\dbp@b),
  \n1={\dbp@anga+90},\n2={\n1-\dbp@firstangle},
  \n3={\dbp@angb-90},\n4={\n3-\dbp@lastangle}
  in
  arc(\n1:\n2:\dbp@rradius pt)
  -- ([shift={(\p{tl})}]\n4:\dbp@lradius pt)
  arc(\n4:\n3:\dbp@lradius pt)
}

\newcommand\dubinspath@lsr{% s, sa, t, ta, as, len, at, r
  let
  \p{tr}=([shift={(\dbp@angb-90:\dbp@radius pt)}]\dbp@b),
  \n1={\dbp@anga-90},\n2={\n1+\dbp@firstangle},
  \n3={\dbp@angb+90},\n4={\n3+\dbp@lastangle}
  in
  arc(\n1:\n2:\dbp@lradius pt)
  -- ([shift={(\p{tr})}]\n4:\dbp@rradius pt)
  arc(\n4:\n3:\dbp@rradius pt)
}

\newcommand\dubinspath@lrl{% s, sa, t, ta, as, ai, at, r
  let
  \n1={\dbp@anga-90},\n2={\n1+\dbp@firstangle},
  \n3={\dbp@angb-90},\n4={\n3-\dbp@lastangle}
  in
  arc(\n1:\n2:\dbp@lradius pt)
  arc(\n2+180:\n2+180-\dbp@midparam:\dbp@rradius pt)
  arc(\n4:\n3:\dbp@lradius pt)
}

\newcommand\dubinspath@rlr{% s, sa, t, ta, as, ai, at, r
  let
  \n1={\dbp@anga+90},\n2={\n1-\dbp@firstangle},
  \n3={\dbp@angb+90},\n4={\n3+\dbp@lastangle}
  in
  arc(\n1:\n2:\dbp@rradius pt)
  arc(\n2+180:\n2+180+\dbp@midparam:\dbp@lradius pt)
  arc(\n4:\n3:\dbp@rradius pt)
}
\newcommand\dubinspath@rev@lsl{\dubinspath@rsr}
\newcommand\dubinspath@rev@rsr{\dubinspath@lsl}
\newcommand\dubinspath@rev@lsr{\dubinspath@lsr}
\newcommand\dubinspath@rev@rsl{\dubinspath@rsl}
\newcommand\dubinspath@rev@lrl{\dubinspath@rlr}
\newcommand\dubinspath@rev@rlr{\dubinspath@lrl}


\newcommand\dubinspath[1]{%
  \pgfextra{
    \dubinspathset{#1}
    \ifbool{dubinspathreverse}{
      \edef\newa{\dbp@b}
      \edef\newb{\dbp@a}
      \pgfmathsetmacro\newanga{180+\dbp@angb}
      \pgfmathsetmacro\newangb{180+\dbp@anga}
      \edef\newfirstangle{\dbp@lastangle}
      \edef\newlastangle{\dbp@firstangle}
      \edef\newmethod{rev@\dbp@method}
      \edef\newlradius{\dbp@rradius}
      \edef\newrradius{\dbp@lradius}
      \dubinspathset{
        start point=\newa,
        end point=\newb,
        start angle=\newanga,
        end angle=\newangb,
        first angle=\newfirstangle,
        last angle=\newlastangle,
        left and right minimum radii=\newlradius pt and \newrradius pt,
        method=\newmethod,
      }
    }{}
    %\typeout{dbp@lradius:\dbp@lradius}
    %\typeout{dbp@rradius:\dbp@rradius}
  }
  \csname dubinspath@\dbp@method\endcsname%
}


% solution
% \xdef\dubinspathmethod{}     % type of dubins path
% \xdef\dubinspathlength{}     % lenght (pt)
% \xdef\dubinspathfirstangle{} % first angle
% \xdef\dubinspathmiddleparam{} % intermediate angle or straight distance
% \xdef\dubinspathlastangle{}  % last angle

\def\dubinspathstore#1#2{%
  \expandafter\xdef\csname dubinspath@store@#1@#2\endcsname%
  {\csname dubinspath#2\endcsname}%
}
\def\dubinspathget#1#2{%
  \csname dubinspath@store@#1@#2\endcsname%
}

%
\def\dubinspathsetparams#1#2#3#4#5{% method, length, fisrt angle, middle param, last angle
  \edef\dubinspathmethod{#1}
  \edef\dubinspathlength{#2}
  \edef\dubinspathfirstangle{#3}
  \edef\dubinspathmiddleparam{#4}
  \edef\dubinspathlastangle{#5}
  \ifdef{\dbp@storename}{
    \foreach \p in {method,length,firstangle,middleparam,lastangle}{
      \dubinspathstore{\dbp@storename}{\p}
    }
  }{}
}

\def\dubinspathupdateparams#1#2#3#4#5{
  \ifpgfmathcond{#2<\dubinspathlength}{
    \dubinspathsetparams{#1}{#2}{#3}{#4}{#5}
  }{}
}

\tikzset{
  dubins path/.is family,
  dubins path,
  start point/.store in=\dbp@a,
  start angle/.store in=\dbp@anga,
  end point/.store in=\dbp@b,
  end angle/.store in=\dbp@angb,
  store/.store in=\dbp@storename,
  use store/.style={
    method=\dubinspathget{#1}{method},
    first angle=\dubinspathget{#1}{firstangle},
    last angle=\dubinspathget{#1}{lastangle},
    middle param=\dubinspathget{#1}{middleparam},
  },
  minimum radius/.code={
    \pgfmathsetmacro\dbp@radius{#1}
    \pgfmathsetmacro\dbp@rradius{#1}
    \pgfmathsetmacro\dbp@lradius{#1}
  },
  left and right minimum radii/.code args={#1 and #2}{
    \pgfmathsetmacro\dbp@lradius{#1}
    \pgfmathsetmacro\dbp@rradius{#2}
    \pgfmathsetmacro\dbp@radius{(\dbp@lradius + \dbp@rradius)/2}
  },
  method/.store in=\dbp@method,
  first angle/.store in=\dbp@firstangle,
  last angle/.store in=\dbp@lastangle,
  middle param/.store in=\dbp@midparam,
  reverse/.is if=dubinspathreverse,
}

\newcommand\dubinspathset[1]{\tikzset{dubins path,#1}}

\newcommand\dubinspathcalc[1]{%
  \begingroup
  \dubinspathset{#1}
  \tikzset{
    declare function={
      angtodist(\a,\r)={abs(\a)*.01745329*\r};
      modangr(\a,\b)={
        (Mod(\a,360)<Mod(\b,360)?Mod(\a,360):Mod(\a,360)-360)+\b-Mod(\b,360)
      };
      modangl(\a,\b)={
        (Mod(\a,360)<Mod(\b,360)?Mod(\a,360)+360:Mod(\a,360))+(\b)-Mod(\b,360)
      };
    },
  }
  
  \pgfmathsetmacro\radius{\dbp@rradius}
  \pgfmathsetmacro\anga{mod((\dbp@anga)+180,360)-180}
  \pgfmathsetmacro\angb{mod((\dbp@angb)+180,360)-180}
  %\typeout{radius:\radius,anga:\anga,angb:\angb}
  \path
  let
  \p{a}=(\dbp@a),
  \p{b}=(\dbp@b),
  \p{ar}=($(\p{a}) + (\anga-90:\radius pt)$),
  \p{al}=($(\p{a}) + (\anga+90:\radius pt)$),
  \p{br}=($(\p{b}) + (\angb-90:\radius pt)$),
  \p{bl}=($(\p{b}) + (\angb+90:\radius pt)$)
  in \pgfextra{
    \pgfinterruptpath

    % RSR (ar et br)
    %\typeout{RSR begin}
    \anglebetween\rsrarbr{\p{ar}}{\p{br}}
    \distancebetween\rsrdarbr{\p{ar}}{\p{br}}
    \pgfmathsetmacro\rsrangone{Mod(\anga-\rsrarbr,360)}
    \pgfmathsetmacro\rsrangtwo{Mod(\rsrarbr-\angb,360)}
    \pgfmathsetmacro\rsrlen{\rsrdarbr}
    \pgfmathsetmacro\rsrdist{
      angtodist(\rsrangone,\radius)
      +\rsrlen
      +angtodist(\rsrangtwo,\radius)
    }
    \dubinspathsetparams{rsr}{\rsrdist}{\rsrangone}{\rsrlen}{\rsrangtwo}
    
    % LSL (al et bl)
    %\typeout{LSL begin}
    \anglebetween\lslalbl{\p{al}}{\p{bl}}
    \distancebetween\lsldalbl{\p{al}}{\p{bl}}
    \pgfmathsetmacro{\lslangone}{mod(\lslalbl-\anga+720,360)}
    \pgfmathsetmacro{\lslangtwo}{mod(\angb-\lslalbl+720,360)}
    \pgfmathsetmacro\lsllen{\lsldalbl}
    \pgfmathsetmacro\lsldist{
      angtodist(\lslangone,\radius)
      +\lsllen
      +angtodist(\lslangtwo,\radius)
    }
    \dubinspathupdateparams{lsl}{\lsldist}{\lslangone}{\lsllen}{\lslangtwo}
    
    % RSL (ar et bl)
    %\typeout{RSL begin}
    \distancebetween\rsldarbl{\p{ar}}{\p{bl}}
    \pgfmathtruncatemacro\rslok{(\rsldarbl>=2*\radius)?1:0}
    \ifnumequal{\rslok}{1}{
      \anglebetween\rslarbl{\p{ar}}{\p{bl}}
      \pgfmathsetmacro\rslanglesup{
        asin(\radius/\rsldarbl*2)}
      \pgfmathsetmacro\rslangone{Mod(\anga-\rslarbl+\rslanglesup,360)}
      \pgfmathsetmacro\rslangtwo{Mod(\angb-\rslarbl+\rslanglesup,360)}
      \pgfmathsetmacro\rsllen{veclen(\rsldarbl,\radius)}
      \pgfmathsetmacro\rsldist{
        angtodist(\rslangone,\radius)
        +\rsllen
        +angtodist(\rslangtwo,\radius)
      }

      \dubinspathupdateparams{rsl}{\rsldist}{\rslangone}{\rsllen}{\rslangtwo}
    }{}

    % LSR (al et br)
    %\typeout{LSR begin}
    \distancebetween\lsrdalbr{\p{al}}{\p{br}}
    \pgfmathtruncatemacro\lsrok{(\lsrdalbr>=2*\radius)?1:0}
    \ifnumequal{\lsrok}{1}{
      \anglebetween\lsralbr{\p{al}}{\p{br}}
      \pgfmathsetmacro\lsranglesup{
        asin(\radius/\lsrdalbr*2)}
      \pgfmathsetmacro\lsrangone{Mod(\lsralbr+\lsranglesup-\anga,360)}
      \pgfmathsetmacro\lsrangtwo{Mod(\lsralbr+\lsranglesup-\angb,360)}
      \pgfmathsetmacro\lsrlen{veclen(\lsrdalbr,\radius)}
      \pgfmathsetmacro\lsrdist{
        angtodist(\lsrangone,\radius)
        +\lsrlen
        +angtodist(\lsrangtwo,\radius)
      }
      \dubinspathupdateparams{lsr}{\lsrdist}{\lsrangone}{\lsrlen}{\lsrangtwo}
    }{}
    
    % LRL (al et bl)
    %\typeout{LRL begin}
    \distancebetween\lrldalbl{\p{al}}{\p{bl}}
    \pgfmathtruncatemacro\lrlok{(\lrldalbl<=4*\radius)?1:0}
    \ifnumequal{\lrlok}{1}{
      % \debug{LRL}
      \anglebetween\lrlalbl{\p{al}}{\p{bl}}
      \pgfmathsetmacro\lrlangsup{acos(\lrldalbl/\radius/4)}
      \pgfmathsetmacro\lrlangone{
        modangl(\lrlalbl+\lrlangsup,\anga-90)-(\anga-90)}
      \pgfmathsetmacro\lrlangtwo{%
        (\angb-90)-modangr(\lrlalbl+180-\lrlangsup,\angb-90)}
      \pgfmathsetmacro\lrlangthree{360-2*(90-\lrlangsup)}
      \pgfmathsetmacro\lrldist{
        angtodist(\lrlangone,\radius)
        +angtodist(\lrlangthree,\radius)
        +angtodist(\lrlangtwo,\radius)
      }
      \dubinspathupdateparams{lrl}{\lrldist}{\lrlangone}{\lrlangthree}{\lrlangtwo}
    }{}

    % RLR (ar et br)
    %\typeout{RLR begin}
    \distancebetween\rlrdarbr{\p{ar}}{\p{br}}
    \pgfmathtruncatemacro\rlrok{(\rlrdarbr<=4*\radius)?1:0}
    \ifnumequal{\rlrok}{1}{
      \anglebetween\rlrarbr{\p{ar}}{\p{br}}
      \pgfmathsetmacro\rlrangsup{acos(\rlrdarbr/\radius/4)}
      \pgfmathsetmacro\rlrangone{
        (\anga+90)-modangr(\rlrarbr-\rlrangsup,\anga+90)}
      \pgfmathsetmacro\rlrangtwo{%
        modangl(\rlrarbr+180+\rlrangsup,\angb+90)-(\angb+90)}
      \pgfmathsetmacro\rlrangthree{360-2*(90-\rlrangsup)}
      \pgfmathsetmacro\rlrdist{
        angtodist(\rlrangone,\radius)
        +angtodist(\rlrangthree,\radius)
        +angtodist(\rlrangtwo,\radius)
      }
      \dubinspathupdateparams{rlr}{\rlrdist}{\rlrangone}{\rlrangthree}{\rlrangtwo}
    }{}

    \endpgfinterruptpath
  };
  \endgroup
}

%%% Local Variables: 
%%% mode: latex
%%% TeX-master: t
%%% End: 
