Secondary branch, TF and limiter solution

much work, such results

11 files changed, 309 insertions, 50 deletions

diff --git a/Project/ControlEditor.cpp b/Project/ControlEditor.cpp
index 0780590..b4976ad 100644
--- a/Project/ControlEditor.cpp
+++ b/Project/ControlEditor.cpp
@@ -616,14 +616,41 @@ void ControlEditor::OnKeyDown(wxKeyEvent& event)
             {
                 RotateSelectedElements(event.GetModifiers() != wxMOD_SHIFT);
             } break;
-            case 'L':
-            {
-                //tests
+            case 'L': {
+                // tests
                 if(event.ControlDown() && event.ShiftDown()) {
-                    
-                    ControlElementSolver solver(this, 1e-3, true, 0.0);
-                    solver.SolveNextStep(1.0);
-                    solver.SolveNextStep(1.12);
+                    double timeStep = 1e-3;
+                    double integrationError = 1e-5;
+                    double simTime = 10.0;
+                    double printStep = 1e-2;
+
+                    ControlElementSolver solver(this, timeStep, integrationError, true, 0.0);
+
+                    double currentTime = 0.0;
+                    double printTime = 0.0;
+                    std::vector<double> time;
+                    std::vector<double> solution;
+                    while(currentTime <= simTime) {
+                        double input = 0.0;
+                        if(currentTime >= 1.0) input = 1.0;
+
+                        solver.SolveNextStep(input);
+                        if(printTime >= printStep) {
+                            time.push_back(currentTime);
+                            solution.push_back(solver.GetLastSolution());
+                            printTime = 0.0;
+                        }
+                        printTime += timeStep;
+                        currentTime += timeStep;
+                    }
+                    std::vector<ElementPlotData> epdList;
+                    ElementPlotData curve1Data(_("TESTES"), ElementPlotData::CT_BUS);
+                    curve1Data.AddData(solution, _("teste 1"));
+                    epdList.push_back(curve1Data);
+
+                    ChartView* cView = new ChartView(this, epdList, time);
+                    cView->Show();
+
                     /*
                     std::vector<double> time, sinC, cosC, tgC;
                     for(int i=0; i<360; ++i) {
@@ -633,21 +660,21 @@ void ControlEditor::OnKeyDown(wxKeyEvent& event)
                         tgC.push_back(std::tan(wxDegToRad(i)));
                     }
                     std::vector<ElementPlotData> epdList;
-                    
+
                     ElementPlotData curve1Data(_("Func. polinomiais 1"), ElementPlotData::CT_BUS);
                     curve1Data.AddData(sinC, _("seno"));
                     epdList.push_back(curve1Data);
-                    
+
                     ElementPlotData curve2Data(_("Func. polinomiais 2"), ElementPlotData::CT_BUS);
                     curve2Data.AddData(tgC, _("tangente"));
                     epdList.push_back(curve2Data);
-                    
+
                     ElementPlotData curve3Data(_("Func. polinomiais 3"), ElementPlotData::CT_SYNC_GENERATOR);
                     curve3Data.AddData(sinC, _("seno"));
                     curve3Data.AddData(cosC, _("cosseno"));
                     curve3Data.AddData(tgC, _("tangente"));
                     epdList.push_back(curve3Data);
-                    
+
                     ChartView* cView = new ChartView(this, epdList, time);
                     cView->Show();*/
                 }
@@ -775,21 +802,19 @@ void ControlEditor::OnImportClick(wxCommandEvent& event)
                                   wxOK | wxCENTRE | wxICON_ERROR);
         msgDialog.ShowModal();
     }
-    
-    //Get the highest id number
+
+    // Get the highest id number
     int majorElementID = 0;
     for(auto it = m_elementList.begin(), itEnd = m_elementList.end(); it != itEnd; ++it) {
         ControlElement* element = *it;
-        if(element->GetID() > majorElementID)
-            majorElementID = element->GetID();
+        if(element->GetID() > majorElementID) majorElementID = element->GetID();
     }
     for(auto it = m_connectionList.begin(), itEnd = m_connectionList.end(); it != itEnd; ++it) {
         ConnectionLine* line = *it;
-        if(line->GetID() > majorElementID)
-            majorElementID = line->GetID();
+        if(line->GetID() > majorElementID) majorElementID = line->GetID();
     }
     m_lastElementID = ++majorElementID;
-    
+
     Redraw();
     event.Skip();
 }
diff --git a/Project/ControlElement.h b/Project/ControlElement.h
index e2b9a29..3744c6f 100644
--- a/Project/ControlElement.h
+++ b/Project/ControlElement.h
@@ -65,8 +65,13 @@ class ControlElement : public Element
 
     virtual bool IsSolved() const { return m_solved; }
     virtual void SetSolved(bool solved = true) { m_solved = solved; }
-    virtual bool Solve(double input) { return true; }
+    virtual bool Solve(double input)
+    {
+        m_output = input * 2.0;
+        return true;
+    }
     virtual double GetOutput() const { return m_output; }
+    virtual void SetOutput(double output) { m_output = output; }
    protected:
     std::vector<Node*> m_nodeList;
     bool m_solved = false;
diff --git a/Project/ControlElementSolver.cpp b/Project/ControlElementSolver.cpp
index d1c9b26..42445f4 100644
--- a/Project/ControlElementSolver.cpp
+++ b/Project/ControlElementSolver.cpp
@@ -15,6 +15,7 @@
 
 ControlElementSolver::ControlElementSolver(ControlEditor* controlEditor,
                                            double timeStep,
+                                           double integrationError,
                                            bool startAllZero,
                                            double input)
 {
@@ -56,10 +57,36 @@ ControlElementSolver::ControlElementSolver(ControlEditor* controlEditor,
     }
 
     m_timeStep = timeStep;
-    if(!startAllZero) InitializeValues(input);
+    m_integrationError = integrationError;
+    InitializeValues(input, startAllZero);
+}
+
+void ControlElementSolver::InitializeValues(double input, bool startAllZero)
+{
+    // Reset Elements values
+    auto elementList = m_ctrlContainer->GetControlElementsList();
+    for(auto it = elementList.begin(), itEnd = elementList.end(); it != itEnd; ++it) {
+        ControlElement* element = *it;
+        element->SetSolved(false);
+        element->SetOutput(0.0);
+    }
+    auto tfList = m_ctrlContainer->GetTFList();
+    for(auto it = tfList.begin(), itEnd = tfList.end(); it != itEnd; ++it) {
+        TransferFunction* tf = *it;
+        tf->CalculateSpaceState(m_timeStep, m_integrationError);
+    }
+    auto connectionLineList = m_ctrlContainer->GetConnectionLineList();
+    for(auto it = connectionLineList.begin(), itEnd = connectionLineList.end(); it != itEnd; ++it) {
+        ConnectionLine* cLine = *it;
+        cLine->SetSolved(false);
+        cLine->SetValue(0.0);
+    }
+
+    if(!startAllZero) {
+        // Calculate the steady-state results according to the input.
+    }
 }
 
-void ControlElementSolver::InitializeValues(double input) {}
 void ControlElementSolver::SolveNextStep(double input)
 {
     // Set all elements as not solved
@@ -73,7 +100,7 @@ void ControlElementSolver::SolveNextStep(double input)
         ConnectionLine* cLine = *it;
         cLine->SetSolved(false);
     }
-    
+
     // Get first node and set input value on connected lines
     ConnectionLine* firstConn = static_cast<ConnectionLine*>(m_inputControl->GetChildList()[0]);
     m_inputControl->SetSolved();
@@ -85,17 +112,45 @@ void ControlElementSolver::SolveNextStep(double input)
     auto constantList = m_ctrlContainer->GetConstantList();
     for(auto it = constantList.begin(), itEnd = constantList.end(); it != itEnd; ++it) {
         Constant* constant = *it;
-        constant->SetSolved();
-        ConnectionLine* child = static_cast<ConnectionLine*>(constant->GetChildList()[0]);
-        child->SetValue(constant->GetValue());
-        child->SetSolved();
-        FillAllConnectedChildren(child);
+        if(constant->GetChildList().size() == 1) {
+            constant->SetSolved();
+            ConnectionLine* child = static_cast<ConnectionLine*>(constant->GetChildList()[0]);
+            child->SetValue(constant->GetValue());
+            child->SetSolved();
+            FillAllConnectedChildren(child);
+        }
     }
-    
+
     ConnectionLine* currentLine = firstConn;
     while(currentLine) {
-        wxMessageBox(wxString::Format("%d", currentLine->GetID()));
+        ConnectionLine* lastLine = currentLine;
         currentLine = SolveNextElement(currentLine);
+        if(!currentLine) m_solutions.push_back(lastLine->GetValue());
+    }
+
+    bool haveUnsolvedElement = true;
+    while(haveUnsolvedElement) {
+        haveUnsolvedElement = false;
+        // Get the solved line connected with unsolved element (elements not connected in the main branch).
+        for(auto it = connectionLineList.begin(), itEnd = connectionLineList.end(); it != itEnd; ++it) {
+            ConnectionLine* cLine = *it;
+            if(cLine->IsSolved()) {
+                auto parentList = cLine->GetParentList();
+                for(auto itP = parentList.begin(), itPEnd = parentList.end(); itP != itPEnd; ++itP) {
+                    ControlElement* parent = static_cast<ControlElement*>(*itP);
+                    if(!parent->IsSolved()) {
+                        haveUnsolvedElement = true;
+                        // Solve secondary branch.
+                        currentLine = cLine;
+                        while(currentLine) {
+                            currentLine = SolveNextElement(currentLine);
+                        }
+                        break;
+                    }
+                }
+            }
+            if(haveUnsolvedElement) break;
+        }
     }
 }
 
@@ -119,22 +174,21 @@ ConnectionLine* ControlElementSolver::SolveNextElement(ConnectionLine* currentLi
         if(!element->IsSolved()) {
             if(!element->Solve(currentLine->GetValue())) return NULL;
             element->SetSolved();
-            
+
             // Get the output node (must have one or will result NULL).
             Node* outNode = NULL;
             auto nodeList = element->GetNodeList();
             for(auto itN = nodeList.begin(), itNEnd = nodeList.end(); itN != itNEnd; ++itN) {
                 Node* node = *itN;
-                if(node->GetNodeType() == Node::NODE_OUT)
-                    outNode = node;
+                if(node->GetNodeType() == Node::NODE_OUT) outNode = node;
             }
             if(!outNode) return NULL;
-            
+
             // Set connection line value associated with the output node.
             auto childList = element->GetChildList();
             for(auto itC = childList.begin(), itCEnd = childList.end(); itC != itCEnd; ++itC) {
                 ConnectionLine* cLine = static_cast<ConnectionLine*>(*itC);
-                if(!cLine->IsSolved()) { // Only check unsolved lines
+                if(!cLine->IsSolved()) {  // Only check unsolved lines
                     // Check if the connection line have the output node on the list
                     auto lineNodeList = cLine->GetNodeList();
                     for(auto itCN = nodeList.begin(), itCNEnd = nodeList.end(); itCN != itCNEnd; ++itCN) {
@@ -142,7 +196,7 @@ ConnectionLine* ControlElementSolver::SolveNextElement(ConnectionLine* currentLi
                         if(childNode == outNode) {
                             // Check if the line connect two elements, otherwise return NULL
                             if(cLine->GetType() != ConnectionLine::ELEMENT_ELEMENT) return NULL;
-                            
+
                             // Set the connection line value and return it.
                             cLine->SetValue(element->GetOutput());
                             cLine->SetSolved();
diff --git a/Project/ControlElementSolver.h b/Project/ControlElementSolver.h
index 9ca6e02..e51c40d 100644
--- a/Project/ControlElementSolver.h
+++ b/Project/ControlElementSolver.h
@@ -23,10 +23,11 @@ class ControlElementSolver
     ControlElementSolver() {}
     ControlElementSolver(ControlEditor* controlEditor,
                          double timeStep = 1e-3,
+                         double integrationError = 1e-3,
                          bool startAllZero = false,
                          double input = 0.0);
     ~ControlElementSolver() {}
-    virtual void InitializeValues(double input);
+    virtual void InitializeValues(double input, bool startAllZero);
     virtual void SolveNextStep(double input);
     virtual std::vector<double> GetSolutions() { return m_solutions; }
     virtual double GetLastSolution() { return m_solutions[m_solutions.size() - 1]; }
@@ -36,6 +37,7 @@ class ControlElementSolver
 
     ControlElementContainer* m_ctrlContainer = NULL;
     double m_timeStep;
+    double m_integrationError;
     std::vector<double> m_solutions;
 
     IOControl* m_inputControl = NULL;
diff --git a/Project/Limiter.cpp b/Project/Limiter.cpp
index 2a0b707..de2b669 100644
--- a/Project/Limiter.cpp
+++ b/Project/Limiter.cpp
@@ -87,3 +87,12 @@ void Limiter::UpdatePoints()
         m_nodeList[1]->SetPosition(m_position + wxPoint2DDouble(0, -18));
     }
 }
+
+bool Limiter::Solve(double input)
+{
+    m_output = input;
+    if(m_output > m_upLimit) m_output = m_upLimit;
+    else if(m_output < m_lowLimit) m_output = m_lowLimit;
+    
+    return true;
+}
diff --git a/Project/Limiter.h b/Project/Limiter.h
index 5f4e55b..b51d8cb 100644
--- a/Project/Limiter.h
+++ b/Project/Limiter.h
@@ -16,6 +16,7 @@ public:
     virtual bool Intersects(wxRect2DDouble rect) const { return m_rect.Intersects(rect); }
     virtual bool ShowForm(wxWindow* parent, Element* element);
     virtual void Rotate(bool clockwise = true);
+    virtual bool Solve(double input);
 
     virtual void UpdatePoints();
     
diff --git a/Project/Project.mk b/Project/Project.mk
index 8d02db8..215c533 100644
--- a/Project/Project.mk
+++ b/Project/Project.mk
@@ -13,7 +13,7 @@ CurrentFileName        :=
 CurrentFilePath        :=
 CurrentFileFullPath    :=
 User                   :=NDSE-69
-Date                   :=06/05/2017
+Date                   :=08/05/2017
 CodeLitePath           :="C:/Program Files/CodeLite"
 LinkerName             :=C:/TDM-GCC-64/bin/g++.exe
 SharedObjectLinkerName :=C:/TDM-GCC-64/bin/g++.exe -shared -fPIC
diff --git a/Project/Sum.cpp b/Project/Sum.cpp
index 606b367..bd35966 100644
--- a/Project/Sum.cpp
+++ b/Project/Sum.cpp
@@ -1,8 +1,8 @@
 #include "Sum.h"
 #include "SumForm.h"
+#include "ConnectionLine.h"
 
-Sum::Sum(int id)
-    : ControlElement(id)
+Sum::Sum(int id) : ControlElement(id)
 {
     m_width = m_height = 36.0;
     Node* nodeIn1 = new Node(m_position + wxPoint2DDouble(-m_width / 2, 9 - m_height / 2), Node::NODE_IN, m_borderSize);
@@ -23,7 +23,6 @@ Sum::Sum(int id)
 }
 
 Sum::~Sum() {}
-
 void Sum::Draw(wxPoint2DDouble translation, double scale) const
 {
     glLineWidth(1.0);
@@ -133,7 +132,7 @@ void Sum::UpdatePoints()
     else if(m_angle == 270.0)
         m_nodeList[m_nodeList.size() - 1]->SetPosition(m_position + wxPoint2DDouble(0, -m_height / 2));
 
-    SetPosition(m_position); // Update rect.
+    SetPosition(m_position);  // Update rect.
 }
 
 void Sum::AddInNode()
@@ -181,3 +180,39 @@ void Sum::Rotate(bool clockwise)
         node->Rotate(clockwise);
     }
 }
+
+bool Sum::Solve(double input)
+{
+    std::vector<double> inputVector;
+    for(auto itN = m_nodeList.begin(), itNEnd = m_nodeList.end(); itN != itNEnd; ++itN) {
+        Node* node = *itN;
+        if(node->GetNodeType() != Node::NODE_OUT) {
+            if(!node->IsConnected()) {
+                inputVector.push_back(0.0);
+            } else {
+                for(auto itC = m_childList.begin(), itCEnd = m_childList.end(); itC != itCEnd; ++itC) {
+                    ConnectionLine* cLine = static_cast<ConnectionLine*>(*itC);
+                    auto nodeList = cLine->GetNodeList();
+                    for(auto itCN = nodeList.begin(), itCNEnd = nodeList.end(); itCN != itCNEnd; ++itCN) {
+                        Node* childNode = *itCN;
+                        if(childNode == node) {
+                            inputVector.push_back(cLine->GetValue());
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    if(m_signalList.size() != inputVector.size()) return false;
+
+    m_output = 0.0;
+    for(unsigned int i = 0; i < m_signalList.size(); ++i) {
+        if(m_signalList[i] == SIGNAL_POSITIVE)
+            m_output += inputVector[i];
+        else if(m_signalList[i] == SIGNAL_NEGATIVE)
+            m_output -= inputVector[i];
+    }
+    return true;
+}
diff --git a/Project/Sum.h b/Project/Sum.h
index a99067e..49059f0 100644
--- a/Project/Sum.h
+++ b/Project/Sum.h
@@ -4,6 +4,7 @@
 #include "ControlElement.h"
 
 class SumForm;
+class ConnectionLine;
 
 class Sum : public ControlElement
 {
@@ -21,6 +22,8 @@ public:
     virtual std::vector<Signal> GetSignalList() const { return m_signalList; }
     virtual void SetSignalList(std::vector<Signal> signalList) { m_signalList = signalList; }
     
+    virtual bool Solve(double input);
+    
     virtual void UpdatePoints();
     void AddInNode();
     void RemoveInNode();
diff --git a/Project/TransferFunction.cpp b/Project/TransferFunction.cpp
index 9c8ffea..2b023ea 100644
--- a/Project/TransferFunction.cpp
+++ b/Project/TransferFunction.cpp
@@ -32,7 +32,6 @@ TransferFunction::TransferFunction(int id) : ControlElement(id)
 }
 
 TransferFunction::~TransferFunction() {}
-
 void TransferFunction::Draw(wxPoint2DDouble translation, double scale) const
 {
     glLineWidth(1.0);
@@ -88,7 +87,7 @@ void TransferFunction::SetText(wxString numerator, wxString denominator)
 
     m_width = nWidth > dWidth ? nWidth : dWidth;
     m_height = m_glStringNum->getheight() + m_glStringDen->getheight() + 2 * m_borderSize;
-    SetPosition(m_position); // Update rect properly.
+    SetPosition(m_position);  // Update rect properly.
 }
 
 wxString TransferFunction::GetSuperscriptNumber(int number)
@@ -248,3 +247,112 @@ void TransferFunction::Rotate(bool clockwise)
         node->Rotate(clockwise);
     }
 }
+
+void TransferFunction::CalculateSpaceState(double timeStep, double error)
+{
+    m_timeStep = timeStep;
+    m_error = error;
+
+    int order = static_cast<int>(m_denominator.size());
+    std::vector<double> denominator = m_denominator;
+    std::vector<double> numerator;
+
+    int k = order;
+    for(int i = 0; i < order; i++) {
+        int numIndex = i - (order - static_cast<int>(m_numerator.size()));
+        if(numIndex < 0)
+            numerator.push_back(0.0);
+        else
+            numerator.push_back(m_numerator[numIndex]);
+        k--;
+    }
+
+    SpaceState ss;
+    for(int i = 0; i < (order - 1); i++) {
+        std::vector<double> lineA;
+        for(int j = 0; j < (order - 1); j++) {
+            if(j == i + 1)
+                lineA.push_back(1.0);
+            else
+                lineA.push_back(0.0);
+        }
+        ss.A.push_back(lineA);
+        ss.B.push_back(0.0);
+        ss.C.push_back(0.0);
+    }
+    for(int i = 0; i < order - 1; i++) {
+        ss.A[order - 2][i] = -(denominator[order - 1 - i] / denominator[0]);
+        ss.C[i] = (numerator[order - 1 - i] - denominator[order - 1 - i] * numerator[0]) / denominator[0];
+    }
+    ss.B[order - 2] = 1.0;
+    ss.D = numerator[0];
+
+    m_ss = ss;
+
+    // Reset state
+    m_x.clear();
+    m_dx.clear();
+
+    for(unsigned int i = 0; i < m_denominator.size(); ++i) {
+        m_x.push_back(0.0);
+        m_dx.push_back(0.0);
+    }
+}
+
+bool TransferFunction::Solve(double input)
+{
+    int maxIter = 100;
+    int order = static_cast<int>(m_ss.A.size());
+
+    std::vector<double> x;
+    std::vector<double> oldx;
+    std::vector<double> dx;
+    std::vector<double> olddx;
+    for(int i = 0; i < order; i++) {
+        x.push_back(m_x[i]);
+        oldx.push_back(m_x[i]);
+
+        dx.push_back(m_dx[i]);
+        olddx.push_back(m_dx[i]);
+    }
+
+    bool exit = false;
+    int iter = 0;
+    while(!exit) {
+        double xError = 0.0;
+        double dxError = 0.0;
+        for(int i = 0; i < order; i++) {
+            // Trapezoidal method
+            x[i] = m_x[i] + 0.5 * m_timeStep * (m_dx[i] + dx[i]);
+
+            if(std::abs(x[i] - oldx[i]) > xError) xError = std::abs(x[i] - oldx[i]);
+
+            oldx[i] = x[i];
+        }
+        for(int i = 0; i < order; i++) {
+            // x' = Ax + Bu
+            dx[i] = 0.0;
+            for(int j = 0; j < order; j++) dx[i] += m_ss.A[i][j] * x[j];
+            dx[i] += m_ss.B[i] * input;
+
+            if(std::abs(dx[i] - olddx[i]) > dxError) dxError = std::abs(dx[i] - olddx[i]);
+
+            olddx[i] = dx[i];
+        }
+        if(std::max(xError, dxError) < m_error) exit = true;
+
+        iter++;
+        if(iter >= maxIter) return false;
+    }
+
+    m_output = 0.0;
+    for(int i = 0; i < order; i++) {
+        m_output += m_ss.C[i] * x[i];
+        m_x[i] = x[i];
+        m_dx[i] = dx[i];
+    }
+
+    m_output += m_ss.D * input;
+
+    return true;
+}
diff --git a/Project/TransferFunction.h b/Project/TransferFunction.h
index 45681ce..38e31bb 100644
--- a/Project/TransferFunction.h
+++ b/Project/TransferFunction.h
@@ -10,7 +10,14 @@ class TransferFunctionForm;
 
 class TransferFunction : public ControlElement
 {
-public:
+   public:
+    struct SpaceState {
+        std::vector<std::vector<double> > A;
+        std::vector<double> B;
+        std::vector<double> C;
+        double D;
+    };
+
     TransferFunction(int id);
     ~TransferFunction();
 
@@ -19,25 +26,35 @@ public:
     virtual bool Intersects(wxRect2DDouble rect) const { return m_rect.Intersects(rect); }
     virtual bool ShowForm(wxWindow* parent, Element* element);
     virtual void Rotate(bool clockwise = true);
-    
+
     virtual std::vector<double> GetNumerator() const { return m_numerator; }
     virtual std::vector<double> GetDenominator() const { return m_denominator; }
     virtual void SetNumerator(std::vector<double> numerator) { m_numerator = numerator; }
     virtual void SetDenominator(std::vector<double> denominator) { m_denominator = denominator; }
     virtual void UpdateTFText();
-    
-protected:
+    virtual SpaceState GetSpaceState() { return m_ss; }
+    virtual void CalculateSpaceState(double timeStep = 1e-3, double error = 1e-3);
+    virtual bool Solve(double input);
+
+   protected:
     virtual void SetText(wxString numerator, wxString denominator);
     virtual wxString GetSuperscriptNumber(int number);
     virtual void GetTFString(wxString& numerator, wxString& denominator);
-    
+
     wchar_t m_supNumber[10];
-    
+
     wxGLString* m_glStringNum = NULL;
     wxGLString* m_glStringDen = NULL;
     int m_fontSize = 10;
+
     std::vector<double> m_numerator;
     std::vector<double> m_denominator;
+    SpaceState m_ss;
+
+    std::vector<double> m_x;
+    std::vector<double> m_dx;
+    double m_timeStep = 1e-3;
+    double m_error = 1e-3;
 };
 
-#endif // TRANSFERFUNCTION_H
+#endif  // TRANSFERFUNCTION_H