MinusNoNeg(e1,e2,T) = if e1-e2 > 0 then Minus(e1,e2,T) else 0

timed csp Spec csp-begin
Timed(OneStep) {

  -- Simplified RoboChart semantics, written within a timed-section

  -- Main process with maximal progress.
  Spec = timed_priority(Initial)

  Initial = EntryMoving;
            Obs;
            EntryTurning;
            -- In the RoboChart model #MBC is reset after obstacle, so we need to subtract 2 units
            -- from that, given that EntryTurning consumes, at least, 2 time units.
            WAIT(MinusNoNeg(Div(const_SimFW_SMovement_PI,const_SimFW_SMovement_av,core_nat),2,core_nat));
            Initial

  EntryMoving = StartBy(SimFW::CMovement::moveCall.const_SimFW_SMovement_lv.0 -> WAIT(1),0)

  Obs = SimFW::CMovement::obstacle.in -> SKIP

  EntryTurning = StartBy(SimFW::CMovement::stopCall -> WAIT(1) ;
                  StartBy(SimFW::CMovement::moveCall.0.const_SimFW_SMovement_av -> WAIT(1),
                  0),
                 0)

  -- For verification using tick-tock need to provide alphabet of Spec process,
  -- which in this case is exactly the same as that of the built-in calculated
  -- by RoboTool
  Spec__sem__events = SimFW::CMovement::sem__events                                  
}
csp-end

timed assertion A0 : SimFW::CMovement equals Spec
timed assertion A1 : SimFW::CMovement equals Spec in the tick-tock model

timed csp Spec csp-begin
Timed(OneStep) {

  -- Simplified RoboSim semantics, written within a timed-section

  -- Main process with maximal progress.
  Spec = timed_priority(StartBy(SimCMovement::registerRead.SimCMovement::i_obstacle?x__ -> Initial,0))

  Initial = SMoving;
            DMoving;
            Waiting;
            STurning;
            DTurning(1);
            Initial      

  SMoving = StartBy(SimCMovement::registerWrite.SimCMovement::o_move.const_SimSMovement_lv.0 -> SKIP,0)

  DMoving = WAIT(cycle_SimCMovement_SimCMovement);
            StartBy(SimCMovement::registerRead.SimCMovement::i_obstacle?obstacle ->
              if obstacle then
                StartBy(SimCMovement::registerWrite.SimCMovement::o_stop -> SKIP,0)
              else
                DMoving,
            0)

  Waiting = WAIT(cycle_SimCMovement_SimCMovement)

  STurning = StartBy(SimCMovement::registerRead.SimCMovement::i_obstacle?x__ ->
        StartBy(SimCMovement::registerWrite.SimCMovement::o_move.0.const_SimSMovement_av -> SKIP,0)
         ,0)

  -- Note the use of a parameter MBC to replicate the current value of the clock MBC
  DTurning(MBC) = WAIT(cycle_SimCMovement_SimCMovement);
                  StartBy(SimCMovement::registerRead.SimCMovement::i_obstacle?x__ ->
                    if (MBC >= Div(const_SimSMovement_PI,const_SimSMovement_av,core_real)) then
                      SKIP
                    else
                      DTurning(MBC+1)
                  ,0)

  -- For verification using tick-tock need to provide alphabet of Spec process,
  -- which in this case is exactly the same as that of the built-in calculated
  -- by RoboTool
  Spec__sem__events = SimCMovement::sem__events            
}
csp-end

timed assertion A0 : SimCMovement equals Spec
timed assertion A1 : SimCMovement equals Spec in the tick-tock model

simulation Sim of SimFW::CMovement {
  cycleDef { cycle == 1 }
}

assertion A0 : SimCMovement refines Sim
assertion A1 : SimCMovement refines Sim in the tick-tock model

// GLOBAL VARIABLES
// SRanger_Base_LHinge_F: vector(real,6)
// ...

const TimeScale: real, // This is the correspondence of a discrete time unit and the continous time (e.g., TimeScale = 5ms indicates that a tock in the d-model takes 5ms)
   	  cycle: real | cycle == 1, // This comes from the definition of cycle
      SRanger_Base_LHinge_AXIS: vector(real,3) = (|0,1,0|),
      SRanger_Base_LHinge_S: vector(real,6) = ScrewAxis(SRanger_Base_LHinge_AXIS, Pitch::Finite(0)),
      ...
SRanger = var timer: real,
        SRanger_Base_LHinge_pV: vector(real,6),
        ...
    @ let
      Init = SKIP
      ResetTimer = timer := 0
      SendToSoftware = (
        (
          if (SRanger_Base_IR_voltage>=3.0)
          then registerRead.obstacle.true -> SKIP
          else registerRead.obstacle.false -> SKIP
        )
      ) 
      ReceiveFromSoftware = (
        registerWrite.moveCall?lv?av -> (
          {SRanger_Base_LHinge_LMotor_das, SRanger_Base_RHinge_RMotor_das}:[
            (SRanger_Base_LHinge_LMotor_das==dsl) 
            and 
            (SRanger_Base_RHinge_RMotor_das==dsr)
            and 
            (lv==RADIUS * (dsl + dsr) / 2) 
            and 
            (av==RADIUS * (dsl - dsr) / axisLength)
          ]
        )
        |||
        registerWrite.stopCall -> (
          {SRanger_Base_LHinge_LMotor_das, SRanger_Base_RHinge_RMotor_das}:[
            (SRanger_Base_LHinge_LMotor_das==0) 
            and 
            (SRanger_Base_RHinge_RMotor_das==0)
          ]
        )
      ) 
      Cycle = SendToSoftware; ReceiveFromSoftware /\ wait(cycle); ResetTimer;
        var SRanger_Base_LHinge_theta: real,
            SRanger_Base_LHinge_v: real,
            ...
          @ (
            // equations
            SRanger_Base_IR_voltage == 4*exp(-0.028784*SRanger_Base_IR_distance)
            and
            SRanger_Base_LHinge_fV == AdT(SRanger_Base_LHinge_theta)*SRanger_Base_LHinge_pV
                                      +
                                      SRanger_Base_LHinge_A*SRanger_Base_LHinge_v
            ...
            // the timer variable controls the execution of the cycle; it is reset at every cycle
            timer' = 1
          ) /\ (
            // domain condition
            timer > TimeScale*cycle
          ); Cycle
    within Init; Cycle

Simulation = (SRanger [|{|tock,registerRead,registerWrite|}|] SimCMovement)\{|tock,registerRead,registerWrite|}