/*/
 * Copyright (c) 2015-2018, 2021-2023, 2025-2026 LAAS/CNRS
 * All rights reserved.
 *
 * Redistribution and use  in source  and binary  forms,  with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *   1. Redistributions of  source  code must retain the  above copyright
 *      notice and this list of conditions.
 *   2. Redistributions in binary form must reproduce the above copyright
 *      notice and  this list of  conditions in the  documentation and/or
 *      other materials provided with the distribution.
 *
 *                                      Anthony Mallet on Thu May 31 2018
 */
#pragma require "openrobots2-idl >= 2.0"

#include "or/pose/pose_estimator.gen"
#include "or/pose/rigid_body.gen"
#include "or/robot/uav.gen"

component uavpos {
  version		"1.4";
  email			"openrobots@laas.fr";
  lang			"c";
  require		"genom3 >= 2.99.41";
  codels-require	"eigen3";

  uses		or_uav;
  uses		or_pose_estimator;

  port in	or_pose_estimator::state state;
  port in	or_rigid_body::state reference;

  exception e_input;
  exception e_sys { short code; string<128> what; };

  native log_s;

  enum control_state {
    idle, active
  };

  ids {
    struct body_s {
      double mass;
      double rxy;	/* cylindric boundary radius (>0: full actuation) */
    } body;

    struct servo_s {
      struct sat_s { double x, v, ix; } sat;

      struct gain_s {
        double Kpxy, Kpz, Kvxy, Kvz;
        double Kixy, Kiz;
      } gain;

      struct emerg_s {
        double descent;
        double dx, dv;
      } emerg;
    } servo;

    or_rigid_body::state reference;
    control_state mode;

    /* logging */
    log_s log;
  };

  attribute set_saturation(in servo.sat);
  attribute set_servo_gain(in servo.gain);
  attribute set_mass(in body.mass);
  attribute set_xyradius(in body.rxy =: "Maximum lateral thrust (N)");
  attribute set_emerg(in servo.emerg = {
    0.1 : "Vertical acceleration for emergency descent (m/s²)",
    0.05 : "Position uncertainty threshold (m)",
    0.2 : "Linear velocity uncertainty threshold (m/s)" }) {
    doc "Set thresholds for emergency descent.";
    doc "";
    doc "`dx` and `dv` represent the maximum uncertainty tolerated in the";
    doc "input <<state>> for position (`dx`) and linear velocity (`dv`),";
    doc "measured as 3 times the standard deviation.";
    doc "By default, `dx` is 5cm and `dv` 20cm/s. Beyond that threshold, an";
    doc "emergency descent is started.";
    doc "";
    doc "As long as the emergency descent is active, the position error or";
    doc "linear velocity error are cancelled (i.e. the desired roll and pitch";
    doc "are set to 0) and a vertical acceleration of `descent` (by default";
    doc "-0.1 m/s²) is requested. The descent is based on the mass only,";
    doc "with no feedback, as the state is considered invalid.";

    validate uavpos_set_emerg(local inout emerg);
  };

  attribute get_servo_parameters(out servo);
  attribute get_body_parameters(out body);

  function get_reference(out or_rigid_body::state reference) {
    codel uavpos_get_reference(ids in reference::internal, local out reference);
  };


  /* --- control ----------------------------------------------------------- */

  const unsigned short control_period_ms = 1;

  task main {
    period control_period_ms ms;

    codel<start> uavpos_main_start(out ::ids, out uav_input)
      yield init;
    codel<init> uavpos_main_init(ids inout reference, inout mode, in state,
                                 out uav_input)
      yield pause::init, pause::control;

    codel<control> uavpos_main_control(in mode, in body, inout servo,
                                       in state, ids inout reference,
                                       inout log, out uav_input)
      yield pause::control, emergency, init;
    codel<emergency> uavpos_main_emergency(in mode, in body, inout servo,
                                           in state, ids inout reference,
                                           inout log, out uav_input)
      yield pause::emergency, control;

    codel<stop> mk_main_stop(out uav_input)
      yield ether;
  };


  activity servo() {
    doc		"Track a desired position";
    task	main;

    codel<start> uavpos_servo_loop(port in reference::in, ids out reference)
      yield pause::start, ether;
    codel<stop> uavpos_servo_stop(ids out reference)
      yield ether;

    throw e_input;
  };

  function set_state(
    in or::t3d::pos pos =: "Position",
    in or::t3d::att att =: "Orientation",
    in or::t3d::vel vel =: "Linear velocity",
    in or::t3d::avel avel =: "Angular velocity",
    in or::t3d::acc acc =: "Acceleration",
    in or::t3d::aacc aacc =: "Angular acceleration",
    in or::t3d::jerk jerk =: "Linear jerk",
    in or::t3d::snap snap =: "Linear snap") {
    doc		"Set the desired state";

    codel uavpos_set_state(in pos, in att,
                           in vel, in avel,
                           in acc, in aacc,
                           in jerk, in snap,
                           ids out reference);

    interrupt servo;
  };

  function set_position(
    in double x, in double y, in double z, in double yaw) {
    doc		"Set the desired position";

    codel uavpos_set_position(in x, in y, in z, in yaw, ids out reference);

    interrupt servo;
  };

  activity set_current_position() {
    doc		"Set the desired position";
    task	main;

    codel<start> uavpos_set_current_position(in state, ids out reference)
      yield ether;

    interrupt servo;
    throw e_input;
  };

  function set_velocity(
    in double x, in double y, in double z, in double yaw) {
    doc "Set the desired velocity";
    doc "";
    doc "The controller will try to reach the given velocity and heading";
    doc "angular velocity. Note that for safety reasons, a watchdog will";
    doc "trigger after 500ms and reset the target velocity to zero.";

    codel uavpos_set_velocity(in x, in y, in z, in yaw, ids out reference);

    interrupt servo;
  };

  function hover() {
    doc "Stop tracking a desired position";
    doc "";
    doc "This sets the current reference velocity and acceleration to 0,";
    doc "maintaining the current position and heading. Any dynamic reference";
    doc "tracking is cancelled (see <<servo>>).";

    codel uavpos_servo_hover(ids out reference);

    interrupt servo;
  };

  function reset() {
    doc "Stop control loop and wait for a new valid reference";
    doc "";
    doc "This function is meant to reset the controller and stop controlling";
    doc "the robot. The component returns to the same state as when it is";
    doc "started, waiting for a first reference state provided either with";
    doc "<<set_state>>, <<set_position>>, <<set_velocity>> or <<servo>> to";
    doc "track a dynamic reference.";

    codel uavpos_servo_reset(out mode, ids out reference);

    interrupt servo;
  };


  /* --- logging ----------------------------------------------------------- */

  function log(in string<64> path = "/tmp/uavpos.log": "Log file name",
               in unsigned long decimation = 1: "Reduced logging frequency") {
    doc		"Log controller data";

    codel uavpos_log(in path, in decimation, inout log);

    throw e_sys;
  };

  function log_stop() {
    doc		"Stop logging";

    codel uavpos_log_stop(out log);
  };

  function log_info(out unsigned long miss = :"Missed log entries",
                    out unsigned long total = :"Total log entries") {
    doc		"Show missed log entries";

    codel uavpos_log_info(in log, out miss, out total);
  };
};
