<commit_before>#include "kernel.h"
#include "errorobject.h"
#include "rootdrawer.h"

using namespace Ilwis;
using namespace Geodrawer;

ComplexDrawer::ComplexDrawer(const QString &name,
                             DrawerInterface* parentDrawer,
                             RootDrawer *rootdrawer,
                             const IOOptions &options) : BaseDrawer(name, parentDrawer, rootdrawer, options)
{

}

bool ComplexDrawer::draw(const IOOptions &options)
{
    if (!isActive() || !isValid())
        return false;

    bool ok = drawSideDrawers( _preDrawers, options)    ;

    for(const auto& drawer : _mainDrawers){
        if ( drawer){
            ok &= drawer->draw( options);
        }
    }

    ok &= drawSideDrawers(_postDrawers, options);

    return ok;
}

bool ComplexDrawer::prepare(DrawerInterface::PreparationType prepType, const IOOptions &options)
{
    BaseDrawer::prepare(prepType, options);

    for( auto& drawer : _preDrawers)    {
        if (!drawer.second->prepare(prepType, options))
            return false;
    }
    for( auto& drawer : _mainDrawers){
        if (!drawer->prepare(prepType, options))
            return false;
    }
    for( auto& drawer : _postDrawers)    {
        if (!drawer.second->prepare(prepType, options))
            return false;
    }
    return true;

}

void ComplexDrawer::unprepare(DrawerInterface::PreparationType prepType)
{
    BaseDrawer::unprepare(prepType);

    for( auto& drawer : _preDrawers)    {
        drawer.second->unprepare(prepType);
    }
    for( auto& drawer : _mainDrawers){
        drawer->unprepare(prepType);
    }
    for( auto& drawer : _postDrawers)    {
        drawer.second->unprepare(prepType);
    }
}

bool ComplexDrawer::prepareChildDrawers(DrawerInterface::PreparationType prepType, const IOOptions &options)
{
    for(auto& drawer : _mainDrawers){
        if ( drawer && drawer->isValid())
            drawer->prepare(prepType, options);
    }
    return true;
}

quint32 ComplexDrawer::drawerCount(DrawerInterface::DrawerType tpe) const
{
    switch(tpe){
    case dtPRE:
        return _postDrawers.size();
    case dtMAIN:
        return _mainDrawers.size();
    case dtPOST:
        return _postDrawers.size();
    default:
        return _postDrawers.size() + _preDrawers.size() + _mainDrawers.size();;
    }
    return iUNDEF;
}

const UPDrawer &ComplexDrawer::drawer(quint32 order, DrawerInterface::DrawerType drawerType) const
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        const DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        const auto& current = drawers.find(order);
        if ( current == drawers.end())
            throw VisualizationError(TR("Invalid drawer number used while drawing"));

        return current->second;
    } else if ( drawerType == dtMAIN){
        if ( order < _mainDrawers.size()){
            return _mainDrawers[order];
        }
    }
    throw VisualizationError(TR(QString("Drawer number %1 is not valid").arg(order)));
}

UPDrawer &ComplexDrawer::drawer(quint32 index, DrawerInterface::DrawerType drawerType)
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        auto current = drawers.find(index);
        if ( current == drawers.end())
            throw VisualizationError(TR("Invalid drawer number used while drawing"));

        return current->second;
    } else if ( drawerType == dtMAIN){
        if ( index < _mainDrawers.size()){
            return _mainDrawers[index];
        }
    }
    throw VisualizationError(TR(QString("Drawer number %1 is not valid").arg(index)));
}

UPDrawer &ComplexDrawer::drawer(const QString &code, DrawerInterface::DrawerType drawerType)
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        for(auto& drawer : drawers){
            if ( drawer.second->code() == code)
                return drawer.second;
        }
        throw VisualizationError(TR("Invalid drawer number used while drawing"));

    } else if ( drawerType == dtMAIN){
        for(auto& drawer : _mainDrawers){
            if ( drawer->code() == code)
                return drawer;
        }
    }
    throw VisualizationError(TR(QString("Drawer number %1 is not valid").arg(code)));
}

void ComplexDrawer::addDrawer(DrawerInterface *drawer, DrawerInterface::DrawerType drawerType, quint32 order,const QString &nme)
{
    if ( !drawer)
        return;

    QString drawername = nme;
    if ( drawerType == dtPOST || drawerType == dtPRE){
        if ( order != iUNDEF){
            DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
            drawers[order].reset(drawer);
            if ( drawername == sUNDEF){
                drawername = (drawerType == dtPOST ? "post_drawer_" : "pre_drawer_") + QString::number(drawer->id()) + "_" + QString::number(order);
            }

        }
    }else if ( drawerType == dtMAIN){
        UPDrawer drawerp(drawer);
        _mainDrawers.push_back(std::move(drawerp));
        if ( drawername == sUNDEF){
            drawername = "drawer_layer_" + QString::number(drawer->id());
        }

    }
    drawer->name(drawername);
}

void ComplexDrawer::setDrawer(quint32 order, DrawerInterface *drawer, DrawerInterface::DrawerType drawerType)
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        const auto& current = drawers.find(order);
        if ( current == drawers.end()){
            return;
        }
        drawers[order].reset(drawer);
    } else if ( drawerType == dtMAIN){
        if ( order < _mainDrawers.size()) {
            _mainDrawers[order].reset(drawer);
        }
    }
}

void ComplexDrawer::removeDrawer(quint32 order, DrawerInterface::DrawerType drawerType)
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        auto current = drawers.find(order);
        if ( current != drawers.end()){
            (*current).second->cleanUp();
            drawers.erase(current)    ;

        }
    } else if ( drawerType == dtMAIN){
        if ( order < _mainDrawers.size()) {
           _mainDrawers[order]->cleanUp();
           _mainDrawers.erase(_mainDrawers.begin() + order);
        }
    }
}

void ComplexDrawer::removeDrawer(const QString &idcode, bool ascode)
{
    std::vector<quint32> selectedDrawers;
    for( auto& drawer : _preDrawers){
        if ( ascode){
            if (drawer.second->code() == idcode){
                selectedDrawers.push_back(drawer.first);
            }
        } else {
            if (drawer.second->name() == idcode){
                selectedDrawers.push_back(drawer.first);
            }
        }
    }
    for(auto order : selectedDrawers)
        removeDrawer(order, DrawerInterface::dtPRE);
    selectedDrawers.clear();
    int count = 0;
    for( auto& drawer : _mainDrawers){
        if ( ascode){
            if (drawer->code() == idcode){
                selectedDrawers.push_back(count);
            }
        } else {
            if (drawer->name() == idcode){
                selectedDrawers.push_back(count);
            }
        }
        ++count;
    }
    for(auto order : selectedDrawers)
        removeDrawer(order, DrawerInterface::dtMAIN);
    selectedDrawers.clear();

    for( auto& drawer : _postDrawers)    {
        if ( ascode){
            if (drawer.second->code() == idcode){
                selectedDrawers.push_back(drawer.first);
            }
        } else {
            if (drawer.second->name() == idcode){
                selectedDrawers.push_back(drawer.first);
            }
        }
    }
    for(auto order : selectedDrawers)
        removeDrawer(order, DrawerInterface::dtPOST);
}

bool ComplexDrawer::drawerAttribute(const QString &drawercode, const QString &key, const QVariant &value)
{
    if ( drawercode == code()){
        setAttribute(key, value);
        return true;
    }

    for( auto& drawer : _preDrawers){
        if (drawer.second->drawerAttribute(drawercode, key, value))
            return true;
    }
    for( auto& drawer : _mainDrawers){
        if (drawer->drawerAttribute(drawercode, key, value))
            return true;
    }
    for( auto& drawer : _postDrawers)    {
        if (drawer.second->drawerAttribute(drawercode, key, value))
            return true;
    }

    return false;
}

bool ComplexDrawer::isSimple() const
{
    return false;
}

void ComplexDrawer::cleanUp()
{
    BaseDrawer::cleanUp();

    for( auto& drawer : _preDrawers)    {
        drawer.second->cleanUp();
    }
    for( auto& drawer : _mainDrawers){
        drawer->cleanUp();
    }
    for( auto& drawer : _postDrawers)    {
        drawer.second->cleanUp();
    }
    _preDrawers.clear();
    _mainDrawers.clear();
    _postDrawers.clear();
    unprepare(ptALL);

}

bool ComplexDrawer::drawSideDrawers(const DrawerMap& drawers, const IOOptions &options) const
{
    if (!isActive())
        return false;

    if ( drawers.size() > 0) {
        for(const auto& current : drawers) {
            const auto& drw = current.second;
            if ( drw)
                drw->draw(options);
        }
    }
    return true;
}

std::vector<QVariant> ComplexDrawer::attributes(const QString &attrNames) const
{
    std::vector<QVariant> results = BaseDrawer::attributes(attrNames);
    QStringList parts = attrNames.split("|");
    for(QString& part : parts)
        part.toLower();

    for(const QString& part : parts){
        results.push_back(attribute(part));
    }
    return results;
}

QVariant ComplexDrawer::attribute(const QString &attrNme) const
{
    QString attrName = attrNme.toLower();
    QVariant var = BaseDrawer::attribute(attrName);
    if ( var.isValid())
        return var;

    if ( attrName == "maindrawercount")
        return drawerCount(DrawerInterface::dtMAIN);
    if ( attrName == "predrawercount")
        return drawerCount(DrawerInterface::dtPRE);
    if ( attrName == "postdrawercount")
        return drawerCount(DrawerInterface::dtPOST);

    return QVariant();
}

QVariant ComplexDrawer::execute(const QString &operationName, const QVariantMap &parameters)
{
    if ( operationName.toLower() == "layerremove"){
        if ( parameters.size() == 1) {
            QVariant var = parameters.begin().value();
            bool ok;
            quint32 layer = var.toUInt(&ok);
            if ( ok){
                removeDrawer(layer);
                return QVariant(true);
            }
        }
    }
    if ( operationName.toLower() == "layerdown"){
        if ( parameters.size() == 1) {
            QVariant var = parameters.begin().value();
            bool ok;
            quint32 layer = var.toUInt(&ok);
            if ( ok && layer <_mainDrawers.size() && _mainDrawers.size() >= 2){
                _mainDrawers[layer].swap(_mainDrawers[layer - 1]);
                return QVariant(true);
            }
        }
    }

    if ( operationName.toLower() == "layerup"){
        if ( parameters.size() == 1) {
            QVariant var = parameters.begin().value();
            bool ok;
            quint32 layer = var.toUInt(&ok);
            if ( ok && layer <_mainDrawers.size() - 1 && _mainDrawers.size() >= 2){
                _mainDrawers[layer].swap(_mainDrawers[layer + 1]);
                return QVariant(true);
            }
        }
    }
    if ( operationName == "code2index" ){
        if ( parameters.size() == 1){
            int index = 0;
            QString cod = parameters["code"].toString();
            for( auto& drawer : _mainDrawers){
                if ( drawer->code() == cod){
                    return QVariant(index);
                }
                ++index;
            }
        }
    }
    if ( operationName == "index2code" ){
        if ( parameters.size() == 1){
            bool ok;
            int index = parameters["index"].toUInt(&ok);
            if ( index < _mainDrawers.size()){
                return QVariant ( _mainDrawers[index]->code());
            }
        }
    }
    return QVariant();
}

QVariant ComplexDrawer::attributeOfDrawer(const QString &drawercode, const QString &attrName) const
{
    if ( code() == drawercode)
        return attribute(attrName);
    if ( drawercode ==  "rootdrawer"){ // special case
        return rootDrawer()->attributeOfDrawer(drawercode, attrName);
    }

    QVariant var;
    for( auto& drawer : _preDrawers){
        var = drawer.second->attributeOfDrawer(drawercode, attrName);
        if ( var.isValid())
            return var;

    }
    for( auto& drawer : _mainDrawers){
        var = drawer->attributeOfDrawer(drawercode, attrName);
        if ( var.isValid())
            return var;
    }
    for( auto& drawer : _postDrawers)    {
        var = drawer.second->attributeOfDrawer(drawercode, attrName);
        if ( var.isValid())
            return var;
    }
    return var;
}
<commit_msg>wrong attribute method was called when retrieving attributes of the rootdrawer<commit_after>#include "kernel.h"
#include "errorobject.h"
#include "rootdrawer.h"

using namespace Ilwis;
using namespace Geodrawer;

ComplexDrawer::ComplexDrawer(const QString &name,
                             DrawerInterface* parentDrawer,
                             RootDrawer *rootdrawer,
                             const IOOptions &options) : BaseDrawer(name, parentDrawer, rootdrawer, options)
{

}

bool ComplexDrawer::draw(const IOOptions &options)
{
    if (!isActive() || !isValid())
        return false;

    bool ok = drawSideDrawers( _preDrawers, options)    ;

    for(const auto& drawer : _mainDrawers){
        if ( drawer){
            ok &= drawer->draw( options);
        }
    }

    ok &= drawSideDrawers(_postDrawers, options);

    return ok;
}

bool ComplexDrawer::prepare(DrawerInterface::PreparationType prepType, const IOOptions &options)
{
    BaseDrawer::prepare(prepType, options);

    for( auto& drawer : _preDrawers)    {
        if (!drawer.second->prepare(prepType, options))
            return false;
    }
    for( auto& drawer : _mainDrawers){
        if (!drawer->prepare(prepType, options))
            return false;
    }
    for( auto& drawer : _postDrawers)    {
        if (!drawer.second->prepare(prepType, options))
            return false;
    }
    return true;

}

void ComplexDrawer::unprepare(DrawerInterface::PreparationType prepType)
{
    BaseDrawer::unprepare(prepType);

    for( auto& drawer : _preDrawers)    {
        drawer.second->unprepare(prepType);
    }
    for( auto& drawer : _mainDrawers){
        drawer->unprepare(prepType);
    }
    for( auto& drawer : _postDrawers)    {
        drawer.second->unprepare(prepType);
    }
}

bool ComplexDrawer::prepareChildDrawers(DrawerInterface::PreparationType prepType, const IOOptions &options)
{
    for(auto& drawer : _mainDrawers){
        if ( drawer && drawer->isValid())
            drawer->prepare(prepType, options);
    }
    return true;
}

quint32 ComplexDrawer::drawerCount(DrawerInterface::DrawerType tpe) const
{
    switch(tpe){
    case dtPRE:
        return _postDrawers.size();
    case dtMAIN:
        return _mainDrawers.size();
    case dtPOST:
        return _postDrawers.size();
    default:
        return _postDrawers.size() + _preDrawers.size() + _mainDrawers.size();;
    }
    return iUNDEF;
}

const UPDrawer &ComplexDrawer::drawer(quint32 order, DrawerInterface::DrawerType drawerType) const
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        const DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        const auto& current = drawers.find(order);
        if ( current == drawers.end())
            throw VisualizationError(TR("Invalid drawer number used while drawing"));

        return current->second;
    } else if ( drawerType == dtMAIN){
        if ( order < _mainDrawers.size()){
            return _mainDrawers[order];
        }
    }
    throw VisualizationError(TR(QString("Drawer number %1 is not valid").arg(order)));
}

UPDrawer &ComplexDrawer::drawer(quint32 index, DrawerInterface::DrawerType drawerType)
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        auto current = drawers.find(index);
        if ( current == drawers.end())
            throw VisualizationError(TR("Invalid drawer number used while drawing"));

        return current->second;
    } else if ( drawerType == dtMAIN){
        if ( index < _mainDrawers.size()){
            return _mainDrawers[index];
        }
    }
    throw VisualizationError(TR(QString("Drawer number %1 is not valid").arg(index)));
}

UPDrawer &ComplexDrawer::drawer(const QString &code, DrawerInterface::DrawerType drawerType)
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        for(auto& drawer : drawers){
            if ( drawer.second->code() == code)
                return drawer.second;
        }
        throw VisualizationError(TR("Invalid drawer number used while drawing"));

    } else if ( drawerType == dtMAIN){
        for(auto& drawer : _mainDrawers){
            if ( drawer->code() == code)
                return drawer;
        }
    }
    throw VisualizationError(TR(QString("Drawer number %1 is not valid").arg(code)));
}

void ComplexDrawer::addDrawer(DrawerInterface *drawer, DrawerInterface::DrawerType drawerType, quint32 order,const QString &nme)
{
    if ( !drawer)
        return;

    QString drawername = nme;
    if ( drawerType == dtPOST || drawerType == dtPRE){
        if ( order != iUNDEF){
            DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
            drawers[order].reset(drawer);
            if ( drawername == sUNDEF){
                drawername = (drawerType == dtPOST ? "post_drawer_" : "pre_drawer_") + QString::number(drawer->id()) + "_" + QString::number(order);
            }

        }
    }else if ( drawerType == dtMAIN){
        UPDrawer drawerp(drawer);
        _mainDrawers.push_back(std::move(drawerp));
        if ( drawername == sUNDEF){
            drawername = "drawer_layer_" + QString::number(drawer->id());
        }

    }
    drawer->name(drawername);
}

void ComplexDrawer::setDrawer(quint32 order, DrawerInterface *drawer, DrawerInterface::DrawerType drawerType)
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        const auto& current = drawers.find(order);
        if ( current == drawers.end()){
            return;
        }
        drawers[order].reset(drawer);
    } else if ( drawerType == dtMAIN){
        if ( order < _mainDrawers.size()) {
            _mainDrawers[order].reset(drawer);
        }
    }
}

void ComplexDrawer::removeDrawer(quint32 order, DrawerInterface::DrawerType drawerType)
{
    if ( drawerType == dtPOST || drawerType == dtPRE){
        DrawerMap& drawers = drawerType == dtPRE ? _preDrawers : _postDrawers;
        auto current = drawers.find(order);
        if ( current != drawers.end()){
            (*current).second->cleanUp();
            drawers.erase(current)    ;

        }
    } else if ( drawerType == dtMAIN){
        if ( order < _mainDrawers.size()) {
           _mainDrawers[order]->cleanUp();
           _mainDrawers.erase(_mainDrawers.begin() + order);
        }
    }
}

void ComplexDrawer::removeDrawer(const QString &idcode, bool ascode)
{
    std::vector<quint32> selectedDrawers;
    for( auto& drawer : _preDrawers){
        if ( ascode){
            if (drawer.second->code() == idcode){
                selectedDrawers.push_back(drawer.first);
            }
        } else {
            if (drawer.second->name() == idcode){
                selectedDrawers.push_back(drawer.first);
            }
        }
    }
    for(auto order : selectedDrawers)
        removeDrawer(order, DrawerInterface::dtPRE);
    selectedDrawers.clear();
    int count = 0;
    for( auto& drawer : _mainDrawers){
        if ( ascode){
            if (drawer->code() == idcode){
                selectedDrawers.push_back(count);
            }
        } else {
            if (drawer->name() == idcode){
                selectedDrawers.push_back(count);
            }
        }
        ++count;
    }
    for(auto order : selectedDrawers)
        removeDrawer(order, DrawerInterface::dtMAIN);
    selectedDrawers.clear();

    for( auto& drawer : _postDrawers)    {
        if ( ascode){
            if (drawer.second->code() == idcode){
                selectedDrawers.push_back(drawer.first);
            }
        } else {
            if (drawer.second->name() == idcode){
                selectedDrawers.push_back(drawer.first);
            }
        }
    }
    for(auto order : selectedDrawers)
        removeDrawer(order, DrawerInterface::dtPOST);
}

bool ComplexDrawer::drawerAttribute(const QString &drawercode, const QString &key, const QVariant &value)
{
    if ( drawercode == code()){
        setAttribute(key, value);
        return true;
    }

    for( auto& drawer : _preDrawers){
        if (drawer.second->drawerAttribute(drawercode, key, value))
            return true;
    }
    for( auto& drawer : _mainDrawers){
        if (drawer->drawerAttribute(drawercode, key, value))
            return true;
    }
    for( auto& drawer : _postDrawers)    {
        if (drawer.second->drawerAttribute(drawercode, key, value))
            return true;
    }

    return false;
}

bool ComplexDrawer::isSimple() const
{
    return false;
}

void ComplexDrawer::cleanUp()
{
    BaseDrawer::cleanUp();

    for( auto& drawer : _preDrawers)    {
        drawer.second->cleanUp();
    }
    for( auto& drawer : _mainDrawers){
        drawer->cleanUp();
    }
    for( auto& drawer : _postDrawers)    {
        drawer.second->cleanUp();
    }
    _preDrawers.clear();
    _mainDrawers.clear();
    _postDrawers.clear();
    unprepare(ptALL);

}

bool ComplexDrawer::drawSideDrawers(const DrawerMap& drawers, const IOOptions &options) const
{
    if (!isActive())
        return false;

    if ( drawers.size() > 0) {
        for(const auto& current : drawers) {
            const auto& drw = current.second;
            if ( drw)
                drw->draw(options);
        }
    }
    return true;
}

std::vector<QVariant> ComplexDrawer::attributes(const QString &attrNames) const
{
    std::vector<QVariant> results = BaseDrawer::attributes(attrNames);
    QStringList parts = attrNames.split("|");
    for(QString& part : parts)
        part.toLower();

    for(const QString& part : parts){
        results.push_back(attribute(part));
    }
    return results;
}

QVariant ComplexDrawer::attribute(const QString &attrNme) const
{
    QString attrName = attrNme.toLower();
    QVariant var = BaseDrawer::attribute(attrName);
    if ( var.isValid())
        return var;

    if ( attrName == "maindrawercount")
        return drawerCount(DrawerInterface::dtMAIN);
    if ( attrName == "predrawercount")
        return drawerCount(DrawerInterface::dtPRE);
    if ( attrName == "postdrawercount")
        return drawerCount(DrawerInterface::dtPOST);

    return QVariant();
}

QVariant ComplexDrawer::execute(const QString &operationName, const QVariantMap &parameters)
{
    if ( operationName.toLower() == "layerremove"){
        if ( parameters.size() == 1) {
            QVariant var = parameters.begin().value();
            bool ok;
            quint32 layer = var.toUInt(&ok);
            if ( ok){
                removeDrawer(layer);
                return QVariant(true);
            }
        }
    }
    if ( operationName.toLower() == "layerdown"){
        if ( parameters.size() == 1) {
            QVariant var = parameters.begin().value();
            bool ok;
            quint32 layer = var.toUInt(&ok);
            if ( ok && layer <_mainDrawers.size() && _mainDrawers.size() >= 2){
                _mainDrawers[layer].swap(_mainDrawers[layer - 1]);
                return QVariant(true);
            }
        }
    }

    if ( operationName.toLower() == "layerup"){
        if ( parameters.size() == 1) {
            QVariant var = parameters.begin().value();
            bool ok;
            quint32 layer = var.toUInt(&ok);
            if ( ok && layer <_mainDrawers.size() - 1 && _mainDrawers.size() >= 2){
                _mainDrawers[layer].swap(_mainDrawers[layer + 1]);
                return QVariant(true);
            }
        }
    }
    if ( operationName == "code2index" ){
        if ( parameters.size() == 1){
            int index = 0;
            QString cod = parameters["code"].toString();
            for( auto& drawer : _mainDrawers){
                if ( drawer->code() == cod){
                    return QVariant(index);
                }
                ++index;
            }
        }
    }
    if ( operationName == "index2code" ){
        if ( parameters.size() == 1){
            bool ok;
            int index = parameters["index"].toUInt(&ok);
            if ( index < _mainDrawers.size()){
                return QVariant ( _mainDrawers[index]->code());
            }
        }
    }
    return QVariant();
}

QVariant ComplexDrawer::attributeOfDrawer(const QString &drawercode, const QString &attrName) const
{
    if ( code() == drawercode)
        return attribute(attrName);
    if ( drawercode ==  "rootdrawer"){ // special case
        return rootDrawer()->attribute(attrName);
    }

    QVariant var;
    for( auto& drawer : _preDrawers){
        var = drawer.second->attributeOfDrawer(drawercode, attrName);
        if ( var.isValid())
            return var;

    }
    for( auto& drawer : _mainDrawers){
        var = drawer->attributeOfDrawer(drawercode, attrName);
        if ( var.isValid())
            return var;
    }
    for( auto& drawer : _postDrawers)    {
        var = drawer.second->attributeOfDrawer(drawercode, attrName);
        if ( var.isValid())
            return var;
    }
    return var;
}
<|endoftext|>


<commit_before>// Copyright (c) 2012, the Dart project authors.  Please see the AUTHORS file
// for details. All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.

#include "vm/globals.h"
#if defined(TARGET_ARCH_ARM)

#include "vm/cpu.h"
#include "vm/cpu_arm.h"

#include "vm/assembler.h"
#include "vm/cpuinfo.h"
#include "vm/heap.h"
#include "vm/isolate.h"
#include "vm/object.h"
#include "vm/simulator.h"

#if !defined(USING_SIMULATOR)
#include <sys/syscall.h> /* NOLINT */
#include <unistd.h>      /* NOLINT */
#endif

// ARM version differences.
// We support three major 32-bit ARM ISA versions: ARMv5TE, ARMv6 and variants,
// and ARMv7 and variants. For each of these we detect the presence of vfp,
// neon, and integer division instructions. Considering ARMv5TE as the baseline,
// later versions add the following features/instructions that we use:
//
// ARMv6:
// - PC read offset in store instructions is 8 rather than 12, matching the
//   offset in read instructions,
// - strex, ldrex, and clrex load/store/clear exclusive instructions,
// - umaal multiplication instruction,
// ARMv7:
// - movw, movt 16-bit immediate load instructions,
// - mls multiplication instruction,
// - vmovs, vmovd floating point immediate load instructions.
//
// If an aarch64 CPU is detected, we generate ARMv7 code.
//
// If an instruction is missing on ARMv5TE or ARMv6, we emulate it, if possible.
// Where we are missing vfp, we do not unbox doubles, or generate intrinsics for
// floating point operations. Where we are missing neon, we do not unbox SIMD
// values, or inline operations on SIMD values. Where we are missing integer
// division, we do not inline division operations, and we do not generate
// intrinsics that do division. See the feature tests in flow_graph_optimizer.cc
// for details.
//
// Alignment:
//
// Before ARMv6, that is only for ARMv5TE, unaligned accesses will cause a
// crash. This includes the ldrd and strd instructions, which must use addresses
// that are 8-byte aligned. Since we don't always guarantee that for our uses
// of ldrd and strd, these instructions are emulated with two load or store
// instructions on ARMv5TE. On ARMv6 and on, we assume that the kernel is
// set up to fixup unaligned accesses. This can be verified by checking
// /proc/cpu/alignment on modern Linux systems.

namespace dart {

#if defined(TARGET_ARCH_ARM_5TE)
DEFINE_FLAG(bool, use_vfp, false, "Use vfp instructions if supported");
DEFINE_FLAG(bool, use_neon, false, "Use neon instructions if supported");
DEFINE_FLAG(bool,
            use_integer_division,
            false,
            "Use integer division instruction if supported");
#elif defined(TARGET_ARCH_ARM_6)
DEFINE_FLAG(bool, use_vfp, true, "Use vfp instructions if supported");
DEFINE_FLAG(bool, use_neon, false, "Use neon instructions if supported");
DEFINE_FLAG(bool,
            use_integer_division,
            false,
            "Use integer division instruction if supported");
#else
DEFINE_FLAG(bool, use_vfp, true, "Use vfp instructions if supported");
DEFINE_FLAG(bool, use_neon, true, "Use neon instructions if supported");
DEFINE_FLAG(bool,
            use_integer_division,
            true,
            "Use integer division instruction if supported");
#endif

#if defined(USING_SIMULATOR)
#if defined(TARGET_ARCH_ARM_5TE) || defined(TARGET_OS_ANDROID)
DEFINE_FLAG(bool, sim_use_hardfp, false, "Use the hardfp ABI.");
#else
DEFINE_FLAG(bool, sim_use_hardfp, true, "Use the hardfp ABI.");
#endif
#endif

void CPU::FlushICache(uword start, uword size) {
#if HOST_OS_IOS
  // Precompilation never patches code so there should be no I cache flushes.
  UNREACHABLE();
#endif

#if !defined(USING_SIMULATOR) && !HOST_OS_IOS
  // Nothing to do. Flushing no instructions.
  if (size == 0) {
    return;
  }

// ARM recommends using the gcc intrinsic __clear_cache on Linux, and the
// library call cacheflush from unistd.h on Android:
// blogs.arm.com/software-enablement/141-caches-and-self-modifying-code/
#if defined(__linux__) && !defined(ANDROID)
  extern void __clear_cache(char*, char*);
  char* beg = reinterpret_cast<char*>(start);
  char* end = reinterpret_cast<char*>(start + size);
  ::__clear_cache(beg, end);
#elif defined(ANDROID)
  cacheflush(start, start + size, 0);
#else
#error FlushICache only tested/supported on Linux and Android
#endif
#endif
}

const char* CPU::Id() {
  return
#if defined(USING_SIMULATOR)
      "sim"
#endif  // defined(USING_SIMULATOR)
      "arm";
}

bool HostCPUFeatures::integer_division_supported_ = false;
bool HostCPUFeatures::vfp_supported_ = false;
bool HostCPUFeatures::neon_supported_ = false;
bool HostCPUFeatures::hardfp_supported_ = false;
const char* HostCPUFeatures::hardware_ = NULL;
ARMVersion HostCPUFeatures::arm_version_ = ARMvUnknown;
intptr_t HostCPUFeatures::store_pc_read_offset_ = 8;
#if defined(DEBUG)
bool HostCPUFeatures::initialized_ = false;
#endif

#if !defined(USING_SIMULATOR)
#if HOST_OS_IOS
void HostCPUFeatures::InitOnce() {
  // TODO(24743): Actually check the CPU features and fail if we're missing
  // something assumed in a precompiled snapshot.
  hardware_ = "";
  // When the VM is targetted to ARMv7, pretend that the CPU is ARMv7 even if
  // the CPU is actually AArch64.
  arm_version_ = ARMv7;
  // Always assume we have floating point unit since we dont support ARMv6 in
  // this path.
  vfp_supported_ = FLAG_use_vfp;
  integer_division_supported_ = FLAG_use_integer_division;
  neon_supported_ = FLAG_use_neon;
  hardfp_supported_ = true;
#if defined(DEBUG)
  initialized_ = true;
#endif
}
#else  // HOST_OS_IOS
void HostCPUFeatures::InitOnce() {
  bool is_arm64 = false;
  CpuInfo::InitOnce();
  hardware_ = CpuInfo::GetCpuModel();

  // Check for ARMv5TE, ARMv6, ARMv7, or aarch64.
  // It can be in either the Processor or Model information fields.
  if (CpuInfo::FieldContains(kCpuInfoProcessor, "aarch64") ||
      CpuInfo::FieldContains(kCpuInfoModel, "aarch64") ||
      CpuInfo::FieldContains(kCpuInfoArchitecture, "8") ||
      CpuInfo::FieldContains(kCpuInfoArchitecture, "AArch64")) {
    // pretend that this arm64 cpu is really an ARMv7
    arm_version_ = ARMv7;
    is_arm64 = true;
  } else if (CpuInfo::FieldContains(kCpuInfoProcessor, "ARM926EJ-S") ||
             CpuInfo::FieldContains(kCpuInfoModel, "ARM926EJ-S")) {
    // Lego Mindstorm EV3.
    arm_version_ = ARMv5TE;
    // On ARMv5, the PC read offset in an STR or STM instruction is either 8 or
    // 12 bytes depending on the implementation. On the Mindstorm EV3 it is 12
    // bytes.
    store_pc_read_offset_ = 12;
  } else if (CpuInfo::FieldContains(kCpuInfoProcessor, "Feroceon 88FR131") ||
             CpuInfo::FieldContains(kCpuInfoModel, "Feroceon 88FR131")) {
    // This is for the DGBox. For the time-being, assume it is similar to the
    // Lego Mindstorm.
    arm_version_ = ARMv5TE;
    store_pc_read_offset_ = 12;
  } else if (CpuInfo::FieldContains(kCpuInfoProcessor, "ARMv6") ||
             CpuInfo::FieldContains(kCpuInfoModel, "ARMv6")) {
    // Raspberry Pi, etc.
    arm_version_ = ARMv6;
  } else {
    ASSERT(CpuInfo::FieldContains(kCpuInfoProcessor, "ARMv7") ||
           CpuInfo::FieldContains(kCpuInfoModel, "ARMv7"));
    arm_version_ = ARMv7;
  }

  // Has floating point unit.
  vfp_supported_ =
      (CpuInfo::FieldContains(kCpuInfoFeatures, "vfp") || is_arm64) &&
      FLAG_use_vfp;

  // Has integer division.
  // Special cases:
  // - Qualcomm Krait CPUs (QCT APQ8064) in Nexus 4 and 7 incorrectly report
  //   that they lack integer division.
  // - Marvell Armada 370/XP incorrectly reports that it has integer division.
  // - Qualcomm Snapdragon 820/821 CPUs (MSM 8996 and MSM8996pro) in Xiaomi MI5
  // and Pixel lack integer division even though ARMv8 requires it in A32.
  bool is_krait = CpuInfo::FieldContains(kCpuInfoHardware, "QCT APQ8064");
  bool is_armada_370xp =
      CpuInfo::FieldContains(kCpuInfoHardware, "Marvell Armada 370/XP");
  bool is_snapdragon = CpuInfo::FieldContains(kCpuInfoHardware, "MSM8996");
  if (is_krait) {
    integer_division_supported_ = FLAG_use_integer_division;
  } else if (is_armada_370xp || is_snapdragon) {
    integer_division_supported_ = false;
  } else {
    integer_division_supported_ =
        (CpuInfo::FieldContains(kCpuInfoFeatures, "idiva") || is_arm64) &&
        FLAG_use_integer_division;
  }
  neon_supported_ =
      (CpuInfo::FieldContains(kCpuInfoFeatures, "neon") || is_arm64) &&
      FLAG_use_vfp && FLAG_use_neon;

// Use the cross-compiler's predefined macros to determine whether we should
// use the hard or soft float ABI.
#if defined(__ARM_PCS_VFP)
  hardfp_supported_ = true;
#else
  hardfp_supported_ = false;
#endif

#if defined(DEBUG)
  initialized_ = true;
#endif
}
#endif  // HOST_OS_IOS

void HostCPUFeatures::Cleanup() {
  DEBUG_ASSERT(initialized_);
#if defined(DEBUG)
  initialized_ = false;
#endif
  ASSERT(hardware_ != NULL);
  free(const_cast<char*>(hardware_));
  hardware_ = NULL;
  CpuInfo::Cleanup();
}

#else

void HostCPUFeatures::InitOnce() {
  CpuInfo::InitOnce();
  hardware_ = CpuInfo::GetCpuModel();

#if defined(TARGET_ARCH_ARM_5TE)
  arm_version_ = ARMv5TE;
#elif defined(TARGET_ARCH_ARM_6)
  arm_version_ = ARMv6;
#else
  arm_version_ = ARMv7;
#endif

  integer_division_supported_ = FLAG_use_integer_division;
  vfp_supported_ = FLAG_use_vfp;
  neon_supported_ = FLAG_use_vfp && FLAG_use_neon;
  hardfp_supported_ = FLAG_sim_use_hardfp;
#if defined(DEBUG)
  initialized_ = true;
#endif
}

void HostCPUFeatures::Cleanup() {
  DEBUG_ASSERT(initialized_);
#if defined(DEBUG)
  initialized_ = false;
#endif
  ASSERT(hardware_ != NULL);
  free(const_cast<char*>(hardware_));
  hardware_ = NULL;
  CpuInfo::Cleanup();
}
#endif  // !defined(USING_SIMULATOR)

}  // namespace dart

#endif  // defined TARGET_ARCH_ARM
<commit_msg>Conservatively disable use of integer division on Android ARM64 devices.<commit_after>// Copyright (c) 2012, the Dart project authors.  Please see the AUTHORS file
// for details. All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.

#include "vm/globals.h"
#if defined(TARGET_ARCH_ARM)

#include "vm/cpu.h"
#include "vm/cpu_arm.h"

#include "vm/assembler.h"
#include "vm/cpuinfo.h"
#include "vm/heap.h"
#include "vm/isolate.h"
#include "vm/object.h"
#include "vm/simulator.h"

#if !defined(USING_SIMULATOR)
#include <sys/syscall.h> /* NOLINT */
#include <unistd.h>      /* NOLINT */
#endif

// ARM version differences.
// We support three major 32-bit ARM ISA versions: ARMv5TE, ARMv6 and variants,
// and ARMv7 and variants. For each of these we detect the presence of vfp,
// neon, and integer division instructions. Considering ARMv5TE as the baseline,
// later versions add the following features/instructions that we use:
//
// ARMv6:
// - PC read offset in store instructions is 8 rather than 12, matching the
//   offset in read instructions,
// - strex, ldrex, and clrex load/store/clear exclusive instructions,
// - umaal multiplication instruction,
// ARMv7:
// - movw, movt 16-bit immediate load instructions,
// - mls multiplication instruction,
// - vmovs, vmovd floating point immediate load instructions.
//
// If an aarch64 CPU is detected, we generate ARMv7 code.
//
// If an instruction is missing on ARMv5TE or ARMv6, we emulate it, if possible.
// Where we are missing vfp, we do not unbox doubles, or generate intrinsics for
// floating point operations. Where we are missing neon, we do not unbox SIMD
// values, or inline operations on SIMD values. Where we are missing integer
// division, we do not inline division operations, and we do not generate
// intrinsics that do division. See the feature tests in flow_graph_optimizer.cc
// for details.
//
// Alignment:
//
// Before ARMv6, that is only for ARMv5TE, unaligned accesses will cause a
// crash. This includes the ldrd and strd instructions, which must use addresses
// that are 8-byte aligned. Since we don't always guarantee that for our uses
// of ldrd and strd, these instructions are emulated with two load or store
// instructions on ARMv5TE. On ARMv6 and on, we assume that the kernel is
// set up to fixup unaligned accesses. This can be verified by checking
// /proc/cpu/alignment on modern Linux systems.

namespace dart {

#if defined(TARGET_ARCH_ARM_5TE)
DEFINE_FLAG(bool, use_vfp, false, "Use vfp instructions if supported");
DEFINE_FLAG(bool, use_neon, false, "Use neon instructions if supported");
DEFINE_FLAG(bool,
            use_integer_division,
            false,
            "Use integer division instruction if supported");
#elif defined(TARGET_ARCH_ARM_6)
DEFINE_FLAG(bool, use_vfp, true, "Use vfp instructions if supported");
DEFINE_FLAG(bool, use_neon, false, "Use neon instructions if supported");
DEFINE_FLAG(bool,
            use_integer_division,
            false,
            "Use integer division instruction if supported");
#else
DEFINE_FLAG(bool, use_vfp, true, "Use vfp instructions if supported");
DEFINE_FLAG(bool, use_neon, true, "Use neon instructions if supported");
DEFINE_FLAG(bool,
            use_integer_division,
            true,
            "Use integer division instruction if supported");
#endif

#if defined(USING_SIMULATOR)
#if defined(TARGET_ARCH_ARM_5TE) || defined(TARGET_OS_ANDROID)
DEFINE_FLAG(bool, sim_use_hardfp, false, "Use the hardfp ABI.");
#else
DEFINE_FLAG(bool, sim_use_hardfp, true, "Use the hardfp ABI.");
#endif
#endif

void CPU::FlushICache(uword start, uword size) {
#if HOST_OS_IOS
  // Precompilation never patches code so there should be no I cache flushes.
  UNREACHABLE();
#endif

#if !defined(USING_SIMULATOR) && !HOST_OS_IOS
  // Nothing to do. Flushing no instructions.
  if (size == 0) {
    return;
  }

// ARM recommends using the gcc intrinsic __clear_cache on Linux, and the
// library call cacheflush from unistd.h on Android:
// blogs.arm.com/software-enablement/141-caches-and-self-modifying-code/
#if defined(__linux__) && !defined(ANDROID)
  extern void __clear_cache(char*, char*);
  char* beg = reinterpret_cast<char*>(start);
  char* end = reinterpret_cast<char*>(start + size);
  ::__clear_cache(beg, end);
#elif defined(ANDROID)
  cacheflush(start, start + size, 0);
#else
#error FlushICache only tested/supported on Linux and Android
#endif
#endif
}

const char* CPU::Id() {
  return
#if defined(USING_SIMULATOR)
      "sim"
#endif  // defined(USING_SIMULATOR)
      "arm";
}

bool HostCPUFeatures::integer_division_supported_ = false;
bool HostCPUFeatures::vfp_supported_ = false;
bool HostCPUFeatures::neon_supported_ = false;
bool HostCPUFeatures::hardfp_supported_ = false;
const char* HostCPUFeatures::hardware_ = NULL;
ARMVersion HostCPUFeatures::arm_version_ = ARMvUnknown;
intptr_t HostCPUFeatures::store_pc_read_offset_ = 8;
#if defined(DEBUG)
bool HostCPUFeatures::initialized_ = false;
#endif

#if !defined(USING_SIMULATOR)
#if HOST_OS_IOS
void HostCPUFeatures::InitOnce() {
  // TODO(24743): Actually check the CPU features and fail if we're missing
  // something assumed in a precompiled snapshot.
  hardware_ = "";
  // When the VM is targetted to ARMv7, pretend that the CPU is ARMv7 even if
  // the CPU is actually AArch64.
  arm_version_ = ARMv7;
  // Always assume we have floating point unit since we dont support ARMv6 in
  // this path.
  vfp_supported_ = FLAG_use_vfp;
  integer_division_supported_ = FLAG_use_integer_division;
  neon_supported_ = FLAG_use_neon;
  hardfp_supported_ = true;
#if defined(DEBUG)
  initialized_ = true;
#endif
}
#else  // HOST_OS_IOS
void HostCPUFeatures::InitOnce() {
  bool is_arm64 = false;
  CpuInfo::InitOnce();
  hardware_ = CpuInfo::GetCpuModel();

  // Check for ARMv5TE, ARMv6, ARMv7, or aarch64.
  // It can be in either the Processor or Model information fields.
  if (CpuInfo::FieldContains(kCpuInfoProcessor, "aarch64") ||
      CpuInfo::FieldContains(kCpuInfoModel, "aarch64") ||
      CpuInfo::FieldContains(kCpuInfoArchitecture, "8") ||
      CpuInfo::FieldContains(kCpuInfoArchitecture, "AArch64")) {
    // pretend that this arm64 cpu is really an ARMv7
    arm_version_ = ARMv7;
    is_arm64 = true;
  } else if (CpuInfo::FieldContains(kCpuInfoProcessor, "ARM926EJ-S") ||
             CpuInfo::FieldContains(kCpuInfoModel, "ARM926EJ-S")) {
    // Lego Mindstorm EV3.
    arm_version_ = ARMv5TE;
    // On ARMv5, the PC read offset in an STR or STM instruction is either 8 or
    // 12 bytes depending on the implementation. On the Mindstorm EV3 it is 12
    // bytes.
    store_pc_read_offset_ = 12;
  } else if (CpuInfo::FieldContains(kCpuInfoProcessor, "Feroceon 88FR131") ||
             CpuInfo::FieldContains(kCpuInfoModel, "Feroceon 88FR131")) {
    // This is for the DGBox. For the time-being, assume it is similar to the
    // Lego Mindstorm.
    arm_version_ = ARMv5TE;
    store_pc_read_offset_ = 12;
  } else if (CpuInfo::FieldContains(kCpuInfoProcessor, "ARMv6") ||
             CpuInfo::FieldContains(kCpuInfoModel, "ARMv6")) {
    // Raspberry Pi, etc.
    arm_version_ = ARMv6;
  } else {
    ASSERT(CpuInfo::FieldContains(kCpuInfoProcessor, "ARMv7") ||
           CpuInfo::FieldContains(kCpuInfoModel, "ARMv7"));
    arm_version_ = ARMv7;
  }

  // Has floating point unit.
  vfp_supported_ =
      (CpuInfo::FieldContains(kCpuInfoFeatures, "vfp") || is_arm64) &&
      FLAG_use_vfp;

  // Has integer division.
  // Special cases:
  // - Qualcomm Krait CPUs (QCT APQ8064) in Nexus 4 and 7 incorrectly report
  //   that they lack integer division.
  // - Marvell Armada 370/XP incorrectly reports that it has integer division.
  bool is_krait = CpuInfo::FieldContains(kCpuInfoHardware, "QCT APQ8064");
  bool is_armada_370xp =
      CpuInfo::FieldContains(kCpuInfoHardware, "Marvell Armada 370/XP");
#if defined(HOST_OS_ANDROID)
  bool is_android = true;
#else
  bool is_android = false;
#endif
  if (is_krait) {
    integer_division_supported_ = FLAG_use_integer_division;
  } else if (is_android && is_arm64) {
    // Various Android ARM64 devices, including the Qualcomm Snapdragon 820/821
    // CPUs (MSM 8996 and MSM8996pro) in Xiaomi MI5 and Pixel lack integer
    // division even though ARMv8 requires it in A32. Instead of attempting to
    // track all of these devices, we conservatively disable use of integer
    // division on Android ARM64 devices.
    // TODO(29270): /proc/self/auxv might be more reliable here.
    integer_division_supported_ = false;
  } else if (is_armada_370xp) {
    integer_division_supported_ = false;
  } else {
    integer_division_supported_ =
        (CpuInfo::FieldContains(kCpuInfoFeatures, "idiva") || is_arm64) &&
        FLAG_use_integer_division;
  }
  neon_supported_ =
      (CpuInfo::FieldContains(kCpuInfoFeatures, "neon") || is_arm64) &&
      FLAG_use_vfp && FLAG_use_neon;

// Use the cross-compiler's predefined macros to determine whether we should
// use the hard or soft float ABI.
#if defined(__ARM_PCS_VFP)
  hardfp_supported_ = true;
#else
  hardfp_supported_ = false;
#endif

#if defined(DEBUG)
  initialized_ = true;
#endif
}
#endif  // HOST_OS_IOS

void HostCPUFeatures::Cleanup() {
  DEBUG_ASSERT(initialized_);
#if defined(DEBUG)
  initialized_ = false;
#endif
  ASSERT(hardware_ != NULL);
  free(const_cast<char*>(hardware_));
  hardware_ = NULL;
  CpuInfo::Cleanup();
}

#else

void HostCPUFeatures::InitOnce() {
  CpuInfo::InitOnce();
  hardware_ = CpuInfo::GetCpuModel();

#if defined(TARGET_ARCH_ARM_5TE)
  arm_version_ = ARMv5TE;
#elif defined(TARGET_ARCH_ARM_6)
  arm_version_ = ARMv6;
#else
  arm_version_ = ARMv7;
#endif

  integer_division_supported_ = FLAG_use_integer_division;
  vfp_supported_ = FLAG_use_vfp;
  neon_supported_ = FLAG_use_vfp && FLAG_use_neon;
  hardfp_supported_ = FLAG_sim_use_hardfp;
#if defined(DEBUG)
  initialized_ = true;
#endif
}

void HostCPUFeatures::Cleanup() {
  DEBUG_ASSERT(initialized_);
#if defined(DEBUG)
  initialized_ = false;
#endif
  ASSERT(hardware_ != NULL);
  free(const_cast<char*>(hardware_));
  hardware_ = NULL;
  CpuInfo::Cleanup();
}
#endif  // !defined(USING_SIMULATOR)

}  // namespace dart

#endif  // defined TARGET_ARCH_ARM
<|endoftext|>


<commit_before><commit_msg>pdr whittle away until it looks sane<commit_after><|endoftext|>


<commit_before>#include "helper-funcs.hpp"

#include "mbed.h"
#include "rtos.h"

#include "logger.hpp"
#include "assert.hpp"

/**
 * Initializes the peripheral nested vector interrupt controller (PNVIC) with
 * appropriate values. Low values have the higest priority (with system
 * interrupts having negative priority). All maskable interrupts are
 * diabled; do not intialize any interrupt frameworks before this funtion is
 * called. PNVIC interrupt priorities are independent of thread and NVIC
 * priorities.
 *
 * The PNVIC is independent of the NVIC responsible for system NMI's. The NVIC
 * is not accissable through the library, so in this context the NVIC functions
 * refer to the PNVIC. The configuration system for PNVIC priorities is strange
 * and different from X86. If you haven't already, look over
 * doc/ARM-Cortex-M_Interrupt-Priorities.pdf from RJ root and the online
 * documentation regarding Interrupt Priority Registers (IPRs) first.
 */
void setISRPriorities() {
    __disable_irq();

    // set two bits for preemptive data, two bits for priority as the
    // structure for the IPRs. Grouping is global withing the IPRs so
    // this value should only be changed here.
    NVIC_SetPriorityGrouping(5);
    uint32_t priorityGrouping = NVIC_GetPriorityGrouping();

    // set preemptive priority default to 2 (0..3)
    // set priority default to 1 (0..3)
    uint32_t defaultPriority = NVIC_EncodePriority(priorityGrouping, 2, 1);

    // When the kernel initialzes the PNVIC, all ISRs are set to the
    // highest priority, making it impossible to elevate a few over
    // the rest, so the default priority is lowered globally for the
    // table first.
    //
    // Consult LPC17xx.h under IRQn_Type for PNVIC ranges, this is LPC1768
    // specific
    for (uint32_t IRQn = TIMER0_IRQn; IRQn <= CANActivity_IRQn; IRQn++)
        NVIC_SetPriority((IRQn_Type)IRQn, defaultPriority);

    ////////////////////////////////////
    //  begin raise priority section  //
    ////////////////////////////////////

    // reestablish watchdog
    NVIC_SetPriority(WDT_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 0));

    // reestablish timers
    // NVIC_SetPriority(TIMER0_IRQn, NVIC_EncodePriority(priorityGrouping, 0,
    // 1));
    // NVIC_SetPriority(TIMER1_IRQn, NVIC_EncodePriority(priorityGrouping, 0,
    // 2));
    // NVIC_SetPriority(TIMER2_IRQn, NVIC_EncodePriority(priorityGrouping, 0,
    // 3));
    // NVIC_SetPriority(TIMER3_IRQn, NVIC_EncodePriority(priorityGrouping, 0,
    // 4));

    // make TIMER #2 2nd in line t the watchdog timer
    NVIC_SetPriority(TIMER2_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 1));

    // this is the timer used in the mbed Ticker library
    NVIC_SetPriority(TIMER3_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 2));

    // Brown-Out Detect
    NVIC_SetPriority(BOD_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 4));

    // The I2C interface that's in use
    NVIC_SetPriority(I2C2_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 1));

    // The SPI interface that's in use.
    NVIC_SetPriority(SPI_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 2));
    // NVIC_SetPriority(SSP0_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 2));

    ////////////////////////////////////
    //  begin lower priotity section  //
    ////////////////////////////////////

    // set UART (console) interrupts to minimal priority
    // when debugging radio and other time sensitive operations, this
    // interrupt will need to be deferred.
    NVIC_SetPriority(UART0_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 4));

    // NVIC_SetPriority(UART1_IRQn, NVIC_EncodePriority(priorityGrouping, 3,
    // 2));
    // NVIC_SetPriority(UART2_IRQn, NVIC_EncodePriority(priorityGrouping, 3,
    // 2));
    // NVIC_SetPriority(UART3_IRQn, NVIC_EncodePriority(priorityGrouping, 3,
    // 1));

    // NVIC_EnableIRQ(TIMER0_IRQn);
    // NVIC_EnableIRQ(TIMER1_IRQn);
    // NVIC_EnableIRQ(TIMER2_IRQn);
    // NVIC_EnableIRQ(TIMER3_IRQn);

    __enable_irq();
}

/**
 * Timer interrupt based light flicker. If this stops, the code triggered
 * a fault.
 */
void imAlive(void const* args) {
    DigitalOut* led = (DigitalOut*)args;

    *led = !(*led);
    Thread::wait(40);
    *led = !(*led);
}

/**
 * @brief      { Flash an LED }
 */
void strobeStatusLED(void const* args) {
    DigitalInOut* led = (DigitalInOut*)args;

    *led = !(*led);
    Thread::wait(30);
    *led = !(*led);
}

// Helper function for creating a class that uses a
// member function for the object's thread operations.
// Take note that this allocates a stack!
void define_thread(osThreadDef_t& t, void (*task)(void const* arg),
                   osPriority priority, uint32_t stack_size) {
#ifdef CMSIS_OS_RTX
    t.pthread = task;
    t.tpriority = priority;
    t.stacksize = stack_size;
    t.stack_pointer = (uint32_t*)new unsigned char[t.stacksize];
    ASSERT(t.stack_pointer != nullptr);
#endif
}
<commit_msg>removing commented out lines and adding cpp casting conventions to some functions with void* arguments<commit_after>#include "helper-funcs.hpp"

#include "mbed.h"
#include "rtos.h"

#include "logger.hpp"
#include "assert.hpp"

/**
 * Initializes the peripheral nested vector interrupt controller (PNVIC) with
 * appropriate values. Low values have the higest priority (with system
 * interrupts having negative priority). All maskable interrupts are
 * diabled; do not intialize any interrupt frameworks before this funtion is
 * called. PNVIC interrupt priorities are independent of thread and NVIC
 * priorities.
 *
 * The PNVIC is independent of the NVIC responsible for system NMI's. The NVIC
 * is not accissable through the library, so in this context the NVIC functions
 * refer to the PNVIC. The configuration system for PNVIC priorities is strange
 * and different from X86. If you haven't already, look over
 * doc/ARM-Cortex-M_Interrupt-Priorities.pdf from RJ root and the online
 * documentation regarding Interrupt Priority Registers (IPRs) first.
 */
void setISRPriorities() {
    __disable_irq();

    // set two bits for preemptive data, two bits for priority as the
    // structure for the IPRs. Grouping is global withing the IPRs so
    // this value should only be changed here.
    NVIC_SetPriorityGrouping(5);
    uint32_t priorityGrouping = NVIC_GetPriorityGrouping();

    // set preemptive priority default to 2 (0..3)
    // set priority default to 1 (0..3)
    uint32_t defaultPriority = NVIC_EncodePriority(priorityGrouping, 2, 1);

    // When the kernel initialzes the PNVIC, all ISRs are set to the
    // highest priority, making it impossible to elevate a few over
    // the rest, so the default priority is lowered globally for the
    // table first.
    //
    // Consult LPC17xx.h under IRQn_Type for PNVIC ranges, this is LPC1768
    // specific
    for (uint32_t IRQn = TIMER0_IRQn; IRQn <= CANActivity_IRQn; IRQn++)
        NVIC_SetPriority((IRQn_Type)IRQn, defaultPriority);

    // reestablish watchdog
    NVIC_SetPriority(WDT_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 0));

    // make TIMER #2 2nd in line t the watchdog timer
    NVIC_SetPriority(TIMER2_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 1));

    // this is the timer used in the mbed Ticker library
    NVIC_SetPriority(TIMER3_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 2));

    // Brown-Out Detect
    NVIC_SetPriority(BOD_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 4));

    // The I2C interface that's in use
    NVIC_SetPriority(I2C2_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 1));

    // The SPI interface that's in use.
    NVIC_SetPriority(SPI_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 2));

    // set UART (console) interrupts to minimal priority
    // when debugging radio and other time sensitive operations, this
    // interrupt will need to be deferred.
    NVIC_SetPriority(UART0_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 4));

    __enable_irq();
}

/**
 * Timer interrupt based light flicker. If this stops, the code triggered
 * a fault.
 */
void imAlive(void const* arg) {
    DigitalOut* led =
        const_cast<DigitalOut*>(reinterpret_cast<const DigitalOut*>(arg));

    *led = !(*led);
    Thread::wait(40);
    *led = !(*led);
}

/**
 * @brief      { Flash an LED }
 */
void strobeStatusLED(void const* arg) {
    DigitalInOut* led =
        const_cast<DigitalInOut*>(reinterpret_cast<const DigitalInOut*>(arg));

    *led = !(*led);
    Thread::wait(30);
    *led = !(*led);
}

// Helper function for creating a class that uses a
// member function for the object's thread operations.
// Take note that this allocates a stack!
void define_thread(osThreadDef_t& t, void (*task)(void const* arg),
                   osPriority priority, uint32_t stack_size) {
#ifdef CMSIS_OS_RTX
    t.pthread = task;
    t.tpriority = priority;
    t.stacksize = stack_size;
    t.stack_pointer = (uint32_t*)new unsigned char[t.stacksize];
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wwrite-strings"
    ASSERT(t.stack_pointer != nullptr);
#pragma GCC diagnostic pop

#endif
}
<|endoftext|>


<commit_before>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "encoderfoaw.h"
#include "packet/frame.h"
#include "packet/serialize.h"
#include "simulation.pb.h"
#include "messageutil.h"
#include "filter/movingaverage.h"

#include "angle.h"
#include "blink.h"
#include "usbconfig.h"
#include "saconfig.h"

#include "parameters.h"

#include <algorithm>
#include <array>
#include <type_traits>

#include "bicycle/kinematic.h" /* simplified bicycle model */
#include "bicycle/whipple.h" /* whipple bicycle model */
#include "oracle.h" /* oracle observer */
#include "kalman.h" /* kalman filter observer */
#include "haptic.h" /* handlebar feedback */
#include "simbicycle.h"

namespace {
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleKinematic;
    using observer_t = observer::Oracle<model_t>;
    using haptic_t = haptic::HandlebarStatic;
#else // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleWhipple;
    using observer_t = observer::Kalman<model_t>;
    using haptic_t = haptic::HandlebarDynamic;
#endif // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using bicycle_t = sim::Bicycle<model_t, observer_t, haptic_t>;

    /* sensors */
    Analog analog;
    Encoder encoder_steer(sa::RLS_ROLIN_ENC, sa::RLS_ROLIN_ENC_INDEX_CFG);
    EncoderFoaw<float, 32> encoder_rear_wheel(sa::RLS_GTS35_ENC,
                                              sa::RLS_GTS35_ENC_CFG,
                                              MS2ST(1), 3.0f);
    filter::MovingAverage<float, 5> velocity_filter;

    constexpr float fixed_velocity = 7.0f;

    /* transmission */
    std::array<uint8_t, SimulationMessage_size + packet::frame::PACKET_OVERHEAD> encode_buffer;
    std::array<uint8_t, SimulationMessage_size + packet::frame::PACKET_OVERHEAD> packet_buffer;
    SimulationMessage msg;
    constexpr SimulationMessage msg_init = SimulationMessage_init_zero;

    /* kinematics loop */
    constexpr systime_t kinematics_loop_time = US2ST(8333); /* update kinematics at 120 Hz */

    /* dynamics loop */
    constexpr model::real_t dynamics_period = 0.005; /* 5 ms -> 200 Hz */
    time_measurement_t computation_time_measurement;
    time_measurement_t transmission_time_measurement;

    dacsample_t set_handlebar_torque(float handlebar_torque) {
        int32_t saturated_value = (handlebar_torque/sa::MAX_KOLLMORGEN_TORQUE * 2048) +
            sa::KOLLMORGEN_DAC_ZERO_OFFSET;
        saturated_value = std::min<int32_t>(std::max<int32_t>(saturated_value, 0), 4096);
        dacsample_t aout = static_cast<dacsample_t>(saturated_value);
        dacPutChannelX(sa::KOLLM_DAC, 0, aout);
        return aout;
    }

    void update_and_transmit_kinematics(bicycle_t& bicycle) {
        bicycle.update_kinematics();
        /*
         * TODO: Pose message should be transmitted asynchronously.
         * After starting transmission, the simulation should start to calculate pose for the next timestep.
         * This is currently not necessary given the observed timings.
         */
        //size_t bytes_encoded = packet::serialize::encode_delimited(bicycle.pose(),
        //        encode_buffer.data(), encode_buffer.size());
        //TODO: clear message
        //message::set_simulation_loop_values(&msg, bicycle);
        //size_t bytes_encoded = packet::serialize::encode_delimited(
        //        msg, encode_buffer.data(), encode_buffer.size());
        //size_t bytes_written = packet::frame::stuff(
        //        encode_buffer.data(), packet_buffer.data(), bytes_encoded);
        //if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
        //    usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
        //}
    }

    template <typename T>
    struct observer_initializer{
        template <typename S = T>
        typename std::enable_if<std::is_same<S, observer::Kalman<model_t>>::value, void>::type
        initialize(S& observer) {
            /* We start with steer angle equal to the measurement and all other state elements at zero.  */
            model_t::state_t x0 = model_t::state_t::Zero();
            model_t::set_state_element(x0, model_t::state_index_t::steer_angle,
                    angle::encoder_count<float>(encoder_steer));
            observer.set_x(x0);
            observer.set_Q(parameters::defaultvalue::kalman::Q(observer.dt()));
            // Reduce steer measurement noise covariance
            observer.set_R(parameters::defaultvalue::kalman::R/1000);
        }
        template <typename S = T>
        typename std::enable_if<!std::is_same<S, observer::Kalman<model_t>>::value, void>::type
        initialize(S& observer) {
            // no-op
            (void)observer;
        }
    };

    THD_WORKING_AREA(wa_kinematics_thread, 2048);
    THD_FUNCTION(kinematics_thread, arg) {
        bicycle_t& bicycle = *static_cast<bicycle_t*>(arg);

        chRegSetThreadName("kinematics");
        while (true) {
            systime_t starttime = chVTGetSystemTime();
            update_and_transmit_kinematics(bicycle);
            systime_t sleeptime = kinematics_loop_time + starttime - chVTGetSystemTime();
            if (sleeptime >= kinematics_loop_time) {
                continue;
            } else {
                chThdSleep(sleeptime);
            }
        }
    }

    size_t write_message_to_encode_buffer(const SimulationMessage& m) {
        const size_t bytes_encoded = packet::serialize::encode_delimited(
                m, encode_buffer.data(), encode_buffer.size());
        return packet::frame::stuff(
                encode_buffer.data(), packet_buffer.data(), bytes_encoded);
    }
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initialize a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activate the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /* create the blink thread and print state monitor */
    chBlinkThreadCreateStatic();

    /*
     * Start sensors.
     * Encoder:
     *   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
     *   Pins for encoder driver 3 are already set in board.h.
     */
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder_steer.start();
    encoder_rear_wheel.start();
    analog.start(1000); /* trigger ADC conversion at 1 kHz */

    /*
     * Set torque measurement enable line low.
     * The output of the Kistler torque sensor is not valid until after a falling edge
     * on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
     * reversed due to NPN switch Q1.
     */
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    /*
     * Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
     * The default line configuration is OUTPUT_OPENDRAIN_PULLUP  for SPI1_ENC1_NSS
     * and must be changed to use as analog output.
     */
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);
    set_handlebar_torque(0.0f);

    /*
     * Initialize bicycle. Velocity doesn't matter as we immediately use the measured value.
     */
    bicycle_t bicycle(fixed_velocity, dynamics_period);

    /*
     * Initialize HandlebarDynamic object to estimate torque due to handlebar inertia.
     * TODO: naming here is poor
     */
    haptic::HandlebarDynamic handlebar_model(bicycle.model(), sa::HANDLEBAR_INERTIA);

    observer_initializer<observer_t> oi;
    oi.initialize(bicycle.observer());

    chTMObjectInit(&computation_time_measurement);
    chTMObjectInit(&transmission_time_measurement);

    /*
     * Transmit initial message containing gitsha1, model, and observer data.
     * This initial transmission is blocking.
     */
    msg = msg_init;
    msg.timestamp = chVTGetSystemTime();
    message::set_simulation_full_model_observer(&msg, bicycle);
    size_t bytes_written = write_message_to_encode_buffer(msg);

    /* block until ready */
    while ((SDU1.config->usbp->state != USB_ACTIVE) || (SDU1.state != SDU_READY)) {
        chThdSleepMilliseconds(10);
    }

    /* transmit data */
    if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
        chTMStartMeasurementX(&transmission_time_measurement);
        usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
        chTMStopMeasurementX(&transmission_time_measurement);
    }

    /*
     * Normal main() thread activity, in this demo it simulates the bicycle
     * dynamics in real-time (roughly).
     */
    chThdCreateStatic(wa_kinematics_thread, sizeof(wa_kinematics_thread),
            NORMALPRIO - 1, kinematics_thread, static_cast<void*>(&bicycle));
    while (true) {
        systime_t starttime = chVTGetSystemTime();
        chTMStartMeasurementX(&computation_time_measurement);
        constexpr float roll_torque = 0.0f;

        /* get sensor measurements */
        const float kistler_torque = static_cast<float>(
                analog.get_adc12()*2.0f*sa::MAX_KISTLER_TORQUE/4096 -
                sa::MAX_KISTLER_TORQUE);
        const float motor_torque = static_cast<float>(
                analog.get_adc13()*2.0f*sa::MAX_KOLLMORGEN_TORQUE/4096 -
                sa::MAX_KOLLMORGEN_TORQUE);
        const float steer_angle = angle::encoder_count<float>(encoder_steer);
        const float rear_wheel_angle = -angle::encoder_count<float>(encoder_rear_wheel);
        const float v = velocity_filter.output(
                -sa::REAR_WHEEL_RADIUS*(angle::encoder_rate(encoder_rear_wheel)));

        /* yaw angle, just use previous state value */
        const float yaw_angle = angle::wrap(bicycle.pose().yaw);

        /* calculate rider applied torque */
        const float inertia_torque = -handlebar_model.feedback_torque(bicycle.observer().state());
        const float steer_torque = kistler_torque - inertia_torque;

        /* simulate bicycle */
        bicycle.set_v(fixed_velocity);
        bicycle.update_dynamics(roll_torque, steer_torque, yaw_angle, steer_angle, rear_wheel_angle);

        /* generate handlebar torque output */
        const dacsample_t handlebar_torque_dac = set_handlebar_torque(bicycle.handlebar_feedback_torque());
        chTMStopMeasurementX(&computation_time_measurement);

        /* prepare message for transmission */
        msg = msg_init;
        msg.timestamp = starttime;
        message::set_bicycle_input(&msg.input,
                (model_t::input_t() << roll_torque, steer_torque).finished());
        msg.has_input = true;
        message::set_simulation_state(&msg, bicycle);
        message::set_simulation_auxiliary_state(&msg, bicycle);
        if (std::is_same<observer_t, typename observer::Kalman<model_t>>::value) {
            message::set_kalman_gain(&msg.kalman, bicycle.observer());
            msg.has_kalman = true;
        }
        message::set_simulation_actuators(&msg, handlebar_torque_dac);
        message::set_simulation_sensors(&msg,
                analog.get_adc12(), analog.get_adc13(),
                encoder_steer.count(), encoder_rear_wheel.count());
        message::set_simulation_timing(&msg,
                computation_time_measurement.last, transmission_time_measurement.last);
        msg.feedback_torque = bicycle.handlebar_feedback_torque();
        msg.has_feedback_torque = true;
        size_t bytes_written = write_message_to_encode_buffer(msg);

        /* transmit message */
        if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
            chTMStartMeasurementX(&transmission_time_measurement);
            usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
            chTMStopMeasurementX(&transmission_time_measurement);
        }

        const systime_t looptime = MS2ST(static_cast<systime_t>(1000*bicycle.dt()));
        const systime_t sleeptime = looptime + starttime - chVTGetSystemTime();
        if (sleeptime >= looptime) {
            //chDbgAssert(false, "deadline missed");
            continue;
        } else {
            chThdSleep(sleeptime);
        }
    }
}
<commit_msg>Prime observer object before starting simulation loop<commit_after>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "encoderfoaw.h"
#include "packet/frame.h"
#include "packet/serialize.h"
#include "simulation.pb.h"
#include "messageutil.h"
#include "filter/movingaverage.h"

#include "angle.h"
#include "blink.h"
#include "usbconfig.h"
#include "saconfig.h"

#include "parameters.h"

#include <algorithm>
#include <array>
#include <type_traits>

#include "bicycle/kinematic.h" /* simplified bicycle model */
#include "bicycle/whipple.h" /* whipple bicycle model */
#include "oracle.h" /* oracle observer */
#include "kalman.h" /* kalman filter observer */
#include "haptic.h" /* handlebar feedback */
#include "simbicycle.h"

namespace {
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleKinematic;
    using observer_t = observer::Oracle<model_t>;
    using haptic_t = haptic::HandlebarStatic;
#else // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleWhipple;
    using observer_t = observer::Kalman<model_t>;
    using haptic_t = haptic::HandlebarDynamic;
#endif // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using bicycle_t = sim::Bicycle<model_t, observer_t, haptic_t>;

    /* sensors */
    Analog analog;
    Encoder encoder_steer(sa::RLS_ROLIN_ENC, sa::RLS_ROLIN_ENC_INDEX_CFG);
    EncoderFoaw<float, 32> encoder_rear_wheel(sa::RLS_GTS35_ENC,
                                              sa::RLS_GTS35_ENC_CFG,
                                              MS2ST(1), 3.0f);
    filter::MovingAverage<float, 5> velocity_filter;

    constexpr float fixed_velocity = 7.0f;

    /* transmission */
    std::array<uint8_t, SimulationMessage_size + packet::frame::PACKET_OVERHEAD> encode_buffer;
    std::array<uint8_t, SimulationMessage_size + packet::frame::PACKET_OVERHEAD> packet_buffer;
    SimulationMessage msg;
    constexpr SimulationMessage msg_init = SimulationMessage_init_zero;

    /* kinematics loop */
    constexpr systime_t kinematics_loop_time = US2ST(8333); /* update kinematics at 120 Hz */

    /* dynamics loop */
    constexpr model::real_t dynamics_period = 0.005; /* 5 ms -> 200 Hz */
    time_measurement_t computation_time_measurement;
    time_measurement_t transmission_time_measurement;

    dacsample_t set_handlebar_torque(float handlebar_torque) {
        int32_t saturated_value = (handlebar_torque/sa::MAX_KOLLMORGEN_TORQUE * 2048) +
            sa::KOLLMORGEN_DAC_ZERO_OFFSET;
        saturated_value = std::min<int32_t>(std::max<int32_t>(saturated_value, 0), 4096);
        dacsample_t aout = static_cast<dacsample_t>(saturated_value);
        dacPutChannelX(sa::KOLLM_DAC, 0, aout);
        return aout;
    }

    void update_and_transmit_kinematics(bicycle_t& bicycle) {
        bicycle.update_kinematics();
        /*
         * TODO: Pose message should be transmitted asynchronously.
         * After starting transmission, the simulation should start to calculate pose for the next timestep.
         * This is currently not necessary given the observed timings.
         */
        //size_t bytes_encoded = packet::serialize::encode_delimited(bicycle.pose(),
        //        encode_buffer.data(), encode_buffer.size());
        //TODO: clear message
        //message::set_simulation_loop_values(&msg, bicycle);
        //size_t bytes_encoded = packet::serialize::encode_delimited(
        //        msg, encode_buffer.data(), encode_buffer.size());
        //size_t bytes_written = packet::frame::stuff(
        //        encode_buffer.data(), packet_buffer.data(), bytes_encoded);
        //if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
        //    usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
        //}
    }

    template <typename T>
    struct observer_initializer{
        template <typename S = T>
        typename std::enable_if<std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
        initialize(S& bicycle) {
            typename S::observer_t& observer = bicycle.observer();
            observer.set_Q(parameters::defaultvalue::kalman::Q(observer.dt()));
            /* Reduce steer measurement noise covariance */
            observer.set_R(parameters::defaultvalue::kalman::R/1000);

            /* prime the Kalman gain matrix */
            bicycle.prime_observer();

            /* We start with steer angle equal to the measurement and all other state elements at zero.  */
            model_t::state_t x0 = model_t::state_t::Zero();
            model_t::set_state_element(x0, model_t::state_index_t::steer_angle,
                    angle::encoder_count<float>(encoder_steer));
            observer.set_x(x0);
        }
        template <typename S = T>
        typename std::enable_if<!std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
        initialize(S& bicycle) {
            // no-op
            (void)bicycle;
        }
    };

    THD_WORKING_AREA(wa_kinematics_thread, 2048);
    THD_FUNCTION(kinematics_thread, arg) {
        bicycle_t& bicycle = *static_cast<bicycle_t*>(arg);

        chRegSetThreadName("kinematics");
        while (true) {
            systime_t starttime = chVTGetSystemTime();
            update_and_transmit_kinematics(bicycle);
            systime_t sleeptime = kinematics_loop_time + starttime - chVTGetSystemTime();
            if (sleeptime >= kinematics_loop_time) {
                continue;
            } else {
                chThdSleep(sleeptime);
            }
        }
    }

    size_t write_message_to_encode_buffer(const SimulationMessage& m) {
        const size_t bytes_encoded = packet::serialize::encode_delimited(
                m, encode_buffer.data(), encode_buffer.size());
        return packet::frame::stuff(
                encode_buffer.data(), packet_buffer.data(), bytes_encoded);
    }
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initialize a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activate the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /* create the blink thread and print state monitor */
    chBlinkThreadCreateStatic();

    /*
     * Start sensors.
     * Encoder:
     *   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
     *   Pins for encoder driver 3 are already set in board.h.
     */
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder_steer.start();
    encoder_rear_wheel.start();
    analog.start(1000); /* trigger ADC conversion at 1 kHz */

    /*
     * Set torque measurement enable line low.
     * The output of the Kistler torque sensor is not valid until after a falling edge
     * on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
     * reversed due to NPN switch Q1.
     */
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    /*
     * Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
     * The default line configuration is OUTPUT_OPENDRAIN_PULLUP  for SPI1_ENC1_NSS
     * and must be changed to use as analog output.
     */
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);
    set_handlebar_torque(0.0f);

    /*
     * Initialize bicycle. The initial velocity is important as we use it to prime
     * the Kalman gain matrix.
     */
    bicycle_t bicycle(fixed_velocity, dynamics_period);

    /*
     * Initialize HandlebarDynamic object to estimate torque due to handlebar inertia.
     * TODO: naming here is poor
     */
    haptic::HandlebarDynamic handlebar_model(bicycle.model(), sa::HANDLEBAR_INERTIA);

    observer_initializer<observer_t> oi;
    oi.initialize(bicycle);

    chTMObjectInit(&computation_time_measurement);
    chTMObjectInit(&transmission_time_measurement);

    /*
     * Transmit initial message containing gitsha1, model, and observer data.
     * This initial transmission is blocking.
     */
    msg = msg_init;
    msg.timestamp = chVTGetSystemTime();
    message::set_simulation_full_model_observer(&msg, bicycle);
    size_t bytes_written = write_message_to_encode_buffer(msg);

    /* block until ready */
    while ((SDU1.config->usbp->state != USB_ACTIVE) || (SDU1.state != SDU_READY)) {
        chThdSleepMilliseconds(10);
    }

    /* transmit data */
    if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
        chTMStartMeasurementX(&transmission_time_measurement);
        usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
        chTMStopMeasurementX(&transmission_time_measurement);
    }

    /*
     * Normal main() thread activity, in this demo it simulates the bicycle
     * dynamics in real-time (roughly).
     */
    chThdCreateStatic(wa_kinematics_thread, sizeof(wa_kinematics_thread),
            NORMALPRIO - 1, kinematics_thread, static_cast<void*>(&bicycle));
    while (true) {
        systime_t starttime = chVTGetSystemTime();
        chTMStartMeasurementX(&computation_time_measurement);
        constexpr float roll_torque = 0.0f;

        /* get sensor measurements */
        const float kistler_torque = static_cast<float>(
                analog.get_adc12()*2.0f*sa::MAX_KISTLER_TORQUE/4096 -
                sa::MAX_KISTLER_TORQUE);
        const float motor_torque = static_cast<float>(
                analog.get_adc13()*2.0f*sa::MAX_KOLLMORGEN_TORQUE/4096 -
                sa::MAX_KOLLMORGEN_TORQUE);
        const float steer_angle = angle::encoder_count<float>(encoder_steer);
        const float rear_wheel_angle = -angle::encoder_count<float>(encoder_rear_wheel);
        const float v = velocity_filter.output(
                -sa::REAR_WHEEL_RADIUS*(angle::encoder_rate(encoder_rear_wheel)));

        /* yaw angle, just use previous state value */
        const float yaw_angle = angle::wrap(bicycle.pose().yaw);

        /* calculate rider applied torque */
        const float inertia_torque = -handlebar_model.feedback_torque(bicycle.observer().state());
        const float steer_torque = kistler_torque - inertia_torque;

        /* simulate bicycle */
        bicycle.set_v(fixed_velocity);
        bicycle.update_dynamics(roll_torque, steer_torque, yaw_angle, steer_angle, rear_wheel_angle);

        /* generate handlebar torque output */
        const dacsample_t handlebar_torque_dac = set_handlebar_torque(bicycle.handlebar_feedback_torque());
        chTMStopMeasurementX(&computation_time_measurement);

        /* prepare message for transmission */
        msg = msg_init;
        msg.timestamp = starttime;
        message::set_bicycle_input(&msg.input,
                (model_t::input_t() << roll_torque, steer_torque).finished());
        msg.has_input = true;
        message::set_simulation_state(&msg, bicycle);
        message::set_simulation_auxiliary_state(&msg, bicycle);
        if (std::is_same<observer_t, typename observer::Kalman<model_t>>::value) {
            message::set_kalman_gain(&msg.kalman, bicycle.observer());
            msg.has_kalman = true;
        }
        message::set_simulation_actuators(&msg, handlebar_torque_dac);
        message::set_simulation_sensors(&msg,
                analog.get_adc12(), analog.get_adc13(),
                encoder_steer.count(), encoder_rear_wheel.count());
        message::set_simulation_timing(&msg,
                computation_time_measurement.last, transmission_time_measurement.last);
        msg.feedback_torque = bicycle.handlebar_feedback_torque();
        msg.has_feedback_torque = true;
        size_t bytes_written = write_message_to_encode_buffer(msg);

        /* transmit message */
        if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
            chTMStartMeasurementX(&transmission_time_measurement);
            usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
            chTMStopMeasurementX(&transmission_time_measurement);
        }

        const systime_t looptime = MS2ST(static_cast<systime_t>(1000*bicycle.dt()));
        const systime_t sleeptime = looptime + starttime - chVTGetSystemTime();
        if (sleeptime >= looptime) {
            //chDbgAssert(false, "deadline missed");
            continue;
        } else {
            chThdSleep(sleeptime);
        }
    }
}
<|endoftext|>


<commit_before>#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 6/14/2020, 3:53:32 AM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- Solve -----

class Solve
{
  ll W, H, K;

public:
  Solve(ll W, ll H, ll K) : W{W}, H{H}, K{K} {}

  ll answer()
  {
    ll ans{0};
    for (auto s{0LL}; s < W - 1; ++s)
    {
      for (auto y{H - 1}; 2 * K - (H - y) * s + H - 2 >= 0 && y >= 0LL; --y)
      {
        ll tmp{calc(s, y)};
        if (s == 0)
        {
          ans += tmp;
        }
        else
        {
          ans += 2 * tmp;
        }
      }
    }
    return ans;
  }

private:
  ll calc(ll s, ll y)
  {
    ll R{2 * K - (H - y) * s + gcd(H, s) - 2};
    if (R < 0)
    {
      return 0;
    }
    ll ans{min(W - s - 1, R / H)};
    ll x{R / H + 1};
    if (x + s < W && H * x - gcd(H - y, x) - gcd(y, x + s) <= R)
    {
      ++ans;
    }
    return ans;
  }
};

// ----- main() -----

int main()
{
  ll W, H, K;
  cin >> W >> H >> K;
  Solve solve(W, H, K), solve2(H, W, K);
  ll ans{2 * solve.answer() + 2 * solve2.answer()};
  cout << ans << endl;
}
<commit_msg>tried F.cpp to 'F'<commit_after>#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 6/14/2020, 3:53:32 AM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- Solve -----

class Solve
{
  ll W, H, K;

public:
  Solve(ll W, ll H, ll K) : W{W}, H{H}, K{K} {}

  ll answer()
  {
    ll ans{0};
    for (auto s{0LL}; s < W - 1; ++s)
    {
      for (auto y{H - 1}; rhs_bound(s, y) >= 0 && y >= 0LL; --y)
      {
        ll tmp{calc(s, y)};
        if (s == 0)
        {
          ans += tmp;
        }
        else
        {
          ans += 2 * tmp;
        }
      }
    }
    return ans;
  }

private:
  ll rhs(ll s, ll y)
  {
    return 2 * K - s * (H - y) + gcd(s, H) - 2;
  }

  ll rhs_bound(ll s, ll y)
  {
    return 2 * K - s * (H - y) + H - 2;
  }

  ll lhs(ll x, ll s, ll y)
  {
    return H * x - gcd(H - y, x) - gcd(y, x + s);
  }

  ll calc(ll s, ll y)
  {
    ll R{rhs(s, y)};
    if (R < 0)
    {
      return 0;
    }
    ll ans{min(W - s - 1, R / H)};
    ll x{R / H + 1};
    if (x + s < W && lhs(x, s, y) <= R)
    {
      ++ans;
    }
    return ans;
  }
};

// ----- main() -----

int main()
{
  ll W, H, K;
  cin >> W >> H >> K;
  Solve solve(W, H, K), solve2(H, W, K);
  ll ans{2 * solve.answer() + 2 * solve2.answer()};
  cout << ans << endl;
}
<|endoftext|>