///|
/// Protocol-neutral CiA 402 profile MVP for Isochronon W0.

///|
pub fn package_id() -> String {
  "fieldbus_core/profile_cia402"
}

///|
pub fn cia402_profile_epoch() -> @core.VTime {
  @core.VTime::from_ns(0L)
}

///|
pub let cia402_controlword_index : UInt = 0x6040U

///|
pub let cia402_statusword_index : UInt = 0x6041U

///|
pub let cia402_mode_of_operation_index : UInt = 0x6060U

///|
pub let cia402_actual_position_index : UInt = 0x6064U

///|
pub let cia402_target_position_index : UInt = 0x607AU

///|
pub let cia402_fault_index : UInt = 0x603FU

///|
pub(all) enum Cia402State {
  NotReadyToSwitchOn
  SwitchOnDisabled
  ReadyToSwitchOn
  SwitchedOn
  OperationEnabled
  QuickStopActive
  FaultReactionActive
  Fault
} derive(Eq, Debug)

///|
pub fn Cia402State::label(self : Cia402State) -> String {
  match self {
    NotReadyToSwitchOn => "not_ready_to_switch_on"
    SwitchOnDisabled => "switch_on_disabled"
    ReadyToSwitchOn => "ready_to_switch_on"
    SwitchedOn => "switched_on"
    OperationEnabled => "operation_enabled"
    QuickStopActive => "quick_stop_active"
    FaultReactionActive => "fault_reaction_active"
    Fault => "fault"
  }
}

///|
pub fn Cia402State::statusword_pattern(self : Cia402State) -> UInt {
  match self {
    NotReadyToSwitchOn => 0x0000U
    SwitchOnDisabled => 0x0040U
    ReadyToSwitchOn => 0x0021U
    SwitchedOn => 0x0023U
    OperationEnabled => 0x0027U
    QuickStopActive => 0x0007U
    FaultReactionActive => 0x000FU
    Fault => 0x0008U
  }
}

///|
pub fn decode_statusword(statusword : UInt) -> Cia402State? {
  match statusword & 0x006FU {
    0x0000U => Some(NotReadyToSwitchOn)
    0x0040U => Some(SwitchOnDisabled)
    0x0021U => Some(ReadyToSwitchOn)
    0x0023U => Some(SwitchedOn)
    0x0027U => Some(OperationEnabled)
    0x0007U => Some(QuickStopActive)
    0x000FU => Some(FaultReactionActive)
    _ => if (statusword & 0x0008U) != 0U { Some(Fault) } else { None }
  }
}

///|
pub(all) enum ControlwordCommand {
  Shutdown
  SwitchOn
  EnableOperation
  DisableOperation
  DisableVoltage
  QuickStop
  FaultReset
  FaultDetected
} derive(Eq, Debug)

///|
pub fn ControlwordCommand::label(self : ControlwordCommand) -> String {
  match self {
    Shutdown => "shutdown"
    SwitchOn => "switch_on"
    EnableOperation => "enable_operation"
    DisableOperation => "disable_operation"
    DisableVoltage => "disable_voltage"
    QuickStop => "quick_stop"
    FaultReset => "fault_reset"
    FaultDetected => "fault_detected"
  }
}

///|
pub fn ControlwordCommand::controlword_value(
  self : ControlwordCommand,
) -> UInt? {
  match self {
    Shutdown => Some(0x0006U)
    SwitchOn => Some(0x0007U)
    EnableOperation => Some(0x000FU)
    DisableOperation => Some(0x0007U)
    DisableVoltage => Some(0x0000U)
    QuickStop => Some(0x0002U)
    FaultReset => Some(0x0080U)
    FaultDetected => None
  }
}

///|
pub(all) struct Cia402Transition {
  from : Cia402State
  command : ControlwordCommand
  to : Cia402State
  accepted : Bool
  reason : String
} derive(Eq, Debug)

///|
pub fn Cia402Transition::is_accepted(self : Cia402Transition) -> Bool {
  self.accepted
}

///|
pub fn transition(
  from : Cia402State,
  command : ControlwordCommand,
) -> Cia402Transition {
  let next = next_state(from, command)
  match next {
    Some(to) => { from, command, to, accepted: true, reason: "" }
    None =>
      {
        from,
        command,
        to: from,
        accepted: false,
        reason: "illegal " + command.label() + " from " + from.label(),
      }
  }
}

///|
fn next_state(from : Cia402State, command : ControlwordCommand) -> Cia402State? {
  match (from, command) {
    (_, FaultDetected) => Some(Fault)
    (SwitchOnDisabled, Shutdown) => Some(ReadyToSwitchOn)
    (ReadyToSwitchOn, SwitchOn) => Some(SwitchedOn)
    (SwitchedOn, EnableOperation) => Some(OperationEnabled)
    (OperationEnabled, DisableOperation) => Some(SwitchedOn)
    (SwitchedOn, Shutdown) => Some(ReadyToSwitchOn)
    (OperationEnabled, Shutdown) => Some(ReadyToSwitchOn)
    (ReadyToSwitchOn, DisableVoltage) => Some(SwitchOnDisabled)
    (SwitchedOn, DisableVoltage) => Some(SwitchOnDisabled)
    (OperationEnabled, DisableVoltage) => Some(SwitchOnDisabled)
    (ReadyToSwitchOn, QuickStop) => Some(QuickStopActive)
    (SwitchedOn, QuickStop) => Some(QuickStopActive)
    (OperationEnabled, QuickStop) => Some(QuickStopActive)
    (Fault, FaultReset) => Some(SwitchOnDisabled)
    _ => None
  }
}

///|
pub(all) struct DriveSwitchOnDisabled {
  axis_id : String
} derive(Eq, Debug)

///|
pub(all) struct DriveReadyToSwitchOn {
  axis_id : String
} derive(Eq, Debug)

///|
pub(all) struct DriveSwitchedOn {
  axis_id : String
} derive(Eq, Debug)

///|
pub(all) struct DriveOperationEnabled {
  axis_id : String
} derive(Eq, Debug)

///|
pub(all) struct DriveFault {
  axis_id : String
} derive(Eq, Debug)

///|
pub fn DriveSwitchOnDisabled::new(axis_id~ : String) -> DriveSwitchOnDisabled {
  { axis_id, }
}

///|
pub fn DriveSwitchOnDisabled::shutdown(
  self : DriveSwitchOnDisabled,
) -> DriveReadyToSwitchOn {
  { axis_id: self.axis_id }
}

///|
pub fn DriveReadyToSwitchOn::switch_on(
  self : DriveReadyToSwitchOn,
) -> DriveSwitchedOn {
  { axis_id: self.axis_id }
}

///|
pub fn DriveSwitchedOn::enable_operation(
  self : DriveSwitchedOn,
) -> DriveOperationEnabled {
  { axis_id: self.axis_id }
}

///|
pub fn DriveOperationEnabled::disable_operation(
  self : DriveOperationEnabled,
) -> DriveSwitchedOn {
  { axis_id: self.axis_id }
}

///|
pub fn DriveOperationEnabled::fault_detected(
  self : DriveOperationEnabled,
) -> DriveFault {
  { axis_id: self.axis_id }
}

///|
pub fn DriveFault::fault_reset(self : DriveFault) -> DriveSwitchOnDisabled {
  { axis_id: self.axis_id }
}

///|
pub fn cia402_coe_bindings() -> Array[@isocontract.ObjectBinding] {
  [
    @isocontract.ObjectBinding::make(
      object_id="axis.controlword",
      binding_id="coe." + cia402_controlword_index.to_string() + ".write",
      medium_id="trace",
      label="cia402.controlword.write",
      direction=@trace.Tx,
      kind=@isocontract.CoeSdo,
      index=cia402_controlword_index,
      subindex=b'\x00',
      channel="sdo",
      authority=@isocontract.Role("controller"),
    ),
    @isocontract.ObjectBinding::make(
      object_id="axis.statusword",
      binding_id="coe." + cia402_statusword_index.to_string() + ".read",
      medium_id="trace",
      label="cia402.statusword.read",
      direction=@trace.Rx,
      kind=@isocontract.CoeSdo,
      index=cia402_statusword_index,
      subindex=b'\x00',
      channel="sdo",
      authority=@isocontract.Role("controller"),
    ),
    @isocontract.ObjectBinding::make(
      object_id="axis.mode",
      binding_id="coe." + cia402_mode_of_operation_index.to_string() + ".write",
      medium_id="trace",
      label="cia402.mode.write",
      direction=@trace.Tx,
      kind=@isocontract.CoeSdo,
      index=cia402_mode_of_operation_index,
      subindex=b'\x00',
      channel="sdo",
      authority=@isocontract.Role("controller"),
    ),
    @isocontract.ObjectBinding::make(
      object_id="axis.target_position",
      binding_id="coe." + cia402_target_position_index.to_string() + ".write",
      medium_id="trace",
      label="cia402.target_position.write",
      direction=@trace.Tx,
      kind=@isocontract.EtherCatPdo,
      index=cia402_target_position_index,
      subindex=b'\x00',
      channel="rxpdo",
      authority=@isocontract.Role("controller"),
    ),
    @isocontract.ObjectBinding::make(
      object_id="axis.actual_position",
      binding_id="coe." + cia402_actual_position_index.to_string() + ".read",
      medium_id="trace",
      label="cia402.actual_position.read",
      direction=@trace.Rx,
      kind=@isocontract.EtherCatPdo,
      index=cia402_actual_position_index,
      subindex=b'\x00',
      channel="txpdo",
      authority=@isocontract.Role("controller"),
    ),
    @isocontract.ObjectBinding::make(
      object_id="axis.fault",
      binding_id="coe." + cia402_fault_index.to_string() + ".detected",
      medium_id="trace",
      label="cia402.fault.detected",
      direction=@trace.Rx,
      kind=@isocontract.CoeSdo,
      index=cia402_fault_index,
      subindex=b'\x00',
      channel="sdo",
      authority=@isocontract.Role("controller"),
    ),
  ]
}

///|
pub fn cia402_object_contracts() -> @isocontract.ContractRegistry {
  cia402_contracts_for_bindings(cia402_coe_bindings())
}

///|
pub fn cia402_contracts_for_bindings(
  bindings : Array[@isocontract.ObjectBinding],
) -> @isocontract.ContractRegistry {
  let registry = @isocontract.ContractRegistry::new()
  for binding in bindings {
    registry.register(contract_for_binding(binding))
  }
  registry
}

///|
fn contract_for_binding(
  binding : @isocontract.ObjectBinding,
) -> @isocontract.ObjectContract {
  match binding.object_id {
    "axis.controlword" =>
      command_contract(
        binding,
        schema=@isocontract.U16,
        max_age_ns=1_000_000L,
        valid_for_ns=1_000_000L,
      )
    "axis.mode" =>
      command_contract(
        binding,
        schema=@isocontract.I8,
        max_age_ns=10_000_000L,
        valid_for_ns=10_000_000L,
      )
    "axis.target_position" =>
      command_contract(
        binding,
        schema=@isocontract.I32,
        max_age_ns=1_000_000L,
        valid_for_ns=1_000_000L,
      )
    "axis.statusword" =>
      signal_contract(binding, schema=@isocontract.U16, max_age_ns=2_000_000L)
    "axis.actual_position" =>
      signal_contract(binding, schema=@isocontract.I32, max_age_ns=2_000_000L)
    "axis.fault" =>
      @isocontract.ObjectContract::make(
        id=binding.object_id,
        kind=@isocontract.Event,
        schema=@isocontract.Bool,
        timing=@isocontract.TimingContract::make(max_age_ns=5_000_000L),
        safety=@isocontract.SafetyContract::make(
          authority=@isocontract.ReadOnly,
        ),
      ).with_binding(binding)
    _ =>
      @isocontract.ObjectContract::make(
        id=binding.object_id,
        kind=@isocontract.Query,
        schema=@isocontract.Text,
        timing=@isocontract.TimingContract::empty(),
        safety=@isocontract.SafetyContract::make(),
      ).with_binding(binding)
  }
}

///|
fn command_contract(
  binding : @isocontract.ObjectBinding,
  schema~ : @isocontract.Schema,
  max_age_ns~ : Int64,
  valid_for_ns~ : Int64,
) -> @isocontract.ObjectContract {
  @isocontract.ObjectContract::make(
    id=binding.object_id,
    kind=@isocontract.Command,
    schema~,
    timing=@isocontract.TimingContract::make(max_age_ns~, valid_for_ns~),
    safety=@isocontract.SafetyContract::make(
      authority=@isocontract.Role("controller"),
      timeout_action="hold_last",
    ),
  ).with_binding(binding)
}

///|
fn signal_contract(
  binding : @isocontract.ObjectBinding,
  schema~ : @isocontract.Schema,
  max_age_ns~ : Int64,
) -> @isocontract.ObjectContract {
  @isocontract.ObjectContract::make(
    id=binding.object_id,
    kind=@isocontract.Signal,
    schema~,
    timing=@isocontract.TimingContract::make(period_ns=1_000_000L, max_age_ns~),
    safety=@isocontract.SafetyContract::make(authority=@isocontract.ReadOnly),
  ).with_binding(binding)
}

///|
pub fn cia402_enable_fault_reset_protocol() -> @protocol_ast.GlobalProtocol {
  let controller = @protocol_ast.role(name="controller")
  let drive = @protocol_ast.role(name="drive")
  @protocol_ast.GlobalProtocol::new(protocol_id="CiA402EnableFaultReset")
  .send(
    from=controller,
    to=drive,
    label="cia402.controlword.write",
    object_id="axis.controlword",
  )
  .send(
    from=drive,
    to=controller,
    label="cia402.statusword.read",
    object_id="axis.statusword",
  )
  .send(
    from=controller,
    to=drive,
    label="cia402.mode.write",
    object_id="axis.mode",
  )
  .send(
    from=controller,
    to=drive,
    label="cia402.target_position.write",
    object_id="axis.target_position",
  )
  .send(
    from=drive,
    to=controller,
    label="cia402.actual_position.read",
    object_id="axis.actual_position",
  )
  .send(
    from=drive,
    to=controller,
    label="cia402.fault.detected",
    object_id="axis.fault",
  )
  .send(
    from=controller,
    to=drive,
    label="cia402.controlword.write",
    object_id="axis.controlword",
  )
  .send(
    from=drive,
    to=controller,
    label="cia402.statusword.read",
    object_id="axis.statusword",
  )
}