ControlElementSolver.cpp

/*
 *  Copyright (C) 2017  Thales Lima Oliveira <thales@ufu.br>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
 
#include "ControlElementSolver.h"
 
#include "../../editors/ControlEditor.h"
 
#include "ConnectionLine.h"
#include "Constant.h"
#include "ControlElementContainer.h"
#include "Exponential.h"
#include "Gain.h"
#include "IOControl.h"
#include "Limiter.h"
#include "Multiplier.h"
#include "RateLimiter.h"
#include "Sum.h"
#include "TransferFunction.h"
 
ControlElementSolver::ControlElementSolver(ControlEditor* controlEditor, double timeStep, double integrationError)
{
    m_ctrlContainer = new ControlElementContainer();
    m_ownsCtrlContainer = true;
    m_ctrlContainer->FillContainer(controlEditor);
    Initialize(controlEditor, timeStep, integrationError);
}
 
ControlElementSolver::ControlElementSolver(ControlElementContainer* ctrlContainer,
    double timeStep,
    double integrationError,
    wxWindow* parent)
{
    m_ctrlContainer = ctrlContainer;
    m_ownsCtrlContainer = false;
    Initialize(parent, timeStep, integrationError);
}
 
ControlElementSolver::~ControlElementSolver()
{
    if (m_inputToSolve) delete[] m_inputToSolve;
    if (m_ownsCtrlContainer && m_ctrlContainer) delete m_ctrlContainer;
}
 
void ControlElementSolver::Initialize(wxWindow* parent, double timeStep, double integrationError)
{
    // Init the input array size
    if (m_inputToSolve) delete[] m_inputToSolve;
    m_inputToSolve = new double[3];
    // Check if the sistem have one input and one output
    bool fail = false;
    auto ioList = m_ctrlContainer->GetIOControlList();
    if (ioList.size() < 2) {
        fail = true;
        m_failMessage = _("The control system must have at least one input and one output.");
    }
    bool haveInput, haveOutput;
    haveInput = haveOutput = false;
    for (auto it = ioList.begin(), itEnd = ioList.end(); it != itEnd; ++it) {
        IOControl* io = *it;
        if (io->GetType() == Node::NodeType::NODE_OUT && !haveInput) {
            m_inputControl = io;
            haveInput = true;
        }
        else if (io->GetType() == Node::NodeType::NODE_IN) {
            m_outputControl = io;
            haveOutput = true;
        }
    }
    if (!fail && !haveInput) {
        fail = true;
        m_failMessage = _("There is no input in the control system.");
    }
    if (!fail && !haveOutput) {
        fail = true;
        m_failMessage = _("There is no output in the control system.");
    }
    if (!fail) {
        if (m_inputControl->GetChildList().size() == 0) {
            fail = true;
            m_failMessage = _("Input not connected.");
        }
    }
 
    m_timeStep = timeStep;
    m_integrationError = integrationError;
    if (!fail) {
        if (!InitializeValues(true)) {
            fail = true;
            m_failMessage = _("It was not possible to initialize the control system.");
        }
    }
 
    if (fail) {
        wxMessageDialog msgDialog(parent, m_failMessage, _("Error"), wxOK | wxCENTRE | wxICON_ERROR);
        msgDialog.ShowModal();
    }
    else {
        m_isOK = true;
    }
}
 
bool ControlElementSolver::InitializeValues(bool startAllZero)
{
    // Reset Elements values
    auto elementList = m_ctrlContainer->GetControlElementsList();
    for (auto it = elementList.begin(), itEnd = elementList.end(); it != itEnd; ++it) {
        if (!(*it)->Initialize()) return false;
    }
    auto connectionLineList = m_ctrlContainer->GetConnectionLineList();
    for (auto it = connectionLineList.begin(), itEnd = connectionLineList.end(); it != itEnd; ++it) {
        if (!(*it)->Initialize()) return false;
    }
    auto tfList = m_ctrlContainer->GetTFList();
    for (auto it = tfList.begin(), itEnd = tfList.end(); it != itEnd; ++it) {
        (*it)->CalculateSpaceState(100, m_integrationError);
    }
 
    if (!startAllZero) {
        double origTimeStep = m_timeStep;
        double minStep = m_timeStep / 10;
        double maxStep = m_timeStep * 10;
        // Calculate the steady-state results according to the input.
        double minError = 1e-7 * m_timeStep;
        int maxIteration = 100 / m_timeStep;
 
        double prevSol = 0.0;
        double currentSol = 1.0;
        double error = 1.0;
        double prevError = 1.0;
        int numIt = 0;
        while (error > minError) {
            prevSol = currentSol;
            prevError = error;
            SolveNextStep();
            currentSol = GetLastSolution();
            numIt++;
            error = std::abs(prevSol - currentSol);
            if (std::abs(error - prevError) < 1e-1) {
                if (m_timeStep < maxStep) { m_timeStep *= 1.5; }
            }
            else if (std::abs(error - prevError) > 10) {
                if (m_timeStep > minStep) { m_timeStep /= 1.5; }
            }
            if (numIt >= maxIteration) {
                m_failMessage = _("It was not possible to initialize the control system.");
                return false;
            }
        }
        m_timeStep = origTimeStep;
        m_solutions.clear();
    }
 
    return true;
}
 
void ControlElementSolver::SolveNextStep()
{
    // Set all elements as not solved
    auto elementList = m_ctrlContainer->GetControlElementsList();
    for (auto& element : elementList) {
        element->SetSolved(false);
    }
    auto connectionLineList = m_ctrlContainer->GetConnectionLineList();
    for (auto& cLine : connectionLineList) {
        cLine->SetSolved(false);
    }
 
    // Get first node connection
    ConnectionLine* firstConn = static_cast<ConnectionLine*>(m_inputControl->GetChildList()[0]);
 
    // Set value to the connected lines in constants
    auto constantList = m_ctrlContainer->GetConstantList();
    for (auto it = constantList.begin(), itEnd = constantList.end(); it != itEnd; ++it) {
        Constant* constant = *it;
        if (constant->GetChildList().size() == 1) {
            constant->SetSolved();
            ConnectionLine* child = static_cast<ConnectionLine*>(constant->GetChildList()[0]);
            child->SetValue(constant->GetValue());
            child->SetSolved();
            FillAllConnectedChildren(child);
        }
    }
 
    // Solve math expression without inputs (but connected)
    auto mathExprList = m_ctrlContainer->GetMathExprList();
    for (auto it = mathExprList.begin(), itEnd = mathExprList.end(); it != itEnd; ++it) {
        MathExpression* mathExpr = *it;
        if (mathExpr->GetVariables().size() == 0) {  // No variables, no inputs.
            m_inputToSolve[0] = 0.0;
            m_inputToSolve[1] = m_currentTime;
            m_inputToSolve[2] = m_switchStatus;
            mathExpr->Solve(m_inputToSolve, m_timeStep);
            mathExpr->SetSolved();
            ConnectionLine* child = static_cast<ConnectionLine*>(mathExpr->GetChildList()[0]);
            child->SetValue(mathExpr->GetOutput());
            child->SetSolved();
            FillAllConnectedChildren(child);
        }
    }
 
    // Set value to the connected lines in inputs
    auto ioList = m_ctrlContainer->GetIOControlList();
    for (auto it = ioList.begin(), itEnd = ioList.end(); it != itEnd; ++it) {
        IOControl* io = *it;
        if (io->GetChildList().size() == 1) {
            io->SetSolved();
            ConnectionLine* child = static_cast<ConnectionLine*>(io->GetChildList()[0]);
            if (m_inputControl == io) firstConn = child;
            bool inputType = true;
            io->SetSolved();
            switch (io->GetValue()) {
            case IOControl::IN_TERMINAL_VOLTAGE: {
                child->SetValue(m_terminalVoltage);
            } break;
            case IOControl::IN_VELOCITY: {
                child->SetValue(m_velocity);
            } break;
            case IOControl::IN_ACTIVE_POWER: {
                child->SetValue(m_activePower);
            } break;
            case IOControl::IN_REACTIVE_POWER: {
                child->SetValue(m_reactivePower);
            } break;
            case IOControl::IN_INITIAL_TERMINAL_VOLTAGE: {
                child->SetValue(m_initTerminalVoltage);
            } break;
            case IOControl::IN_INITIAL_MEC_POWER: {
                child->SetValue(m_initMecPower);
            } break;
            case IOControl::IN_INITIAL_VELOCITY: {
                child->SetValue(m_initVelocity);
            } break;
            case IOControl::IN_DELTA_VELOCITY: {
                child->SetValue(m_deltaVelocity);
            } break;
            case IOControl::IN_DELTA_ACTIVE_POWER: {
                child->SetValue(m_deltaPe);
            } break;
            case IOControl::IN_TEST: {
                child->SetValue(io->GetTestValue());
            } break;
            default: {
                inputType = false;
                io->SetSolved(false);
            } break;
            }
            if (inputType) {
                child->SetSolved();
                FillAllConnectedChildren(child);
            }
        }
    }
 
    ConnectionLine* currentLine = firstConn;
    while (currentLine) {
        currentLine = SolveNextElement(currentLine);
        if (!m_isOK) return;
    }
 
    bool haveUnsolvedElement = true;
    while (haveUnsolvedElement) {
        haveUnsolvedElement = false;
        // Get the solved line connected with unsolved element (elements not connected in the main branch).
        for (auto& cLine : connectionLineList) {
            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.get();
                        while (currentLine) {
                            currentLine = SolveNextElement(currentLine);
                            if (!m_isOK) return;
                        }
                        break;
                    }
                }
            }
            if (haveUnsolvedElement) break;
        }
    }
 
    // Set the control system output.
    for (auto it = ioList.begin(), itEnd = ioList.end(); it != itEnd; ++it) {
        IOControl* io = *it;
        if (io->GetChildList().size() == 1) {
            io->SetSolved();
            ConnectionLine* child = static_cast<ConnectionLine*>(io->GetChildList()[0]);
            switch (io->GetValue()) {
            case IOControl::OUT_MEC_POWER: {
                m_mecPower = child->GetValue();
                m_solutions.push_back(m_mecPower);
            } break;
            case IOControl::OUT_FIELD_VOLTAGE: {
                m_fieldVoltage = child->GetValue();
                m_solutions.push_back(m_fieldVoltage);
            } break;
            default:
                break;
            }
        }
    }
}
 
void ControlElementSolver::FillAllConnectedChildren(ConnectionLine* parent)
{
    auto childList = parent->GetLineChildList();
    for (auto it = childList.begin(), itEnd = childList.end(); it != itEnd; ++it) {
        ConnectionLine* child = *it;
        child->SetValue(parent->GetValue());
        child->SetSolved();
        FillAllConnectedChildren(child);
    }
}
 
ConnectionLine* ControlElementSolver::SolveNextElement(ConnectionLine* currentLine)
{
    auto parentList = currentLine->GetParentList();
    for (auto it = parentList.begin(), itEnd = parentList.end(); it != itEnd; ++it) {
        ControlElement* element = static_cast<ControlElement*>(*it);
        // Solve the unsolved parent.
        if (!element->IsSolved()) {
            m_inputToSolve[0] = currentLine->GetValue();
            if (!std::isfinite(m_inputToSolve[0])) {
                m_inputToSolve[0] = 0.0;
                m_isOK = false;
                m_failMessage = _("The control system solution is diverging. Try to reduce the time step.");
                return nullptr;
            }
            m_inputToSolve[1] = m_currentTime;
            m_inputToSolve[2] = m_switchStatus;
            if (!element->Solve(m_inputToSolve, m_timeStep)) {
                m_isOK = false;
                if(auto tf = dynamic_cast<TransferFunction*>(element)) {
                    wxString numerator, denominator;
                    tf->GetTFString(numerator, denominator);
                    m_failMessage = wxString::Format(_("Unable to compute the transfer function: (%s)/(%s)"), numerator, denominator);
                }
                return nullptr;
            }
            element->SetSolved();
 
            // Get the output node (must have one or will result nullptr).
            Node* outNode = nullptr;
            auto nodeList = element->GetNodeList();
            for (auto itN = nodeList.begin(), itNEnd = nodeList.end(); itN != itNEnd; ++itN) {
                Node* node = *itN;
                if (node->GetNodeType() == Node::NodeType::NODE_OUT) outNode = node;
            }
            if (!outNode) return nullptr;
 
            // 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
                    // Check if the connection line have the output node on the list
                    auto lineNodeList = cLine->GetNodeList();
                    for (auto itCN = lineNodeList.begin(), itCNEnd = lineNodeList.end(); itCN != itCNEnd; ++itCN) {
                        Node* childNode = *itCN;
                        if (childNode == outNode) {
                            // Check if the line connect two elements, otherwise return nullptr
                            if (cLine->GetType() != ConnectionLine::ConnectionLineType::ELEMENT_ELEMENT) return nullptr;
 
                            // Set the connection line value and return it.
                            cLine->SetValue(element->GetOutput());
                            cLine->SetSolved();
                            FillAllConnectedChildren(cLine);
                            return cLine;
                        }
                    }
                }
            }
        }
    }
    return nullptr;
}