%%%%%%%%%%%%%%
% go.tex

% Code to try to get us somewhere...

% Directions/speeds

\def\R{R}\def\r{r}\def\L{L}\def\l{l}
\def\-{-}\def\a{a}\def\b{b}

\def\namedef#1{\expandafter\def\csname#1\endcsname}

\def\turn@RR{}
\def\turn@Rr{l}
\namedef{turn@R-}{ll}
\def\turn@Rl{lll}
\def\turn@RL{llll}
\def\turn@rR{r}
\def\turn@rr{}
\namedef{turn@r-}{l}
\def\turn@rl{ll}
\def\turn@rL{lll}
\namedef{turn@-R}{rr}
\namedef{turn@-r}{r}
\namedef{turn@--}{}
\namedef{turn@-l}{l}
\namedef{turn@-L}{ll}
\def\turn@lR{rrr}
\def\turn@lr{rr}
\namedef{turn@l-}{r}
\def\turn@ll{}
\def\turn@lL{l}
\def\turn@LR{rrrr}
\def\turn@Lr{rrr}
\namedef{turn@L-}{rr}
\def\turn@Ll{r}
\def\turn@LL{}

\def\acc@ab{bb}
\namedef{acc@a-}{b}
\def\acc@aa{}
\namedef{acc@-a}{a}
\namedef{acc@--}{}
\namedef{acc@-b}{b}
\def\acc@bb{}
\namedef{acc@b-}{a}
\def\acc@ba{aa}

% Math code

% if all this slows us too much, we'll limit the
% time between recomputations...

\newdimen\lA\newdimen\lB
\newdimen\dirx \newdimen\diry % projection of dir
\newdimen\dirtheta
\newdimen\distance % this one is tough w/o overflow

\newdimen\ct\newdimen\st  % scratch

\newdimen\obs@x\newdimen\obs@y\newdimen\obs@r
\def\xyr@assign(#1,#2,#3){%
  \obs@x=#1\p@ \advance\obs@x-\vx
  \obs@y=#2\p@ \advance\obs@y-\vy
  \obs@r=#3\relax % pt isn't missing here!
  %\multiply\obs@r\obsfactor
}
% Note: the line is always the same!  pre-process!!!
% ALSO.... this doesn't check front/back...!
%   so ---|-----x----y---- => ``|'' obstructs
\def\precomputeline{%
  \lA=\ty \advance\lA-\vy
  \lB=\vx \advance\lB-\tx
  % first make @=sqrt(A^2+B^2) denominator
  \pythag\lA\lB\dimen@ % could overflow?!?
  \div\lA\dimen@ \div\lB\dimen@
  % Now r=Ax+By (+C, but C=0 b/c we moved origin!)
  \dirx-\lB \diry\lA
  % compute distance R = x/(x/R)
  \distance\tx \advance\distance-\vx
  \div\distance\dirx
  \atan\diry\dirx\dirtheta
}

\def\pointdir{% pretends like tx,ty is where we're pointing
  \distance\maxspeed % how far ahead to look...
  \div\distance\maxhardturn
  \dimen@90\p@\distance=\mul\distance\dimen@
  %
  % now pretend vx=vy=0 is origin FROM NOW ON!
  \ct=\vdir \docos\ct   \st=\vdir \dosin\st
  \tx=\mul\distance\ct  \ty=\mul\distance\st
  \advance\tx\vx        \advance\ty\vy
}

\def\obstructs#1{%
  \xyr@assign#1%
  \def\theobj{#1}% save it if it's the one...
  \obs@dircheck
}
\def\obs@dircheck{%
  % 1. right direction...?
  \dimen@\mul\obs@x\dirx
  \advance\dimen@\mul\obs@y\diry
  \ifdim\dimen@>\z@
    \expandafter\obs@linecheck
  \fi
}
\def\obs@linecheck{%
  \dimen@ = \mul\lA\obs@x
  \advance\dimen@ \mul\lB\obs@y
  % now tempdima = distance
  \ifdim\AbsValD\dimen@<\obs@r
    % obstructs - suggest a new way?
    \expandafter\obs@distancecheck
  \else\ifdim\AbsValD\dimen@<3\obs@r
    \obs@slowcheck
  \fi\fi
}
\def\obs@distancecheck{%
  \pythag\obs@x\obs@y\dimen@
  \advance\dimen@-\obs@r
  \ifdim\dimen@<\braketime\maxspeed
    \advance\slowcount\tw@ % slow us down...
  \fi
  \ifdim\dimen@<\distance
    \expandafter\obs@obstruct
  \fi
}
\def\obs@slowcheck{%
  \pythag\obs@x\obs@y\dimen@
  \advance\dimen@-\obs@r
  \ifdim\dimen@<\braketime\maxspeed
    \advance\slowcount\@ne % slow us down a little...
  \fi
}  

\def\obs@obstruct{%
  \glet\obs@obj\theobj
  \debug1{obstruction}
  %\debug1{Obstruction: from \the\vx,\the\vy to \the\tx,\the\ty (\the\distance)}%
  %\debug1{at (\the\obs@x,\the\obs@y,\the\obs@r) at distance \the\dimen@ < \the\distance}
  %\debug1{\space\space x0=\pointless\vx; y0=\pointless\vy;}
  %\debug1{\space\space xo=\pointless\obs@x; yo=\pointless\obs@y;}
  %\debug1{\space\space r=\pointless\obs@r;}
  \distance\dimen@
  \dimen@\mul\dirx\obs@x \advance\dimen@\mul\diry\obs@y
  % \dimen@=dot product
  \@tempdima-\obs@x \advance\@tempdima\mul\dimen@\dirx
  \@tempdimb-\obs@y \advance\@tempdimb\mul\dimen@\diry
  % now a,b = -(object vector perp to path) -> normalize
  \unitvector\@tempdima\@tempdimb\dimen@
  % figure out which direction we are... (for other methods)
  \dimen@\mul\vy\@tempdima \dimen@-\dimen@
  \advance\dimen@\mul\vx\@tempdimb
  \ifdim\dimen@<\z@\glet\obsturn\L\else\glet\obsturn\R\fi  
  % Now chart a course
  \obs@r=\swerve\obs@r % destroy...
  \tx\vx \ty\vy % note: obs@ is relative to v; a,b to obs
  \advance\tx\obs@x \global\advance\tx \mul\obs@r\@tempdima
  \advance\ty\obs@y \global\advance\ty \mul\obs@r\@tempdimb
  %\debug1{\space\space x1=\pointless\tx; y1=\pointless\ty;}
  %\debug1{\space\space plot([x0 xt],[y0 yt],'b-',x0+xo+r*ct}
  %\debug1{\space\space\space\space ,y0+yo+r*st,'r-',[x0 0],[y0 0],'g-')}
  %\debug1{New target: \the\tx,\the\ty (\the\distance)}
  %\debug1{Impact parameter of old path: \the\dimen@}
  %\debug1{================================================}
  \@obstructedtrue % obstruction.
}
% In reality, this ``new way'' is relative to the LINE to
% the target, NOT the direction we're going...

\def\check@obstructed{%
  \slowcount\tw@
  \@obstructedfalse
  \pointdir
  \precomputeline
  \let\\\obstructs
  \the\boulders
  \the\craters
  %\if@obstructed\else\debug1{No obstruction}\fi
  %\the\martians
  % Check if home is in front - then not obstructed!
  \@testfalse
  \if@obstructed\@testtrue
    \@obstructedfalse
    \obstructs{(0,0,5\p@)}% =\homerad
    \if@obstructed\@testfalse\fi
  \fi
  \if@test\@obstructedtrue\else\@obstructedfalse\fi
}

\def\turntowardstarget{%
  \lA = \tx \advance\lA-\vx
  \lB = \ty \advance\lB-\vy
  % Let's actually look for the origin!
  \lA=-\vx \lB=-\vy
  \atan\lB\lA\dimen@
  \whichway{\the\dimen@}{\the\vdir}%
}

%%%%%%%%%%%%% all new brain

\newif\if@swerving
\newcount\swerve@end
\newcount\turn@end

\def\decide{%
  \if@swerving
    \do@swerve
  \else
    \expandafter\do@run
  \fi
}

\def\do@run{%
  \gdef\obsfactor{1}
  % clearly I wanted to put something here...?
  \check@obstructed
  \if@obstructed
    \startswerve
  \else
    \global\tx\z@ \global\ty\z@
    \expandafter\turntowardstarget
  \fi
}

\def\do@swerve{\if@talemdata % only do this when we're sure...?
  \ifx\turnstate\- % we're done turning - now pass the obstacle
    % copied from \obs@dircheck
    \xa\xyr@assign\obs@obj
    \debug1{obs@x=\the\obs@x, obs@y=\the\obs@y, dirx=\the\dirx, diry=\the\diry}%
    \dimen@\mul\obs@x\dirx
    \advance\dimen@\mul\obs@y\diry
    \ifdim\dimen@<\z@ % clear!
      \global\@swervingfalse
      \debug1{Ending swerve}%
      \expandafter\do@run
    \fi
    % Also, check if we're about to hit anything
    % (we don't want to run completely blind!)
    \def\obsfactor{1}%
    \check@obstructed
    \if@obstructed\expandafter\startswerve\fi
  \else % still turning, wait till obstacle is out of the way
    \def\obsfactor{1.2}%
    \@obstructedfalse
    \pointdir
    \precomputeline
    \xa\obstructs\xa{\obs@obj}%
    \if@obstructed\else
      \glet\wantturn\-
      \getwhatiwant
    \fi
  \fi
\fi}

\def\startswerve{%
  \debug1{Starting swerve: \the\obs@r\space to the \obsturn: obj @ \the\distance}%
  \global\@swervingtrue
  \glet\wantturn\obsturn
  %
  % Figure out how long to turn for
  %\atan\obs@r\distance\dimen@ % theta to turn
  %\div\dimen@\maxhardturn     % time to spend
  %\xa\count@\pointless\dimen@
  %\advance\count@20% for good measure
  %
  % figure out tspeed based on this!
  %\tspeed\distance \divide\tspeed\count@
  %\global\multiply\tspeed1000
  % Double the original slowcount speed...:
  \tspeed=4\maxspeed \divide\tspeed\slowcount
  \debug1{want speed \the\tspeed}%
  %
  \getwhatiwant
}



% A better idea:
% 1. commit to a single path
% 2. if an obstruction arises while heading on it, then
%    try the main path again
% Maybe just set up a FINITE STATE MACHINE
%

\def\whichway#1#2{%
  % want to point #1, am pointing #2, which way to turn?
  \debug2{Want to point #1, think am pointing #2}%
  \begingroup
  \dimen@#1\relax\advance\dimen@-#2\relax
  \normalizeangle\dimen@
  %\global\@acceltrue\global\@brakefalse
  \glet\wantacc\a % new version...
  \glet\wantturn\-
  \ifdim\dimen@>\z@ % changed from 8,40...
    \ifdim\dimen@>1\p@ \glet\wantturn\l \fi
    %\ifdim\dimen@>15\p@ \glet\wantacc\- \fi
    \ifdim\dimen@>10\p@ \glet\wantturn\L \fi
    %\ifdim\dimen@>90\p@ \glet\wantacc\b \fi
    \global\@golefttrue
  \else
    \ifdim\dimen@<-1\p@ \glet\wantturn\r \fi
    %\ifdim\dimen@<-15\p@ \glet\wantacc\- \fi
    \ifdim\dimen@<-10\p@ \glet\wantturn\R \fi
    %\ifdim\dimen@<-90\p@ \glet\wantacc\b \fi
    \global\@gorighttrue
  \fi
  % No compute speed: want v=(vmax)*cos(theta) (th=d@)
  \docos\dimen@
  \global\tspeed=\mul\dimen@\maxspeed
  % Don't slow down for crowding on runs...
  %\multiply\tspeed\tw@ \divide\tspeed\slowcount
  \debug3{Want \wantacc\wantturn}%
  \endgroup
  %
  \if@obstructed % if the obstruction is too close
                 % then hit the brakes!
    \dimen@\collrate\distance
    \xa\global\xa\tspeed\MaxD\tspeed\dimen@ % try this?
    %\xa\dimen@\MaxD\tspeed\vspeed
    %\dimen@=\brakethresh\dimen@ % where will we be?
    %\ifdim\dimen@>\distance % uh oh
    %  \tspeed-\maxspeed % !!!
    %\fi
  \fi   
  %
  \getwhatiwant
}

\def\getwhatiwant{\whichacc\whichturn\sendturns}

\def\whichacc{%
  % schmitt trigger: tolerance=10% maxspeed
  \dimen@=\tspeed\advance\dimen@-\vspeed
  \glet\wantacc\-
  \ifdim\dimen@>.1\maxspeed \glet\wantacc\a \fi
  \ifdim\dimen@<-.1\maxspeed \glet\wantacc\b \fi
  \edef\acc{\csname acc@\accstate\wantacc\endcsname}%
  %\xa\ifx\acc\relax\let\acc\@empty\fi % signals...?
}

\def\whichturn{%
  \ifx\wantturn\@empty\debug1{whichturn: No turn wanted!}\fi
  \edef\turn{\csname turn@\turnstate\wantturn\endcsname}%
  %\xa\ifx\turn\relax\let\turn\@empty\fi % prevent bad
}

\def\fst#1#2:#3#4{\def#3{#1}\gdef#4{#2}}
\def\sendturns{%
  \debug3{Sending commands [\acc\turn]}%
  \@testtrue
  \def\msg{}%
  \@whilesw\if@test\fi{%
    \@testfalse
    \ifx\turn\@empty\def\thisturn{}\else
      \expandafter\fst\turn:\thisturn\turn
      \@testtrue
    \fi
    \ifx\acc\@empty\def\thisacc{}\else
      \expandafter\fst\acc:\thisacc\acc
      \@testtrue
    \fi
    \edef\thiscom{\thisacc\thisturn}%
    \ifx\thiscom\@empty\else
      \edef\msg{\msg\thiscom;}%
    \fi
  }%
  \ifx\msg\@empty\else\send{\msg}\fi % send all at once
  % ignore changing times, latency, ...
  \debug2{changing state to \wantacc\wantturn}%
  \glet\accstate\wantacc\glet\turnstate\wantturn
}

\def\processtelemetry{%
  \if@accel\if@brake
    \global\@accelfalse\global\@brakefalse
  \fi\fi
  %\if@goleft\if@goright
  %  \global\@goleftfalse\global\@gorightfalse
  %\fi\fi
  \iflatex % draw position, but skip some frames...
    %\debug1{PROCESS TELEM: \the\skippedframes, \the\frameskip}%
    %\ifnum\skippedframes>\frameskip
      \drawtelemetry{blue}\global\skippedframes\z@
    %\else
    %  \advance\skippedframes\@ne
    %\fi
  \fi
  \decide
}

\def\turninfo@l{%
  \@turninglefttrue\@turningrightfalse
  \@turningsofttrue\@turninghardfalse
}
\def\turninfo@L{%
  \@turninglefttrue\@turningrightfalse
  \@turningsoftfalse\@turninghardtrue
}
\def\turninfo@r{%
  \@turningleftfalse\@turningrighttrue
  \@turningsofttrue\@turninghardfalse
}
\def\turninfo@R{%
  \@turningleftfalse\@turningrighttrue
  \@turningsoftfalse\@turninghardtrue
}
\namedef{turninfo@-}{%
  \@turningleftfalse\@turningrightfalse
  \@turningsoftfalse\@turninghardfalse
}
\def\turnstateinfo{\csname turninfo@\turnstate\endcsname}

\def\projecttelemetry{%
  \debug3{projection: dt=\the\dt, vspeed=\the\vspeed, turn=\turnstate}%
  %
  %\@testfalse % should we do any turning?
  \ifdim\vspeed>\z@
    \@tempdima=.001\vspeed \multiply\@tempdima\dt
    \@tempdimb\vdir \docos\@tempdimb
    \global\advance\vx\mul\@tempdimb\@tempdima \echo3\vx
    \@tempdimb\vdir \dosin\@tempdimb
    \global\advance\vy\mul\@tempdimb\@tempdima \echo3\vy
    %\@testtrue
    %\iflatex\drawtelemetry{lightblue}\fi
  \fi
  %
  \turnstateinfo
  \if@turningsoft
    \dimen@.001\maxturn \multiply\dimen@\dt
  \else\if@turninghard
    \dimen@.001\maxhardturn \multiply\dimen@\dt
  \fi\fi
  \if@turningleft
    \global\advance\vdir\dimen@ %\@testtrue
  \else\if@turningright
    \global\advance\vdir-\dimen@ %\@testtrue
  \fi\fi
  \echo3\vdir
  %
  \decide
}