//
// Academic License - for use in teaching, academic research, and meeting
// course requirements at degree granting institutions only.  Not for
// government, commercial, or other organizational use.
//
// File: Controler.cpp
//
// Code generated for Simulink model 'Controler'.
//
// Model version                  : 5.115
// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Tue Aug 12 11:26:01 2025
//
// Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Linux 64)
// Code generation objectives:
//    1. Execution efficiency
//    2. RAM efficiency
// Validation result: Not run
//
#include "Controler.h"
#include <cmath>
#include "rtwtypes.h"
#include "cmath"

// Model step function
void Controler::step()
{
  real_T mod_error;
  real_T rtb_DeadZone;
  real_T rtb_FilterCoefficient;
  real_T rtb_FilterCoefficient_a;
  real_T rtb_IntegralGain;
  real_T rtb_Integrator_l;
  real_T rtb_Sum;
  real_T rtb_Switch;
  real_T rtb_depth_error;
  int8_T tmp;
  int8_T tmp_0;
  boolean_T rtb_RelationalOperator;
  boolean_T rtb_fixforDTpropagationissue;

  // Outputs for Atomic SubSystem: '<Root>/Controler'
  // MATLAB Function: '<S1>/MATLAB Function1' incorporates:
  //   Inport: '<Root>/heading_cmd'
  //   Inport: '<Root>/psi'

  mod_error = rtU.psi - rtU.heading_cmd;
  if (!std::isnan(mod_error)) {
    if (std::isinf(mod_error)) {
      mod_error = (rtNaN);
    } else if (mod_error == 0.0) {
      mod_error = 0.0;
    } else {
      mod_error = std::fmod(mod_error, 360.0);
      if (mod_error == 0.0) {
        mod_error = 0.0;
      } else if (mod_error < 0.0) {
        mod_error += 360.0;
      }
    }
  }

  if (mod_error >= 180.0) {
    mod_error -= 360.0;
  }

  // End of MATLAB Function: '<S1>/MATLAB Function1'

  // Gain: '<S98>/Filter Coefficient' incorporates:
  //   DiscreteIntegrator: '<S90>/Filter'
  //   Gain: '<S88>/Derivative Gain'
  //   Sum: '<S90>/SumD'

  rtb_FilterCoefficient = (rtP.pid_heading.D * mod_error - rtDW.Filter_DSTATE) *
    rtP.pid_heading.N;

  // Sum: '<S104>/Sum' incorporates:
  //   DiscreteIntegrator: '<S95>/Integrator'
  //   Gain: '<S100>/Proportional Gain'

  rtb_Integrator_l = (rtP.pid_heading.P * mod_error + rtDW.Integrator_DSTATE) +
    rtb_FilterCoefficient;

  // DeadZone: '<S87>/DeadZone'
  if (rtb_Integrator_l > 35.0) {
    rtb_IntegralGain = rtb_Integrator_l - 35.0;
  } else if (rtb_Integrator_l >= -35.0) {
    rtb_IntegralGain = 0.0;
  } else {
    rtb_IntegralGain = rtb_Integrator_l - -35.0;
  }

  // End of DeadZone: '<S87>/DeadZone'

  // RelationalOperator: '<S85>/Relational Operator' incorporates:
  //   Constant: '<S85>/Clamping_zero'

  rtb_RelationalOperator = (rtb_IntegralGain != 0.0);

  // RelationalOperator: '<S85>/fix for DT propagation issue' incorporates:
  //   Constant: '<S85>/Clamping_zero'

  rtb_fixforDTpropagationissue = (rtb_IntegralGain > 0.0);

  // Gain: '<S92>/Integral Gain'
  rtb_IntegralGain = rtP.pid_heading.I * mod_error;

  // Switch: '<S85>/Switch1' incorporates:
  //   Constant: '<S85>/Constant'
  //   Constant: '<S85>/Constant2'

  if (rtb_fixforDTpropagationissue) {
    tmp = 1;
  } else {
    tmp = -1;
  }

  // Switch: '<S85>/Switch2' incorporates:
  //   Constant: '<S85>/Clamping_zero'
  //   Constant: '<S85>/Constant3'
  //   Constant: '<S85>/Constant4'
  //   RelationalOperator: '<S85>/fix for DT propagation issue1'

  if (rtb_IntegralGain > 0.0) {
    tmp_0 = 1;
  } else {
    tmp_0 = -1;
  }

  // Switch: '<S85>/Switch' incorporates:
  //   Constant: '<S85>/Constant1'
  //   Logic: '<S85>/AND3'
  //   RelationalOperator: '<S85>/Equal1'
  //   Switch: '<S85>/Switch1'
  //   Switch: '<S85>/Switch2'

  if (rtb_RelationalOperator && (tmp == tmp_0)) {
    rtb_Switch = 0.0;
  } else {
    rtb_Switch = rtb_IntegralGain;
  }

  // End of Switch: '<S85>/Switch'

  // Saturate: '<S102>/Saturation'
  if (rtb_Integrator_l > 35.0) {
    rtb_Integrator_l = 35.0;
  } else if (rtb_Integrator_l < -35.0) {
    rtb_Integrator_l = -35.0;
  }

  // End of Saturate: '<S102>/Saturation'

  // Sum: '<S1>/Sum1' incorporates:
  //   Inport: '<Root>/depth_cmd'
  //   Inport: '<Root>/z'

  rtb_depth_error = rtU.depth_cmd - rtU.z;

  // Gain: '<S38>/Integral Gain'
  rtb_IntegralGain = rtP.pid_depth.I * rtb_depth_error;

  // Gain: '<S44>/Filter Coefficient' incorporates:
  //   DiscreteIntegrator: '<S36>/Filter'
  //   Gain: '<S34>/Derivative Gain'
  //   Sum: '<S36>/SumD'

  rtb_FilterCoefficient_a = (rtP.pid_depth.D * rtb_depth_error -
    rtDW.Filter_DSTATE_m) * rtP.pid_depth.N;

  // Sum: '<S50>/Sum' incorporates:
  //   DiscreteIntegrator: '<S41>/Integrator'
  //   Gain: '<S46>/Proportional Gain'

  rtb_Sum = (rtP.pid_depth.P * rtb_depth_error + rtDW.Integrator_DSTATE_o) +
    rtb_FilterCoefficient_a;

  // DeadZone: '<S33>/DeadZone' incorporates:
  //   Saturate: '<S48>/Saturation'

  if (rtb_Sum > 30.0) {
    rtb_DeadZone = rtb_Sum - 30.0;
    rtb_Sum = 30.0;
  } else {
    if (rtb_Sum >= -30.0) {
      rtb_DeadZone = 0.0;
    } else {
      rtb_DeadZone = rtb_Sum - -30.0;
    }

    if (rtb_Sum < -30.0) {
      rtb_Sum = -30.0;
    }
  }

  // End of DeadZone: '<S33>/DeadZone'

  // Update for DiscreteIntegrator: '<S95>/Integrator'
  rtDW.Integrator_DSTATE += rtb_Switch;
  if (rtDW.Integrator_DSTATE > 15.0) {
    rtDW.Integrator_DSTATE = 15.0;
  } else if (rtDW.Integrator_DSTATE < -15.0) {
    rtDW.Integrator_DSTATE = -15.0;
  }

  // End of Update for DiscreteIntegrator: '<S95>/Integrator'

  // Update for DiscreteIntegrator: '<S90>/Filter'
  rtDW.Filter_DSTATE += 0.1 * rtb_FilterCoefficient;

  // Switch: '<S31>/Switch1' incorporates:
  //   Constant: '<S31>/Clamping_zero'
  //   Constant: '<S31>/Constant'
  //   Constant: '<S31>/Constant2'
  //   RelationalOperator: '<S31>/fix for DT propagation issue'

  if (rtb_DeadZone > 0.0) {
    tmp = 1;
  } else {
    tmp = -1;
  }

  // Switch: '<S31>/Switch2' incorporates:
  //   Constant: '<S31>/Clamping_zero'
  //   Constant: '<S31>/Constant3'
  //   Constant: '<S31>/Constant4'
  //   RelationalOperator: '<S31>/fix for DT propagation issue1'

  if (rtb_IntegralGain > 0.0) {
    tmp_0 = 1;
  } else {
    tmp_0 = -1;
  }

  // Switch: '<S31>/Switch' incorporates:
  //   Constant: '<S31>/Clamping_zero'
  //   Constant: '<S31>/Constant1'
  //   Logic: '<S31>/AND3'
  //   RelationalOperator: '<S31>/Equal1'
  //   RelationalOperator: '<S31>/Relational Operator'
  //   Switch: '<S31>/Switch1'
  //   Switch: '<S31>/Switch2'

  if ((rtb_DeadZone != 0.0) && (tmp == tmp_0)) {
    rtb_IntegralGain = 0.0;
  }

  // Update for DiscreteIntegrator: '<S41>/Integrator' incorporates:
  //   Switch: '<S31>/Switch'

  rtDW.Integrator_DSTATE_o += rtb_IntegralGain;
  if (rtDW.Integrator_DSTATE_o > 10.0) {
    rtDW.Integrator_DSTATE_o = 10.0;
  } else if (rtDW.Integrator_DSTATE_o < -10.0) {
    rtDW.Integrator_DSTATE_o = -10.0;
  }

  // End of Update for DiscreteIntegrator: '<S41>/Integrator'

  // Update for DiscreteIntegrator: '<S36>/Filter'
  rtDW.Filter_DSTATE_m += 0.1 * rtb_FilterCoefficient_a;

  // Outport: '<Root>/MainThrusterSpeedCmd' incorporates:
  //   Constant: '<S1>/Constant1'
  //   Inport: '<Root>/speed_cmd'
  //   Product: '<S1>/Divide'

  rtY.MainThrusterSpeedCmd = rtU.speed_cmd / 0.025;

  // End of Outputs for SubSystem: '<Root>/Controler'

  // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
  rtY.DirectUpperRudderServoAngleCmd = rtb_Integrator_l;

  // Outport: '<Root>/DirectLowerRudderServoAngleCmd'
  rtY.DirectLowerRudderServoAngleCmd = rtb_Integrator_l;

  // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
  rtY.DirectLeftRudderServoAngleCmd = rtb_Sum;

  // Outport: '<Root>/DirectRightRudderServoAngleCmd'
  rtY.DirectRightRudderServoAngleCmd = rtb_Sum;

  // Outport: '<Root>/heading_error'
  rtY.heading_error = mod_error;

  // Outport: '<Root>/pitch_error' incorporates:
  //   Constant: '<S1>/Constant'

  rtY.pitch_error = 0.0;

  // Outport: '<Root>/depth_error'
  rtY.depth_error = rtb_depth_error;
}

// Model initialize function
void Controler::initialize()
{
  // (no initialization code required)
}

const char_T* Controler::RT_MODEL::getErrorStatus() const
{
  return (errorStatus);
}

void Controler::RT_MODEL::setErrorStatus(const char_T* const volatile
  aErrorStatus)
{
  (errorStatus = aErrorStatus);
}

// Constructor
Controler::Controler() :
  rtU(),
  rtY(),
  rtDW(),
  rtM()
{
  // Currently there is no constructor body generated.
}

// Destructor
// Currently there is no destructor body generated.
Controler::~Controler() = default;

// Real-Time Model get method
Controler::RT_MODEL * Controler::getRTM()
{
  return (&rtM);
}

//
// File trailer for generated code.
//
// [EOF]
//
