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

Require Export Vector.
Require Export ROSCyberPhysicalSystem.

Require Export MathClasses.interfaces.canonical_names.
Require Export MCInstances.
Require Export CartCR.

Definition initialVel : (Polar2D Q) := {|rad:=0; θ:=0|}.

Require Export CartIR.

Notation FConst := ConstTContR.
Notation FSin:= CFSine.
Notation FCos:= CFCos.

Record iCreate : Type := {
  position :> Cart2D TContR;
  theta : TContR;
  transVel : TContR;
  omega : TContR;

Initial (at time:=0) Conditions
Robot is asked to go to the target relative to current position. This function defines the list of messages that the robot will send to the motor so that it will go to the target position. X axis of target points towards the direction that robot will move and Y points to its left side. it might be better to make Y point in robot's direction. Then add 90 in cartesian <-> polar conversion.

Section HardwareAgents.

  `{rtopic : RosTopicType RosTopic}
  `{dteq : Deq RosTopic}
  `{etype : @EventType _ _ _ Event LocT minGap tdeq}.

Variable VELOCITY : RosTopic.
Hypothesis VelTOPICType : (topicType VELOCITY Polar2D Q).

Variable reacTime : QTime.
It is more sensible to change the type to QNonNeg as the value certainly does not represent time. However, the coercion QT2Q does not work then
Variable motorPrec : Polar2D Q Polar2D QTime.

Hypothesis motorPrec0 : motorPrec {| rad :=0 ; θ :=0 |} {| rad :=0 ; θ :=0 |}.

Close Scope Q_scope.

Definition latestVelPayloadAndTime
  (evs : natoption Event) (t : QTime) : ((Polar2D Q) × QTime) :=
(@transport _ _ _ (λ t, tt ** QTime) VelTOPICType
   (lastPayloadAndTime VELOCITY evs t)) initialVel.

Definition correctVelDuring
  (lastVelCmd : (Polar2D Q))
  (lastTime: QTime)
  (uptoTime : QTime)
  (robot: iCreate) :=

    (transVel robot)
    lastTime uptoTime
    (rad lastVelCmd)
    (rad (motorPrec lastVelCmd))
    (omega robot)
    lastTime uptoTime
    (θ lastVelCmd)
    (θ (motorPrec lastVelCmd)).

Definition corrSinceLastVel
  (evs : natoption Event)
  (uptoTime : QTime)
  (robot: iCreate) :=
let (lastVel, lastTime) := latestVelPayloadAndTime evs uptoTime in
correctVelDuring lastVel lastTime uptoTime robot.

Definition HwAgent : Device iCreate :=
λ (robot: iCreate) (evs : natoption Event) ,
  ( t: QTime, corrSinceLastVel evs t robot)
   n:nat, isDeqEvtOp (evs n).

Definition onlyRecvEvts (evs : natoption Event) : Prop :=
n:nat, isDeqEvtOp (evs n).

Definition ∊ᵥ a b : Q :=
 (rad (motorPrec {|rad:= a; θ:= b|})).

Definition ∊w a b : Q :=
 (θ (motorPrec {|rad:= a; θ:= b|})).

partial simplified definition shown in the paper :
Definition HwAgentP (ic: iCreate) (evs : natoption Event): Prop :=
onlyRecvEvts evs t: QTime,
  let (lastCmd, tm ) := latestVelPayloadAndTime evs t in
  let a : Q := rad (lastCmd) in
  let b : Q := θ (lastCmd) in
   tr : QTime, (tm tr tm + reacTime)
     ( t' : QTime, (tm t' tr)
         (Min ({transVel ic} tm) (a - ∊ᵥ a b)
             {transVel ic} t' Max ({transVel ic} tm) (a+ ∊ᵥ a b)))
     ( t' : QTime, (tr t' t) |{transVel ic} t' - a | Q2R (∊ᵥ a b)).

Lemma HwAgentPCorr1 :
   icr evs,
  HwAgent icr evsHwAgentP icr evs.
  intros ? ? H.
  destruct H as [Hr H].
  split; [assumption|]. clear H.
  intros t.
  specialize (Hr t).
  unfold corrSinceLastVel in Hr.
  destruct (latestVelPayloadAndTime evs t) as [lc lt].
  apply proj1 in Hr.
  destruct Hr as [qtrans Hr].
  unfold le, Le_instance_QTime.
  autounfold with IRMC. unfold Plus_instance_QTime.
  unfoldMC. repnd.
  split; auto.
  unfold ∊ᵥ.
  destruct lc.
  simpl Vector.rad.
  simpl Vector.rad in Hrrl, Hrrr.
  unfold Q2R. setoid_rewrite inj_Q_inv.
  split; auto.
  unfold between, Q2R in Hrrr.
  intros ? H.
  apply Hrrr in H.
  autorewrite with QSimpl in H.
  exact H.
End HardwareAgents.