//**********Mustafa Chougale***************
//**********Simulation & Microcontroller HW***********

//Plant Variables and Parameters:
float omega = 0; // motor speed [rad/s]
float tqBalance = 0; // resulting torque from tqInrLim, tqFriction and tqDisturbance [Nm]
float tqFriction = 0; 
float tqDisturbance = 0; // Load torque shall appear after 10 s with 6 Nm 
float inertia = 0.3; // [kg m^2]   !!!!!   => use here this small value => later change it to 0.4 [kg m^2]  
float gain = 1/inertia; // gain multipied
float damping = 0.05; // [Nm/(rad/s)]

// -----------------------------Input--------------------------------- 
float tqInrLim = 0; // limited inner motor torque [Nm], shall appear with 10 Nm 2 sec after simulation start

// Scheduling Variables and Parameters:
unsigned long h = 10; // h in msec
unsigned long printInterval = 50; // in msec
unsigned long oldMillis = 0; // for model if
unsigned long oldPrintMillis = 0; // for print if

//------------------------Constant Inputs------------------------------
const float T_step = 10.0; // Step Input Torque (Nm)
const float T_disturbance = 0; // Disturbance Torque (Nm)
const unsigned long T_step_time = 2000; // 2 seconds
const unsigned long T_disturbance_time = 10000; // 10 seconds
const unsigned long T_unlimited_time = 2000; // Time to apply unlimited torque [ms]

//-----------------------Limiting Torque-------------------------------
float tqInrUnLim = 500; // unlimited inner motor torque from controller [Nm]
float omegaEdge = 250; // edge speed [rad/s]
float pwrAbsInrMax = 50000; // max./min. power is +/-50000 W
float sign = 0; //sign setup

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
}

void loop() {
  // put your main code here, to run repeatedly:
unsigned long currentMillis = millis();

  // Update the simulation at the defined time step
    if (currentMillis - oldMillis >= h) {
        oldMillis = currentMillis;

//----------------------Limiting Torque Code----------------------------
        // Apply torque input after 2 seconds
        if (currentMillis >= T_step_time && currentMillis < T_step_time + T_unlimited_time) {
        tqInrLim = tqInrUnLim; // Set inner torque to unlimited torque for 2 seconds
       } else if (currentMillis >= T_step_time + T_unlimited_time) {
        tqInrLim = 0; // Reset inner torque after the unlimited period
        }

        // Limiting torque code second part
        float omegaAbsLimEdge = max(abs(omega), omegaEdge); //maximum of omega uptil 250
        float tqunlimandomegaAbsLimEdge = tqInrLim*abs(omegaAbsLimEdge);
        float pwr_AbsInrLimDes = min(tqunlimandomegaAbsLimEdge,pwrAbsInrMax);
        float tqAbsInrLimDes = pwr_AbsInrLimDes/omegaAbsLimEdge;

        if (tqInrUnLim >= 0) {sign = 1;}
        else {sign = -1;} //sign(0) = 1
        tqInrLim = tqAbsInrLimDes*sign;

        
        // Apply disturbance torque after 10 seconds
        if (currentMillis >= T_disturbance_time) {
            tqDisturbance = T_disturbance;
        }

        // Calculate total balance torque
        tqBalance = tqInrLim - tqFriction - tqDisturbance;

        // Calculate angular acceleration
        float torque = gain * tqBalance;

        // Apply damping to balance torque
        tqFriction = damping * omega;

        // motor speed (omega) using Euler integration y = y + (float)h/1000/T*u;
        omega = omega + (h * torque)/1000; // Convert ms to seconds

    }
    
// Printing in Printing Loop (here in 50 ms that the display speed in the Arduino IDE fits well)
 if (currentMillis-oldPrintMillis >= printInterval){

Serial.print(0); 	// Nr. 1 printing => set 0 for the lowest value
Serial.print(" ");
Serial.print(omega/4);  // Nr. 2 printing => read omega by multiplying the value with 4 
Serial.print(" ");
Serial.print(tqInrLim);  // Nr. 3 for printing => limited torque 
Serial.print(" ");
Serial.println(250);  // Nr. 4 printing => set max. value to 250 for printing the highest value

  oldPrintMillis += printInterval;
  }

}