(** * Composition of functors *)
Require Import Category.Core Functor.Core.

Set Universe Polymorphism.
Set Implicit Arguments.
Generalizable All Variables.
Set Asymmetric Patterns.

Local Open Scope morphism_scope.

Section composition.
  Variables B C D E : PreCategory.
  Variable G : Functor D E.
  Variable F : Functor C D.

  (** We usually don't want to see the proofs of composition in functors, because the proofs are hProps, and so we don't care about them.  But occasionally, we want to be able to reduce the proofs.  Having the proofs transparent allows the composition of the identity functor with itself to be judgementally the identity.  Since the only way to hide something from within a proof is [abstract], and that makes the definitions opaque, we need to define the laws separately. *)

  Local Notation c_object_of c := (G (F c)) (only parsing).
  Local Notation c_morphism_of m := (G _1 (F _1 m)) (only parsing).
  Definition compose_composition_of s d d'
      (m1 : morphism C s d) (m2 : morphism C d d')
  : c_morphism_of (m2 o m1) = c_morphism_of m2 o c_morphism_of m1
    := transport (@paths _ (c_morphism_of (m2 o m1)))
                 (composition_of G _ _ _ _ _)
                 (ap (fun m => G _1 m) (composition_of F _ _ _ m1 m2)).

  Definition compose_identity_of x
  : c_morphism_of (identity x) = identity (c_object_of x)
    := transport (@paths _ _)
                 (identity_of G _)
                 (ap (fun m => G _1 m) (identity_of F x)).

  Definition compose : Functor C E
    := Build_Functor
         C E
         (fun c => G (F c))
         (fun _ _ m => G _1 (F _1 m))
         compose_composition_of
         compose_identity_of.
End composition.

Global Arguments compose_composition_of / .
Global Arguments compose_identity_of / .

Module Export FunctorCompositionCoreNotations.
  Infix "o" := compose : functor_scope.
End FunctorCompositionCoreNotations.


Require Export XR_R.
Require Export XR_Rmult.

Local Open Scope R_scope.
Implicit Type r : R.

Lemma Rmult_eq_compat_r : forall r r1 r2, r1 = r2 -> r1 * r = r2 * r.
Proof.
  intros x y z.
  intro heq.
  subst z.
  reflexivity.
Qed.


Set Universe Polymorphism.
Set Primitive Projections.
Unset Printing Primitive Projection Compatibility.

Inductive unit := tt.

Record sig A (P : A -> Type) := exist {
  wit : A;
  prf : P wit;
}.

Arguments wit {_ _} _.
Arguments prf {_ _} _.

Definition M A := forall Ω : Prop, (A -> Ω) -> Ω.
Definition ret {A} (x : A) : M A := fun Ω k => k x.

(** The underlying type is the type of stacks *)
Inductive Typeᴿ : M (Prop -> Type) -> Type :=
| IsType : forall (A : Prop -> Type),
  ((forall Ω : Prop, A Ω -> Ω) -> Type) -> Typeᴿ (ret A)
.

Definition TYPE := sig (M (Prop -> Type)) Typeᴿ.

Definition Typeᶠ : TYPE :=
 exist _ _ _ (IsType (fun Ω : Prop => (Prop -> Type) -> Ω) Typeᴿ).

Definition C (Ω : Prop) (A : TYPE) : Type :=
  match A.(prf) with IsType A Aᴿ => A Ω end.

Definition Cᴿ (A : TYPE) : (forall Ω : Prop, C Ω A -> Ω) -> Type :=
  match A.(prf) as T return
    (forall Ω, match T with IsType A _ => _ end -> _) -> Type
  with IsType A Aᴿ => Aᴿ end.

Definition El (A : TYPE) : Type := sig (forall Ω : Prop, C Ω A -> Ω) (Cᴿ A).

Check Typeᶠ : El Typeᶠ.

Definition Prodᶠ (A : TYPE) (B : El A -> TYPE) : El Typeᶠ.
Proof.
unshelve refine (exist _ _ (ret _) _).
+ refine (sig (El A) (fun x => C (B x))).
+ refine (IsType _ (fun f => _)).
  refine (forall x : El A, Cᴿ (B x) (fun ω => f (exist _ _ x ω))).
Defined.

Notation "A →ᶠ B" := (Prodᶠ A (fun _ => B)) (at level 99, right associativity, B at level 200).
Notation "'Πᶠ'  x .. y , P" := (Prodᶠ _ (fun x => .. (Prodᶠ _ (fun y => P)) ..))
  (at level 200, x binder, y binder, right associativity).

Definition Lamᶠ {A B} (f : forall x : El A, El (B x)) : El (Prodᶠ A B).
Proof.
unshelve refine (exist _ _ (fun ω => _) _).
+ unshelve refine ((f ω.(wit)).(wit) ω.(prf)).
+ unshelve refine (fun x => (f x).(prf)).
Defined.

Notation "'λᶠ'  x .. y , t" := (Lamᶠ (fun x => .. (Lamᶠ (fun y => t)) ..))
  (at level 200, x binder, y binder, right associativity).

Definition Appᶠ {A B} (f : El (Prodᶠ A B)) (x : El A) : El (B x).
Proof.
unshelve refine (exist _ _ (fun ω => f.(wit) (exist _ _ x ω)) _).
unshelve refine (f.(prf) _).
Defined.

Eval compute in eq_refl : (fun A B f x => Appᶠ (@Lamᶠ A B f) x) = (fun A B f x => f x).
Eval compute in eq_refl : (fun A B f => @Lamᶠ A B (fun x => Appᶠ f x)) = (fun A B f => f).

Notation "t · u" := (Appᶠ t u) (at level 11, left associativity).

Inductive Falseᴿ : M False -> Type :=.

Definition Falseᶠ : TYPE :=
  exist _ _ (ret (False -> Ω)) (IsType _ Falseᴿ).

Inductive boolᴿ : M bool -> Type :=
| trueᴿ : boolᴿ (ret true)
| falseᴿ : boolᴿ (ret false).

Definition boolᶠ : TYPE :=
  exist _ _ (ret (bool -> Ω)) (IsType _ boolᴿ).

Definition trueᶠ : El boolᶠ := exist _ _ _ trueᴿ.
Definition falseᶠ : El boolᶠ := exist _ _ _ falseᴿ.

Definition bool_rectᶠ : El
  (Πᶠ (P : El (boolᶠ →ᶠ Typeᶠ)), P · trueᶠ →ᶠ P · falseᶠ →ᶠ
  Πᶠ (b : El boolᶠ), P · b).
Proof.
refine (λᶠ P Pt Pf b, _).
refine (
match b.(prf) as br in boolᴿ b return El (Appᶠ P (exist _ _ _ br)) with
| trueᴿ => Pt
| falseᴿ => Pf
end
).
Defined.

Eval compute in eq_refl :
  (fun P Pt Pf => bool_rectᶠ · P · Pt · Pf · trueᶠ) = (fun P Pt Pf => Pt).
Eval compute in eq_refl :
  (fun P Pt Pf => bool_rectᶠ · P · Pt · Pf · falseᶠ) = (fun P Pt Pf => Pf).

Definition squash : El (Typeᶠ →ᶠ Typeᶠ).
Proof.
refine (λᶠ A, _).
unshelve refine (exist _ _ (ret (C A)) (IsType _ _)).
unshelve refine (fun _ => unit).
Defined.

Notation "[[ A ]]" := (squash · A).

Definition inhabits : El (Πᶠ (A : El Typeᶠ), A →ᶠ [[ A ]]).
Proof.
refine (λᶠ A x, _).
unshelve refine (exist _ _ x.(wit) tt).
Defined.

Lemma cc : El (Πᶠ (A B : El Typeᶠ), ((A →ᶠ [[ B ]]) →ᶠ A) →ᶠ [[ A ]]).
Proof.
refine (λᶠ A B f, _).
simple refine (exist _ _ (fun ω => _) _).
+ unshelve refine ((f).(wit) (exist _ _ _ _)); [|refine ω].
  unshelve refine (exist _ _ (fun k => (k.(wit).(wit) ω)) _).
  refine (fun _ => tt).
+ refine tt.
Defined.

Lemma squash_consistency : El ([[ Falseᶠ ]]) -> False.
Proof.
refine (fun e => match e.(wit) (False_rect _) with end).
Defined.

Definition squash_bool_caseᶠ :
  El (Πᶠ (P : El Typeᶠ) (Pt : El [[ P ]]) (Pf : El [[ P ]]) (b : El [[ boolᶠ ]]), [[ P ]]).
Proof.
unshelve refine (λᶠ P Pt Pf b, exist _ _ (fun ω => _) _).
+ refine (b.(wit) (fun b => match b with true => Pt.(wit) ω | false => Pf.(wit) ω end)).
+ exact tt.
Defined.

(* Not true : we would need extensional unit types *)
(* Check (eq_refl : (fun P Pt Pf => squash_bool_caseᶠ · P · Pt · Pf · (inhabits · boolᶠ · trueᶠ)) = (fun P Pt Pf => Pt)). *)
(* Check (eq_refl : (fun P Pt Pf => squash_bool_caseᶠ · P · Pt · Pf · (inhabits · boolᶠ · falseᶠ)) = (fun P Pt Pf => Pf)). *)

Lemma squash_bool_caseᶠ_true :
  forall P Pt Pf, squash_bool_caseᶠ · P · Pt · Pf · (inhabits · boolᶠ · trueᶠ) = Pt.
Proof.
intros P Pt Pf; compute.
destruct Pt as [? [ ] ]; reflexivity.
Defined.

Lemma squash_bool_caseᶠ_false :
  forall P Pt Pf, squash_bool_caseᶠ · P · Pt · Pf · (inhabits · boolᶠ · falseᶠ) = Pf.
Proof.
intros P Pt Pf; compute.
destruct Pf as [? [ ] ]; reflexivity.
Defined.

Inductive psigma (A : Type) (B : A -> Type) :=
| pexist : forall x, B x -> psigma A B.

Inductive sigmaᴿ (A : El Typeᶠ) (B : El (A →ᶠ Typeᶠ)) :
  M (psigma (El A) (fun x => El (B · x))) -> Type :=
| existᴿ : forall x y, sigmaᴿ A B (ret (pexist _ _ x y)).

Definition sigmaᶠ : El (Πᶠ (A : El Typeᶠ) (B : El (A →ᶠ Typeᶠ)), Typeᶠ).
Proof.
refine (λᶠ A B, _).
unshelve refine (exist _ _ (ret (_ -> Ω)) (IsType _ _)).
+ refine (psigma (El A) (fun x => El (B · x))).
+ refine (sigmaᴿ _ _).
Defined.

Definition existᶠ : El (
  Πᶠ (A : El Typeᶠ) (B : El (A →ᶠ Typeᶠ)) (x : El A) (y : El (B · x)),
    sigmaᶠ · A · B).
Proof.
refine (λᶠ A B x y, _).
refine (exist _ _ (ret _) (existᴿ A B x y)).
Defined.

Definition sigma_rectᶠ : El
  (Πᶠ (A : El Typeᶠ) (B : El (A →ᶠ Typeᶠ)) (P : El (sigmaᶠ · A · B →ᶠ Typeᶠ)),
  (Πᶠ x y, P · (existᶠ · A · B · x · y)) →ᶠ Πᶠ (p : El (sigmaᶠ · A · B)), P · p).
Proof.
refine (λᶠ A B P Pex p, _).
refine (
match p.(prf) as pr in sigmaᴿ _ _ p return El (P · (exist _ _ _ pr)) with
| existᴿ _ _ x y => Pex · x · y
end
).
Defined.

Definition choice : El (
  Πᶠ (A B : El Typeᶠ) (P : El (A →ᶠ B →ᶠ Typeᶠ)),
    (Πᶠ x, [[ sigmaᶠ · B · (λᶠ y, P · x · y) ]]) →ᶠ
    [[ sigmaᶠ · (A →ᶠ B) · (λᶠ f, Πᶠ x, P · x · (f · x)) ]]
).
Proof.
refine (λᶠ A B P f, _).
revert f; intros [f _].
unshelve refine (exist _ _ _ tt).
cbn in *.
refine (fun k => _).
assert (k' := fun f y => k (pexist _ _ f y)); clear k; rename k' into k.
cbn in *.
assert (f' := fun x y => f (exist _ _ x y)); clear f; rename f' into f.
cbn in *.

unshelve refine (k _ _).
+ unshelve refine (exist _ _ _ _).
  - cbn; intros [x ω].
    refine (f x _).
    intros [y _]; refine (y.(wit) ω).
  - intros x.
cbn.

Abort.

Definition choice : El (
  Πᶠ (A : El Typeᶠ) (B : El (A →ᶠ Typeᶠ)),
    (Πᶠ x, [[ B · x ]]) →ᶠ
    [[ Πᶠ x, B · x ]]
).
Proof.
refine (λᶠ A B f, _).
refine (exist _ _ _ tt).
cbn; intros [x k].
refine (f.(wit) (exist _ _ x k)).
Defined.

Inductive sum (A B : Type) :=
| inl : A -> sum A B
| inr : B -> sum A B.

Inductive sumᴿ (A B : El Typeᶠ) :
  M (sum (El A) (El B)) -> Type :=
| inlᴿ : forall x, sumᴿ A B (ret (inl _ _ x))
| inrᴿ : forall y, sumᴿ A B (ret (inr _ _ y)).

Definition sumᶠ : El (Typeᶠ →ᶠ Typeᶠ →ᶠ Typeᶠ).
Proof.
refine (λᶠ A B, _).
refine (exist _ _ (ret _) (IsType _ (sumᴿ A B))).
Defined.

Definition em : El (Πᶠ A, [[ sumᶠ · A · (A →ᶠ Falseᶠ) ]]).
Proof.
refine (λᶠ A, _).
refine (exist _ _ _ tt).
cbn.
refine (fun ω => ω (inr _ _ (λᶠ x, _))).
destruct (ω (inl _ _ x)).
Defined.


(**********************************************************************)
(* This program is free software; you can redistribute it and/or      *)
(* modify it under the terms of the GNU Lesser General Public License *)
(* as published by the Free Software Foundation; either version 2.1   *)
(* of the License, or (at your option) any later version.             *)
(*                                                                    *)
(* This program is distributed in the hope that it will be useful,    *)
(* but WITHOUT ANY WARRANTY; without even the implied warranty of     *)
(* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the      *)
(* GNU General Public License for more details.                       *)
(*                                                                    *)
(* You should have received a copy of the GNU Lesser General Public   *)
(* License along with this program; if not, write to the Free         *)
(* Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA *)
(* 02110-1301 USA                                                     *)
(**********************************************************************)

(**********************************************************************)
(*               Delayed Substitution Lambda Calculus                 *)
(*                                                                    *)
(* is implemented in Coq by adapting the implementation of            *)
(* Lambda Calculus from Project Coq                                   *)
(* 2015                                                               *)
(**********************************************************************)

(**********************************************************************)
(*                        Tuple_Terms.v                               *)
(*                                                                    *)
(* adapted from Terms.v for Lambda Calculus                           *)
(*                                                                    *)
(*                          Barry Jay                                 *)
(*                                                                    *)
(**********************************************************************)

Require Import Arith.
Require Import General.

(* lambda terms using de Bruijn's indices *)

Inductive lambda:  Set :=
| Unit  : lambda
| Pair: lambda -> lambda -> lambda
| Ref : nat -> lambda -> lambda
| Raise: lambda -> nat -> nat -> lambda
| Abs : nat -> lambda -> lambda
| App : lambda -> lambda -> lambda .

Lemma decidable_term_equality : forall (M N: lambda), M = N \/ M<> N.
Proof.
  induction M; induction N; intros; try (left; congruence); try(right; congruence).
  (* 5 *)
  elim(IHM1 N1); elim(IHM2 N2); auto; (right; congruence; fail) || left; congruence.
  (* 4 *)
  elim(decidable_nats n0 n); auto. elim(IHM N); split_all. left; congruence. right; congruence.
  right; congruence.
  (* 3 *)
  elim(decidable_nats n1 n); auto. elim(decidable_nats n0 n2); auto.
  elim(IHM N); split_all. left; congruence. right; congruence.
  right; congruence.  right; congruence.
  (* 2 *)
  elim(IHM N); split_all.  elim(decidable_nats n0 n); auto. left; congruence.
  right; congruence.  right; congruence.
  (* 1 *)
   elim(IHM1 N1); elim(IHM2 N2); auto; (right; congruence; fail) || left; congruence.
Qed.


Require Import Coq.Lists.List.
Require Import Coq.Bool.Bool.
Require Import NetCore.NetCoreEval.
Require Import Common.Types.
Require Import Classifier.Classifier.
Require Import Word.WordInterface.
Require Import Pattern.Pattern.
(* TODO: MJR Move 'switchId' from messagesDef so that we don't have to include this whole thing *)
Require Import OpenFlow.OpenFlow0x01Types.
Require Import Network.NetworkPacket.

Set Implicit Arguments.

Import ListNotations.

Fixpoint compile_pred (opt : Classifier bool -> Classifier bool)
         (pr : pred) (sw : switchId) : Classifier bool :=
  match pr with
    | PrHdr pat => [(pat, true)]
    | PrOnSwitch sw' =>
      match Word64.eq_dec sw sw' with
        | left _ => [(Pattern.all, true)]
        | right _ => []
      end
    | PrOr pr1 pr2 => opt (union orb (compile_pred opt pr1 sw)
                                 (compile_pred opt pr2 sw))
    | PrAnd pr1 pr2 => opt (inter andb (compile_pred opt pr1 sw)
                                  (compile_pred opt pr2 sw))
    | PrNot pr' =>
      opt (map (second negb)
               (compile_pred opt pr' sw ++ [(Pattern.all, false)]))
    | PrAll => [(Pattern.all, true)]
    | PrNone => []
  end.

Definition apply_act (a : list act) (b : bool) :=
  match b with
    | true => a
    | false => nil
  end.

(** TODO(arjun): rank-2 polymorphism. The extracted code makes me nervous. *)
Fixpoint compile_pol
  (opt : forall (A : Type), Classifier A -> Classifier A)
  (p : pol) (sw : switchId) : Classifier (list act) :=
  match p with
    | PoAtom pr act =>
      opt _ (map (second (apply_act act))
                 (compile_pred (opt bool) pr sw ++ [(Pattern.all, false)]))
    | PoUnion pol1 pol2 =>
      opt _ (union (@app act)
                   (compile_pol opt pol1 sw)
                   (compile_pol opt pol2 sw))
  end.

Fixpoint strip_empty_rules (A : Type) (cf : Classifier A) : Classifier A :=
  match cf with
    | nil => nil
    | (pat, acts) :: cf =>
      if Pattern.is_empty pat
      then strip_empty_rules cf
      else (pat, acts) :: strip_empty_rules cf
  end.

Definition no_opt (A : Type) := @Datatypes.id (Classifier A).

Definition compile_no_opt := compile_pol no_opt.

Definition compile_opt :=
  compile_pol ((fun A x  => @strip_empty_rules A (@elim_shadowed A x))).


Require Import ExtLib.Data.Sum.
Require Import ExtLib.Tactics.
Require Import MirrorCore.Instantiate.
Require Import MirrorCore.RTac.Core.
Require Import MirrorCore.RTac.Simplify.

Require Import MirrorCore.Util.Forwardy.

Set Implicit Arguments.
Set Strict Implicit.

Section parameterized.
  Context {typ : Set}.
  Context {expr : Set}.
  Context {RType_typ : RType typ}.
  Context {RTypeOk_typ : RTypeOk}.
  Context {Expr_expr : Expr typ expr}.
  Context {ExprOk_expr : ExprOk Expr_expr}.
  Context {Typ0_Prop : Typ0 _ Prop}.

  Definition INSTANTIATE
  : rtac typ expr :=
    @SIMPLIFY typ expr
              (fun subst Subst_subst _ctx sub =>
                 instantiate (fun u => subst_lookup u sub) 0).

  Theorem INSTANTIATE_sound : rtac_sound INSTANTIATE.
  Proof.
    intros. eapply SIMPLIFY_sound.
    red. intros; forward.
    unfold propD, exprD_typ0 in *.
    forward.
    eapply (@instantiate_sound_ho  _ _ _ _ _ _ _ _ _ nil) in H3;
      [ | | eapply sem_preserves_if_ho_ctx_lookup; eauto ]; eauto.
    { forward_reason; inv_all; subst.
      change_rewrite H3.
      autorewrite with eq_rw. eexists; split; eauto.
      intros.
      gather_facts.
      eapply Pure_pctxD; eauto.
      clear. firstorder. specialize (H HList.Hnil).
      revert H1. autorewrite with eq_rw.
      simpl in *. rewrite H.
      exact (fun x => x). }
    constructor.
    { intros. eapply Pure_pctxD; eauto. }
    { intros.
      specialize (H6 us vs).
      eapply Ap_pctxD; eauto. }
  Qed.

End parameterized.


Require Import Coq.Vectors.Vector.
        Import VectorNotations.
Require Import Coq.Sorting.Permutation.
Require Import Coq.Arith.Le.
Require Import Coq.Arith.Lt.
Require Import Coq.Arith.Gt.
Require Import Coq.Program.Equality.

Fixpoint uniques  {n : nat} {A : Type} (deck : t A n) : Prop :=
      match deck with
        | nil _            => True
        | cons _ c0 _ rest => (Forall (fun c : A => c <> c0) rest ) /\ uniques rest
      end.

Module Type PokerFunctions.

  (* Of Cards and Decks *)
  Parameter  Card              : Type.
  Parameter  standard_deck     : t Card 52.
  Axiom unique_deck : uniques standard_deck.

  Parameter  shuffle           : forall n : nat, t Card n -> t Card n.

  Axiom      shuffle_changes   : forall n : nat, forall deck : t Card n,
                                        n > 1 -> uniques deck -> shuffle n deck <> deck.
  Axiom      Shuffle_perumute  : forall n : nat, forall deck : t Card n,
                                 Permutation (to_list deck) (to_list (shuffle n deck)).
  Parameter  count_cards : Card -> nat.
  Axiom countable_cards : forall n : nat,
      n < 14 -> exists c0 c1 c2 c3 : Card,
            (count_cards c0 = count_cards c1) /\
            (count_cards c2 = count_cards c3) /\
            (count_cards c0 = count_cards c3).
  Definition hand              := option (prod Card Card).

  (* Of Players *)
  Definition Player           : Type := (nat * hand).
  Definition get_hand  : Player -> hand    := snd .
  Definition get_money : Player -> nat := fst.
  Parameter  number_of_players : nat.
  Definition max_players       := 6.
  Axiom      multiple_players  : (number_of_players > 1) /\ (number_of_players <= max_players).
  Definition Players           := t Player number_of_players.
  Definition Active {n : nat} {c0 c1 : Card} (p: Player) : Prop := p = (n,Some (c0,c1)).

  (* Of Card Flops *)
  Definition flop : Type       := (Card * Card * Card).
  Definition turn              := prod Card flop.
  Definition river             := prod Card turn.
  Definition get_flop { n : nat } (deck : t Card (S ( S ( S ( S ( S n)))))) :
                                 ((t Card (S (S n))) * flop ):=
      let c1 := hd deck                 in
      let c2 := hd (tl deck)            in
      let c3 := hd (tl (tl deck))       in
      let new_deck := tl (tl (tl deck)) in
      (new_deck, (c1,c2,c3)).
  Definition get_turn {n : nat} (deck : t Card (S (S n))) (fl : flop) :
    ((t Card (S n)) * turn) :=
    (tl deck, (hd deck, fl)).
  Definition get_river {n : nat} (deck : t Card (S n)) (fl : flop) :
    ((t Card n) * turn) :=
    (tl deck, (hd deck, fl)).

  (* Of Betting *)
  Definition bet (pl : Player) (pot : nat) (bet : nat)
    : (Player * nat)  :=
    ( (get_money pl - bet, get_hand pl) , pot + bet).

End PokerFunctions.

Print All.

Module Type single_n.
  Parameter n : nat.
  Axiom    in_range : (n > 1) /\ n < 7.
End single_n.

Module impl (n' : single_n) <: PokerFunctions.

Inductive Name : Type :=
| Ace   : Name
| Two   : Name
| Three : Name
| Four  : Name
| Five  : Name
| Six   : Name
| Seven : Name
| Eight : Name
| Nine  : Name
| Ten   : Name
| Jack  : Name
| Queen : Name
| King  : Name
.

Definition Names : t Name 13  :=
  [
    Ace   ;
    Two   ;
    Three ;
    Four  ;
    Five  ;
    Six   ;
    Seven ;
    Eight ;
    Nine  ;
    Ten   ;
    Jack  ;
    Queen ;
    King
  ]
.

Inductive Card' : Type :=
| Heart   : Name -> Card'
| Spade   : Name -> Card'
| Club    : Name -> Card'
| Diamond : Name -> Card'
.

Definition Card := Card'.

Definition standard_deck : t Card 52 :=
    map Heart   Names ++
    map Spade   Names ++
    map Club    Names ++
    map Diamond Names
.

Definition unique_deck : uniques standard_deck.
Proof.

constructor.
all:constructor.
all:simpl.

all:repeat (
(try discriminate) ;
(constructor)
).
Qed.

Definition shuffle {n : nat} (deck : t Card n) : t Card n := rev (A:=Card) (n:=n) deck.
Definition shuffle_changes : forall n : nat, forall deck : t Card n,
                                        n > 1 -> (uniques deck) -> (shuffle (n:=n) deck) <> deck.
Proof.
intros.
inversion H.
Check deck.
subst.
dependent induction deck.
dependent induction deck.
dependent induction deck.
simpl.
cut (h <> h0).
all:(try
(dependent induction deck ||
dependent induction deck  ||
dependent induction deck
)).
intros.
Check shuffle.
Compute (shuffle (n:=2) [h; h0]).

unfold shuffle.
unfold rev.
unfold rev_append.
unfold rev_append_tail.
unfold eq_rect_r.
unfold eq_rect.
unfold eq_sym.
simpl.
pattern shuffle.

Check .
dependent induction n.

inversion deck.
case deck.
assert (n <> 0).
intuition.

absurd H1 H.

End impl.


(**
This file is part of the Flocq formalization of floating-point
arithmetic in Coq: http://flocq.gforge.inria.fr/

Copyright (C) 2010-2018 Sylvie Boldo
#<br />#
Copyright (C) 2010-2018 Guillaume Melquiond

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
COPYING file for more details.
*)

(** * Helper functions and theorems for computing the rounded square root of a floating-point number. *)

From Coq Require Import ZArith Reals Lia.

Require Import Zaux Raux Defs Digits Generic_fmt Float_prop Bracket.

Set Implicit Arguments.
Set Strongly Strict Implicit.

Section Fcalc_sqrt.

Variable beta : radix.
Notation bpow e := (bpow beta e).

Variable fexp : Z -> Z.

(** Computes a mantissa of precision p, the corresponding exponent,
    and the position with respect to the real square root of the
    input floating-point number.

The algorithm performs the following steps:
- Shift the mantissa so that it has at least 2p-1 digits;
  shift it one digit more if the new exponent is not even.
- Compute the square root s (at least p digits) of the new
  mantissa, and its remainder r.
- Compute the position according to the remainder:
  -- r == 0  =>  Eq,
  -- r <= s  =>  Lo,
  -- r >= s  =>  Up.

Complexity is fine as long as p1 <= 2p-1.
*)

Lemma mag_sqrt_F2R :
  forall m1 e1,
  (0 < m1)%Z ->
  mag beta (sqrt (F2R (Float beta m1 e1))) = Z.div2 (Zdigits beta m1 + e1 + 1) :> Z.
Proof.
intros m1 e1 Hm1.
rewrite <- (mag_F2R_Zdigits beta m1 e1) by now apply Zgt_not_eq.
apply mag_sqrt.
now apply F2R_gt_0.
Qed.

Definition Fsqrt_core m1 e1 e :=
  let d1 := Zdigits beta m1 in
  let m1' := (m1 * Zpower beta (e1 - 2 * e))%Z in
  let (q, r) := Z.sqrtrem m1' in
  let l :=
    if Zeq_bool r 0 then loc_Exact
    else loc_Inexact (if Zle_bool r q then Lt else Gt) in
  (q, l).

Theorem Fsqrt_core_correct :
  forall m1 e1 e,
  (0 < m1)%Z ->
  (2 * e <= e1)%Z ->
  let '(m, l) := Fsqrt_core m1 e1 e in
  inbetween_float beta m e (sqrt (F2R (Float beta m1 e1))) l.
Proof.
intros m1 e1 e Hm1 He.
unfold Fsqrt_core.
set (m' := Zmult _ _).
assert (0 <= m')%Z as Hm'.
{ apply Z.mul_nonneg_nonneg.
  now apply Zlt_le_weak.
  apply Zpower_ge_0. }
assert (sqrt (F2R (Float beta m1 e1)) = sqrt (IZR m') * bpow e)%R as Hf.
{ rewrite <- (sqrt_Rsqr (bpow e)) by apply bpow_ge_0.
  rewrite <- sqrt_mult.
  unfold Rsqr, m'.
  rewrite mult_IZR, IZR_Zpower by lia.
  rewrite Rmult_assoc, <- 2!bpow_plus.
  now replace (_ + _)%Z with e1 by ring.
  now apply IZR_le.
  apply Rle_0_sqr. }
generalize (Z.sqrtrem_spec m' Hm').
destruct Z.sqrtrem as [q r].
intros [Hq Hr].
rewrite Hf.
unfold inbetween_float, F2R. simpl Fnum.
apply inbetween_mult_compat.
apply bpow_gt_0.
rewrite Hq.
case Zeq_bool_spec ; intros Hr'.
(* .. r = 0 *)
rewrite Hr', Zplus_0_r, mult_IZR.
fold (Rsqr (IZR q)).
rewrite sqrt_Rsqr.
now constructor.
apply IZR_le.
clear -Hr ; lia.
(* .. r <> 0 *)
constructor.
split.
(* ... bounds *)
apply Rle_lt_trans with (sqrt (IZR (q * q))).
rewrite mult_IZR.
fold (Rsqr (IZR q)).
rewrite sqrt_Rsqr.
apply Rle_refl.
apply IZR_le.
clear -Hr ; lia.
apply sqrt_lt_1.
rewrite mult_IZR.
apply Rle_0_sqr.
rewrite <- Hq.
now apply IZR_le.
apply IZR_lt.
lia.
apply Rlt_le_trans with (sqrt (IZR ((q + 1) * (q + 1)))).
apply sqrt_lt_1.
rewrite <- Hq.
now apply IZR_le.
rewrite mult_IZR.
apply Rle_0_sqr.
apply IZR_lt.
ring_simplify.
lia.
rewrite mult_IZR.
fold (Rsqr (IZR (q + 1))).
rewrite sqrt_Rsqr.
apply Rle_refl.
apply IZR_le.
clear -Hr ; lia.
(* ... location *)
rewrite Rcompare_half_r.
generalize (Rcompare_sqr (2 * sqrt (IZR (q * q + r))) (IZR q + IZR (q + 1))).
rewrite 2!Rabs_pos_eq.
intros <-.
replace ((2 * sqrt (IZR (q * q + r))) * (2 * sqrt (IZR (q * q + r))))%R
  with (4 * Rsqr (sqrt (IZR (q * q + r))))%R by (unfold Rsqr ; ring).
rewrite Rsqr_sqrt.
rewrite <- plus_IZR, <- 2!mult_IZR.
rewrite Rcompare_IZR.
replace ((q + (q + 1)) * (q + (q + 1)))%Z with (4 * (q * q) + 4 * q + 1)%Z by ring.
generalize (Zle_cases r q).
case (Zle_bool r q) ; intros Hr''.
change (4 * (q * q + r) < 4 * (q * q) + 4 * q + 1)%Z.
lia.
change (4 * (q * q + r) > 4 * (q * q) + 4 * q + 1)%Z.
lia.
rewrite <- Hq.
now apply IZR_le.
rewrite <- plus_IZR.
apply IZR_le.
clear -Hr ; lia.
apply Rmult_le_pos.
now apply IZR_le.
apply sqrt_ge_0.
Qed.

Definition Fsqrt (x : float beta) :=
  let (m1, e1) := x in
  let e' := (Zdigits beta m1 + e1 + 1)%Z in
  let e := Z.min (fexp (Z.div2 e')) (Z.div2 e1) in
  let '(m, l) := Fsqrt_core m1 e1 e in
  (m, e, l).

Theorem Fsqrt_correct :
  forall x,
  (0 < F2R x)%R ->
  let '(m, e, l) := Fsqrt x in
  (e <= cexp beta fexp (sqrt (F2R x)))%Z /\
  inbetween_float beta m e (sqrt (F2R x)) l.
Proof.
intros [m1 e1] Hm1.
apply gt_0_F2R in Hm1.
unfold Fsqrt.
set (e := Z.min _ _).
assert (2 * e <= e1)%Z as He.
{ assert (e <= Z.div2 e1)%Z by apply Z.le_min_r.
  rewrite (Zdiv2_odd_eqn e1).
  destruct Z.odd ; lia. }
generalize (Fsqrt_core_correct m1 e1 e Hm1 He).
destruct Fsqrt_core as [m l].
apply conj.
apply Z.le_trans with (1 := Z.le_min_l _ _).
unfold cexp.
rewrite (mag_sqrt_F2R m1 e1 Hm1).
apply Z.le_refl.
Qed.

End Fcalc_sqrt.


(** Grothendieck groups of exact categories  *)
Require Export UniMath.Tactics.EnsureStructuredProofs.

Require Import UniMath.Foundations.All.
Require Import UniMath.MoreFoundations.All.
Require Export UniMath.CategoryTheory.ExactCategories.ExactCategories.
Require Import UniMath.Algebra.Groups.
Require Import UniMath.Algebra.Free_Monoids_and_Groups.
Import AddNotation.
Local Open Scope addmonoid.

Section setquot.
  Lemma setquot_map_epi {X : Type} {R : eqrel X} {Y : hSet} {h h' : setquot R → Y} :
    h ∘ setquotpr R ~ h' ∘ setquotpr R -> h ~ h'.
  Proof.
    use (surjectionisepitosets _ _ _ (issurjsetquotpr R) (setproperty Y)).
  Defined.
  Lemma setquot_map_recovery {X : Type} (R : eqrel X) (Y : hSet) (h : setquot R → Y) :
    ∏ w, setquotuniv R Y (h ∘ setquotpr R)
                     (λ x x' (r : R x x'), maponpaths h (iscompsetquotpr R x x' r)) w =
         h w.
  Proof.
    exact (setquot_map_epi (λ x, idpath (h (setquotpr R x)))).
  Qed.
  Definition setquot_universal_property (X:Type) (R:eqrel X) (Y:hSet) :
    (setquot R -> Y) ≃ (∑ f : X -> Y, iscomprelfun R f).
  Proof.
    use weq_iso.
    - intros h. exists (h ∘ setquotpr R).
      intros x x' r. simpl. apply maponpaths. apply iscompsetquotpr. exact r.
    - intros f. exact (setquotuniv R Y (pr1 f) (pr2 f)).
    - intros h. apply funextsec. unfold pr1,pr2. intros w. apply setquot_map_recovery.
    - intros f. cbn beta. apply subtypePath.
      * intros f'. apply isapropiscomprelfun.
      * cbn. reflexivity.
  Defined.
  Goal ∏ (X:Type) (R:eqrel X) (Y:hSet) (h : setquot R -> Y),
    pr1 (setquot_universal_property X R Y h) = h ∘ setquotpr R.
  Proof.
    reflexivity.
  Defined.
  Goal ∏ (X:Type) (R:eqrel X) (Y:hSet) (q : ∑ f : X -> Y, iscomprelfun R f),
    invmap (setquot_universal_property X R Y) q = setquotuniv R Y (pr1 q) (pr2 q).
  Proof.
    reflexivity.
  Defined.
End setquot.

(* new stuff *)
Definition K_0_hrel (M:ExactCategory) : hrel (free_abgr (π₀ (ob M)))
  := λ g h, ∃ E : ShortExactSequence M,
      g = free_abgr_unit (component (Ob2 E)) ×
      h = free_abgr_unit (component (Ob1 E)) + free_abgr_unit (component (Ob3 E)).
Definition K_0_related {M:ExactCategory} (E : ShortExactSequence M)
  : K_0_hrel M (free_abgr_unit (component (Ob2 E)))
               (free_abgr_unit (component (Ob1 E)) + free_abgr_unit (component (Ob3 E)))
  := hinhpr ( E ,, idpath _ ,, idpath _).
Definition K_0 (M:ExactCategory) : abgr
  := presented_abgr (π₀ (ob M)) (K_0_hrel M).
Definition K_0_map {M N:ExactCategory} (F:ExactFunctor M N) : monoidfun (K_0 M) (K_0 N).
Proof.
  use (presented_abgrfun (π₀_map (functor_on_objects F))). intros g h.
  use hinhfun. intros [E [e e']]. exists (applyFunctorToShortExactSequence F E).
  now induction (!e), (!e').
Defined.
Definition K_0_class {M:ExactCategory} : ob M -> K_0 M
  := λ A, @presented_abgr_intro (π₀ M) _ (pi0pr _ A).
Lemma K_0_eqn {M:ExactCategory} (E : ShortExactSequence M) :
  K_0_class (Ob2 E) = K_0_class (Ob1 E) + K_0_class (Ob3 E).
Proof.
  apply iscompsetquotpr. apply generated_binopeqrel_intro. apply hinhpr.
  exists E. split; reflexivity.
Defined.
Lemma K_0_map_universal_property {M:ExactCategory} {G:abgr} :
  monoidfun (K_0 M) G
  ≃
  ∑ f : ob M -> G, ∏ E:ShortExactSequence M, f(Ob2 E) = f(Ob1 E) + f(Ob3 E).
Proof.
  apply (weqcomp (presented_abgr_universal_property (K_0_hrel M) G)).
  apply (weqtotal2 π₀_universal_property). intros h. apply weqiff.
  + split.
    * intros i E. exact (i _ _ (K_0_related E)).
    * intros k w w' r. apply (squash_to_prop r (setproperty _ _ _)).
      clear r; intros [E [e e']]. induction (!e), (!e'); clear e e'. exact (k E).
  + apply isapropiscomprelfun.
  + apply impred_isaprop; intros E. apply setproperty.
Defined.
Definition K_0_universal_map {M:ExactCategory} {G:abgr} (f : ob M -> G) :
  (∏ E:ShortExactSequence M, f(Ob2 E) = f(Ob1 E) + f(Ob3 E)) -> monoidfun (K_0 M) G.
Proof.
  intros c. exact (invmap K_0_map_universal_property (f,,c)).
Defined.
Goal ∏ (M:ExactCategory) (G:abgr) (f : ob M -> G)
      (add : ∏ E:ShortExactSequence M, f(Ob2 E) = f(Ob1 E) + f(Ob3 E)) (A:M),
  K_0_universal_map f add (K_0_class A) = f A.
Proof.
  reflexivity.
Qed.
Goal ∏ (M:ExactCategory) (G:abgr) (h : monoidfun (K_0 M) G),
  pr1 (K_0_map_universal_property h) = h ∘ K_0_class.
Proof.
  reflexivity.
Qed.
Goal ∏ (M:ExactCategory) (G:abgr)
     (f : ob M -> G) (add : ∏ E:ShortExactSequence M, f(Ob2 E) = f(Ob1 E) + f(Ob3 E)),
  invmap K_0_map_universal_property (f,,add) = K_0_universal_map f add.
Proof.
  intros.
  apply pathsinv0.              (* omit this and it is slow! *)
  reflexivity.
Qed.


(* coq-robot (c) 2017 AIST and INRIA. License: LGPL-2.1-or-later. *)
From mathcomp Require Import all_ssreflect ssralg ssrint ssrnum rat.
From mathcomp Require Import closed_field polyrcf matrix mxalgebra mxpoly zmodp.
From mathcomp Require Import realalg complex fingroup perm.
From mathcomp.analysis Require Import boolp reals Rstruct classical_sets signed.
From mathcomp.analysis Require Import topology normedtype landau forms derive.
From mathcomp Require Import functions.
Require Import ssr_ext derive_matrix euclidean frame rot skew rigid.

(******************************************************************************)
(* This file is wip. It contains an on-going formalization of                 *)
(* [sciavicco] chap. 3.                                                       *)
(******************************************************************************)

(* contents:
  5. Module BoundVect.
     Module RFrame.
     relative frame
     (NB: to be moved to frame.v)
  6. Section kinematics
     velocity composition rule
  7. Section link_velocity.
  8. Lemma ang_vel_mx_Rz
     example: angular velocity of Rz as a spinor
  9. Section chain. (wip)
  10. Section geometric_jacobian_computation.
      geometric jacobian
  11. Section scara_geometric_jacobian.
      computation of SCARA's geometric jacobian
*)

Set Implicit Arguments.
Unset Strict Implicit.
Unset Printing Implicit Defensive.
Import Order.TTheory GRing.Theory Num.Def Num.Theory.

Local Open Scope ring_scope.
Local Open Scope frame_scope.

Module BoundVect. (* i.e., point of application prescribed *)
Section bound_vector.
Variable T : ringType.
Record t (F : tframe T) := mk { endp : 'rV[T]_3 }.
Definition startp F (v : t F) : 'rV[T]_3 := \o{F}.
End bound_vector.
End BoundVect.
Notation bvec := BoundVect.t.
Coercion boundvectendp T (F : tframe T) (v : bvec F) := BoundVect.endp v.

Definition bvec0 T (F : tframe T) : bvec F := BoundVect.mk _ 0.

Reserved Notation "a \+b b" (at level 39).
Reserved Notation "a \-b b" (at level 39).

Section about_bound_vectors.

Variables (T : ringType) (F : tframe T).

Definition FramedVect_of_Bound (p : bvec F) : fvec F := `[ BoundVect.endp p $ F ].

Definition BoundVect_add (a b : bvec F) : bvec F :=
  BoundVect.mk F (BoundVect.endp a + BoundVect.endp b).

Definition BoundFramed_add (a : bvec F) (b : fvec F) : bvec F :=
  BoundVect.mk F (BoundVect.endp a + FramedVect.v b).

Local Notation "a \+b b" := (BoundFramed_add a b).

Lemma BoundFramed_addA (a : bvec F) (b c : fvec F) :
  a \+b b \+b c = a \+b (b \+f c).
Proof. by rewrite /BoundFramed_add /= addrA. Qed.

Definition BoundVect_sub (F : tframe T) (a b : bvec F) : fvec F :=
  `[ BoundVect.endp a - BoundVect.endp b $ F ].

Local Notation "a \-b b" := (BoundVect_sub a b).

Lemma bv_eq (a b : bvec F) : a = b :> 'rV[T]_3 -> a = b.
Proof. by move: a b => [a] [b] /= ->. Qed.

End about_bound_vectors.

Notation "a \+b b" := (BoundFramed_add a b).
Notation "a \-b b" := (BoundVect_sub a b).

Lemma derive1mx_BoundFramed_add (R : realFieldType) (F : tframe [ringType of R^o])
  (Q : R -> bvec F) (Z : R -> fvec F) t :
  derivable_mx (fun x => BoundVect.endp (Q x)) t 1 ->
  derivable_mx (fun x => FramedVect.v (Z x)) t 1 ->
  derive1mx (fun x => BoundVect.endp (Q x \+b Z x)) t =
  derive1mx (fun x => BoundVect.endp (Q x)) t +
    derive1mx (fun x => FramedVect.v (Z x)) t.
Proof.
move=> H H'.
rewrite (_ : (fun x : R => _) = (fun x : R => BoundVect.endp (Q x) +
  (FramedVect.v (Z x)))); last by rewrite funeqE.
rewrite derive1mxD.
- by [].
- exact: H.
- exact H'.
Qed.

Module RFrame.
Section rframe.
Variable T : ringType.
Record t (F0 : tframe T) := mk {
  o : bvec F0 ;
  i : fvec F0 ;
  j : fvec F0 ;
  k : fvec F0 ;
  F : tframe T ;
  _ : BoundVect.endp o = \o{F} ;
  _ : FramedVect.v i = F~i ;
  _ : FramedVect.v j = F~j ;
  _ : FramedVect.v k = F~k ;
}.
End rframe.
End RFrame.
Notation rframe := RFrame.t.
Coercion tframe_of_rframe R (F : tframe R) (F : rframe F) : tframe R := RFrame.F F.

Lemma RFrame_o T (F0 : tframe T) (F1 : rframe F0) :
  BoundVect.endp (RFrame.o F1) = \o{RFrame.F F1}.
Proof. by destruct F1. Qed.

(* lemmas about relative frames *)

Lemma BoundVect0_fixed (R : realFieldType) T (F : tframe T) (F1 : R -> rframe F) t :
  BoundVect.endp (bvec0 (F1 t)) = BoundVect.endp (bvec0 (F1 0)).
Proof. by []. Qed.

Section derivable_FromTo.

Lemma derivable_mx_FromTo' (R : realFieldType) (F : R -> tframe [ringType of R^o])
  (G' : forall t, rframe (F t)) (G : forall t, rframe (F t)) t :
  derivable_mx F t 1 -> derivable_mx G t 1 -> derivable_mx G' t 1 ->
  derivable_mx (fun x => (G x) _R^ (G' x)) t 1.
Proof.
move=> HF HG HG' a b.
rewrite (_ : (fun x => _) = (fun x => row a (G x) *d row b (G' x))); last first.
  by rewrite funeqE => t'; rewrite !mxE.
evar (f : 'I_3 -> R^o -> R^o).
rewrite (_ : (fun x => _) = \sum_i f i); last first.
  rewrite funeqE => t'; rewrite dotmulE fct_sumE; apply: eq_bigr => /= i _.
  rewrite !mxE /f; reflexivity.
rewrite {}/f.
apply: derivable_sum => i.
apply: derivableM; [exact: HG | exact: HG'].
Qed.

Lemma derivable_mx_FromTo (R : realFieldType) (F : R -> tframe [ringType of R^o])
  (G : forall t, rframe (F t)) t :
  derivable_mx F t 1 -> derivable_mx G t 1 ->
  derivable_mx (fun x => (G x) _R^ (F x)) t 1.
Proof.
move=> HF HG a b.
have @G' : forall t0, rframe (F t0).
  move=> t0.
  exact: (@RFrame.mk _ _ (@BoundVect.mk _ _ \o{F t0}) `[(F t0)~i $ F t0] `[(F t0)~j $ F t0] `[(F t0)~k $ F t0] (F t0)).
apply: (@derivable_mx_FromTo' R F G' G).
by [].
by [].
rewrite {}/G' /=.
exact: HF.
Qed.

Lemma derivable_mx_FromTo_fixed (R : realFieldType)
  (F : tframe [ringType of R^o]) (G : R -> rframe F) t :
  derivable_mx G t 1 -> derivable_mx (fun x => (G x) _R^ F) t 1.
Proof.
move=> H; apply derivable_mx_FromTo; [move=> a b; exact: ex_derive | exact H].
Qed.

Lemma derivable_mx_FromTo_tr (R : realFieldType)
  (F : tframe [ringType of R^o]) (G : R -> rframe F) t :
  derivable_mx (fun x => F _R^ (G x)) t 1 = derivable_mx (fun x => F _R^ (G x)) t 1.
Proof. by rewrite trmx_derivable. Qed.

End derivable_FromTo.

(* the coordinate transformation of a point P1 from frame F1 to frame F
  (eqn 2.33, 3.10) *)
Definition coortrans (R : realType) (F : tframe [ringType of R^o]) (F1 : rframe F)
    (P1 : bvec F1) : bvec F :=
  RFrame.o F1 \+b rmap F (FramedVect_of_Bound P1).

(* motion of P1 w.r.t. the fixed frame F (eqn B.2) *)
Definition motion (R : realType) (F : tframe [ringType of R^o]) (F1 : R -> rframe F)
  (P1 : forall t, bvec (F1 t)) t : bvec F := coortrans (P1 t).

(* [sciavicco] p.351-352  *)
Section kinematics.

Variables (R : realType) (F : tframe [ringType of R^o]). (* fixed frame *)
Variable F1 : R -> rframe F. (* time-varying frame (origin and basis in F) *)
Hypothesis derivable_F1 : forall t, derivable_mx F1 t 1.
Hypothesis derivable_F1o : forall t, derivable_mx (@TFrame.o [ringType of R^o] \o F1) t 1.

Variable P1 : forall t, bvec (F1 t). (* point with coordinates in F1 *)
(* NB: compared with [sciavicco] p.351, P1 not necessarily fixed in F1 *)
Hypothesis derivable_mxP1 : forall t, derivable_mx (fun x => BoundVect.endp (P1 x)) t 1.

Let P : R -> bvec F := motion P1.

Variable Q1 : forall t, bvec (F1 t).
Hypothesis Q1_fixed_in_F1 : forall t, BoundVect.endp (Q1 t) = BoundVect.endp (Q1 0).

Let Q : R -> bvec F := motion Q1.

(* eqn B.3 *)
Lemma eqnB3 t : P t = Q t \+b rmap F (P1 t \-b Q1 t).
Proof.
rewrite /P /Q /motion /coortrans BoundFramed_addA; congr (_ \+b _).
apply fv_eq => /=; rewrite -mulmxDl; congr (_ *m _).
by rewrite addrCA subrr addr0.
Qed.

Lemma derivable_mx_Q t : derivable_mx (fun x => BoundVect.endp (Q x)) t 1.
Proof.
rewrite /Q /=; apply derivable_mxD.
  move=> a b.
  move: (@derivable_F1o t a b).
  rewrite (_ : (fun x => \o{F1 x} a b) =
    (fun x => BoundVect.endp (RFrame.o (F1 x)) a b)) // funeqE => x.
  destruct (F1 x) => /=; by rewrite e.
apply derivable_mxM; last exact: derivable_mx_FromTo.
rewrite (_ : (fun x => _) = (fun _ => BoundVect.endp (Q1 0))); last first.
  rewrite funeqE => x; by rewrite Q1_fixed_in_F1.
move=> a b; exact: ex_derive.
Qed.

Let Rot := fun t => (F1 t) _R^ F.

(* generalization of B.4  *)
Lemma velocity_composition_rule (t : R) :
  derive1mx (fun x => BoundVect.endp (P x)) t =
  derive1mx (fun x => BoundVect.endp (Q x)) t +
  derive1mx P1 t *m Rot t (* rate of change of the coordinates P1 expressed in the frame F *) +
  ang_vel Rot t *v FramedVect.v (P t \-b Q t).
Proof.
rewrite {1}(_ : P = fun t => Q t \+b rmap F (P1 t \-b Q1 t)); last first.
  by rewrite funeqE => t'; rewrite eqnB3.
rewrite (derive1mx_BoundFramed_add (@derivable_mx_Q t)); last first.
  apply derivable_mxM; last exact: derivable_mx_FromTo.
  rewrite (_ : (fun x => _) = (fun x => FramedVect.v (FramedVect_of_Bound (P1 x)) -
    FramedVect.v (FramedVect_of_Bound (Q1 0)))); last first.
    rewrite funeqE => x; by rewrite /= Q1_fixed_in_F1.
  apply: derivable_mxB => /=.
  exact: derivable_mxP1.
  move=> a b; exact: ex_derive.
rewrite -addrA; congr (_ + _).
rewrite [in LHS]/rmap [in LHS]/= derive1mxM; last 2 first.
  rewrite {1}(_ : (fun x  => _) = (fun x  => BoundVect.endp (P1 x) -
    BoundVect.endp (Q1 0))); last first.
    by rewrite funeqE => ?; rewrite Q1_fixed_in_F1.
  apply: derivable_mxB.
    exact: derivable_mxP1.
    move=> a b; exact: ex_derive.
  exact: derivable_mx_FromTo.
rewrite derive1mxB; last 2 first.
  exact: derivable_mxP1.
  rewrite (_ : (fun x => _) = cst (BoundVect.endp (Q1 0))); last first.
    by rewrite funeqE => x; rewrite Q1_fixed_in_F1.
  exact: derivable_mx_cst.
congr (_*m _  + _).
  rewrite [in X in _ + X = _](_ : (fun x => _) = cst (BoundVect.endp (Q1 0))); last first.
    by rewrite funeqE => x; rewrite Q1_fixed_in_F1.
  by rewrite derive1mx_cst subr0.
rewrite -spinE unspinK; last first.
  rewrite ang_vel_mx_is_so; first by [].
  move=> t'; by rewrite FromTo_is_O.
  move=> t'; exact: derivable_mx_FromTo.
rewrite /ang_vel_mx mulmxA; congr (_ *m _).
rewrite /P /Q /= opprD addrACA subrr add0r mulmxBl -!mulmxA.
by rewrite orthogonal_mul_tr ?FromTo_is_O // !mulmx1.
Qed.

Hypothesis P1_fixed_in_F1 : forall t, BoundVect.endp (P1 t) = BoundVect.endp (P1 0).

(* eqn B.4 *)
Lemma velocity_composition_rule_spec (t : R) :
  derive1mx (fun x => BoundVect.endp (P x)) t =
  derive1mx (fun x => BoundVect.endp (Q x)) t +
  ang_vel Rot t *v (FramedVect.v (P t \-b Q t)).
Proof.
rewrite velocity_composition_rule; congr (_ + _).
suff -> : derive1mx P1 t = 0 by rewrite mul0mx addr0.
apply/matrixP => a b; rewrite !mxE.
rewrite (_ : (fun x => _) = cst (P1 0 a b)); last first.
  rewrite funeqE => x /=; by rewrite /boundvectendp (P1_fixed_in_F1 x).
by rewrite derive1_cst.
Qed.

End kinematics.

(* [sciavicco] p.81-82 *)
Section derivative_of_a_rotation_matrix_contd.

Variables (R : realType) (F : tframe [ringType of R^o]).
Variable F1 : R -> rframe F.
Hypothesis derivable_F1 : forall t, derivable_mx F1 t 1.
Variable p1 : forall t, bvec (F1 t).
Hypothesis derivable_mx_p1 : forall t, derivable_mx (fun x => BoundVect.endp (p1 x)) t 1.
Hypothesis derivable_F1o : forall t, derivable_mx (@TFrame.o [ringType of R^o] \o F1) t 1.

Definition p0 := motion p1.

Lemma eqn312 t :
  derive1mx (fun x => BoundVect.endp (motion p1 x)) t =
  derive1mx (fun x => BoundVect.endp (motion (fun=> bvec0 (F1 x)) t)) t +
  derive1mx p1 t *m (F1 t) _R^ F +
  ang_vel (fun t => (F1 t) _R^ F) t *v (p1 t *m (F1 t) _R^ F).
Proof.
rewrite (@velocity_composition_rule _ F _ derivable_F1 derivable_F1o p1 derivable_mx_p1
  (fun t => bvec0 (F1 t)) (@BoundVect0_fixed _ _ _ F1)).
congr (_ + _ *v _).
by rewrite /= mul0mx addr0 addrAC subrr add0r.
Qed.

End derivative_of_a_rotation_matrix_contd.

Require Import screw.

(* [murray p.54] chapter 2, section 4.2*)
Section rigid_body_velocity.

Section spatial_velocity.

Variables (R : realType) (M : R -> 'M[R^o]_4).
Hypothesis derivableM : forall t, derivable_mx M t 1.
Hypothesis MSE : forall t, M t \in 'SE3[R].

Definition spatial_velocity t : 'M_4 := (M t)^-1 * derive1mx M t.

Definition spatial_lin_vel :=
  let r : R -> 'M[R^o]_3 := @rot_of_hom _ \o M in
  let p : R -> 'rV[R^o]_3:= @trans_of_hom _ \o M in
  fun t => - p t *m (r t)^T *m derive1mx r t + derive1mx p t.

Lemma spatial_velocityE t :
  let r : R -> 'M[R^o]_3 := @rot_of_hom _ \o M in
  spatial_velocity t = block_mx (ang_vel_mx r t) 0 (spatial_lin_vel t) 0.
Proof.
move=> r.
rewrite /spatial_velocity.
transitivity (inv_hom (M t) * derive1mx M t) => //.
  by rewrite inv_homE.
rewrite /inv_hom.
rewrite /hom.
rewrite derive1mx_SE //.
rewrite (_ : rot_of_hom (M t) = r t) // -/r.
rewrite -mulmxE.
rewrite (mulmx_block (r t)^T _ _ _ (derive1mx r t)).
rewrite !(mul0mx,add0r,mul1mx,mulmx0,trmx0,addr0,mulmx1).
by rewrite mulmxE -/(ang_vel_mx r t).
Qed.

Lemma spatial_velocity_in_se3 x : spatial_velocity x \in 'se3[[rcfType of R^o]].
Proof.
rewrite spatial_velocityE. set r := @rot_of_hom _.
rewrite qualifE block_mxKul block_mxKur block_mxKdr 2!eqxx 2!andbT.
rewrite ang_vel_mx_is_so // => t0.
by rewrite rotation_sub // rot_of_hom_is_SO.
exact: derivable_rot_of_hom.
Qed.

Lemma spatial_velocity_is_twist x :
  let r : R -> 'M[R^o]_3 := @rot_of_hom _ \o M in
  spatial_velocity x = wedge \T( spatial_lin_vel x , unspin (ang_vel_mx r x)).
Proof.
move=> r.
rewrite spatial_velocityE.
rewrite /wedge lin_tcoorE ang_tcoorE unspinK //.
rewrite ang_vel_mx_is_so // => t0.
by rewrite rotation_sub // rot_of_hom_is_SO.
exact: derivable_rot_of_hom.
Qed.

End spatial_velocity.

Section body_velocity.

Variables (R : realType) (M : R -> 'M[R^o]_4).
Hypothesis derivableM : forall t, derivable_mx M t 1.
Hypothesis MSE : forall t, M t \in 'SE3[R].

Definition body_velocity t : 'M_4 := derive1mx M t * (M t)^-1.

Definition body_lin_vel :=
  let r : R -> 'M[R^o]_3 := @rot_of_hom _ \o M in
  let p : R -> 'rV[R^o]_3:= @trans_of_hom _ \o M in
  fun t => derive1mx p t *m (r t)^T.

Lemma body_ang_vel_is_so t : body_ang_vel_mx (@rot_of_hom _ \o M) t \is 'so[R]_3.
Proof.
rewrite /body_ang_vel_mx.
have : forall t, (@rot_of_hom [rcfType of R^o] \o M) t \is 'O[[ringType of R]]_3.
  move=> t0; by rewrite rotation_sub // rot_of_hom_is_SO.
move/ang_vel_mx_is_so => /(_ (derivable_rot_of_hom derivableM))/(_ t).
rewrite /ang_vel_mx.
move/(conj_so (((rot_of_hom (T:=[rcfType of R^o]) \o M) t)^T)).
rewrite !mulmxA !trmxK orthogonal_mul_tr ?rotation_sub // ?rot_of_hom_is_SO //.
by rewrite mul1mx.
Qed.

Lemma body_velocityE t : let r : R -> 'M[R^o]_3 := @rot_of_hom _ \o M in
  body_velocity t = block_mx (body_ang_vel_mx r t) 0 (body_lin_vel t) 0.
Proof.
move=> r.
rewrite /body_velocity.
transitivity (derive1mx M t * inv_hom (M t)).
  by rewrite inv_homE.
rewrite /inv_hom.
rewrite /hom.
rewrite derive1mx_SE //.
rewrite (_ : rot_of_hom (M t) = r t) // -/r.
rewrite -mulmxE.
rewrite (mulmx_block (derive1mx r t) _ _ _ (r t)^T).
rewrite !(mul0mx,add0r,mul1mx,mulmx0,trmx0,addr0,mulmx1).
by rewrite -/(body_ang_vel_mx _) -/(body_lin_vel _).
Qed.

Lemma body_velocity_is_se x : body_velocity x \is 'se3[R].
Proof.
rewrite body_velocityE.
rewrite qualifE block_mxKul block_mxKur block_mxKdr 2!eqxx 2!andbT.
by rewrite body_ang_vel_is_so.
Qed.

End body_velocity.

Section spatial_body_adjoint.

Variables (R : realType) (M : R -> 'M[R^o]_4).
Hypothesis derivableM : forall t, derivable_mx M t 1.
Hypothesis MSE : forall t, M t \in 'SE3[R].

Lemma spatial_body_velocity x :
  vee (spatial_velocity M x) = vee (body_velocity M x) *m Adjoint (M x).
Proof.
rewrite -/(SE3_action _ _) action_Adjoint; last by [].
congr vee; rewrite /spatial_velocity -mulmxE -mulmxA; congr (_ * _).
rewrite veeK; last by rewrite body_velocity_is_se.
by rewrite /body_velocity -mulmxA mulVmx ?mulmx1 // SE3_in_unitmx.
Qed.

End spatial_body_adjoint.

End rigid_body_velocity.

(* [sciavicco] p.83 *)
Section link_velocity.

Variables (R : realType) (F : tframe [ringType of R^o]).
Variable F1 : R -> rframe F.
Variable F2 : R -> rframe F.
Let o1 t : bvec F := RFrame.o (F1 t).
Let o2 t : bvec F := RFrame.o (F2 t).

Let r12 : forall t : R, bvec (F1 t) := fun t =>
  BoundVect.mk (F1 t)
    (FramedVect.v (rmap (F1 t) `[ \o{F2 t} - \o{F1 t} $ F ])).

Hypothesis derivable_F1 : forall t, derivable_mx F1 t 1.
Hypothesis derivable_F1o : forall t, derivable_mx (@TFrame.o [ringType of R^o] \o F1) t 1.
Hypothesis derivable_r12 : forall t, derivable_mx (fun x => BoundVect.endp (r12 x)) t 1.
Hypothesis derivable_F2 : forall t, derivable_mx F2 t 1.

Lemma eqn310' t : o2 t = o1 t \+b rmap F (FramedVect_of_Bound (r12 t)).
Proof.
rewrite /o2 /o1 /= /r12 /= /rmap /= /BoundFramed_add /=; apply bv_eq => /=.
rewrite -mulmxA FromTo_comp FromToI mulmx1 /= addrCA RFrame_o /=.
by rewrite subrr addr0 -RFrame_o.
Qed.

Definition w1 := ang_vel (fun t => (F1 t) _R^ F).

Lemma eqn314_helper t : FramedVect.v (rmap F `[r12 t $ F1 t]) = \o{F2 t} - \o{F1 t}.
Proof. by rewrite /= -mulmxA FromTo_comp FromToI mulmx1. Qed.

(* lin. vel. of Link i as a function of
   the translational and rotational velocities of Link i-1 *)
Lemma eqn314 t : derive1mx o2 t = derive1mx o1 t +
  FramedVect.v (rmap F `[derive1mx r12 t $ F1 t])
    (* velocity of the origin of Frame i w.r.t. the origin of Frame i-1 *) +
  w1 t *v (\o{F2 t} - \o{F1 t}).
Proof.
rewrite -eqn314_helper.
move: (@eqn312 _ F _ derivable_F1 _ derivable_r12 derivable_F1o t).
have -> : derive1mx (fun x => BoundVect.endp (motion r12 x)) t = derive1mx o2 t.
  rewrite (_ : (fun x => BoundVect.endp (motion r12 x)) = o2) //.
  rewrite funeqE => t' /=; rewrite -mulmxA FromTo_comp FromToI mulmx1.
  rewrite addrCA RFrame_o subrr addr0.
  by rewrite /o2 -RFrame_o.
move=> ->.
rewrite -!{1}addrA /=; apply: f_equal2; last by [].
rewrite (_ : (fun x => BoundVect.endp (motion (fun _ : R => bvec0 (F1 x)) t)) = o1) //.
rewrite funeqE => t'.
by rewrite /motion /= mul0mx addr0.
Qed.

Definition w2 : R -> 'rV_3 := ang_vel (fun t => (F2 t) _R^ F).
Definition w12 : R -> 'rV_3 := ang_vel (fun t => (F2 t) _R^ (F1 t)).

(* ang. vel. of Link i as a function of the ang. vel. of Link i-1 and of Link i w.r.t. Link i-1 *)
Lemma eqn316 t : w2 t = w1 t + w12 t *m ((F1 t) _R^ F).
Proof.
have : (fun t => (F2 t) _R^ F) = (fun t => ((F2 t) _R^ (F1 t)) *m ((F1 t) _R^ F)).
  by rewrite funeqE => ?; rewrite FromTo_comp.
move/(congr1 (fun x => derive1mx x)).
rewrite funeqE.
move/(_ t).
rewrite derive1mxM; last 2 first.
  move=> t'; exact: derivable_mx_FromTo'.
  move=> t'; exact: derivable_mx_FromTo.
rewrite derive1mx_ang_vel; last 2 first.
  move=> t'; by rewrite FromTo_is_O.
  move=> t'; exact: derivable_mx_FromTo.
rewrite derive1mx_ang_vel; last 2 first.
  move=> t'; by rewrite FromTo_is_O.
  move=> t'; exact: derivable_mx_FromTo'.
rewrite derive1mx_ang_vel; last 2 first.
  move=> t'; by rewrite FromTo_is_O.
  move=> t'; exact: derivable_mx_FromTo.
rewrite ang_vel_mxE; last 2 first.
  move=> t'; by rewrite FromTo_is_O.
  move=> t'; exact: derivable_mx_FromTo.
rewrite ang_vel_mxE; last 2 first.
  move=> t'; by rewrite FromTo_is_O.
  move=> t'; exact: derivable_mx_FromTo'.
rewrite ang_vel_mxE; last 2 first.
  move=> t'; by rewrite FromTo_is_O.
  move=> t'; exact: derivable_mx_FromTo.
rewrite mulmxE -[in X in _ = X + _](mulr1 ((F2 t) _R^ (F1 t))).
rewrite -(@orthogonal_tr_mul _ _ (F _R^ (F1 t))) ?FromTo_is_O //.
rewrite -{2}(trmx_FromTo (F1 t) F).
rewrite -!mulrA.
rewrite (mulrA _ _ ((F1 t) _R^ F)).
rewrite (@spin_similarity _ ((F1 t) _R^ F)) ?FromTo_is_SO //.
rewrite mulrA -mulmxE.
rewrite trmx_FromTo.
rewrite FromTo_comp.
rewrite mulmxA.
rewrite FromTo_comp.
rewrite -mulmxDr.
rewrite -spinD.
rewrite -/w2 -/w1 -/w12.
rewrite mulmxE.
move/mulrI.
rewrite FromTo_unit => /(_ isT)/eqP.
rewrite spin_inj => /eqP.
by rewrite addrC.
Qed.

End link_velocity.

From mathcomp Require Import trigo.

Lemma derive1_cos (R : realType) (t : R) : derive1 (cos : _ -> R^o) t = - sin t.
Proof.
rewrite derive1E.
have u := is_derive_cos t.
by have := @derive_val _ _ _ _ _ _ _ u.
Qed.

Lemma derive1_sin (R : realType) (t : R) : derive1 (sin : _ -> R^o) t = cos t.
Proof.
rewrite derive1E.
have u := is_derive_sin t.
by have := @derive_val _ _ _ _ _ _ _ u.
Qed.

Lemma derivable_sin (R : realType) (t : R) : derivable (sin : R^o -> R^o) t 1.
Proof. exact: ex_derive. Qed.

Lemma derivable_cos (R : realType) (t : R) : derivable (cos : R^o -> R^o) t 1.
Proof. exact: ex_derive. Qed.

Lemma derivable_cos_comp  (R : realType) (t : R) (a : R^o -> R^o) :
  derivable a t 1 -> derivable (cos \o a : _ -> R^o) t 1.
Proof. by move=> /derivableP Hs; exact: ex_derive. Qed.

Lemma derivable_sin_comp (R : realType) (t : R) (a : R^o -> R^o) :
  derivable a t 1 -> derivable ((sin : _ -> R^o) \o a) t 1.
Proof. by move=> /derivableP Hs; exact: ex_derive. Qed.

Lemma derive1_cos_comp (R : realType) t (a : R^o -> R^o) : derivable a t 1 ->
  derive1 (cos \o a : _ -> R^o) t = - (derive1 a t) * sin (a t).
Proof.
move=> H; rewrite (derive1_comp H); last exact: derivable_cos.
by rewrite derive1_cos mulrC mulNr mulrN.
Qed.

Lemma derive1mx_RzE (R : realType) (a : R^o -> R^o) t : derivable a t 1 ->
  derive1mx (fun x => Rz (a x) : 'M[R^o]__) t =
  derive1 a t *: col_mx3 (row3 (- sin (a t)) (cos (a t)) 0)
                         (row3 (- cos (a t)) (- sin (a t)) 0)
                         0.
Proof.
move=> Ha.
apply/matrix3P/and9P; split; rewrite !mxE /=.
- rewrite (_ : (fun _ => _) = cos \o a); last by rewrite funeqE => x; rewrite !mxE.
  rewrite (derive1_comp Ha); last exact/derivable_cos.
  by rewrite derive1_cos mulrC.
- rewrite (_ : (fun _ => _) = sin \o a); last by rewrite funeqE => x; rewrite !mxE.
  rewrite (derive1_comp Ha); last exact/derivable_sin.
  by rewrite derive1_sin mulrC.
- rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE.
  by rewrite derive1_cst mulr0.
- rewrite (_ : (fun _ => _) = - sin \o a); last by rewrite funeqE => x; rewrite !mxE.
  rewrite (_ : - _ \o _ = - (sin \o a)) // derive1E deriveN; last first.
    apply/derivable1_diffP/differentiable_comp.
    exact/derivable1_diffP.
    exact/derivable1_diffP/derivable_sin.
  rewrite -derive1E (derive1_comp Ha); last exact/derivable_sin.
  by rewrite derive1_sin mulrN mulrC.
- rewrite (_ : (fun _ => _) = cos \o a); last by rewrite funeqE => x; rewrite !mxE.
  rewrite (derive1_comp Ha); last exact/derivable_cos.
  by rewrite derive1_cos mulrN mulNr mulrC.
- rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE.
  by rewrite derive1_cst mulr0.
- rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE.
  by rewrite derive1_cst mulr0.
- rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE.
  by rewrite derive1_cst mulr0.
- rewrite (_ : (fun _ => _) = cst 1); last by rewrite funeqE => x; rewrite !mxE.
  by rewrite derive1_cst mulr0.
Qed.

(* example 3.1 [sciavicco]*)
(* rotational motion of one degree of freedom manipulator *)
Lemma ang_vel_mx_Rz (R : realType) (a : R^o -> R^o) t : derivable a t 1 ->
  ang_vel_mx (@Rz _ \o a) t = \S(row3 0 0 (derive1 a t)).
Proof.
move=> Ha.
rewrite /ang_vel_mx trmx_Rz (derive1mx_RzE Ha) /Rz.
rewrite -!scalerAr -col_mx3_mul.
rewrite mulmx_row3_col3 scale0r addr0.
rewrite mulmx_row3_col3 scale0r addr0.
rewrite e2row mulmx_row3_col3 !(scale0r,scaler0,addr0).
rewrite !row3Z !(cosN,sinN,opprK,mulr0,mulrN,mulNr).
rewrite !row3D mulrC addrC subrr addr0.
rewrite -!expr2 cos2Dsin2 -opprD addrC cos2Dsin2.
by apply/matrix3P; rewrite !spinij !mxE /= !(eqxx,oppr0,mulr0,mulrN1,mulr1).
Qed.

(* see also the end of dh.v *)
Section chain.

Inductive joint (R : rcfType) :=
  Revolute of (R^o -> (*angle*) R) | Prismatic of (R^o -> R).

Definition joint_variable (R : realType) (j : joint [rcfType of R]) : R^o -> R :=
  fun t => match j with Revolute a => a t | Prismatic a => a t end.

Record chain (R : rcfType) n (* number of joints *) := mkChain {
  joint_of_chain : 'I_n -> joint R ;
  frame_of_chain : 'I_n.+1 -> (R^o -> tframe R) }.

End chain.

Section geometric_jacobian_computation.

Variables (n : nat) (R : realType) (c : chain [rcfType of R^o] n).

Definition prev_frame (i : 'I_n) : 'I_n.+1 := widen_ord (leqnSn n) i.
Definition next_frame (i : 'I_n) : 'I_n.+1 := fintype.lift ord0 i.
Lemma next_frameE i : next_frame i = i.+1 :> nat. Proof. by []. Qed.
Definition last_frame : 'I_n.+1 := ord_max.

Section geo_jac_row.
Variable i : 'I_n.
Let j : joint [rcfType of R^o] := joint_of_chain c i.
Let q : R^o -> R^o := joint_variable j.
Let Fim1 := frame_of_chain c (prev_frame i).
(*Let Fi := frame_of_chain c (next_frame i).*)
Let Fmax := frame_of_chain c last_frame.
(* contribution to the angular velocity *)
Definition geo_jac_ang t : 'rV_3 := if j is Revolute _ then (Fim1 t)~k else 0.
(* contribution to the linear velocity *)
Definition geo_jac_lin t : 'rV_3 :=
  if j is Revolute _ then (Fim1 t)~k *v (\o{Fmax t} - \o{Fim1 t}) else (Fim1 t)~k.
End geo_jac_row.

Definition geo_jac t : 'M_(n, 6) :=
  row_mx (\matrix_(i < n) geo_jac_lin i t) (\matrix_(i < n) geo_jac_ang i t).

End geometric_jacobian_computation.

Require Import scara.

Section scara_geometric_jacobian.
Variable R : realType.

Variable theta1 : R -> R.
Variable a1 : R.
Variable theta2 : R -> R.
Variable a2 : R.
Variable d3 : R -> R.
Variable theta4 : R -> R.
Variable d4 : R.
(* direct kinematics equation written in function of the joint variables,
   from scara.v  *)
Let rot t := scara_rot (theta1 t) (theta2 t) (theta4 t).
Let trans t := scara_trans (theta1 t) a1 (theta2 t) a2 (d3 t) d4.
Definition scara_end_effector t : 'M[R]_4 := hom (rot t) (trans t).

Let scara_lin_vel : R -> 'rV[R]_3 :=
  derive1mx (@trans_of_hom [ringType of R^o] \o scara_end_effector).
Let scara_ang_vel : R -> 'rV[R]_3 :=
  ang_vel (@rot_of_hom [ringType of R^o] \o scara_end_effector).

Definition scara_joints : 'I_4 -> joint R :=
  [eta (fun=> Prismatic 0) with
  0 |-> Revolute theta1,
  1 |-> Revolute theta2,
  2%:R |-> Prismatic d3,
  3%:R |-> Revolute theta4].

Definition scara_joint_variables t : 'rV[R^o]_4 :=
  \row_i (joint_variable (scara_joints i) t).

Let scara_joint_velocities : R -> 'rV[R^o]_4 := derive1mx scara_joint_variables.

(* specification of scara frames *)
Variables scara_frames : 'I_5 -> R -> tframe R.
Let Fim1 (i : 'I_4) := scara_frames (prev_frame i).
Let Fi (i : 'I_4) := scara_frames (next_frame i).
Let Fmax := scara_frames ord_max.
Hypothesis Hzvec : forall t i, (Fim1 i t)~k = 'e_2%:R.
Hypothesis o0E : forall t, \o{Fim1 0 t} = 0.
Hypothesis o1E : forall t, \o{Fim1 1 t} =
  a1 * cos (theta1 t) *: 'e_0 + a1 * sin (theta1 t) *: 'e_1.
Hypothesis o2E : forall t, \o{Fim1 2%:R t} = \o{Fim1 1 t} +
  (a2 * cos (theta1 t + theta2 t)) *: 'e_0 +
  (a2 * sin (theta1 t + theta2 t)) *: 'e_1.
Hypothesis o3E : forall t, \o{Fim1 3%:R t} = \o{Fim1 2%:R t} + (d3 t) *: 'e_2.
Hypothesis o4E : forall t, \o{Fmax t} = \o{Fim1 3%:R t} + d4 *: 'e_2.

Lemma scale_realType (K : realType) (k1 : K) (k2 : K^o) : k1 *: k2 = k1 * k2.
Proof. by []. Qed.

Import rv3LieAlgebra.Exports.

Lemma scara_geometric_jacobian t :
  derivable (theta1 : R^o -> R^o) t 1 ->
  derivable (theta2 : R^o -> R^o) t 1 ->
  derivable (d3 : R^o -> R^o) t 1 ->
  derivable (theta4 : R^o -> R^o) t 1 ->
  scara_joint_velocities t *m geo_jac (mkChain scara_joints scara_frames) t =
  row_mx (scara_lin_vel t) (scara_ang_vel t).
Proof.
move=> H1 H2 H3 H4.
rewrite /geo_jac; set a := (X in _ *m @row_mx _ _ 3 3 X _).
rewrite (mul_mx_row _ a) {}/a; congr (@row_mx _ _ 3 3 _ _).
- rewrite /scara_lin_vel (_ : @trans_of_hom [rcfType of R] \o _ = trans); last first.
    rewrite funeqE => x /=; exact: trans_of_hom_hom.
  rewrite /trans /scara_trans derive1mxE [RHS]row3_proj /= ![in RHS]mxE [in RHS]/=.
  transitivity (
      derive1 (theta1 : R^o -> R^o) t *: (Fim1 0 t)~k *v (\o{Fmax t} - \o{Fim1 0 t}) +
      derive1 (theta2 : R^o -> R^o) t *: (Fim1 1 t)~k *v (\o{Fmax t} - \o{Fim1 1 t}) +
      derive1 (d3 : R^o -> R^o) t *: (Fim1 2 t)~k +
      derive1 (theta4 : R^o -> R^o) t *: (Fim1 3%:R t)~k *v (\o{Fmax t} - \o{Fim1 3%:R t})).
    rewrite /scara_joint_velocities /scara_joint_variables derive1mxE /geo_jac_lin /=.
    apply/rowP => i; rewrite 3![in RHS]mxE [in LHS]mxE sum4E;
          (repeat apply: f_equal2).
    - by rewrite 2!mxE /= linearZl_LR [in RHS]mxE.
    - by rewrite 2!mxE /= linearZl_LR [in RHS]mxE.
    - by rewrite 2!mxE [in RHS]mxE.
    - by rewrite 2!mxE /= linearZl_LR [in RHS]mxE.
  rewrite o0E subr0.
  rewrite {1}o4E.
  rewrite linearDr /=.
  rewrite (linearZr_LR _ _ _ 'e_2)
   /= {2}Hzvec (linearZl_LR _ _ _ 'e_2) /= liexx 2!{1}scaler0 addr0.
  rewrite (_ : \o{Fmax t} - \o{Fim1 1 t} =
    (a2 * cos (theta1 t + theta2 t) *: 'e_0 +
    (a1 * sin (theta1 t) + a2 * sin (theta1 t + theta2 t) - a1 * sin (theta1 t)) *: 'e_1 +
    (d3 t + d4) *: 'e_2)); last first.
    rewrite (addrC (a1 * sin _)) addrK.
    rewrite o4E o3E o2E o1E.
    set a := _ *: 'e_0 + _ *: 'e_1.
    rewrite -3!addrA addrC 3!addrA subrK.
    rewrite addrC -[in RHS]addrA; congr (_ + _).
    by rewrite -addrA -scalerDl.
  rewrite linearDr /=.
  rewrite (linearZr_LR _ _ _ 'e_2) /= (linearZl_LR (crossmul_bilinear _) 'e_2)
          {2}(Hzvec t 1) /= liexx 2!{1}scaler0 addr0.
  rewrite (addrC (a1 * sin _)) addrK.
  rewrite (_ : \o{Fmax t} - \o{Fim1 3%:R t} = d4 *: 'e_2%:R); last first.
    by rewrite o4E -addrA addrC subrK.
  rewrite (linearZr_LR _ _ _ 'e_2) /= (linearZl_LR (crossmul_bilinear _) 'e_2)
          {1}(Hzvec t 3%:R) /= liexx 2!scaler0 addr0.
  rewrite o3E.
  rewrite linearDr /= (linearZr_LR _ _ _ 'e_2) (linearZl_LR _ 'e_2)
          {2}(Hzvec t 0) /= liexx 2!scaler0 addr0.
  rewrite {1}o2E {1}o1E.
  rewrite (_ : (fun _ => _) =
                (a2 \*: (cos \o (theta2 + theta1) : R^o -> R^o)) +
                (a1 *: (cos \o theta1 : R^o -> R^o))) //.
  rewrite (_ : (fun _ => _) = (a2 \*: (sin \o (theta2 + theta1) : _ -> R^o)) +
                              (a1 *: (sin \o theta1 : _ -> R^o))) //.
  rewrite row3e2; congr (_ + _ *: _); last first.
  - by rewrite Hzvec.
  - rewrite [in RHS]derive1E [in RHS]deriveD; last 2 first.
      exact: ex_derive.
      exact: H3.
    by rewrite deriveE // diff_cst add0r derive1E.
  - rewrite !linearDr /= !(linearZr_LR (crossmul_bilinear _))
            !(linearZl_LR (crossmul_bilinear _)) /= !Hzvec veckj vecki
            !{1}scalerN.
    rewrite -!addrA addrCA addrC -!addrA (addrCA (- _)) !addrA.
    rewrite -2!addrA [in RHS]addrC; congr (_ + _).
    - rewrite !scalerA -2!scalerDl row3e1; congr (_ *: _).
      rewrite [in RHS]derive1E deriveD; last 2 first.
        apply/derivableZ/derivable_sin_comp/derivableD; [exact H2 | exact H1].
        exact/derivableZ/derivable_sin_comp.
      rewrite deriveZ /=; last first.
        apply/derivable_sin_comp/derivableD; [exact H2 | exact H1].
      rewrite deriveZ /=; last exact/derivable_sin_comp.
      rewrite -[in RHS]derive1E [in RHS]derive1_comp; last 2 first.
      - by apply: derivableD; [exact: H2 | exact: H1].
      - by apply: derivable_sin.
      rewrite derive1_sin.
      rewrite -derive1E [in RHS]derive1_comp; last 2 first.
      - exact: H1.
      - exact: derivable_sin.
      rewrite derive1_sin -addrA addrC; congr (_ + _); last first.
        by rewrite -mulrA.
      rewrite -mulrDr [in RHS]derive1E deriveD; last 2 first.
        exact: H2.
        exact: H1.
      rewrite -mulrA scale_realType; apply: f_equal2; first by [].
      rewrite addrC; apply: f_equal2; first by [].
      by rewrite addrC !derive1E.
    - rewrite !{1}scalerA !addrA -3!{1}scaleNr -2!scalerDl row3e0.
      congr (_ *: _).
      rewrite [in RHS]derive1E deriveD; last 2 first.
        by apply/derivableZ/derivable_cos_comp/derivableD; [exact H2 | exact H1].
        by apply/derivableZ/derivable_cos_comp; exact H1.
      rewrite deriveZ /=; last first.
        by apply/derivable_cos_comp/derivableD.
      rewrite deriveZ /=; last exact/derivable_cos_comp.
      rewrite -derive1E [in RHS]derive1_comp; last 2 first.
      - by apply: derivableD; [exact: H2 | exact: H1].
      - exact: derivable_cos.
      rewrite derive1_cos mulNr scalerN.
      rewrite -derive1E [in RHS]derive1_comp; last 2 first.
      - exact: H1.
      - exact: derivable_cos.
      rewrite derive1_cos mulNr scalerN; congr (_ - _); last first.
        by rewrite -mulrA.
      rewrite -opprD; congr (- _).
      rewrite [in RHS]derive1E deriveD; last 2 first.
        exact: H2.
        exact: H1.
      rewrite -mulrDr -!derive1E.
      rewrite addrC (addrC (_^`() _)).
      by rewrite !scalerAl.
- rewrite /scara_ang_vel (_ : @rot_of_hom [ringType of R^o] \o _ = rot); last first.
    rewrite funeqE => x /=; exact: rot_of_hom_hom.
  rewrite /rot /ang_vel /scara_rot.
  rewrite (_ : (fun _ => _) = (@Rz _ \o (theta1 + theta2 + theta4))); last first.
    by rewrite funeqE.
  rewrite ang_vel_mx_Rz; last first.
    apply derivableD; [apply/derivableD|by []].
    exact: H1.
    exact: H2.
  rewrite [RHS]spinK.
  transitivity (derive1 (theta1 : R^o -> R^o) t *: (Fim1 0 t)~k +
                derive1 (theta2 : R^o -> R^o) t *: (Fim1 1 t)~k +
                derive1 (theta4 : R^o -> R^o) t *: (Fim1 3%:R t)~k).
    rewrite /scara_joint_velocities /scara_joint_variables derive1mxE /geo_jac_ang /=.
    apply/rowP => i; rewrite !mxE sum4E !mxE {1}mulr0 addr0.
    by rewrite -!/(Fim1 _) [Fim1 0 _]lock [Fim1 1 _]lock [Fim1 3%:R _]lock /= -!lock.
  rewrite !Hzvec -2!scalerDl e2row row3Z mulr0 mulr1.
  rewrite [in RHS]derive1E deriveD; [|apply/derivableD|by []]; last 2 first.
    exact: H1.
    exact: H2.
  rewrite deriveD; [| exact: H1| exact H2].
  by rewrite 3!derive1E.
Qed.

End scara_geometric_jacobian.

Section analytical_jacobian.

Variable n' : nat.
Let n := n'.+1.
Variable (R : realType) (p : 'rV[R^o]_n -> 'rV[R^o]_3).
Let Jp := jacobian p.
Variable (phi : 'rV[R^o]_n -> 'rV[R^o]_3).
Let Jphi := jacobian phi.

Lemma dp (q : R^o -> 'rV[R^o]_n) t :
  derive1mx (p \o q) t = derive1mx q t *m Jp (q t). (* 3.56 *)
Proof.
rewrite /Jp /jacobian mul_rV_lin1.
rewrite /derive1mx.
Abort.

End analytical_jacobian.

(*Section tangent_vectors.

Variable R : realType.
Let vector := 'rV[R]_3.
Let point := 'rV[R]_3.
Implicit Types p : point.

(* tangent vector *)
Record tvec p := TVec {tvec_field :> vector}.
Definition vtvec p (v : tvec p) := let: TVec v := v in v.

Local Notation "p .-vec" := (tvec p) (at level 5).
Local Notation "u :@ p" := (TVec p u) (at level 11).

Definition addt (p : point) (u : p.-vec) (v : vector) : p.-vec :=
  TVec p (tvec_field u + v).

Local Notation "p +@ u" := (addt p u) (at level 41).

End tangent_vectors.

Coercion vtvec_field_coercion := vtvec.

Notation "p .-vec" := (tvec p) (at level 5).
Notation "u :@ p" := (TVec p u) (at level 11).
Notation "p +@ u" := (addt p u) (at level 41).*)