freetype ROTATION == { Clockwise
                     , CounterClock
                     , rIdle
                     }

freetype ARM_MOTOR == { Extend
                      , Retract
                      , aIdle
                      }

freetype ON_OFF  == { Off, On }

typealias ANGLE == FLOAT
typealias EXTENSION == FLOAT

static function Arm1ToTable   == 50.0
static function Arm2ToPress   == 35.0
static function Arm2ToDepBelt == fsub(0.0,45.0)
static function Arm1ToPress   == fsub(0.0,90.0)

static function Retracted      == 0.0
static function OverTable      == 0.5208
static function Arm2IntoPress  == 0.7971
static function OverDepBelt    == 0.5707
static function Arm1IntoPress  == 0.6458

dynamic function RobotRotationMot : ROTATION
initially rIdle

dynamic function Arm1Mot : ARM_MOTOR
initially aIdle

dynamic function Arm2Mot : ARM_MOTOR
initially aIdle

dynamic function Arm1Mag : ON_OFF
initially Off

dynamic function Arm2Mag : ON_OFF
initially Off

output function Arm1Ext : EXTENSION

output function Arm2Ext : EXTENSION

output function Angle : ANGLE

derived relation ArmsRectracted ==

  Arm1Ext = Retracted and Arm2Ext = Retracted


derived relation RobotIdle == 

  RobotRotationMot = rIdle and Arm1Mot = aIdle and Arm2Mot = aIdle


derived relation WaitingInUnloadTablePos ==

  Angle = Arm1ToTable and ArmsRectracted and RobotIdle
          and Arm1Mag = Off and Arm2Mag = Off


derived relation WaitingInUnloadPressPos ==

  Angle = Arm2ToPress and ArmsRectracted and RobotIdle
          and Arm1Mag = On and Arm2Mag = Off


derived relation WaitingInLoadDepBeltPos ==

  Angle = Arm2ToDepBelt and ArmsRectracted and RobotIdle
          and Arm1Mag = On and Arm2Mag = On


derived relation WaitingInLoadPressPos ==

  Angle = Arm1ToPress and ArmsRectracted and RobotIdle
          and Arm1Mag = On and Arm2Mag = Off


derived relation ExtendingArm1ToTable ==

  Angle = Arm1ToTable and Arm1Mot = Extend


derived relation ExtendingArm2ToPress ==

  Angle = Arm2ToPress and Arm2Mot = Extend


derived relation ExtendingArm2ToDepBelt ==

  Angle = Arm2ToDepBelt and Arm2Mot = Extend


derived relation ExtendingArm1ToPress ==

  Angle = Arm1ToPress and Arm1Mot = Extend


derived relation ExtendedOverTable ==

  Angle = Arm1ToTable and Arm1Ext = OverTable and Arm1Mot = aIdle


derived relation ExtendedArm2IntoPress ==

  Angle = Arm2ToPress and Arm2Ext = Arm2IntoPress and Arm2Mot = aIdle


derived relation ExtendedOverDepBelt ==

  Angle = Arm2ToDepBelt and Arm2Ext = OverDepBelt and Arm2Mot = aIdle


derived relation ExtendedArm1IntoPress ==

  Angle = Arm1ToPress and Arm1Ext = Arm1IntoPress and Arm1Mot = aIdle


derived relation RetractingArm1ToTable ==

  Angle = Arm1ToTable and Arm1Mot = Retract


derived relation RetractingArm2ToPress ==

  Angle = Arm2ToPress and Arm2Mot = Retract


derived relation RetractingArm2ToDepBelt ==

  Angle = Arm2ToDepBelt and Arm2Mot = Retract


derived relation RetractingArm1ToPress ==

  Angle = Arm1ToPress and Arm1Mot = Retract


derived relation Arm1ToTableCompleted ==

  RetractingArm1ToTable and Arm1Ext = Retracted


derived relation Arm2ToPressCompleted ==

  RetractingArm2ToPress and Arm2Ext = Retracted


derived relation Arm2ToDepBeltCompleted ==

  RetractingArm2ToDepBelt and Arm2Ext = Retracted


derived relation Arm1ToPressCompleted ==

  RetractingArm1ToPress and Arm1Ext = Retracted

derived relation interval : FLOAT * FLOAT * FLOAT
  == fn(value,a,b) -> value >= a and value <= b

derived relation MovingToUnloadPressPos ==

  ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and
  RobotRotationMot = CounterClock and
  Arm1Mag = On and Arm2Mag = Off and
  interval(Angle,Arm2ToPress,Arm1ToTable)
  

derived relation MovingToLoadDepBeltPos ==

  ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and
  RobotRotationMot = CounterClock and 
  Arm1Mag = On and Arm2Mag = On and
  interval(Angle, Arm2ToDepBelt, Arm2ToPress)


derived relation MovingToLoadPressPos ==

  ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and
  RobotRotationMot = CounterClock and 
  Arm1Mag = On and Arm2Mag = Off and
  interval(Angle, Arm1ToPress, Arm2ToDepBelt)


derived relation MovingToUnloadTablePos ==

  ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and
  RobotRotationMot = Clockwise and 
  Arm1Mag = Off and Arm2Mag = Off and
  interval(Angle,Arm1ToPress, Arm1ToTable)


derived relation UnloadPressPosReached ==

  Angle = Arm2ToPress


derived relation LoadDepBeltPosReached ==

  Angle = Arm2ToDepBelt


derived relation LoadPressPosReached ==

  Angle = Arm1ToPress


derived relation UnloadTablePosReached ==

  Angle = Arm1ToTable


derived relation PressInUnloadPosition ==

  PressInBottomPosition and PressMot = udIdle

derived relation TableInUnloadPosition ==

  ErtInTopPosition and MaxRotation


derived relation TableReadyForUnloading ==

  TableInUnloadPosition and TableLoaded


derived relation PressReadyForUnloading ==

  PressInUnloadPosition and PressLoaded



derived relation PressReadyForLoading ==

  PressInLoadPosition and not ( PressLoaded )


derived relation PressInLoadPosition ==

  PressInMiddlePosition and PressMot = udIdle



transition WAITING ==

  block

   if WaitingInUnloadTablePos and TableReadyForUnloading
   then Arm1Mot := Extend
   endif

   if WaitingInUnloadPressPos and PressReadyForUnloading
   then Arm2Mot := Extend
   endif

   if WaitingInUnloadPressPos and not (PressLoaded)
   then RobotRotationMot := CounterClock
   endif

   if WaitingInLoadDepBeltPos and DepositBeltReadyForLoading
   then Arm2Mot := Extend
   endif

   if WaitingInLoadPressPos and PressReadyForLoading
   then Arm1Mot := Extend
   endif

  endblock


transition ACTION_extension ==

  block

   if ExtendingArm1ToTable and Arm1Ext = OverTable
   then Arm1Mot := aIdle
        Arm1Mag := On
   endif

   if ExtendingArm2ToPress and Arm2Ext = Arm2IntoPress
   then Arm2Mot := aIdle
        Arm2Mag := On
   endif

   if ExtendingArm2ToDepBelt and Arm2Ext = OverDepBelt
   then Arm2Mot := aIdle
        Arm2Mag := Off
   endif

   if ExtendingArm1ToPress and Arm1Ext = Arm1IntoPress
   then Arm1Mot := aIdle
        Arm1Mag := Off
   endif

  endblock


transition ACTION_proper ==

  block

    if ExtendedOverTable and Arm1Mag = On
    then Arm1Mot := Retract
    endif

    if ExtendedArm2IntoPress and Arm2Mag = On
    then Arm2Mot := Retract
    endif

    if ExtendedOverDepBelt and Arm2Mag = Off
    then Arm2Mot := Retract
    endif

    if ExtendedArm1IntoPress and Arm1Mag = Off
    then Arm1Mot := Retract
    endif
    
  endblock


transition ACTION_retraction ==

  block
 
   if RetractingArm1ToTable and Arm1Ext = Retracted
   then Arm1Mot          := aIdle
        RobotRotationMot := CounterClock
        TableLoaded      := false
   endif

   if RetractingArm2ToPress and Arm2Ext = Retracted
   then Arm2Mot          := aIdle
        RobotRotationMot := CounterClock
        PressLoaded      := false
   endif

   if RetractingArm2ToDepBelt and Arm2Ext = Retracted
   then Arm2Mot                    := aIdle
        RobotRotationMot           := CounterClock
        DepositBeltReadyForLoading := false
   endif

   if RetractingArm1ToPress and Arm1Ext = Retracted
   then Arm1Mot          := aIdle
        RobotRotationMot := Clockwise
        PressLoaded      := true
   endif

  endblock



transition MOVING ==

  block

    if MovingToUnloadPressPos and UnloadPressPosReached
    then RobotRotationMot := rIdle
    endif

    if MovingToLoadDepBeltPos and LoadDepBeltPosReached
    then RobotRotationMot := rIdle
    endif

    if MovingToLoadPressPos and LoadPressPosReached
    then RobotRotationMot := rIdle
    endif

    if MovingToUnloadTablePos and UnloadTablePosReached
    then RobotRotationMot := rIdle
    endif

  endblock