From 8612206eaa6d73b4970e4c652405d5714efa4f8e Mon Sep 17 00:00:00 2001
From: Federico Bergero <fedebergero@gmail.com>
Date: Thu, 12 May 2011 11:45:50 +0000
Subject: [PATCH] Implementing QSS2

git-svn-id: https://openmodelica.org/svn/OpenModelica/trunk@8957 f25d12d1-65f4-0310-ae8a-bbce733d8d8e
---
 c_runtime/solver_qss/integrator.cpp      | 70 +++++++++++++++++++++++-
 c_runtime/solver_qss/integrator.h        | 16 ++++++
 c_runtime/solver_qss/static_function.cpp | 27 +++++++--
 3 files changed, 108 insertions(+), 5 deletions(-)

diff --git a/c_runtime/solver_qss/integrator.cpp b/c_runtime/solver_qss/integrator.cpp
index 2d917ea7e16..5bfb61f9aee 100644
--- a/c_runtime/solver_qss/integrator.cpp
+++ b/c_runtime/solver_qss/integrator.cpp
@@ -6,7 +6,6 @@ IntegratorQSS::IntegratorQSS(double dqm, double dqr)
   order=1;
   dQmin=dqm;
   dQrel=dqr;
-
 }
 
 void IntegratorQSS::init(Time t, unsigned int i)
@@ -66,3 +65,72 @@ void IntegratorQSS::update(Time t) {
   }
   X[state].sampledAt(t);
 }
+
+
+/// QSS 2
+IntegratorQSS2::IntegratorQSS2(double dqm, double dqr)
+{
+  order=2;
+  dQmin=dqm;
+  dQrel=dqr;
+}
+
+void IntegratorQSS2::init(Time t, unsigned int i)
+{
+  index=i;
+  for (int j=0;j<inputRows;j++)
+  {
+    if (inputMatrix[j*2]==index) {
+      cout << "Integrator " << index << " computes state " << stateNumber(inputMatrix[j*2+1]) <<endl;
+      state = stateNumber(inputMatrix[j*2+1]);
+      break;
+    }
+  }
+ 
+  sigma=0;
+  X[state].setCoeff(0,globalData->states[state]);
+  q[state].setOrder(order-1);
+  X[state].setOrder(order);
+  derX[state].setOrder(order);
+  q[state].setCoeff(0,X[state].value()); 
+  X[state].sampledAt(t);
+  q[state].sampledAt(t);
+  dQ = fabs(X[state].value())*dQrel;
+  if (dQ<dQmin)
+    dQ = dQmin;
+}
+
+void IntegratorQSS2::makeStep(Time t)
+{
+  X[state].advanceBy(sigma);
+  X[state].sampledAt(t);
+  q[state].sampledAt(t);
+  q[state].setCoeff(0,X[state].value()); 
+  q[state].setCoeff(1,X[state].coeff(1)); 
+  dQ = fabs(X[state].value())*dQrel;
+  if (dQ<dQmin)
+    dQ = dQmin;
+  if (X[state].coeff(2)==0.0)
+    sigma = INF;
+  else 
+    sigma = sqrt(fabs(dQ/X[state].coeff(2)));
+}
+
+void IntegratorQSS2::update(Time t) {
+  X[state].advanceTo(t);
+  X[state].setCoeff(1,derX[state].coeff(0));
+  X[state].setCoeff(2,derX[state].coeff(1)/2);
+  if (sigma>0) {
+    q[state].advanceTo(t);
+    q[state].sampledAt(t);
+    QssSignal diff = q[state]- X[state];
+    const double sigmaUpper = diff.offsetBy(dQ).minPosRoot();
+    const double sigmaLower = diff.offsetBy(-dQ).minPosRoot();
+    sigma=std::min(sigmaUpper,sigmaLower);
+    if (fabs(X[state].value()-q[state].value())>dQ)
+      sigma=0.0;
+  }
+  X[state].sampledAt(t);
+}
+
+
diff --git a/c_runtime/solver_qss/integrator.h b/c_runtime/solver_qss/integrator.h
index 7f51cadd158..348ec30b99b 100644
--- a/c_runtime/solver_qss/integrator.h
+++ b/c_runtime/solver_qss/integrator.h
@@ -19,3 +19,19 @@ class IntegratorQSS: public Simulator
   int order;
 };
 
+class IntegratorQSS2: public Simulator
+{
+public:
+  IntegratorQSS2(double dqm, double dqr);
+  virtual void init(Time t, unsigned int i);
+  virtual void makeStep(Time t);
+  virtual void update(Time t);
+private:
+  unsigned int state;
+  unsigned int index;
+  double dQmin;
+  double dQrel;
+  double dQ;
+  int order;
+};
+
diff --git a/c_runtime/solver_qss/static_function.cpp b/c_runtime/solver_qss/static_function.cpp
index 0afe2681a54..c2798108b16 100644
--- a/c_runtime/solver_qss/static_function.cpp
+++ b/c_runtime/solver_qss/static_function.cpp
@@ -54,21 +54,23 @@ void StaticFunction::init(Time t, unsigned int i)
  
 void StaticFunction::makeStep(Time t)
 {
+  // Evaluate at t
   advanceInputs(t);
   function_staticBlocks(index,t,inp,out);
 
   if (order>1) 
   {
+    // Evaluate at t-dt
     advanceInputs(t-dt);
-    function_staticBlocks(index,t,inp,out_dt);
+    function_staticBlocks(index,t-dt,inp,out_dt);
 
+    // Evaluate at t+dt
     advanceInputs(t+dt);
     function_staticBlocks(index,t+dt,inp,outdt);
-    
   }
 
-  writeOutputs(t);
   sigma=INF;
+  writeOutputs(t);
   // Take back the time
   globalData->timeValue=t;
 }
@@ -86,6 +88,7 @@ void StaticFunction::writeOutputs(Time t)
   {
     zc[indexCrossing].sampledAt(t);
     zc[indexCrossing].setCoeff(0,out[0]);
+    zc[indexCrossing].setCoeff(1,(outdt[i]-out_dt[i])/(dt*2));
     return;
   }
   for (;it!=computes.end();it++,i++)
@@ -95,11 +98,27 @@ void StaticFunction::writeOutputs(Time t)
       cout << "Block " << devsIndex << " writes der " << stateNumber(*it) << endl;
       derX[stateNumber(*it)].sampledAt(t);
       derX[stateNumber(*it)].setCoeff(0,out[i]);
-      // If order>1...
+      if (order>1) 
+        derX[stateNumber(*it)].setCoeff(1,(outdt[i]-out_dt[i])/(dt*2));
     } else {
       cout << "Block " << devsIndex << " writes algebraic " << algNumber(*it) << endl;
       alg[algNumber(*it)].sampledAt(t);
       alg[algNumber(*it)].setCoeff(0,out[i]);
+      if (order>1)
+        alg[algNumber(*it)].setCoeff(1,(outdt[i]-out_dt[i])/(dt*2));
+    }
+    if (order==2) 
+    {
+      const double ddf=(outdt[i]-2*out[i]+out_dt[i])/(dt*dt*2);
+      const double tolerr=dQmin+dQrel*fabs(out[i]);
+      if (ddf!=0) 
+      {
+        const double s=.9*sqrt(fabs(ddf/tolerr));
+        if (s<sigma) {
+          cout << "Adjusting sigma to " << s << endl;
+          sigma=s;
+        }
+      }
     }
   } 
 }