Require Export Coq.Program.Tactics.
Require Export LibTactics.

(** printing × $\times$ #×# *)
(** printing :> $:$ #:># *)
(** printing ≡ $\equiv$ #≡# *)
(** printing [*] $*$ #*# *)
(** printing ∧ $\wedge$ #∧# *)
(** printing ∀ $\forall$ #∀# *)
(** printing → $\rightarrow$ #→# *)
(** printing ∃ $\exists$ #∃# *)
(** printing ≤ $\le$ #≤# *)
(** printing θ $\theta$ #θ# *)
(** printing eeev $\epsilon_v$ #∊ᵥ# *)
(** printing eeew $\epsilon_w$ #∊w# *)
(** printing tm $t_m$ #tm# *)
(** printing IR $\mathbb{R}$ #IR# *)
(** printing CR $\mathbb{R}$ #CR# *)
(** printing tr $t_r$ #tr# *)
(** remove printing * *)

(** printing ' $ $ #'# *)

Require Import Vector.
Require Import CPS.
Require Import CPSUtils.

Require Import MathClasses.interfaces.canonical_names.
Require Import MCInstances.
Require Import CartCR.
Require Export ackermannSteering.
Require Import car.errorBounds.
Require Export ackermannSteeringProp.

  Require Import IRMisc.LegacyIRRing.

Require Import atomicMoves.

Require Export CartIR.

Require Import String.

Require Import haskellExtractionDirectives.
(*Require Import ocamlExtractionDirectives.*)

Local Opaque CSine.
Local Opaque CCos.
Local Opaque Sine.
Local Opaque Cosine.
Local Opaque Sin.
Local Opaque Cos.

Set Implicit Arguments.
Require Import geometry2D.

(** enough data to render a car in a picture, which will be a part of an animation*)
Record carState (A:Type) : Type :=
{
  csrigid2D : Rigid2DState A;
  cs_tc :  A (*turn curvatire, determines the position of steering wheel*)
}.

Open Scope CR_scope.

Open Scope mc_scope.

Close Scope CR_scope.

Typeclasses  eauto := 100.

Require Import fastReals.misc.
Require Import fastReals.interface.
Require Import fastReals.implCR.

Require Import MathClasses.interfaces.functors.

Definition carStateAfterAtomicMove
  (cs : carState CR) (dm : @DAtomicMove CR _ _ _): carState CR:=
  {|csrigid2D := (*sfmap compress*) (stateAfterAtomicMove (csrigid2D cs) dm)
    ; cs_tc := (am_tc (projT1 dm)) |}.

Section LineRounding.
(** A perfect rouding from a real to an integer is undecidable.
  This value is an upper bound on suboptimality. e.g. values between
   1.5-eps and 1.5+eps may be rounded to 1 or 2. *)
Variable eps: Qpos.

Global Instance  CastZCR : Cast Z CR := fun x => inject_Q_CR (inject_Z x).

Definition finerRes : Z := 100.

Definition roundPointRZ (p: Cart2D CR) : Cart2D Z :=
{|X:= R2ZApprox ('finerRes * (X p)) eps; Y:=R2ZApprox ('finerRes * (Y p)) eps |}.

Definition roundLineRZ (p: Line2D CR) : Line2D Z :=
{|lstart := roundPointRZ (lstart p); lend:=roundPointRZ (lend p) |}.

End LineRounding.

Axiom ZtoString : Z -> string.
(** [Z] maps to [Prelude.Integer] and [string] map to Prelude.?? .
  So Prelude.show works *)
Extract Constant ZtoString => "PleaseFixMe".
Definition sconcat (l:list string) : string :=
  List.fold_left append  l EmptyString.

Definition newLineChar : Ascii.ascii := Ascii.ascii_of_nat 10.
Definition newLineString : string := String newLineChar EmptyString.

Definition tikZPoint (p: Cart2D Z) : string :=
  "(" ++ ZtoString ((X p)) ++ "," ++ ZtoString (Y p) ++ ")".

Definition tikZLine (l: Line2D Z) : string :=
  "\draw" ++ tikZPoint (lstart l) ++ "--" ++ tikZPoint (lend l) ++ ";" ++
  newLineString.

Definition tikZLineStyled (s:string) (l: Line2D Z) : string :=
  sconcat ["\draw[";s;"]"; tikZPoint (lstart l) ; "--"; tikZPoint (lend l); ";";
            newLineString]%string.

Definition tikZFilledRect (color : string) (l: Line2D Z) : string :=
  "\draw[fill=" ++ color  ++ "," ++ color ++ ", fill opacity=0.3]" ++ tikZPoint (lstart l)
  ++ " rectangle " ++ tikZPoint (lend l) ++ ";" ++ newLineString.

Definition tikZColoredLine (color : string) (l: Line2D Z) : string :=
  "\draw[" ++ color ++ "]" ++ tikZPoint (lstart l) ++ "--" ++ tikZPoint (lend l) ++ ";" ++
  newLineString.

Definition tikZLines (l: list (Line2D Z)) : string :=
  sconcat  (List.map tikZLine l).

Definition tikZOptions : string :=
 "[scale=0.02]".

Definition tikZHeaderFooter (contents : string) : string :=
  "\begin{tikzpicture}"++tikZOptions++newLineString++contents++newLineString
  ++ "\end{tikzpicture}".

Definition beamerFrameHeaderFooter (title contents : string) : string :=
  "\begin{frame}{"++title++"}"++newLineString++contents++newLineString
    ++ "\end{frame}"++newLineString.

Definition tikZPicLines (l: list (Line2D Z)) : string :=
  tikZHeaderFooter (tikZLines l).

Definition beamerFrameLines (title: string)
    (l: list (Line2D Z)) : string :=
  beamerFrameHeaderFooter title (tikZPicLines l).

Definition computeBoundingRectLines `{MinClass A}`{MaxClass A} `{Zero A}
  (ll : list (Line2D A)) : BoundingRectangle A :=
computeBoundingRect (List.map (@lstart _) ll ++ List.map (@lend _) ll).

Section CornerPos.
(** position of the 4 corners of the car *)
Variable eps:Qpos.

Definition tikZBoundingClip (l: BoundingRectangle Z) : string :=
  let maxtoDim := {|lstart := lstart l; lend := lend l - lstart l|} in
  "\clip" ++ tikZPoint (lstart l) ++ " rectangle " ++ tikZPoint (lend l) ++ ";" ++
  newLineString.

Global Instance  CastQCartCR : Cast Q (Cart2D CR)
  := fun x => sameXY (inject_Q_CR x).

Variable cd :CarDimensions CR.
Variable cs :Rigid2DState CR.

Definition carBoundingRectCR : BoundingRectangle CR :=
  carMinMaxXY cd cs.

Definition leftWheelCenter : Cart2D CR :=
  (pos2D cs) +
  '(Qmake 3 4)
    * ((frontUnitVec cs)* '(lengthFront cd)
        + (rightSideUnitVec cs) * '(width cd)).

Definition rightWheelCenter : Cart2D CR :=
  (pos2D cs) +
  '(Qmake 3 4)
    * ((frontUnitVec cs)* '(lengthFront cd)
        - (rightSideUnitVec cs) * '(width cd)).

Definition carWheels (α : CR) : list (Line2D CR) :=
  List.map
    (centredLineAtAngle α ((lengthFront cd) * '(Qmake 1 8)))
    [leftWheelCenter; rightWheelCenter].

Definition drawCarZAux  (α : CR) : list (Line2D Z):=
  List.map (roundLineRZ eps) ((carOutline cd cs)++carWheels α).

Definition drawCarTikZOld (θ : CR) : string :=
  tikZLines (drawCarZAux θ).

(** illustrate the current angle *)
Definition angleLineLength : CR := '(Qmake 4 3) * (totalLength cd).

Definition angleLineXLength : CR := angleLineLength.

Definition Pair (T:Type) : Type := T*T.

Definition angleLines : Pair (Line2D CR) :=
let ls := (pos2D cs) - ('lengthBack cd) * (frontUnitVec cs) in
({|
  lstart := ls;
  lend := ls + ('(angleLineLength))*(frontUnitVec cs)
|},
{|
  lstart := ls;
  lend := ls + {|X:=angleLineXLength; Y:=0|}
|}
).

Definition textPos : Cart2D CR := (pos2D cs)
  + ('totalLength cd) * (unitVec (½*(θ2D cs))).

Definition angleLabel (label:string) :=
sconcat ["\node[right] at "; tikZPoint (roundPointRZ eps textPos)
            ; "{" ; label ; "};"]%string.

Definition illustrateAngle (alabel: string): string :=
sconcat[
  tikZLineStyled "dashed" (roundLineRZ eps (fst angleLines));
  tikZLineStyled "dotted" (roundLineRZ eps (snd angleLines));
  angleLabel alabel
   ].

End CornerPos.

Definition  comparisonAsZ  (c : Datatypes.comparison) : Z :=
match c with
| Datatypes.Eq => 0
| Datatypes.Lt => -1
| Datatypes.Gt => 1
end.

Record CarStateRenderingInfo :=
{
  frameLabel : string;
  drawAngle : bool;
  angleString : string;
  pauseBefore : bool;
  emphBackRightCorner : bool
}.

Definition mkRenderingInfo (name: Pair string):CarStateRenderingInfo :=
{|
  frameLabel := fst name;
  angleString := snd name;
  drawAngle := false;
  pauseBefore := false;
  emphBackRightCorner := false
|}.

Definition mkTransitionRenderingInfo (name: Pair string):CarStateRenderingInfo :=
{|
  frameLabel := fst name;
  angleString := snd name;
  drawAngle := false; (* may want to set this true to true*)
  pauseBefore := true;
  emphBackRightCorner := false
|}.

Section DrawCar.
Variable eps:Qpos.
Variable cd :CarDimensions CR.
Variable cs :carState CR.

Local Definition wheelAngle :CR :=
  (θ2D (csrigid2D cs)) +
  '(comparisonAsZ (CR_epsilon_sign_dec (QposMake 1 10000) (cs_tc cs)))
  * '(Qmake 1 6) * π.

Definition drawCarZ : list (Line2D Z) :=
drawCarZAux eps cd (csrigid2D cs) wheelAngle.

End DrawCar.

(**lets compute a concrete bounding box*)
Open Scope Z_scope.
(** A perfect rouding from a real to an integer is undecidable.
  This value is an upper bound on suboptimality. e.g. values between
   1.5-1/100 and 1.5+1/100 may be rounded to 1 or 2. *)
Local Definition eps : Qpos := QposMake 1 100.

Definition imaginaryCarDimZ : CarDimensions Z :=
{|lengthFront :=  60; lengthBack :=  15;
 width := 25|}.

Require Import exampleDimensions.

Extract Inlined Constant cbvApply => "(Prelude.$!)".

Axiom showQQQQ : (list (Z ** (list Z))) -> string.
Axiom showZZ : (Z ** Z) -> string.
Axiom showN : (nat) -> string.
Extract Constant showQQQQ => "(Prelude.show)".
Extract Constant showZZ => "(Prelude.show)".
Extract Constant showN => "(Prelude.show)".

Section simulator.
Variable carGeo : CarGeometry Z.

Definition myCarDimZ : CarDimensions Z := carDim carGeo.
Close Scope Z_scope.

Definition myCarDim : CarDimensions CR := 'myCarDimZ.

Definition initSt : carState CR :=
 {| csrigid2D := {|pos2D := 0; θ2D := 0|}; cs_tc :=0 |} .

Global Instance  CastQposCR : Cast Qpos CR := fun x =>  '(`x).

Require Import MathClasses.interfaces.orders.

Typeclasses eauto := 10.

(* Move *)

Local Definition straightMove : DAtomicMove CR :=
  (mkStraightMove (cast Z CR 100))%Z.

Definition NamedCarState := prod CarStateRenderingInfo  (carState CR).

Definition drawCarFrameZ (ns : NamedCarState) : ((string * string) * list (Line2D Z))
:=
let newF : string :=
  (if (pauseBefore (fst ns)) then "\newframe*" else "\newframe")%string in
let angle : string :=
  illustrateAngle eps myCarDim (csrigid2D (snd ns)) (angleString (fst ns)) in
let angle : string :=
  if (drawAngle (fst ns)) then angle else EmptyString in
(frameLabel (fst ns), sconcat [angle; newF; newLineString],
   drawCarZ eps myCarDim (snd ns)).

Definition carStatesFrames  (l:list NamedCarState)
: list ((string * string) * list (Line2D Z))  :=
List.map (drawCarFrameZ) l.

Fixpoint movesStates (l:list (DAtomicMove CR)) (init : carState CR) :
  list (carState CR) :=
match l with
| [] => [init]
| hm::t => let midState := carStateAfterAtomicMove init hm in
      init::(movesStates t midState)
end.

Definition DAtomicMoves := list (DAtomicMove CR).

Definition getAtomicMove (d: (DAtomicMove CR)) : AtomicMove CR := projT1 d.

(*
*)

Fixpoint mapDiagonal  `(f: A->A) (prefix tail: list A) : list (list A) :=
match tail with
| [] => []
| h::tl => (prefix ++ ((f h)::tl))::(mapDiagonal f (prefix++[h]) tl)
end.

Require Import sidewaysMoveImpl.

(*
Local Definition wriggleMove : DAtomicMoves :=
  (DWriggle (QposMake 1 150) ('(Qmake 32799 822050))).
*)

Definition totalAvailXExtraSpace : Qpos := (QposMake 35 1).

Local Definition sidewaysMoveAndRightShift :  CR * DAtomicMoves :=
  optimalSidewaysMoveMazda totalAvailXExtraSpace.

Open Scope string_scope.
Definition moveNamesWriggle : list string :=
  ["\hll{($\alpha$,d)}; (-$\alpha$,-d)" ;"($\alpha$,d); \hll{(-$\alpha$,-d)}"].

Definition initStNameWriggle : string :=
  "($\alpha$,d); (-$\alpha$,-d)".

Definition atomicMoveNamesSideways : list string :=
  ["($\alpha$,d)"; "(-$\alpha$,-d)"; "$\;$(0,d')" ; "$\;$(-$\alpha$,d)" ; "($\alpha$,-d)"].

Local Definition spacedMoves := List.map (fun x => x++" ")atomicMoveNamesSideways.

Definition moveNamesSideways : list string :=
  List.map sconcat (mapDiagonal (fun x => sconcat ["\hll{";x;"}"]) [] spacedMoves).

Definition angleLabelsSideways : list string :=
  ["$\theta$"; "$2\theta$"; "$2\theta$"; "$\theta$"; "0"].

Definition sidewaysMovesInfo : list ( (Pair string)) :=
  List.zip moveNamesSideways angleLabelsSideways.

Close Scope string_scope.

Definition NameDAtomicMove := prod (Pair string)  (DAtomicMove CR).

Definition scaleAtomicMove (m: AtomicMove CR) (s:CR): AtomicMove CR:=
  {|am_tc := am_tc m; am_distance := s*(am_distance m) |}.

Definition DscaleAtomicMove  (m: (DAtomicMove CR)) (s:Q) : (DAtomicMove CR) :=
  existT _ (scaleAtomicMove (getAtomicMove m) (inject_Q_CR s)) (projT2 m).

Definition finerAtomicMoves (d:Z⁺) (m: (DAtomicMove CR)) : list (DAtomicMove CR) :=
  List.map (DscaleAtomicMove m) (equiMidPoints d).

Definition finerStates (d:Z⁺) (dm : NameDAtomicMove) (init : carState CR) :
  NamedCarState * list NamedCarState :=
  let (name,dm) := dm in
  ((mkTransitionRenderingInfo name,carStateAfterAtomicMove init dm),
    List.map
      (fun m => (mkRenderingInfo name,carStateAfterAtomicMove init m))
      (finerAtomicMoves d dm)).

Fixpoint finerMovesStates (d:Z⁺) (l:list NameDAtomicMove) (init : NamedCarState) :
   list NamedCarState :=
match l with
| [] => ( [init])
| hm::t => let (midState,interS) := finerStates d hm (snd init) in
          ([init]++(interS)++(finerMovesStates d t midState))
end.

Definition textHt : Z := 25*finerRes.

Definition Rect2D := Line2D.

Definition sideCars (global init :BoundingRectangle Z): (BoundingRectangle Z) * list (Rect2D Z) :=
  let cardim : Cart2D Z  := (sameXY finerRes)*{|X:= (lengthFront myCarDimZ) ; Y:= 2 * (width myCarDimZ) |} in
  let ymax := Y (lend init) in
  let lcarMaxXY : Cart2D Z := {|X:= X (lstart global) ; Y:= ymax |}  in
  let rcarMinXY : Cart2D Z := {|X:= X (lend global) ; Y:= ymax - (Y cardim) |}  in
  (boundingUnion global {|lstart := lcarMaxXY - cardim; lend := rcarMinXY + cardim |},
    [
      {|lstart := lcarMaxXY - cardim; lend := lcarMaxXY |} ;
      {|lstart := rcarMinXY ; lend := rcarMinXY + cardim |}
    ]).

Definition extendRectForText (b:BoundingRectangle Z)  : BoundingRectangle Z :=
  {|lstart := (lstart b) - {|X:= 0 ; Y:= textHt  |};
    lend := (lend b) + {|X:= 0 ; Y:= textHt  |} |}.

Open Scope string_scope.

Definition drawEnv (global init:BoundingRectangle Z)  : string * Cart2D Z:=
  let (bc, sc) := sideCars global init in
  let textPos := lstart bc in
  let bf := extendRectForText bc in
  let bottomLineStart := {| X := X (lstart bc); Y := Y (lstart bc) |} in
  let bottomLineEnd := {| X := X (lend bc); Y := Y (lstart bc) |} in
  let bottomLineZ   := {|lstart := bottomLineStart; lend := bottomLineEnd|} in
  let clip : string :=  tikZBoundingClip bf in
  let sideCars : list string :=  List.map  (tikZFilledRect "red") sc in
  let bottomLine : string :=  tikZColoredLine "red" bottomLineZ in
  (sconcat (sideCars ++ [clip;bottomLine]), textPos).

Definition frameWithLines (suffix:string) (lines : list (Line2D Z)) : string :=
    sconcat [newLineString; tikZLines lines; newLineString; suffix; newLineString].

  Local Notation minxy := (lstart).
  Local Notation maxxy := (lend).

Definition globalBound (initb :  BoundingRectangle Z) (rightExtra upExtra downExtra : CR ): BoundingRectangle Z :=
let rightExtraZ  :=  roundPointRZ eps {| X:= rightExtra; Y:= upExtra|} + {| X:= 1; Y:= 0|} in
let leftExtra : CR :=  'totalAvailXExtraSpace - rightExtra in
let leftExtraZ   :=  roundPointRZ eps {| X:= leftExtra; Y:=downExtra|} + {| X:= 1; Y:=0|} in
initb + {| minxy := -leftExtraZ ; maxxy := rightExtraZ|}.

Definition animation (n: Z⁺): string :=
  let (rs, sidewaysMove) := sidewaysMoveAndRightShift in
  let sidewaysMove := List.zip sidewaysMovesInfo sidewaysMove  in
  let initStp : NamedCarState :=
    (mkRenderingInfo ((sconcat spacedMoves),EmptyString),initSt) in
  let cs : list NamedCarState := (finerMovesStates n sidewaysMove initStp) in
  let namedLines : list ((string * string) * list (Line2D Z))
    := carStatesFrames  cs in
  let allLines : list (Line2D Z) :=  (*cbvApply*) (flat_map snd) namedLines in
  match namedLines return string with
  | [] => ""
  | h::tl =>
      let initb := computeBoundingRectLines (snd h) in
      (* the 2 items below are just guesses. TODO : compute them from the bounds derived in sidewaysMove.v *)
      let upExtra :  CR := '(Zdiv (width (myCarDimZ)) (5%Z)) in
      let downExtra : CR := '(Zdiv (width (myCarDimZ)) (5%Z)) in
      let globalB : BoundingRectangle Z := globalBound initb rs upExtra downExtra  in
      let (preface, textPos) := drawEnv globalB initb in
      let textTikZ  : string -> string
        := fun label => "\node[below right] at " ++ tikZPoint textPos
            ++ "{" ++ label ++ "};" ++ newLineString in
      let frames :=
        List.map (fun p =>
          frameWithLines
            (sconcat [ preface ; textTikZ (fst (fst p)); snd (fst p)])
            (snd p))
          namedLines in
      sconcat frames
  end.

Fixpoint  fold_left_inter {A B : Type} (f : A → B → A) (l : list B)
  (a0 : A) {struct l} : list A :=
  match l with
  | [] => [a0]
  | b :: t => (f a0 b)::(fold_left_inter f t (f a0 b))
  end.

Definition animateSpaceReqOptimalMove (n: Z⁺) : string :=
  let (_, sidewaysMove) := sidewaysMoveAndRightShift in
  let sidewaysMove := List.zip sidewaysMovesInfo sidewaysMove  in
  let initStp := (mkRenderingInfo ((sconcat spacedMoves),EmptyString),initSt) in
  let cs := (finerMovesStates n sidewaysMove initStp) in
  let namedLines : list ((string * string) * list (Line2D Z))
      := carStatesFrames cs in
  let boundRects : list (Line2D Z)
    :=  List.map (computeBoundingRectLines∘snd) namedLines in
  let boundRectsCum : list (Line2D Z)
    := fold_left_inter boundingUnion  boundRects 0 in
  let globalB :  (Line2D Z)
    := last boundRectsCum 0 in
  let lineRects
    := List.zip namedLines  boundRectsCum in
  match lineRects return string with
  | [] => ""
  | (h,_)::tl =>
      let initb := computeBoundingRectLines (snd h) in
      let (pp, textPos) := drawEnv globalB initb in
      let textTikZ  : string -> string
        := fun label => "\node[below right] at " ++ tikZPoint textPos
            ++ "{" ++ label ++ "};" ++ newLineString in
      let frames :=
        List.map (fun p =>
        let bnd := snd p in
        let p := fst p in
        let preface :=  sconcat [pp;tikZFilledRect "green" bnd] in
          frameWithLines
            (sconcat [ preface ; textTikZ (fst (fst p)); snd (fst p)])
            (snd p))
          lineRects in
      sconcat frames
  end.

(* Move *)
Definition mkDAtomicMoveQ (qa: AtomicMove Q) : (DAtomicMove CR).
  destruct qa.
  exists (@mkAtomicMove  CR ('am_distance) ('am_tc)).
  remember (Qeq_bool am_tc 0)%Q as qd. destruct qd; symmetry in Heqqd; simpl.
- left.  simpl. apply Qeq_bool_iff in Heqqd.
  rewrite Heqqd.
  apply preserves_0.
- right. apply Qeq_bool_neq in Heqqd. apply Qap_CRap. assumption.
Qed.

Definition mkRelativeMove (rel : Q*Q) : DAtomicMove CR :=
  let (turnCurv, distance) := rel in
  mkDAtomicMoveQ  (mkAtomicMove (Qmult distance (width myCarDimZ)) (Qdiv turnCurv (minTR carGeo))).

Definition mkNamedRelativeMove (rel : Q*Q) :NameDAtomicMove :=
  ((EmptyString (* convert rel to string*) , EmptyString), mkRelativeMove rel).

Require Import ROSCOQ.geometry2D.
Definition relGlobalBound (initb :  BoundingRectangle Z) (extra : BoundingRectangle Q) : BoundingRectangle Z :=
let extra := (''' finerRes) * extra in
let rightExtraZ  := sfmap Qround.Qfloor ({|X:=' totalLength myCarDimZ; Y:=' width myCarDimZ|} * (maxxy extra)) in
let leftExtraZ  :=  sfmap Qround.Qfloor ({|X:=' totalLength myCarDimZ; Y:=' width myCarDimZ|} * (minxy extra)) in
initb + {| minxy := -leftExtraZ ; maxxy := rightExtraZ|}.

Definition animateSpaceReq
   (moves : list (Q*Q))
   (extraSpace : BoundingRectangle Q)
   (framesPerMove : Z⁺) : string :=
  let sidewaysMove : list NameDAtomicMove := List.map mkNamedRelativeMove moves  in
  let initStp := (mkRenderingInfo (EmptyString,EmptyString),initSt) in
  let cs := (finerMovesStates framesPerMove sidewaysMove initStp) in
  let namedLines : list ((string * string) * list (Line2D Z))
      := carStatesFrames cs in
  let boundRects : list (Line2D Z)
    :=  List.map (computeBoundingRectLines∘snd) namedLines in
  let boundRectsCum : list (Line2D Z)
    := fold_left_inter boundingUnion  boundRects 0 in
  let lineRects
    := List.zip namedLines  boundRectsCum in
  match lineRects return string with
  | [] => ""
  | (h,_)::tl =>
      let initb := computeBoundingRectLines (snd h) in
      let globalB :  (Line2D Z)
        := relGlobalBound initb extraSpace in
      let (pp, textPos) := drawEnv globalB initb in
      let textTikZ  : string -> string
        := fun label => "\node[below right] at " ++ tikZPoint textPos
            ++ "{" ++ label ++ "};" ++ newLineString in
      let frames :=
        List.map (fun p =>
        let bnd := snd p in
        let p := fst p in
        let preface :=  sconcat [pp;tikZFilledRect "green" bnd] in
          frameWithLines
            (sconcat [ preface ; textTikZ (fst (fst p)); snd (fst p)])
            (snd p))
          lineRects in
      sconcat frames
  end.

Definition extra :BoundingRectangle Q :=
{| minxy := {|X:=Qmake 1 5; Y:= Qmake 1 10|}; maxxy := {|X:=Qmake 1 5; Y:= Qmake 1 1|} |}.

Definition moves : (list (Q*Q)) :=
[(1, Qmake 1 3); (-1, Qmake 1 3) ].

(*
Definition animationAutoBounding : string :=
  let (rs, sidewaysMove) := sidewaysMoveAndRightShift in
  let sidewaysMove := List.zip moveNamesSideways sidewaysMove  in
  let initStp := (mkRenderingInfo (sconcat spacedMoves),initSt) in
  let cs := (finerMovesStates 3 sidewaysMove initStp) in
  let namedLines : list ((string * string) * list (Line2D Z))
      := carStatesFrames cs in
  let allLines : list (Line2D Z) :=  (*cbvApply*) (flat_map snd) namedLines in
  let globalB : BoundingRectangle Z := computeBoundingRectLines allLines in
  match namedLines return string with
  | [] => ""
  | h::tl =>
      let initb := computeBoundingRectLines (snd h) in
      let (preface, textPos) := drawEnv globalB initb in
      let textTikZ  : string -> string
        := fun label => "\node[below right] at " ++ tikZPoint textPos
            ++ "{" ++ label ++ "};" ++ newLineString in
      let frames := List.map (fun p => frameWithLines
          (preface ++ textTikZ (fst p))  (snd p)) namedLines in
      sconcat frames
  end.
*)

(*
Definition fstMoveOnlyAnimation : string :=
  let (rs, sidewaysMove) := sidewaysMoveAndRightShift in
  let sidewaysMove := List.zip [EmptyString] [mazdaMaxCurvTurnMove ('singleMoveDistance)] in
  let initStp : string * carState CR := (EmptyString,initSt) in
  let cs : list NamedCarState := (finerMovesStates 10 sidewaysMove initStp) in
  let namedLines : list (string * list (Line2D Z)) := carStatesFrames  cs in
  let allLines : list (Line2D Z) :=  (*cbvApply*) (flat_map snd) namedLines in
  match namedLines return string with
  | [] => ""
  | h::tl =>
      let initb := computeBoundingRectLines (snd h) in
      (* the 2 items below are just guesses. TODO : compute them from the bounds derived in sidewaysMove.v *)
      let upExtra :  CR := '(Zdiv (width (myCarDimZ)) (5%Z)) in
      let downExtra : CR := '(Zdiv (width (myCarDimZ)) (5%Z)) in
      let globalB : BoundingRectangle Z := globalBound initb rs upExtra downExtra  in
      let (preface, textPos) := drawEnv globalB initb in
      let textTikZ  : string -> string
        := fun label => "\node[below right] at " ++ tikZPoint textPos
            ++ "{" ++ label ++ "};" ++ newLineString in
      let frames := List.map (fun p => frameWithLines
          (preface ++ textTikZ (fst p))  (snd p)) namedLines in
      sconcat frames
  end.
*)

Definition numXspaceSamples : positive := (60)%positive.

(** Hakell normaizes rationals whenever it wants. So ensuring that
the denominator is 100 in Coq is not sufficient after extraction.
We are extracting (just the type) of Coq rationals to Haskell rationals,
just to access printing, and conversion to/from floats.
*)
Definition spaceXplot : (list (Z ** (list Z))) :=
  let QtoZ q := Qround.Qfloor (q*(100)%Z) in
  let mf (p:(Q ** (list Q)))
     := let (a,b) := p in (QtoZ a, List.map QtoZ b) in
    (List.map mf (plotOptimalSidewaysMoveShiftMazdaQ eps eps numXspaceSamples)).

Definition spaceXplotn (n:nat) : list (Z*Z):=
  List.map
    (fun (p:(Z ** (list Z))) => let (a,b) := p in (fst p, nth n b 0%Z))
    spaceXplot.

Definition endLine (s:string) : string := sconcat [s; newLineString].

Definition colorAndNames : (list (string * string )) :=
[
  ("red" , "angle (degrees)");
  ("green" , "upward shift");
  ("blue" , "straight distance");
  ("magenta" , "wriggle X space");
  ("yellow" , "straight X space")
].

(*Definition singleMoveDistance :Z :=25. *)

Definition spaceXplotnStr (n:nat) : string :=
  let (color, name) := nth n colorAndNames ("exception","garbage") in
  let preamble n :=
    sconcat ["\addplot[color="; color ; "] coordinates {"] in
  let cs:=
    (sconcat (List.map
            (fun p => endLine (showZZ p))
            (spaceXplotn n))) in
    sconcat [preamble n; cs ; "}; \addlegendentry{";name;"}"
      ; newLineString; newLineString].
End simulator.

Definition spaceXplotStr : string := sconcat (List.map  spaceXplotnStr (seq 0 5)).

(*
Definition toPrint : string := animateSpaceReqOptimalMove Mazda3Sedan2014sGT (4)%positive. *)

Definition toPrint : string := animateSpaceReq Mazda3Sedan2014sGT moves extra (4)%positive.

Close Scope string_scope.
Locate posCompareContAbstract43820948120402312.
Extraction "simulator.hs"  (*animation spaceXplot *) toPrint
  ExtrHaskellQ.posCompareContAbstract43820948120402312.


(* TLC in Coq
 *
 * Module: tlc.component.stubborn_link
 * Purpose: Contains the functional implementation of the stubborn link component.
 *)

Require Import mathcomp.ssreflect.ssrbool.
Require Import mathcomp.ssreflect.ssreflect.
Require Import tlc.component.component.
Require Import tlc.semantics.all_semantics.
Require Import tlc.syntax.all_syntax.

Set Implicit Arguments.
Unset Strict Implicit.
Unset Printing Implicit Defensive.

(* Functional implementation *)
Definition stubborn_link :=
  let flc := 0 in
  let initialize :=
    {-t fun: (* n *)
      []
    -} in
  let request :=
    {-t fun: fun: fun: (* n, s, ir *)
      match: $$0 with:
      { CSLSend ' $0 ' $1 (* {n', m} *) ->
        ($$2 :|: [($0, $1)], [(term_of_nat flc, CFLSend ' $0 ' $1)], [])
      }
    -} in
  let indication :=
    {-t fun: fun: fun: (* n, s, ii *)
      match: $$0 with:
      { (pattern_of_nat flc, CFLDeliver ' $0 ' $1) (* {n, m} *) ->
        ($$2, [], [CSLDeliver ' $0 ' $1])
      }
    -} in
  let periodic :=
    {-t fun: fun: (* n, s *)
      ($$0, (fun: (* (n', m) *)
        (term_of_nat flc, CFLSend ' ($$0).1 ' ($$0).2)) <$> $$0, [])
    -} in
  Component
    (Logic.eq_refl : term_computable initialize)
    (Logic.eq_refl : term_computable request)
    (Logic.eq_refl : term_computable indication)
    (Logic.eq_refl : term_computable periodic).


(* -*- coding: utf-8 -*- *)
(************************************************************************)
(*         *   The Coq Proof Assistant / The Coq Development Team       *)
(*  v      *   INRIA, CNRS and contributors - Copyright 1999-2018       *)
(* <O___,, *       (see CREDITS file for the list of authors)           *)
(*   \VV/  **************************************************************)
(*    //   *    This file is distributed under the terms of the         *)
(*         *     GNU Lesser General Public License Version 2.1          *)
(*         *     (see LICENSE file for the text of the license)         *)
(************************************************************************)

(**********************************************************************)
(** * Binary positive numbers, operations *)
(**********************************************************************)

(** Initial development by Pierre Crégut, CNET, Lannion, France *)

(** The type [positive] and its constructors [xI] and [xO] and [xH]
    are now defined in [BinNums.v] *)

Require Export BinNums.

(** Postfix notation for positive numbers, which allows mimicking
    the position of bits in a big-endian representation.
    For instance, we can write [1~1~0] instead of [(xO (xI xH))]
    for the number 6 (which is 110 in binary notation).
*)

Local Notation "1" := xH.

Notation "p ~ 1" := (xI p)
 (at level 7, left associativity, format "p '~' '1'") : positive_scope.
Notation "p ~ 0" := (xO p)
 (at level 7, left associativity, format "p '~' '0'") : positive_scope.

Local Open Scope positive_scope.

Module Pos.

Definition t := positive.

(** * Operations over positive numbers *)

(** ** Successor *)

Fixpoint succ x :=
  match x with
    | p~1 => (succ p)~0
    | p~0 => p~1
    | 1 => 1~0
  end.

(** ** Addition *)

Fixpoint add x y :=
  match x, y with
    | p~1, q~1 => (add_carry p q)~0
    | p~1, q~0 => (add p q)~1
    | p~1, 1 => (succ p)~0
    | p~0, q~1 => (add p q)~1
    | p~0, q~0 => (add p q)~0
    | p~0, 1 => p~1
    | 1, q~1 => (succ q)~0
    | 1, q~0 => q~1
    | 1, 1 => 1~0
  end

with add_carry x y :=
  match x, y with
    | p~1, q~1 => (add_carry p q)~1
    | p~1, q~0 => (add_carry p q)~0
    | p~1, 1 => (succ p)~1
    | p~0, q~1 => (add_carry p q)~0
    | p~0, q~0 => (add p q)~1
    | p~0, 1 => (succ p)~0
    | 1, q~1 => (succ q)~1
    | 1, q~0 => (succ q)~0
    | 1, 1 => 1~1
  end.

Infix "+" := add : positive_scope.

(** ** Operation [x -> 2*x-1] *)

Fixpoint pred_double x :=
  match x with
    | p~1 => p~0~1
    | p~0 => (pred_double p)~1
    | 1 => 1
  end.

(** ** Predecessor *)

Definition pred x :=
  match x with
    | p~1 => p~0
    | p~0 => pred_double p
    | 1 => 1
  end.

(** ** The predecessor of a positive number can be seen as a [N] *)

Definition pred_N x :=
  match x with
    | p~1 => Npos (p~0)
    | p~0 => Npos (pred_double p)
    | 1 => N0
  end.

(** ** An auxiliary type for subtraction *)

Inductive mask : Set :=
| IsNul : mask
| IsPos : positive -> mask
| IsNeg : mask.

(** ** Operation [x -> 2*x+1] *)

Definition succ_double_mask (x:mask) : mask :=
  match x with
    | IsNul => IsPos 1
    | IsNeg => IsNeg
    | IsPos p => IsPos p~1
  end.

(** ** Operation [x -> 2*x] *)

Definition double_mask (x:mask) : mask :=
  match x with
    | IsNul => IsNul
    | IsNeg => IsNeg
    | IsPos p => IsPos p~0
  end.

(** ** Operation [x -> 2*x-2] *)

Definition double_pred_mask x : mask :=
  match x with
    | p~1 => IsPos p~0~0
    | p~0 => IsPos (pred_double p)~0
    | 1 => IsNul
  end.

(** ** Predecessor with mask *)

Definition pred_mask (p : mask) : mask :=
  match p with
    | IsPos 1 => IsNul
    | IsPos q => IsPos (pred q)
    | IsNul => IsNeg
    | IsNeg => IsNeg
  end.

(** ** Subtraction, result as a mask *)

Fixpoint sub_mask (x y:positive) {struct y} : mask :=
  match x, y with
    | p~1, q~1 => double_mask (sub_mask p q)
    | p~1, q~0 => succ_double_mask (sub_mask p q)
    | p~1, 1 => IsPos p~0
    | p~0, q~1 => succ_double_mask (sub_mask_carry p q)
    | p~0, q~0 => double_mask (sub_mask p q)
    | p~0, 1 => IsPos (pred_double p)
    | 1, 1 => IsNul
    | 1, _ => IsNeg
  end

with sub_mask_carry (x y:positive) {struct y} : mask :=
  match x, y with
    | p~1, q~1 => succ_double_mask (sub_mask_carry p q)
    | p~1, q~0 => double_mask (sub_mask p q)
    | p~1, 1 => IsPos (pred_double p)
    | p~0, q~1 => double_mask (sub_mask_carry p q)
    | p~0, q~0 => succ_double_mask (sub_mask_carry p q)
    | p~0, 1 => double_pred_mask p
    | 1, _ => IsNeg
  end.

(** ** Subtraction, result as a positive, returning 1 if [x<=y] *)

Definition sub x y :=
  match sub_mask x y with
    | IsPos z => z
    | _ => 1
  end.

Infix "-" := sub : positive_scope.

(** ** Multiplication *)

Fixpoint mul x y :=
  match x with
    | p~1 => y + (mul p y)~0
    | p~0 => (mul p y)~0
    | 1 => y
  end.

Infix "*" := mul : positive_scope.

(** ** Iteration over a positive number *)

Definition iter {A} (f:A -> A) : A -> positive -> A :=
  fix iter_fix x n := match n with
    | xH => f x
    | xO n' => iter_fix (iter_fix x n') n'
    | xI n' => f (iter_fix (iter_fix x n') n')
  end.

(** ** Power *)

Definition pow (x:positive) := iter (mul x) 1.

Infix "^" := pow : positive_scope.

(** ** Square *)

Fixpoint square p :=
  match p with
    | p~1 => (square p + p)~0~1
    | p~0 => (square p)~0~0
    | 1 => 1
  end.

(** ** Division by 2 rounded below but for 1 *)

Definition div2 p :=
  match p with
    | 1 => 1
    | p~0 => p
    | p~1 => p
  end.

(** Division by 2 rounded up *)

Definition div2_up p :=
 match p with
   | 1 => 1
   | p~0 => p
   | p~1 => succ p
 end.

(** ** Number of digits in a positive number *)

Fixpoint size_nat p : nat :=
  match p with
    | 1 => S O
    | p~1 => S (size_nat p)
    | p~0 => S (size_nat p)
  end.

(** Same, with positive output *)

Fixpoint size p :=
  match p with
    | 1 => 1
    | p~1 => succ (size p)
    | p~0 => succ (size p)
  end.

(** ** Comparison on binary positive numbers *)

Fixpoint compare_cont (r:comparison) (x y:positive) {struct y} : comparison :=
  match x, y with
    | p~1, q~1 => compare_cont r p q
    | p~1, q~0 => compare_cont Gt p q
    | p~1, 1 => Gt
    | p~0, q~1 => compare_cont Lt p q
    | p~0, q~0 => compare_cont r p q
    | p~0, 1 => Gt
    | 1, q~1 => Lt
    | 1, q~0 => Lt
    | 1, 1 => r
  end.

Definition compare := compare_cont Eq.

Infix "?=" := compare (at level 70, no associativity) : positive_scope.

Definition min p p' :=
 match p ?= p' with
 | Lt | Eq => p
 | Gt => p'
 end.

Definition max p p' :=
 match p ?= p' with
 | Lt | Eq => p'
 | Gt => p
 end.

(** ** Boolean equality and comparisons *)

Fixpoint eqb p q {struct q} :=
  match p, q with
    | p~1, q~1 => eqb p q
    | p~0, q~0 => eqb p q
    | 1, 1 => true
    | _, _ => false
  end.

Definition leb x y :=
 match x ?= y with Gt => false | _ => true end.

Definition ltb x y :=
 match x ?= y with Lt => true | _ => false end.

Infix "=?" := eqb (at level 70, no associativity) : positive_scope.
Infix "<=?" := leb (at level 70, no associativity) : positive_scope.
Infix "<?" := ltb (at level 70, no associativity) : positive_scope.

(** ** A Square Root function for positive numbers *)

(** We procede by blocks of two digits : if p is written qbb'
    then sqrt(p) will be sqrt(q)~0 or sqrt(q)~1.
    For deciding easily in which case we are, we store the remainder
    (as a mask, since it can be null).
    Instead of copy-pasting the following code four times, we
    factorize as an auxiliary function, with f and g being either
    xO or xI depending of the initial digits.
    NB: (sub_mask (g (f 1)) 4) is a hack, morally it's g (f 0).
*)

Definition sqrtrem_step (f g:positive->positive) p :=
 match p with
  | (s, IsPos r) =>
    let s' := s~0~1 in
    let r' := g (f r) in
    if s' <=? r' then (s~1, sub_mask r' s')
    else (s~0, IsPos r')
  | (s,_)  => (s~0, sub_mask (g (f 1)) 1~0~0)
 end.

Fixpoint sqrtrem p : positive * mask :=
 match p with
  | 1 => (1,IsNul)
  | 1~0 => (1,IsPos 1)
  | 1~1 => (1,IsPos 1~0)
  | p~0~0 => sqrtrem_step xO xO (sqrtrem p)
  | p~0~1 => sqrtrem_step xO xI (sqrtrem p)
  | p~1~0 => sqrtrem_step xI xO (sqrtrem p)
  | p~1~1 => sqrtrem_step xI xI (sqrtrem p)
 end.

Definition sqrt p := fst (sqrtrem p).

(** ** Greatest Common Divisor *)

Definition divide p q := exists r, q = r*p.
Notation "( p | q )" := (divide p q) (at level 0) : positive_scope.

(** Instead of the Euclid algorithm, we use here the Stein binary
   algorithm, which is faster for this representation. This algorithm
   is almost structural, but in the last cases we do some recursive
   calls on subtraction, hence the need for a counter.
*)

Fixpoint gcdn (n : nat) (a b : positive) : positive :=
  match n with
    | O => 1
    | S n =>
      match a,b with
	| 1, _ => 1
	| _, 1 => 1
	| a~0, b~0 => (gcdn n a b)~0
	| _  , b~0 => gcdn n a b
	| a~0, _   => gcdn n a b
	| a'~1, b'~1 =>
          match a' ?= b' with
	    | Eq => a
	    | Lt => gcdn n (b'-a') a
	    | Gt => gcdn n (a'-b') b
          end
      end
  end.

(** We'll show later that we need at most (log2(a.b)) loops *)

Definition gcd (a b : positive) := gcdn (size_nat a + size_nat b)%nat a b.

(** Generalized Gcd, also computing the division of a and b by the gcd *)
Set Printing Universes.
Fixpoint ggcdn (n : nat) (a b : positive) : (positive*(positive*positive)) :=
  match n with
    | O => (1,(a,b))
    | S n =>
      match a,b with
	| 1, _ => (1,(1,b))
	| _, 1 => (1,(a,1))
	| a~0, b~0 =>
           let (g,p) := ggcdn n a b in
           (g~0,p)
	| _, b~0 =>
           let '(g,(aa,bb)) := ggcdn n a b in
           (g,(aa, bb~0))
	| a~0, _ =>
           let '(g,(aa,bb)) := ggcdn n a b in
           (g,(aa~0, bb))
	| a'~1, b'~1 =>
           match a' ?= b' with
	     | Eq => (a,(1,1))
	     | Lt =>
	        let '(g,(ba,aa)) := ggcdn n (b'-a') a in
	        (g,(aa, aa + ba~0))
	     | Gt =>
		let '(g,(ab,bb)) := ggcdn n (a'-b') b in
		(g,(bb + ab~0, bb))
	   end
      end
  end.

Definition ggcd (a b: positive) := ggcdn (size_nat a + size_nat b)%nat a b.

(** Local copies of the not-yet-available [N.double] and [N.succ_double] *)

Definition Nsucc_double x :=
  match x with
  | N0 => Npos 1
  | Npos p => Npos p~1
  end.

Definition Ndouble n :=
  match n with
  | N0 => N0
  | Npos p => Npos p~0
  end.

(** Operation over bits. *)

(** Logical [or] *)

Fixpoint lor (p q : positive) : positive :=
  match p, q with
    | 1, q~0 => q~1
    | 1, _ => q
    | p~0, 1 => p~1
    | _, 1 => p
    | p~0, q~0 => (lor p q)~0
    | p~0, q~1 => (lor p q)~1
    | p~1, q~0 => (lor p q)~1
    | p~1, q~1 => (lor p q)~1
  end.

(** Logical [and] *)

Fixpoint land (p q : positive) : N :=
  match p, q with
    | 1, q~0 => N0
    | 1, _ => Npos 1
    | p~0, 1 => N0
    | _, 1 => Npos 1
    | p~0, q~0 => Ndouble (land p q)
    | p~0, q~1 => Ndouble (land p q)
    | p~1, q~0 => Ndouble (land p q)
    | p~1, q~1 => Nsucc_double (land p q)
  end.

(** Logical [diff] *)

Fixpoint ldiff (p q:positive) : N :=
  match p, q with
    | 1, q~0 => Npos 1
    | 1, _ => N0
    | _~0, 1 => Npos p
    | p~1, 1 => Npos (p~0)
    | p~0, q~0 => Ndouble (ldiff p q)
    | p~0, q~1 => Ndouble (ldiff p q)
    | p~1, q~1 => Ndouble (ldiff p q)
    | p~1, q~0 => Nsucc_double (ldiff p q)
  end.

(** [xor] *)

Fixpoint lxor (p q:positive) : N :=
  match p, q with
    | 1, 1 => N0
    | 1, q~0 => Npos (q~1)
    | 1, q~1 => Npos (q~0)
    | p~0, 1 => Npos (p~1)
    | p~0, q~0 => Ndouble (lxor p q)
    | p~0, q~1 => Nsucc_double (lxor p q)
    | p~1, 1 => Npos (p~0)
    | p~1, q~0 => Nsucc_double (lxor p q)
    | p~1, q~1 => Ndouble (lxor p q)
  end.

(** Shifts. NB: right shift of 1 stays at 1. *)

Definition shiftl_nat (p:positive) := nat_rect _ p (fun _ => xO).
Definition shiftr_nat (p:positive) := nat_rect _ p (fun _ => div2).

Definition shiftl (p:positive)(n:N) :=
  match n with
    | N0 => p
    | Npos n => iter xO p n
  end.

Definition shiftr (p:positive)(n:N) :=
  match n with
    | N0 => p
    | Npos n => iter div2 p n
  end.

(** Checking whether a particular bit is set or not *)

Fixpoint testbit_nat (p:positive) : nat -> bool :=
  match p with
    | 1 => fun n => match n with
                      | O => true
                      | S _ => false
                    end
    | p~0 => fun n => match n with
                        | O => false
                        | S n' => testbit_nat p n'
                      end
    | p~1 => fun n => match n with
                        | O => true
                        | S n' => testbit_nat p n'
                      end
  end.

(** Same, but with index in N *)

Fixpoint testbit (p:positive)(n:N) :=
  match p, n with
    | p~0, N0 => false
    | _, N0 => true
    | 1, _ => false
    | p~0, Npos n => testbit p (pred_N n)
    | p~1, Npos n => testbit p (pred_N n)
  end.

(** ** From binary positive numbers to Peano natural numbers *)

Definition iter_op {A}(op:A->A->A) :=
  fix iter (p:positive)(a:A) : A :=
  match p with
    | 1 => a
    | p~0 => iter p (op a a)
    | p~1 => op a (iter p (op a a))
  end.

Definition to_nat (x:positive) : nat := iter_op plus x (S O).
Arguments to_nat x: simpl never.
(** ** From Peano natural numbers to binary positive numbers *)

(** A version preserving positive numbers, and sending 0 to 1. *)

Fixpoint of_nat (n:nat) : positive :=
 match n with
   | O => 1
   | S O => 1
   | S x => succ (of_nat x)
 end.

(* Another version that converts [n] into [n+1] *)

Fixpoint of_succ_nat (n:nat) : positive :=
  match n with
    | O => 1
    | S x => succ (of_succ_nat x)
  end.

(** ** Conversion with a decimal representation for printing/parsing *)

Local Notation ten := 1~0~1~0.

Fixpoint of_uint_acc (d:Decimal.uint)(acc:positive) :=
  match d with
  | Decimal.Nil => acc
  | Decimal.D0 l => of_uint_acc l (mul ten acc)
  | Decimal.D1 l => of_uint_acc l (add 1 (mul ten acc))
  | Decimal.D2 l => of_uint_acc l (add 1~0 (mul ten acc))
  | Decimal.D3 l => of_uint_acc l (add 1~1 (mul ten acc))
  | Decimal.D4 l => of_uint_acc l (add 1~0~0 (mul ten acc))
  | Decimal.D5 l => of_uint_acc l (add 1~0~1 (mul ten acc))
  | Decimal.D6 l => of_uint_acc l (add 1~1~0 (mul ten acc))
  | Decimal.D7 l => of_uint_acc l (add 1~1~1 (mul ten acc))
  | Decimal.D8 l => of_uint_acc l (add 1~0~0~0 (mul ten acc))
  | Decimal.D9 l => of_uint_acc l (add 1~0~0~1 (mul ten acc))
  end.

Fixpoint of_uint (d:Decimal.uint) : N :=
  match d with
  | Decimal.Nil => N0
  | Decimal.D0 l => of_uint l
  | Decimal.D1 l => Npos (of_uint_acc l 1)
  | Decimal.D2 l => Npos (of_uint_acc l 1~0)
  | Decimal.D3 l => Npos (of_uint_acc l 1~1)
  | Decimal.D4 l => Npos (of_uint_acc l 1~0~0)
  | Decimal.D5 l => Npos (of_uint_acc l 1~0~1)
  | Decimal.D6 l => Npos (of_uint_acc l 1~1~0)
  | Decimal.D7 l => Npos (of_uint_acc l 1~1~1)
  | Decimal.D8 l => Npos (of_uint_acc l 1~0~0~0)
  | Decimal.D9 l => Npos (of_uint_acc l 1~0~0~1)
  end.

Definition of_int (d:Decimal.int) : option positive :=
  match d with
  | Decimal.Pos d =>
    match of_uint d with
    | N0 => None
    | Npos p => Some p
    end
  | Decimal.Neg _ => None
  end.

Fixpoint to_little_uint p :=
  match p with
  | 1 => Decimal.D1 Decimal.Nil
  | p~1 => Decimal.Little.succ_double (to_little_uint p)
  | p~0 => Decimal.Little.double (to_little_uint p)
  end.

Definition to_uint p := Decimal.rev (to_little_uint p).

Definition to_int n := Decimal.Pos (to_uint n).

Numeral Notation positive of_int to_uint : positive_scope.

End Pos.

(** Re-export the notation for those who just [Import BinPosDef] *)
Numeral Notation positive Pos.of_int Pos.to_uint : positive_scope.


(** * ImpParser_J: Coqでの字句解析と構文解析 *)
(* * ImpParser: Lexing and Parsing in Coq *)

(* $Date: 2011-02-21 10:21:18 -0500 (Mon, 21 Feb 2011) $ *)

(* The development of the [Imp] language in Imp.v completely ignores
    issues of concrete syntax -- how an ascii string that a programmer
    might write gets translated into the abstract syntax trees defined
    by the datatypes [aexp], [bexp], and [com].  In this file we
    illustrate how the rest of the story can be filled in by building
    a simple lexical analyzer and parser using Coq's functional
    programming facilities.

    This development is not intended to be understood in detail: the
    explanations are fairly terse and there are no exercises.  The
    main point is simply to demonstrate that it can be done.  You are
    invited to look through the code -- most of it is not very
    complicated, though the parser relies on some "monadic"
    programming idioms that may require a little work to make out --
    but most readers will probably want to just skip down to the
    Examples section at the very end to get the punchline. *)
(** Imp_J.vでの[Imp]言語の開発は、具象構文の問題を完全に無視しています。
    つまり、プログラマが書くアスキー文字列をデータ型[aexp]、[bexp]、[com]
    で定義された抽象構文木にどうやって変換するか、という問題です。
    このファイルでは、
    Coqの関数プログラミング機能によって簡単な字句解析器と構文解析器(パーサ)を構築することで、
    この残っている問題を終わらせます。

    ここでやることは、細部まで理解する必要はありません。
    説明はかなり少なく、練習問題もありません。
    一番のポイントは単に、それをやることが可能なことを示すことです。
    コードを眺めてみて欲しいところです。ほとんどの部分はそれほど複雑ではありません。
    ただパーサはある「モナド的」プログラミング法をしているので、
    理解するのにちょっと骨が折れるかもしれません。
    しかし、ほとんどの読者は、一番最後の「例」のさわりまで飛ばしたいことでしょう。*)

(* ####################################################### *)
(* * Internals *)
(** * 内部処理 *)

Require Import SfLib_J.
Require Import Imp_J.

Require Import String.
Require Import Ascii.

Open Scope list_scope.

(* ####################################################### *)
(* ** Lexical Analysis *)
(** ** 字句解析 *)

Definition isWhite (c : ascii) : bool :=
  let n := nat_of_ascii c in
  orb (orb (beq_nat n 32)   (* space *)
           (beq_nat n 9))   (* tab *)
      (orb (beq_nat n 10)   (* linefeed *)
           (beq_nat n 13)). (* Carriage return. *)

Notation "x '<=?' y" := (ble_nat x y)
  (at level 70, no associativity) : nat_scope.

Definition isLowerAlpha (c : ascii) : bool :=
  let n := nat_of_ascii c in
    andb (97 <=? n) (n <=? 122).

Definition isAlpha (c : ascii) : bool :=
  let n := nat_of_ascii c in
    orb (andb (65 <=? n) (n <=? 90))
        (andb (97 <=? n) (n <=? 122)).

Definition isDigit (c : ascii) : bool :=
  let n := nat_of_ascii c in
     andb (48 <=? n) (n <=? 57).

Inductive chartype := white | alpha | digit | other.

Definition classifyChar (c : ascii) : chartype :=
  if isWhite c then
    white
  else if isAlpha c then
    alpha
  else if isDigit c then
    digit
  else
    other.

Fixpoint list_of_string (s : string) : list ascii :=
  match s with
  | EmptyString => []
  | String c s => c :: (list_of_string s)
  end.

Fixpoint string_of_list (xs : list ascii) : string :=
  fold_right String EmptyString xs.

Definition token := string.

Fixpoint tokenize_helper (cls : chartype) (acc xs : list ascii)
                       : list (list ascii) :=
  let tk := match acc with [] => [] | _::_ => [rev acc] end in
  match xs with
  | [] => tk
  | (x::xs') =>
    match cls, classifyChar x, x with
    | _, _, "("      => tk ++ ["("]::(tokenize_helper other [] xs')
    | _, _, ")"      => tk ++ [")"]::(tokenize_helper other [] xs')
    | _, white, _    => tk ++ (tokenize_helper white [] xs')
    | alpha,alpha,x  => tokenize_helper alpha (x::acc) xs'
    | digit,digit,x  => tokenize_helper digit (x::acc) xs'
    | other,other,x  => tokenize_helper other (x::acc) xs'
    | _,tp,x         => tk ++ (tokenize_helper tp [x] xs')
    end
  end %char.

Definition tokenize (s : string) : list string :=
  map string_of_list (tokenize_helper white [] (list_of_string s)).

Example tokenize_ex1 :
    tokenize "abc12==3  223*(3+(a+c))" %string
  = ["abc", "12", "==", "3", "223",
       "*", "(", "3", "+", "(",
       "a", "+", "c", ")", ")"]%string.
Proof. reflexivity. Qed.

(* ####################################################### *)
(* ** Parsing *)
(** ** 構文解析 *)

(* ####################################################### *)
(* *** Options with Errors *)
(** *** Option と Error *)

(* An option with error messages. *)
(* エラーメッセージのためのoption。 *)
Inductive optionE (X:Type) : Type :=
  | SomeE : X -> optionE X
  | NoneE : string -> optionE X.

Implicit Arguments SomeE [[X]].
Implicit Arguments NoneE [[X]].

(* Some syntactic sugar to make writing nested match-expressions on
   optionE more convenient. *)
(* ネストされたoptionEの上のマッチ式をより簡単に書くための構文糖衣。*)

Notation "'DO' ( x , y ) <== e1 ;; e2"
   := (match e1 with
         | SomeE (x,y) => e2
         | NoneE err => NoneE err
       end)
   (right associativity, at level 60).

Notation "'DO' ( x , y ) <-- e1 ;; e2 'OR' e3"
   := (match e1 with
         | SomeE (x,y) => e2
         | NoneE err => e3
       end)
   (right associativity, at level 60, e2 at next level).

(* ####################################################### *)
(* *** Symbol Table *)
(** *** シンボルテーブル *)

(* Build a mapping from [tokens] to [nats].  A real parser would do
   this incrementally as it encountered new symbols, but passing
   around the symbol table inside the parsing functions is a bit
   inconvenient, so instead we do it as a first pass. *)
(* [tokens]から[nats]への写像を構築する。
   実際のパーサは新しいシンボルに遭遇するたびにインクリメンタルにこれをやるのかもしれないが、
   パーサ関数の中でシンボルテーブルを渡してまわるのはちょっと不便なので、
   ここではその代わりに、第1パスでこれをやることにする。*)
Fixpoint build_symtable (xs : list token) (n : nat) : (token -> nat) :=
  match xs with
  | [] => (fun s => n)
  | x::xs =>
    if (forallb isLowerAlpha (list_of_string x))
     then (fun s => if string_dec s x then n else (build_symtable xs (S n) s))
     else build_symtable xs n
  end.

(* ####################################################### *)
(* *** Generic Combinators for Building Parsers *)
(** *** パーサ構築のための一般コンビネータ *)

Open Scope string_scope.

Definition parser (T : Type) :=
  list token -> optionE (T * list token).

Fixpoint many_helper {T} (p : parser T) acc steps xs :=
match steps, p xs with
| 0, _ => NoneE "Too many recursive calls"
| _, NoneE _ => SomeE ((rev acc), xs)
| S steps', SomeE (t, xs') => many_helper p (t::acc) steps' xs'
end.

(* A (step-indexed) parser which expects zero or more [p]s *)
(* (stepをインデックスとする)パーサ。0個以上の[p]をとる。*)
Fixpoint many {T} (p : parser T) (steps : nat) : parser (list T) :=
  many_helper p [] steps.

(* A parser which expects a given token, followed by p *)
(* pの前のトークンを引数とするパーサ *)
Definition firstExpect {T} (t : token) (p : parser T) : parser T :=
  fun xs => match xs with
              | x::xs' => if string_dec x t
                           then p xs'
                          else NoneE ("expected '" ++ t ++ "'.")
              | [] =>  NoneE ("expected '" ++ t ++ "'.")
            end.

(* A parser which expects a particular token *)
(* 特定のトークンを引数とするパーサ *)
Definition expect (t : token) : parser unit :=
  firstExpect t (fun xs => SomeE(tt, xs)).

(* ####################################################### *)
(* *** A Recursive-Descent Parser for Imp *)
(** *** Impの再帰下降パーサ *)

(* Identifiers *)
(* 識別子 *)
Definition parseIdentifier (symtable :string->nat) (xs : list token)
                         : optionE (id * list token) :=
match xs with
| [] => NoneE "Expected identifier"
| x::xs' =>
    if forallb isLowerAlpha (list_of_string x) then
      SomeE (Id (symtable x), xs')
    else
      NoneE ("Illegal identifier:'" ++ x ++ "'")
end.

(* Numbers *)
(* 数値 *)
Definition parseNumber (xs : list token) : optionE (nat * list token) :=
match xs with
| [] => NoneE "Expected number"
| x::xs' =>
    if forallb isDigit (list_of_string x) then
      SomeE (fold_left (fun n d =>
                        10 * n + (nat_of_ascii d - nat_of_ascii "0"%char))
                (list_of_string x)
                0,
              xs')
    else
      NoneE "Expected number"
end.

(* Parse arithmetic expressions *)
(* 算術式の構文解析 *)
Fixpoint parsePrimaryExp (steps:nat) symtable (xs : list token)
   : optionE (aexp * list token) :=
  match steps with
  | 0 => NoneE "Too many recursive calls"
  | S steps' =>
      DO (i, rest) <-- parseIdentifier symtable xs ;;
          SomeE (AId i, rest)
      OR DO (n, rest) <-- parseNumber xs ;;
          SomeE (ANum n, rest)
      OR (DO (e, rest)  <== firstExpect "(" (parseSumExp steps' symtable) xs;;
          DO (u, rest') <== expect ")" rest ;;
          SomeE(e,rest'))
  end
with parseProductExp (steps:nat) symtable (xs : list token)  :=
  match steps with
  | 0 => NoneE "Too many recursive calls"
  | S steps' =>
    DO (e, rest) <==
      parsePrimaryExp steps' symtable xs ;;
    DO (es, rest') <==
      many (firstExpect "*" (parsePrimaryExp steps' symtable)) steps' rest;;
    SomeE (fold_left AMult es e, rest')
  end
with parseSumExp (steps:nat) symtable (xs : list token)  :=
  match steps with
  | 0 => NoneE "Too many recursive calls"
  | S steps' =>
    DO (e, rest) <==
      parseProductExp steps' symtable xs ;;
    DO (es, rest') <==
      many (fun xs =>
             DO (e,rest') <--
               firstExpect "+" (parseProductExp steps' symtable) xs;;
                                 SomeE ( (true, e), rest')
             OR DO (e,rest') <==
               firstExpect "-" (parseProductExp steps' symtable) xs;;
                                 SomeE ( (false, e), rest'))
                            steps' rest;;
      SomeE (fold_left (fun e0 term =>
                          match term with
                            (true,  e) => APlus e0 e
                          | (false, e) => AMinus e0 e
                          end)
                       es e,
             rest')
  end.

Definition parseAExp := parseSumExp.

(* Parsing boolean expressions. *)
(* ブール式の構文解析 *)
Fixpoint parseAtomicExp (steps:nat) (symtable : string->nat) (xs : list token)  :=
match steps with
  | 0 => NoneE "Too many recursive calls"
  | S steps' =>
     DO    (u,rest) <-- expect "true" xs;;
         SomeE (BTrue,rest)
     OR DO (u,rest) <-- expect "false" xs;;
         SomeE (BFalse,rest)
     OR DO (e,rest) <-- firstExpect "not" (parseAtomicExp steps' symtable) xs;;
         SomeE (BNot e, rest)
     OR DO (e,rest) <-- firstExpect "(" (parseConjunctionExp steps' symtable) xs;;
          (DO (u,rest') <== expect ")" rest;; SomeE (e, rest'))
     OR DO (e, rest) <== parseProductExp steps' symtable xs ;;
            (DO (e', rest') <--
              firstExpect "==" (parseAExp steps' symtable) rest ;;
              SomeE (BEq e e', rest')
             OR DO (e', rest') <--
               firstExpect "<=" (parseAExp steps' symtable) rest ;;
               SomeE (BLe e e', rest')
             OR
               NoneE "Expected '==' or '<=' after arithmetic expression")
end
with parseConjunctionExp (steps:nat) (symtable : string->nat) (xs : list token)   :=
  match steps with
  | 0 => NoneE "Too many recursive calls"
  | S steps' =>
    DO (e, rest) <==
      parseAtomicExp steps' symtable xs ;;
    DO (es, rest') <==
      many (firstExpect "&&" (parseAtomicExp steps' symtable)) steps' rest;;
    SomeE (fold_left BAnd es e, rest')
  end.

Definition parseBExp := parseConjunctionExp.

(*
Eval compute in
  (parseProductExp 100 (tokenize "x*y*(x*x)*x")).

Eval compute in
  (parseDisjunctionExp 100 (tokenize "not((x==x||x*x<=(x*x)*x)&&x==x)")).
*)

(* Parsing commands *)
(* コマンドの構文解析 *)
Fixpoint parseSimpleCommand (steps:nat) (symtable:string->nat) (xs : list token) :=
  match steps with
  | 0 => NoneE "Too many recursive calls"
  | S steps' =>
    DO (u, rest) <-- expect "SKIP" xs;;
      SomeE (SKIP, rest)
    OR DO (e,rest) <--
         firstExpect "IF" (parseBExp steps' symtable) xs;;
       DO (c,rest')  <==
         firstExpect "THEN" (parseSequencedCommand steps' symtable) rest;;
       DO (c',rest'') <==
         firstExpect "ELSE" (parseSequencedCommand steps' symtable) rest';;
       DO (u,rest''') <==
         expect "END" rest'';;
       SomeE(IFB e THEN c ELSE c' FI, rest''')
    OR DO (e,rest) <--
         firstExpect "WHILE" (parseBExp steps' symtable) xs;;
       DO (c,rest') <==
         firstExpect "DO" (parseSequencedCommand steps' symtable) rest;;
       DO (u,rest'') <==
         expect "END" rest';;
       SomeE(WHILE e DO c END, rest'')
    OR DO (i, rest) <==
         parseIdentifier symtable xs;;
       DO (e, rest') <==
         firstExpect ":=" (parseAExp steps' symtable) rest;;
       SomeE(i ::= e, rest')
  end

with parseSequencedCommand (steps:nat) (symtable:string->nat) (xs : list token) :=
  match steps with
  | 0 => NoneE "Too many recursive calls"
  | S steps' =>
      DO (c, rest) <==
        parseSimpleCommand steps' symtable xs;;
      DO (c', rest') <--
        firstExpect ";" (parseSequencedCommand steps' symtable) rest;;
        SomeE(c ; c', rest')
      OR
        SomeE(c, rest)
  end.

Definition parse (str : string) : optionE (com * list token) :=
  let tokens := tokenize str in
  parseSequencedCommand 1000 (build_symtable tokens 0) tokens.

(* ####################################################### *)
(* * Examples *)
(** * 例 *)

(*
Eval compute in parse "
    IF x == y + 1 + 2 - y * 6 + 3 THEN
      x := x * 1;
      y := 0
    ELSE
      SKIP
    END  ".
====>
    SomeE
       (IFB BEq (AId (Id 0))
                (APlus
                   (AMinus (APlus (APlus (AId (Id 1)) (ANum 1)) (ANum 2))
                      (AMult (AId (Id 1)) (ANum 6)))
                   (ANum 3))
        THEN Id 0 ::= AMult (AId (Id 0)) (ANum 1); Id 1 ::= ANum 0
        ELSE SKIP FI, [])
*)
(**
Eval compute in parse
[[
   "IF x == y + 1 + 2 - y * 6 + 3 THEN
      x := x * 1;
      y := 0
    ELSE
      SKIP
    END  ".
]]
====>
[[
    SomeE
       (IFB BEq (AId (Id 0))
                (APlus
                   (AMinus (APlus (APlus (AId (Id 1)) (ANum 1)) (ANum 2))
                      (AMult (AId (Id 1)) (ANum 6)))
                   (ANum 3))
        THEN Id 0 ::= AMult (AId (Id 0)) (ANum 1); Id 1 ::= ANum 0
        ELSE SKIP FI, [])
]]
*)

(*
Eval compute in parse "
    SKIP;
    z:=x*y*(x*x);
    WHILE x==x DO
      IF z <= z*z && not x == 2 THEN
        x := z;
        y := z
      ELSE
        SKIP
      END;
      SKIP
    END;
    x:=z  ".
====>
     SomeE
        (SKIP;
         Id 0 ::= AMult (AMult (AId (Id 1)) (AId (Id 2)))
                        (AMult (AId (Id 1)) (AId (Id 1)));
         WHILE BEq (AId (Id 1)) (AId (Id 1)) DO
           IFB BAnd (BLe (AId (Id 0)) (AMult (AId (Id 0)) (AId (Id 0))))
                     (BNot (BEq (AId (Id 1)) (ANum 2)))
              THEN Id 1 ::= AId (Id 0); Id 2 ::= AId (Id 0)
              ELSE SKIP FI;
           SKIP
         END;
         Id 1 ::= AId (Id 0),
        [])
*)
(**
Eval compute in parse
[[
   "SKIP;
    z:=x*y*(x*x);
    WHILE x==x DO
      IF z <= z*z && not x == 2 THEN
        x := z;
        y := z
      ELSE
        SKIP
      END;
      SKIP
    END;
    x:=z  ".
]]
====>
[[
     SomeE
        (SKIP;
         Id 0 ::= AMult (AMult (AId (Id 1)) (AId (Id 2)))
                        (AMult (AId (Id 1)) (AId (Id 1)));
         WHILE BEq (AId (Id 1)) (AId (Id 1)) DO
           IFB BAnd (BLe (AId (Id 0)) (AMult (AId (Id 0)) (AId (Id 0))))
                     (BNot (BEq (AId (Id 1)) (ANum 2)))
              THEN Id 1 ::= AId (Id 0); Id 2 ::= AId (Id 0)
              ELSE SKIP FI;
           SKIP
         END;
         Id 1 ::= AId (Id 0),
        [])
]]
*)

(*
Eval compute in parse "
   SKIP;
   z:=x*y*(x*x);
   WHILE x==x DO
     IF z <= z*z && not x == 2 THEN
       x := z;
       y := z
     ELSE
       SKIP
     END;
     SKIP
   END;
   x:=z  ".
=====>
      SomeE
         (SKIP;
          Id 0 ::= AMult (AMult (AId (Id 1)) (AId (Id 2)))
                (AMult (AId (Id 1)) (AId (Id 1)));
          WHILE BEq (AId (Id 1)) (AId (Id 1)) DO
            IFB BAnd (BLe (AId (Id 0)) (AMult (AId (Id 0)) (AId (Id 0))))
                     (BNot (BEq (AId (Id 1)) (ANum 2)))
              THEN Id 1 ::= AId (Id 0);
                   Id 2 ::= AId (Id 0)
              ELSE SKIP
            FI;
            SKIP
          END;
          Id 1 ::= AId (Id 0),
         []).
*)
(**
Eval compute in parse
[[
  "SKIP;
   z:=x*y*(x*x);
   WHILE x==x DO
     IF z <= z*z && not x == 2 THEN
       x := z;
       y := z
     ELSE
       SKIP
     END;
     SKIP
   END;
   x:=z  ".
]]
=====>
[[
      SomeE
         (SKIP;
          Id 0 ::= AMult (AMult (AId (Id 1)) (AId (Id 2)))
                (AMult (AId (Id 1)) (AId (Id 1)));
          WHILE BEq (AId (Id 1)) (AId (Id 1)) DO
            IFB BAnd (BLe (AId (Id 0)) (AMult (AId (Id 0)) (AId (Id 0))))
                     (BNot (BEq (AId (Id 1)) (ANum 2)))
              THEN Id 1 ::= AId (Id 0);
                   Id 2 ::= AId (Id 0)
              ELSE SKIP
            FI;
            SKIP
          END;
          Id 1 ::= AId (Id 0),
         []).
]]
*)


Require Import Crypto.Compilers.Syntax.
Require Import Crypto.Compilers.Wf.
Require Import Crypto.Compilers.InlineConstAndOpByRewriteWf.
Require Import Crypto.Compilers.ZExtended.Syntax.
Require Import Crypto.Compilers.ZExtended.InlineConstAndOpByRewrite.

Module Export Rewrite.
  Definition Wf_InlineConstAndOp {t} (e : Expr t) (Hwf : Wf e)
  : Wf (InlineConstAndOp e)
    := @Wf_InlineConstAndOp _ _ _ _ _ t e Hwf.

  Hint Resolve Wf_InlineConstAndOp : wf.
End Rewrite.


Set Suggest Proof Using.
Require Export Coq.Program.Tactics.
Require Export LibTactics.
(** printing × $\times$ #×# *)
(** printing :> $:$ #:># *)
(** printing ≡ $\equiv$ #≡# *)
(** printing [*] $*$ #*# *)
(** printing ∧ $\wedge$ #∧# *)
(** printing ∀ $\forall$ #∀# *)
(** printing → $\rightarrow$ #→# *)
(** printing ∃ $\exists$ #∃# *)
(** printing ≤ $\le$ #≤# *)
(** printing θ $\theta$ #θ# *)
(** printing eeev $\epsilon_v$ #∊ᵥ# *)
(** printing eeew $\epsilon_w$ #∊w# *)
(** printing tm $t_m$ #tm# *)
(** printing IR $\mathbb{R}$ #IR# *)
(** printing CR $\mathbb{R}$ #CR# *)
(** printing tr $t_r$ #tr# *)
(** remove printing * *)

(** printing ' $ $ #'# *)

Require Import Vector.
Require Import MathClasses.interfaces.canonical_names.
Require Import MCInstances.
Require Export ackermannSteering.
Require Import MCMisc.tactics.
Require Import fastReals.interface.
Require Import fastReals.misc.
Require Import errorBounds.

  Require Import IRMisc.LegacyIRRing.
  Add Ring cart2dir : Cart2DIRRing.

Require Export CartIR.
Require Import geometry2D.
Require Import geometry2DProps.

Local Opaque CSine.
Local Opaque CCos.
Local Opaque Sine.
Local Opaque Cosine.
Local Opaque Sin.
Local Opaque Cos.

  Local Notation minxy := (lstart).
  Local Notation maxxy := (lend).
  Local Notation  "∫" := Cintegral.

(**
* Characterizing the motion under Ackermann steering.

This file is highly experimental.
*)

Hint Unfold cos CosClassIR sin SinClassIR min MinClassIR  max MaxClassIR: IRMC.
Hint Unfold cos CosClassIR sin SinClassIR min MinClassIR  max MaxClassIR: AbstractR.

Global Instance ProperFrontUnitVec : Proper
    (equiv ==> equiv) (@frontUnitVec IR _ _).
Proof.
  intros x y Heq. destruct x, y. apply proj2 in Heq.
  unfold frontUnitVec. simpl in *. rewrite Heq.
  reflexivity.
Qed.

Global Instance ProperRHSUnitVec : Proper
  (equiv ==> equiv) (@rightSideUnitVec IR _ _ _ _ _ _ _).
Proof.
  intros x y Heq. destruct x, y. apply proj2 in Heq.
  unfold rightSideUnitVec. simpl in *. rewrite Heq.
  reflexivity.
Qed.

Global Instance properPosAtTime {maxTurnCurvature : Qpos}
   (acs : AckermannCar maxTurnCurvature) :
Proper (equiv ==> equiv) (posAtTime acs).
Proof.
  intros ? ? Heq; split; simpl;
  rewrite Heq; reflexivity.
Qed.

Global Instance properRigid2DState {maxTurnCurvature : Qpos}
   (acs : AckermannCar maxTurnCurvature) :
Proper (equiv ==> equiv) (rigidStateAtTime acs).
Proof.
  intros ? ? Heq; split; simpl;
  rewrite Heq; reflexivity.
Qed.

Global Instance CastCarDim `{Cast A B}
  : Cast (CarDimensions A) (CarDimensions B) :=
fun a =>  Build_CarDimensions
            ('lengthFront a)
            ('lengthBack a)
            ('width a).

Global Instance EquivCarDim `{Equiv A}: Equiv (CarDimensions A) := fun a b => (width a = width b)
  /\ (lengthFront a = lengthFront b)
  /\ (lengthBack a = lengthBack b).

Global Instance ProperCarDimLengthFront `{Equiv A}:
Proper (equiv ==> equiv) (@lengthFront A).
Proof.
  intros ? ? Heq. destruct Heq. tauto.
Qed.

Global Instance ProperCarDimLengthBack `{Equiv A}:
Proper (equiv ==> equiv) (@lengthBack A).
Proof.
  intros ? ? Heq. destruct Heq. tauto.
Qed.

Global Instance ProperCarDimWidth `{Equiv A}:
Proper (equiv ==> equiv) (@width A).
Proof.
  intros ? ? Heq. destruct Heq. tauto.
Qed.

(**width needs to be nonzero so that we can take its reciprocal in an arctan.
  others can also be required to be strictly positive*)

Global Instance EquivalenceCarDim  `{e:Equiv A}
  `{Equivalence _ e}
   : Equivalence EquivCarDim.
Proof using .
  unfold equiv, EquivCarDim. split.
  - intros x. destruct x. simpl. split; auto with *.
  - intros x y. destruct x,y. simpl. intros Hd; repnd;
      split; auto with relations.

  - intros x y z. destruct x,y,z. simpl. intros H0 H1.
    repnd.
    split; eauto 3
    with relations.
Qed.
Definition plausibleCarDim (cd : CarDimensions IR) : Prop :=
  0 ≤ lengthFront cd /\  0 ≤ width cd /\ 0 ≤ lengthBack cd.

Definition nonTrivialCarDim (cd : CarDimensions Q) :=
  0 < lengthFront cd  ∧  0 < width cd ∧ 0 < lengthBack cd.

Global Instance ProperCarMinMax : Proper
    (equiv ==> equiv ==> equiv) (@carMinMaxXY IR _ _ _ _ _ _ _ _ _ _).
Proof using.
  intros ? ? H0 x y Heq. unfold carMinMaxXY. simpl.
  unfold backLeft, frontLeft, frontRight, backRight.
  rewrite H0.
  rewrite Heq. reflexivity.
Qed.

(**
We characterize the motion of a car at a particular fixed turn curvature.
The speed is not fixed, even though it seems like an enticing temporary simpilification.
The assumption that [linSpeed] is continuous makes it impossible to assume
that the car immediately achieves the desired velocity from a state of rest.
Fortunately, the lack of constanthood assumption of [linSpeed]
does not complicate the integrals.

TODO: Ideally, we should let the turn curvature vary a bit
(upto some epsilon) during the process.
This will SIGNIFICANTLY complicate the integrals.
*)

Section Cases.
Context {maxTurnCurvature : Qpos}
(acs : AckermannCar maxTurnCurvature).

  Variable tstart : Time.
  Variable tend : Time.

  Hypothesis tstartEnd : (tstart ≤ tend).

  Definition confinedDuring (cd :CarDimensions IR) (rect: Line2D IR) :=
   forall  (t :Time),
    tstart ≤ t ≤ tend
    → carMinMaxXY cd (rigidStateAtTime acs t) ⊆ rect.

  Local Notation θ0 := ({theta acs} tstart).

  (** We will consider 2 classes of motions between [tstart] and [tend]. These classes suffice for our purpose

    -1) move with fixed steering wheel ([turnCurvature])
    -2) rotate the steering wheel while remaining stationary ([linVel = 0]).

  *)

  Section FixedSteeringWheel.
  Variable tc : IR.

  (*Move*)
  Lemma absSmall0 : forall x:IR,
    AbsSmall 0 x <-> x=0.
  Proof using.
    intros ?. split; intros Xx.
  - apply AbsSmall_imp_AbsIR in Xx.
    apply AbsIRLe0. assumption.
  - rewrite Xx. split; try reflexivity;[].
    setoid_rewrite negate_0. reflexivity.
  Qed.

(* TODO: It suffices to assume it for just rational times, because of continuity *)
  Hypothesis fixed : forall (t :Time),
    (tstart ≤ t ≤ tend)  -> {turnCurvature acs} t = tc.

  Lemma fixed2 : ∀ (t:Time), (tstart ≤ t ≤ tend)
    -> AbsSmall 0 (({turnCurvature acs} t) - tc).
  Proof using fixed.
    intros. apply absSmall0.
    apply equal_by_zero_sum.
    apply fixed. assumption.
  Qed.

  Lemma fixed3 : ∀ (t1:Time), (tstart ≤ t1 ≤ tend)
    -> forall t, (tstart ≤ t ≤ t1)
    -> AbsSmall 0 (({turnCurvature acs} t) - tc).
  Proof using fixed.
    intros. apply fixed2.
    split; try tauto.
    repnd.
    eapply transitivity; eauto.
  Qed.

  (** [theta] at time [t] is also needed obtain position at time [t] by integration *)
  Lemma fixedSteeringTheta : forall (t :Time)  (p: tstart ≤ t ≤ tend),
(** ib denotes the pair of numbers that goes at the bottom and at the top of ∫ *)
    let ib := @mkIntBnd _ tstart t (proj1 p) in
    ({theta acs} t - {theta acs} tstart) = tc* (∫ ib (linVel acs)).
  Proof using fixed.
    intros ? Hb ?.
    pose proof (fixed3 _ Hb) as Ht.
    apply errorBounds.thetaBall with (timeInc:=proj1 Hb) in Ht.
    rewrite mult_0_l in Ht.
    setoid_rewrite AbsIR_eq_x in Ht;[| reflexivity].
    apply absSmall0 in Ht.
    apply equal_by_zero_sum. assumption.
  Qed.

  Section NoSignChange.
  (** While characterizing the space needed by a move,
    the whole trajectory matters, not just the initial and final
    positions. So, we rule out the case of the car moving both
    forward and backward during an atomic move.*)
  Hypothesis nsc : noSignChangeDuring (linVel acs) tstart tend.

  Hypothesis tcSign : (tc≤0 \/ 0≤tc).

  (** As a result, during an atomic move,
    theta is always between its initial and final value. *)
  Lemma thetaMonotone : forall (t :Time)  (p: tstart ≤ t ≤ tend),
    inBetweenR ({theta acs} t) ({theta acs} tstart) ({theta acs} tend).
  Proof using All.
    eapply nosignChangeInBw;
      [assumption | eapply derivRot |].
    apply nonSignChangeMult; try assumption.
    destruct tcSign; [right|left]; intros t Hb; rewrite fixed; auto.
  Qed.

  End NoSignChange.
  (** We consider the case when the front wheels are not straight, i.e. the
      turn curvature is nonzero. The other case (front wheels are perfectly straight) is simpler,
      but needs to be handled differently due to "divide by 0" issues during integration.*)

  Section TCNZ.
  (**Needed because [tc] shows up as a denominator
     during integration below in [fixedCurvX].*)
  Hypothesis tcNZ : (tc [#] 0).
  Local Notation turnRadius  (* :IR *) := (f_rcpcl tc tcNZ).

  (** [X] coordinate of the [position] at a given time. Note that in CoRN,
      division is a ternary operator. [a[/]b[//][bp]] denotes the real number [a]
      divided by the non-zero real number [b], where [bp] is the proof of non-zero-hood
      of [b].
   *)
  Lemma fixedSteeeringX : forall (t :Time) (_: tstart ≤ t ≤ tend),
    ({X (position acs)} t - {X (position acs)} tstart) =
        ((Sin ({theta acs} t) - Sin ({theta acs} tstart)) [/] tc [//] tcNZ).
  Proof using (fixed tcNZ).
    intros  ? Hb.
    pose proof (fixed3 _ Hb) as Ht.
    apply errorBounds.fixedSteeeringX
        with (timeInc:=proj1 Hb) (tcNZ0:=tcNZ) in Ht.
    unfold cf_div in Ht.
    rewrite mult_0_l in Ht.
    setoid_rewrite mult_0_l in Ht.
    setoid_rewrite AbsIR_eq_x in Ht;[| reflexivity].
    apply absSmall0 in Ht.
    apply equal_by_zero_sum. assumption.
  Qed.

  Lemma tcnegNZ : - tc [#] 0.
  Proof using tcNZ.
    apply inv_resp_ap_zero. exact tcNZ.
  Qed.

  Lemma fixedSteeeringY : forall (t :Time) (_: tstart ≤ t ≤ tend),
    ({Y (position acs)} t - {Y (position acs)} tstart) =
        ((Cos ({theta acs} tstart) - Cos ({theta acs} t)) [/] tc [//] tcNZ).
  Proof using (fixed tcNZ).
    intros  ? Hb.
    pose proof (fixed3 _ Hb) as Ht.
    apply errorBounds.fixedSteeeringY
      with (timeInc:=proj1 Hb) (tcNZ0:=tcNZ) in Ht.
    unfold cf_div in Ht.
    rewrite mult_0_l in Ht.
    setoid_rewrite mult_0_l in Ht.
    setoid_rewrite AbsIR_eq_x in Ht;[| reflexivity].
    apply absSmall0 in Ht.
    apply equal_by_zero_sum. assumption.
  Qed.

  Lemma fixedSteeeringXY : forall (t :Time) (_: tstart ≤ t ≤ tend),
    posAtTime acs t - posAtTime acs tstart =
      'turnRadius * (rhsUnitVecAtTime acs t - rhsUnitVecAtTime acs tstart).
  Proof using (fixed tcNZ).
    intros ? Hb.
    unfold rhsUnitVecAtTime, rightSideUnitVec, rigidStateAtTime.
    simpl. rewrite unitVecMinus90, unitVecMinus90.
    unfold posAtTime. split; simpl;
      [rewrite fixedSteeeringX by assumption| rewrite fixedSteeeringY by assumption];
    autounfold with IRMC; unfold cf_div; ring.
  Qed.

  Section CornersCircle.
  Variable cd :CarDimensions IR.

  (** To characterize the space required, one needs to study the motion of the corners.
      Note that [unitVec ({theta acs} t)] and [rhsUnitVecAtTime acs t] are orthogonal
      to each other at all times [t]. Also, the magnitudes in these directions are exactly
      the coordinates in the reference frame with origin at the turnCenter, and the
      [rhsUnitVecAtTime acs t] axis pointing towards the car. Hence,
      each corner is rotating around the turnCenter, as explained in
      https://www.youtube.com/watch?v=oYMMdjbmQXc
      *)
  Lemma backRightXYAM : forall (t :Time) (Hb : tstart ≤ t ≤ tend),
    backRightAtTime acs t cd - backRightAtTime acs tstart cd =
    '(turnRadius + width cd) * (rhsUnitVecAtTime acs t - rhsUnitVecAtTime acs tstart) -
    'lengthBack cd * (unitVec ({theta acs} t) - unitVec θ0).
  Proof using (cd fixed tcNZ).
    intros ? ?. unfold backRightAtTime,  backRight, frontUnitVec . simpl.
    fold (rhsUnitVecAtTime acs t).
    fold (rhsUnitVecAtTime acs tstart).
    match goal with
    [|- equiv ?l _] => assert
      (l=(posAtTime acs t - posAtTime acs tstart)
          - (' lengthBack cd) * (unitVec ({theta acs} t) - unitVec θ0)
          + (' width cd) * (rhsUnitVecAtTime acs t - rhsUnitVecAtTime acs tstart))
        as Heq by ring; rewrite Heq; clear Heq
    end.

    rewrite fixedSteeeringXY by assumption.
    setoid_rewrite <- sameXYAdd. unfold cast, castCRCart2DCR.
    ring.
  Qed.

  Lemma backLeftXYAM : forall (t :Time) (Hb : tstart ≤ t ≤ tend),
    backLeftAtTime acs t cd - backLeftAtTime acs tstart cd =
    '(turnRadius - width cd) * (rhsUnitVecAtTime acs t - rhsUnitVecAtTime acs tstart) -
    'lengthBack cd * (unitVec ({theta acs} t) - unitVec θ0).
  Proof using (cd fixed tcNZ).
    intros ? ?. unfold backLeftAtTime,  backLeft, frontUnitVec . simpl.
    fold (rhsUnitVecAtTime acs t).
    fold (rhsUnitVecAtTime acs tstart).
    match goal with
    [|- equiv ?l _] => assert
      (l=(posAtTime acs t - posAtTime acs tstart)
          - (' lengthBack cd) * (unitVec ({theta acs} t) - unitVec θ0)
          - (' width cd) * (rhsUnitVecAtTime acs t - rhsUnitVecAtTime acs tstart))
        as Heq by ring; rewrite Heq; clear Heq
    end.

    rewrite fixedSteeeringXY by assumption.
    setoid_rewrite <- sameXYAdd. unfold cast, castCRCart2DCR.
    rewrite sameXYNegate.
    ring.
  Qed.

  Lemma frontRightXYAM : forall (t :Time) (Hb : tstart ≤ t ≤ tend),
    frontRightAtTime acs t cd - frontRightAtTime acs tstart cd =
    '(turnRadius + width cd) * (rhsUnitVecAtTime acs t - rhsUnitVecAtTime acs tstart)
    + 'lengthFront cd * (unitVec ({theta acs} t) - unitVec θ0).
  Proof using (cd fixed tcNZ).
    intros ? ?. unfold frontRightAtTime,  frontRight, frontUnitVec . simpl.
    fold (rhsUnitVecAtTime acs t).
    fold (rhsUnitVecAtTime acs tstart).
    match goal with
    [|- equiv ?l _] => assert
      (l=(posAtTime acs t - posAtTime acs tstart)
          + (' lengthFront cd) * (unitVec ({theta acs} t) - unitVec θ0)
          + (' width cd) * (rhsUnitVecAtTime acs t - rhsUnitVecAtTime acs tstart))
        as Heq by ring; rewrite Heq; clear Heq
    end.

    rewrite fixedSteeeringXY by assumption.
    setoid_rewrite <- sameXYAdd. unfold cast, castCRCart2DCR.
    ring.
  Qed.

  Lemma frontLeftXYAM : forall (t :Time) (Hb : tstart ≤ t ≤ tend),
    frontLeftAtTime acs t cd - frontLeftAtTime acs tstart cd =
    '(turnRadius - width cd) * (rhsUnitVecAtTime acs t - rhsUnitVecAtTime acs tstart)
    + 'lengthFront cd * (unitVec ({theta acs} t) - unitVec θ0).
  Proof using (cd fixed tcNZ).
    intros ? ?. unfold frontLeftAtTime,  frontLeft, frontUnitVec . simpl.
    fold (rhsUnitVecAtTime acs t).
    fold (rhsUnitVecAtTime acs tstart).
    match goal with
    [|- equiv ?l _] => assert
      (l=(posAtTime acs t - posAtTime acs tstart)
          + (' lengthFront cd) * (unitVec ({theta acs} t) - unitVec θ0)
          - (' width cd) * (rhsUnitVecAtTime acs t - rhsUnitVecAtTime acs tstart))
        as Heq by ring; rewrite Heq; clear Heq
    end.

    rewrite fixedSteeeringXY by assumption.
    setoid_rewrite <- sameXYAdd. unfold cast, castCRCart2DCR.
    rewrite sameXYNegate.
    ring.
  Qed.

  End CornersCircle.
    Require Import MCMisc.rings.

  Lemma turnRigidStateAtθCorrect: forall (t :Time)  (p: tstart ≤ t ≤ tend),
    rigidStateAtTime acs t
    = turnRigidStateAtθ
            (rigidStateAtTime acs tstart)
            turnRadius
            ({theta acs} t).
  Proof using fixed.
    intros ? ?.
    split;[| reflexivity].
    simpl. apply RingShiftMinus.
    split;simpl;[apply fixedSteeeringX | apply fixedSteeeringY];
    assumption.
  Qed.

  Lemma holdsDuringAMDistIf : forall(P: (Rigid2DState IR) -> Prop)
    `{@Setoid_Morphism  _ _ _ _ P},
  let tcc := (existT _ tc tcNZ) in
    noSignChangeDuring (linVel acs) tstart tend
    ->
    (∀ (d : IR), inBetweenR d 0 (∫ (mkIntBnd tstartEnd) (linVel acs))
      -> P (turnRigidStateAtDist (rigidStateAtTime acs tstart) tcc d))
     ->  (∀ t : Time, tstart ≤ t ≤ tend → P (rigidStateAtTime acs t)).
  Proof using fixed tstartEnd.
    simpl.
    intros ? ?  Hn hh t Hb.
    specialize (hh ((∫ (mkIntBnd (proj1 Hb)) (linVel acs)))).
    rewrite turnRigidStateAtθCorrect;[| assumption].
    unfold turnRigidStateAtDist, recipT, RecipTIR in hh. simpl in hh.
    rewrite <- fixedSteeringTheta in hh.
    assert  (θ0 + ({theta acs} t - θ0) = {theta acs} t) as Heq by IRring.
    rewrite Heq in hh. clear Heq. repnd.
    apply hh. apply nosignChangeInBwInt; eauto.
  Qed.

  Lemma holdsDuringAMIf : forall(P: (Rigid2DState IR) -> Prop)
    `{@Setoid_Morphism  _ _ _ _ P},
    noSignChangeDuring (linVel acs) tstart tend
    ->
    (∀ (θ : IR), inBetweenR θ ({theta acs} tstart) ({theta acs} tend)
      -> P (turnRigidStateAtθ (rigidStateAtTime acs tstart) turnRadius θ))
     ->  (∀ t : Time, tstart ≤ t ≤ tend → P (rigidStateAtTime acs t)).
  Proof using fixed tstartEnd.
    intros ? ?  Hn hh t Hb.
    specialize (hh ({theta acs}t)).
    rewrite turnRigidStateAtθCorrect;[| assumption].
    apply hh.
    apply thetaMonotone; try assumption.
    pose proof (fst (less_conf_ap _ _ _) tcNZ) as Hdec.
    autounfold with IRMC.
    destruct Hdec; [left|right]; eauto 2 with CoRN.
  Qed.

  Lemma auxConfinedDuringAMIf : forall (confineRect : Line2D IR) cd,
    noSignChangeDuring (linVel acs) tstart tend
    ->
    (∀ (θ : IR), inBetweenR θ ({theta acs} tstart) ({theta acs} tend)
  -> carMinMaxXY cd
     (turnRigidStateAtθ (rigidStateAtTime acs tstart) turnRadius θ)
           ⊆ confineRect)
     ->  confinedDuring cd confineRect.
  Proof using fixed tstartEnd.

    intros ? ? Hn hh t Hb. apply holdsDuringAMIf; auto.
    constructor; eauto 2 with typeclass_instances.
    unfold Setoid. eauto 2 with typeclass_instances.
    intros ? ? Heq. rewrite Heq. reflexivity.
  Qed.

  End TCNZ.
  End FixedSteeringWheel.

  Section LinVel0.
  (** Now consider the second case where the steering wheel may move, but the car remains stationary *)
    Hypothesis lv0 :  forall (t :Time),
      (tstart ≤ t ≤ tend)  -> {linVel acs} t = 0.

    Lemma LV0Theta : forall (t :Time)  (p: tstart ≤ t ≤ tend),
        {theta acs} t = {theta acs} tstart.
    Proof using lv0.
      intros. eapply TDerivativeEq0;[tauto | apply derivRot|].
      intros tt Hb. simpl. rewrite lv0;autounfold with IRMC; [ring|].
      repnd. split; eauto 2 with CoRN.
    Qed.

 Local Opaque FCos.
    Lemma LV0X : forall (t :Time) (p: tstart ≤ t ≤ tend),
      {X (position acs)} t = {X (position acs)} tstart .
    Proof using lv0.
      intros. eapply TDerivativeEq0;[tauto | apply derivX|].
      intros tt Hb.
      simpl. rewrite lv0;autounfold with IRMC; [ring|].
      repnd. split; eauto 2 with CoRN.
    Qed.

    Lemma LV0Y : forall (t :Time) (p: tstart ≤ t ≤ tend),
      {Y (position acs)} t = {Y (position acs)} tstart .
    Proof using lv0.
      intros. eapply TDerivativeEq0;[tauto | apply derivY|].
      intros tt Hb.
      simpl. rewrite lv0;autounfold with IRMC; [ring|].
      repnd. split; eauto 2 with CoRN.
    Qed.

    Lemma LV0 : forall (t :Time) (p: tstart ≤ t ≤ tend),
      rigidStateAtTime acs t = rigidStateAtTime acs tstart.
    Proof using lv0.
      intros. split;[split|]; simpl;
      eauto using LV0Theta, LV0X, LV0Y.
    Qed.

  End LinVel0.
End Cases.