<commit_before>#include <iostream>
#include <unistd.h>
#include <chrono>
#include <thread>

#include "../Hardware.h"
#include "Fido/Fido.h"
#include "../../../Connection.h"

// ---------------------- Helper Functions -----------------------
bool isLeftOfLine(Hardware &hardware) {
    int lineVal = hardware.readLine();
    std::cout << "Line val: " << lineVal << "\n";
    return lineVal < 4;
}

// ---------------------- Experiments ----------------------------

void goStraight() {
    double maxRotate = 100;
    double exploration = 0.2;

    Connection connection;
    int receiverNum;
    do {
        receiverNum = atoi(connection.getString().c_str());
    } while(receiverNum != 5 && receiverNum != 6);

    Hardware hardware;

    rl::WireFitQLearn learner = rl::WireFitQLearn(1, 1, 1, 3, 4, {-1}, {1}, 11, new rl::LSInterpolator(), net::Backpropagation(0.01, 0.9, 0.1, 35000), 0.95, 0.4);
    learner.reset();

    std::cout << "Done with initialization\n";
    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({1}, exploration);
        std::cout << "Action: " << action[0] << "\n";

        hardware.goHolonomic(0, 50, action[0]*maxRotate);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        hardware.goHolonomic(0, 0, 0);

        double reward = connection.getReward();
        if(fabs(reward - (-2)) < 0.001) break;
        learner.applyReinforcementToLastAction(reward, {1});
    }

    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({1}, 0.001);
        hardware.goHolonomic(0, 50, action[0]*maxRotate);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        std::cout << "Action: " << action[0] << "\n";
    }
}

void goStraightKiwi() {
    double maxMove = 100;
    double exploration = 0.2;

    Connection connection;
    int receiverNum;
    do {
        receiverNum = atoi(connection.getString().c_str());
    } while(receiverNum != 5 && receiverNum != 6);

    Hardware hardware;

    rl::WireFitQLearn learner = rl::WireFitQLearn(1, 3, 1, 8, 4, {-1, -1, -1}, {1, 1, 1}, 3, new rl::LSInterpolator(), net::Backpropagation(0.01, 0.9, 0.1, 35000), 0.95, 0.4);
    learner.reset();

    std::cout << "Done with initialization\n";
    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({1}, exploration);
        std::cout << "Action: " << action[0] << " " << action[1] << " " << action[2] << "\n";

        hardware.setMotors(action[0]*maxMove, action[1]*maxMove, action[2]*maxMove);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        hardware.setMotors(0, 0, 0);

        double reward = connection.getReward();
        if(fabs(reward - (-2)) < 0.001) break;
        learner.applyReinforcementToLastAction(reward, {1});
    }

    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({1}, 0.001);
        hardware.setMotors(action[0]*maxMove, action[1]*maxMove, action[2]*maxMove);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        std::cout << "Action: " << action[0] << " " << action[1] << " " << action[2] << "\n";
    }
}

void lineFollow() {
    double maxDisplacementComponent = 100;
    double exploration = 0.2;

    Connection connection;
    int receiverNum;
    do {
        receiverNum = atoi(connection.getString().c_str());
    } while(receiverNum != 5 && receiverNum != 6);

    Hardware hardware;

    rl::WireFitQLearn learner = rl::WireFitQLearn(1, 2, 1, 6, 4, {-1, -1}, {1, 1}, 3, new rl::LSInterpolator(), net::Backpropagation(0.01, 0.9, 0.05, 5000), 1, 0.5);
    learner.reset();

    std::cout << "Done with initialization\n";
    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({!isLeftOfLine(hardware) ? -1 : 1}, exploration);
        std::cout << "Action: " << action[0] << "\n";

        hardware.goHolonomic(action[0]*maxDisplacementComponent, action[1]*maxDisplacementComponent, 0);
        std::this_thread::sleep_for(std::chrono::milliseconds(400));
        hardware.goHolonomic(0, 0, 0);

        double reward = connection.getReward();
        if(fabs(reward - (-2)) < 0.001) break;
        learner.applyReinforcementToLastAction(reward, {!isLeftOfLine(hardware) ? -1 : 1});
    }

    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({!isLeftOfLine(hardware) ? -1 : 1}, 0.001);
        hardware.goHolonomic(action[0]*maxDisplacementComponent, action[1]*maxDisplacementComponent, 0);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        std::cout << "Action: " << action[0] << "\n";
    }
}

void lineFollowKiwi() {
    double maxMove = 100;
    double exploration = 0.2;

    Connection connection;
    int receiverNum;
    do {
        receiverNum = atoi(connection.getString().c_str());
    } while(receiverNum != 5 && receiverNum != 6);

    Hardware hardware;

    rl::WireFitQLearn learner = rl::WireFitQLearn(1, 3, 1, 8, 4, {-1, -1, -1}, {1, 1, 1}, 3, new rl::LSInterpolator(), net::Backpropagation(0.01, 0.9, 0.1, 35000), 0.95, 0.4);
    learner.reset();

    std::cout << "Done with initialization\n";
    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({!!isLeftOfLine(hardware) ? -1 : 1}, exploration);
        std::cout << "Action: " << action[0] << " " << action[1] << " " << action[2] << "\n";
        std::cout << "Is left of line: " << isLeftOfLine(hardware) << "\n";

        hardware.setMotors(action[0]*maxMove, action[1]*maxMove, action[2]*maxMove);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        hardware.setMotors(0, 0, 0);

        double reward = connection.getReward();
        if(fabs(reward - (-2)) < 0.001) break;
        learner.applyReinforcementToLastAction(reward, {!isLeftOfLine(hardware) ? -1 : 1});
    }

    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({!isLeftOfLine(hardware) ? -1 : 1}, 0.001);
        hardware.setMotors(action[0]*maxMove, action[1]*maxMove, action[2]*maxMove);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        std::cout << "Action: " << action[0] << " " << action[1] << " " << action[2] << "\n";
    }
}

int main() {
    lineFollow();

    return 0;
}
<commit_msg>kiwi linefollow tweaks<commit_after>#include <iostream>
#include <unistd.h>
#include <chrono>
#include <thread>

#include "../Hardware.h"
#include "Fido/Fido.h"
#include "../../../Connection.h"

// ---------------------- Helper Functions -----------------------
bool isLeftOfLine(Hardware &hardware) {
    int lineVal = hardware.readLine();
    std::cout << "Line val: " << lineVal << "\n";
    return lineVal < 4;
}

// ---------------------- Experiments ----------------------------

void goStraight() {
    double maxRotate = 100;
    double exploration = 0.2;

    Connection connection;
    int receiverNum;
    do {
        receiverNum = atoi(connection.getString().c_str());
    } while(receiverNum != 5 && receiverNum != 6);

    Hardware hardware;

    rl::WireFitQLearn learner = rl::WireFitQLearn(1, 1, 1, 3, 4, {-1}, {1}, 11, new rl::LSInterpolator(), net::Backpropagation(0.01, 0.9, 0.1, 35000), 0.95, 0.4);
    learner.reset();

    std::cout << "Done with initialization\n";
    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({1}, exploration);
        std::cout << "Action: " << action[0] << "\n";

        hardware.goHolonomic(0, 50, action[0]*maxRotate);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        hardware.goHolonomic(0, 0, 0);

        double reward = connection.getReward();
        if(fabs(reward - (-2)) < 0.001) break;
        learner.applyReinforcementToLastAction(reward, {1});
    }

    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({1}, 0.001);
        hardware.goHolonomic(0, 50, action[0]*maxRotate);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        std::cout << "Action: " << action[0] << "\n";
    }
}

void goStraightKiwi() {
    double maxMove = 100;
    double exploration = 0.2;

    Connection connection;
    int receiverNum;
    do {
        receiverNum = atoi(connection.getString().c_str());
    } while(receiverNum != 5 && receiverNum != 6);

    Hardware hardware;

    rl::WireFitQLearn learner = rl::WireFitQLearn(1, 3, 1, 8, 4, {-1, -1, -1}, {1, 1, 1}, 3, new rl::LSInterpolator(), net::Backpropagation(0.01, 0.9, 0.1, 35000), 0.95, 0.4);
    learner.reset();

    std::cout << "Done with initialization\n";
    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({1}, exploration);
        std::cout << "Action: " << action[0] << " " << action[1] << " " << action[2] << "\n";

        hardware.setMotors(action[0]*maxMove, action[1]*maxMove, action[2]*maxMove);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        hardware.setMotors(0, 0, 0);

        double reward = connection.getReward();
        if(fabs(reward - (-2)) < 0.001) break;
        learner.applyReinforcementToLastAction(reward, {1});
    }

    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({1}, 0.001);
        hardware.setMotors(action[0]*maxMove, action[1]*maxMove, action[2]*maxMove);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        std::cout << "Action: " << action[0] << " " << action[1] << " " << action[2] << "\n";
    }
}

void lineFollow() {
    double maxDisplacementComponent = 100;
    double exploration = 0.2;

    Connection connection;
    int receiverNum;
    do {
        receiverNum = atoi(connection.getString().c_str());
    } while(receiverNum != 5 && receiverNum != 6);

    Hardware hardware;

    rl::WireFitQLearn learner = rl::WireFitQLearn(1, 2, 1, 6, 4, {-1, -1}, {1, 1}, 3, new rl::LSInterpolator(), net::Backpropagation(0.01, 0.9, 0.05, 5000), 1, 0);
    learner.reset();

    std::cout << "Done with initialization\n";
    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({!isLeftOfLine(hardware) ? -1 : 1}, exploration);
        std::cout << "Action: " << action[0] << " " << action[1] << "\n";

        hardware.goHolonomic(action[0]*maxDisplacementComponent, action[1]*maxDisplacementComponent, 0);
        std::this_thread::sleep_for(std::chrono::milliseconds(400));
        hardware.goHolonomic(0, 0, 0);

        double reward = connection.getReward();
        if(fabs(reward - (-2)) < 0.001) break;
        learner.applyReinforcementToLastAction(reward, {!isLeftOfLine(hardware) ? -1 : 1});
    }

    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({!isLeftOfLine(hardware) ? -1 : 1}, 0.001);
        hardware.goHolonomic(action[0]*maxDisplacementComponent, action[1]*maxDisplacementComponent, 0);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        std::cout << "Action: " << action[0] << " " << action[1] << "\n";
    }
}

void lineFollowKiwi() {
    double maxMove = 100;
    double exploration = 0.6;

    Connection connection;
    int receiverNum;
    do {
        receiverNum = atoi(connection.getString().c_str());
    } while(receiverNum != 5 && receiverNum != 6);

    Hardware hardware;

    rl::WireFitQLearn learner = rl::WireFitQLearn(1, 3, 1, 8, 4, {-1, -1, -1}, {1, 1, 1}, 6, new rl::LSInterpolator(), net::Backpropagation(0.01, 0.9, 0.05, 5000), 1, 0);
    learner.reset();

    std::cout << "Done with initialization\n";
    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({!!isLeftOfLine(hardware) ? -1 : 1}, exploration);
        std::cout << "Action: " << action[0] << " " << action[1] << " " << action[2] << "\n";
        std::cout << "Is left of line: " << isLeftOfLine(hardware) << "\n";

        hardware.setMotors(action[0]*maxMove, action[1]*maxMove, action[2]*maxMove);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        hardware.setMotors(0, 0, 0);

        double reward = connection.getReward();
        if(fabs(reward - (-2)) < 0.001) break;
        learner.applyReinforcementToLastAction(reward, {!isLeftOfLine(hardware) ? -1 : 1});
    }

    while(true) {
        rl::Action action = learner.chooseBoltzmanAction({!isLeftOfLine(hardware) ? -1 : 1}, 0.001);
        hardware.setMotors(action[0]*maxMove, action[1]*maxMove, action[2]*maxMove);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        std::cout << "Action: " << action[0] << " " << action[1] << " " << action[2] << "\n";
    }
}

int main() {
    lineFollowKiwi();

    return 0;
}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: b3drange.hxx,v $
 *
 *  $Revision: 1.11 $
 *
 *  last change: $Author: pjunck $ $Date: 2004-11-03 08:35:44 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef _BGFX_RANGE_B3DRANGE_HXX
#define _BGFX_RANGE_B3DRANGE_HXX

#ifndef _BGFX_VECTOR_B3DVECTOR_HXX
#include <basegfx/vector/b3dvector.hxx>
#endif
#ifndef _BGFX_POINT_B3DPOINT_HXX
#include <basegfx/point/b3dpoint.hxx>
#endif
#ifndef _BGFX_TUPLE_B3DTUPLE_HXX
#include <basegfx/tuple/b3dtuple.hxx>
#endif

#ifndef _BGFX_RANGE_BASICRANGE_HXX
#include <basegfx/range/basicrange.hxx>
#endif

namespace basegfx
{
    // predeclarations
    class B3IRange;

    class B3DRange
    {
        typedef ::basegfx::BasicRange< double, DoubleTraits >   MyBasicRange;

        MyBasicRange            maRangeX;
        MyBasicRange            maRangeY;
        MyBasicRange            maRangeZ;

    public:
        B3DRange()
        {
        }

        explicit B3DRange(const B3DTuple& rTuple)
        :   maRangeX(rTuple.getX()),
            maRangeY(rTuple.getY()),
            maRangeZ(rTuple.getZ())
        {
        }

        B3DRange(double x1,
                 double y1,
                 double z1,
                 double x2,
                 double y2,
                 double z2)
        :   maRangeX(x1),
            maRangeY(y1),
            maRangeZ(z1)
        {
            maRangeX.expand(x2);
            maRangeY.expand(y2);
            maRangeZ.expand(z2);
        }

        B3DRange(const B3DTuple& rTuple1,
                 const B3DTuple& rTuple2)
        :   maRangeX(rTuple1.getX()),
            maRangeY(rTuple1.getY()),
            maRangeZ(rTuple1.getZ())
        {
            expand(rTuple2);
        }

        B3DRange(const B3DRange& rRange)
        :   maRangeX(rRange.maRangeX),
            maRangeY(rRange.maRangeY),
            maRangeZ(rRange.maRangeZ)
        {
        }

        bool isEmpty() const
        {
            return (
                maRangeX.isEmpty()
                || maRangeY.isEmpty()
                || maRangeZ.isEmpty()
                );
        }

        void reset()
        {
            maRangeX.reset();
            maRangeY.reset();
            maRangeZ.reset();
        }

        bool operator==( const B3DRange& rRange ) const
        {
            return (maRangeX == rRange.maRangeX
                && maRangeY == rRange.maRangeY
                && maRangeZ == rRange.maRangeZ);
        }

        bool operator!=( const B3DRange& rRange ) const
        {
            return (maRangeX != rRange.maRangeX
                || maRangeY != rRange.maRangeY
                || maRangeZ != rRange.maRangeZ);
        }

        void operator=(const B3DRange& rRange)
        {
            maRangeX = rRange.maRangeX;
            maRangeY = rRange.maRangeY;
            maRangeZ = rRange.maRangeZ;
        }

        double getMinX() const
        {
            return maRangeX.getMinimum();
        }

        double getMinY() const
        {
            return maRangeY.getMinimum();
        }

        double getMinZ() const
        {
            return maRangeZ.getMinimum();
        }

        double getMaxX() const
        {
            return maRangeX.getMaximum();
        }

        double getMaxY() const
        {
            return maRangeY.getMaximum();
        }

        double getMaxZ() const
        {
            return maRangeZ.getMaximum();
        }

        double getWidth() const
        {
            return maRangeX.getRange();
        }

        double getHeight() const
        {
            return maRangeY.getRange();
        }

        double getDepth() const
        {
            return maRangeZ.getRange();
        }

        B3DPoint getMinimum() const
        {
            return B3DPoint(
                maRangeX.getMinimum(),
                maRangeY.getMinimum(),
                maRangeZ.getMinimum()
                );
        }

        B3DPoint getMaximum() const
        {
            return B3DPoint(
                maRangeX.getMaximum(),
                maRangeY.getMaximum(),
                maRangeZ.getMaximum()
                );
        }

        B3DVector getRange() const
        {
            return B3DVector(
                maRangeX.getRange(),
                maRangeY.getRange(),
                maRangeZ.getRange()
                );
        }

        B3DPoint getCenter() const
        {
            return B3DPoint(
                maRangeX.getCenter(),
                maRangeY.getCenter(),
                maRangeZ.getCenter()
                );
        }

        bool isInside(const B3DTuple& rTuple) const
        {
            return (
                maRangeX.isInside(rTuple.getX())
                && maRangeY.isInside(rTuple.getY())
                && maRangeZ.isInside(rTuple.getZ())
                );
        }

        bool isInside(const B3DRange& rRange) const
        {
            return (
                maRangeX.isInside(rRange.maRangeX)
                && maRangeY.isInside(rRange.maRangeY)
                && maRangeZ.isInside(rRange.maRangeZ)
                );
        }

        bool overlaps(const B3DRange& rRange) const
        {
            return (
                maRangeX.overlaps(rRange.maRangeX)
                && maRangeY.overlaps(rRange.maRangeY)
                && maRangeZ.overlaps(rRange.maRangeZ)
                );
        }

        void expand(const B3DTuple& rTuple)
        {
            maRangeX.expand(rTuple.getX());
            maRangeY.expand(rTuple.getY());
            maRangeZ.expand(rTuple.getZ());
        }

        void expand(const B3DRange& rRange)
        {
            maRangeX.expand(rRange.maRangeX);
            maRangeY.expand(rRange.maRangeY);
            maRangeZ.expand(rRange.maRangeZ);
        }

        void grow(double fValue)
        {
            maRangeX.grow(fValue);
            maRangeY.grow(fValue);
            maRangeZ.grow(fValue);
        }
    };

    /** Round double to nearest integer for 3D range

        @return the nearest integer for this range
    */
    B3IRange fround(const B3DRange& rRange);
} // end of namespace basegfx

#endif /* _BGFX_RANGE_B3DRANGE_HXX */
<commit_msg>INTEGRATION: CWS presentationengine01 (1.10.12); FILE MERGED 2004/11/17 18:53:56 thb 1.10.12.3: RESYNC: (1.10-1.11); FILE MERGED 2004/09/23 20:19:06 thb 1.10.12.2: #110496# Added intersect method to all range types 2004/09/22 16:58:37 thb 1.10.12.1: #110496# Added coordinate-wise center accessors<commit_after>/*************************************************************************
 *
 *  $RCSfile: b3drange.hxx,v $
 *
 *  $Revision: 1.12 $
 *
 *  last change: $Author: rt $ $Date: 2004-11-26 18:35:24 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef _BGFX_RANGE_B3DRANGE_HXX
#define _BGFX_RANGE_B3DRANGE_HXX

#ifndef _BGFX_VECTOR_B3DVECTOR_HXX
#include <basegfx/vector/b3dvector.hxx>
#endif
#ifndef _BGFX_POINT_B3DPOINT_HXX
#include <basegfx/point/b3dpoint.hxx>
#endif
#ifndef _BGFX_TUPLE_B3DTUPLE_HXX
#include <basegfx/tuple/b3dtuple.hxx>
#endif

#ifndef _BGFX_RANGE_BASICRANGE_HXX
#include <basegfx/range/basicrange.hxx>
#endif

namespace basegfx
{
    // predeclarations
    class B3IRange;

    class B3DRange
    {
        typedef ::basegfx::BasicRange< double, DoubleTraits >   MyBasicRange;

        MyBasicRange            maRangeX;
        MyBasicRange            maRangeY;
        MyBasicRange            maRangeZ;

    public:
        B3DRange()
        {
        }

        explicit B3DRange(const B3DTuple& rTuple)
        :   maRangeX(rTuple.getX()),
            maRangeY(rTuple.getY()),
            maRangeZ(rTuple.getZ())
        {
        }

        B3DRange(double x1,
                 double y1,
                 double z1,
                 double x2,
                 double y2,
                 double z2)
        :   maRangeX(x1),
            maRangeY(y1),
            maRangeZ(z1)
        {
            maRangeX.expand(x2);
            maRangeY.expand(y2);
            maRangeZ.expand(z2);
        }

        B3DRange(const B3DTuple& rTuple1,
                 const B3DTuple& rTuple2)
        :   maRangeX(rTuple1.getX()),
            maRangeY(rTuple1.getY()),
            maRangeZ(rTuple1.getZ())
        {
            expand(rTuple2);
        }

        B3DRange(const B3DRange& rRange)
        :   maRangeX(rRange.maRangeX),
            maRangeY(rRange.maRangeY),
            maRangeZ(rRange.maRangeZ)
        {
        }

        explicit B3DRange(const B3IRange& rRange);

        bool isEmpty() const
        {
            return (
                maRangeX.isEmpty()
                || maRangeY.isEmpty()
                || maRangeZ.isEmpty()
                );
        }

        void reset()
        {
            maRangeX.reset();
            maRangeY.reset();
            maRangeZ.reset();
        }

        bool operator==( const B3DRange& rRange ) const
        {
            return (maRangeX == rRange.maRangeX
                && maRangeY == rRange.maRangeY
                && maRangeZ == rRange.maRangeZ);
        }

        bool operator!=( const B3DRange& rRange ) const
        {
            return (maRangeX != rRange.maRangeX
                || maRangeY != rRange.maRangeY
                || maRangeZ != rRange.maRangeZ);
        }

        void operator=(const B3DRange& rRange)
        {
            maRangeX = rRange.maRangeX;
            maRangeY = rRange.maRangeY;
            maRangeZ = rRange.maRangeZ;
        }

        double getMinX() const
        {
            return maRangeX.getMinimum();
        }

        double getMinY() const
        {
            return maRangeY.getMinimum();
        }

        double getMinZ() const
        {
            return maRangeZ.getMinimum();
        }

        double getMaxX() const
        {
            return maRangeX.getMaximum();
        }

        double getMaxY() const
        {
            return maRangeY.getMaximum();
        }

        double getMaxZ() const
        {
            return maRangeZ.getMaximum();
        }

        double getWidth() const
        {
            return maRangeX.getRange();
        }

        double getHeight() const
        {
            return maRangeY.getRange();
        }

        double getDepth() const
        {
            return maRangeZ.getRange();
        }

        B3DPoint getMinimum() const
        {
            return B3DPoint(
                maRangeX.getMinimum(),
                maRangeY.getMinimum(),
                maRangeZ.getMinimum()
                );
        }

        B3DPoint getMaximum() const
        {
            return B3DPoint(
                maRangeX.getMaximum(),
                maRangeY.getMaximum(),
                maRangeZ.getMaximum()
                );
        }

        B3DVector getRange() const
        {
            return B3DVector(
                maRangeX.getRange(),
                maRangeY.getRange(),
                maRangeZ.getRange()
                );
        }

        B3DPoint getCenter() const
        {
            return B3DPoint(
                maRangeX.getCenter(),
                maRangeY.getCenter(),
                maRangeZ.getCenter()
                );
        }

        double getCenterX() const
        {
            return maRangeX.getCenter();
        }

        double getCenterY() const
        {
            return maRangeY.getCenter();
        }

        double getCenterZ() const
        {
            return maRangeZ.getCenter();
        }

        bool isInside(const B3DTuple& rTuple) const
        {
            return (
                maRangeX.isInside(rTuple.getX())
                && maRangeY.isInside(rTuple.getY())
                && maRangeZ.isInside(rTuple.getZ())
                );
        }

        bool isInside(const B3DRange& rRange) const
        {
            return (
                maRangeX.isInside(rRange.maRangeX)
                && maRangeY.isInside(rRange.maRangeY)
                && maRangeZ.isInside(rRange.maRangeZ)
                );
        }

        bool overlaps(const B3DRange& rRange) const
        {
            return (
                maRangeX.overlaps(rRange.maRangeX)
                && maRangeY.overlaps(rRange.maRangeY)
                && maRangeZ.overlaps(rRange.maRangeZ)
                );
        }

        void expand(const B3DTuple& rTuple)
        {
            maRangeX.expand(rTuple.getX());
            maRangeY.expand(rTuple.getY());
            maRangeZ.expand(rTuple.getZ());
        }

        void expand(const B3DRange& rRange)
        {
            maRangeX.expand(rRange.maRangeX);
            maRangeY.expand(rRange.maRangeY);
            maRangeZ.expand(rRange.maRangeZ);
        }

        void intersect(const B3DRange& rRange)
        {
            maRangeX.intersect(rRange.maRangeX);
            maRangeY.intersect(rRange.maRangeY);
            maRangeZ.intersect(rRange.maRangeZ);
        }

        void grow(double fValue)
        {
            maRangeX.grow(fValue);
            maRangeY.grow(fValue);
            maRangeZ.grow(fValue);
        }
    };

    /** Round double to nearest integer for 3D range

        @return the nearest integer for this range
    */
    B3IRange fround(const B3DRange& rRange);
} // end of namespace basegfx

#endif /* _BGFX_RANGE_B3DRANGE_HXX */
<|endoftext|>


<commit_before>#include <QPainter>
#include <QAbstractEventDispatcher>
#include <QApplication>
#include <QPaintEvent>
#include <QFontMetrics>
#include <qterm.h>
#include <stdio.h>
#include <QKeyEvent>
#include <QTimer>
#include <sys/select.h>
#include <errno.h>
#ifdef __QNX__
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <bps/bps.h>
#ifdef BPS_VERSION
#include <bps/virtualkeyboard.h>
#else
#include <bbsupport/Keyboard>
#include <bbsupport/Notification>
#endif
#endif

#define WIDTH    80
#define HEIGHT    17
#define BLINK_SPEED 1000

QTerm::QTerm(QWidget *parent) : QWidget(parent)
{
    term_create( &terminal );
    term_begin( terminal, WIDTH, HEIGHT, 0 );
    init();
}

QTerm::QTerm(QWidget *parent, term_t terminal) : QWidget(parent)
{
    this->terminal = terminal;
    init();
}

void QTerm::init()
{
    char_width = 0;
    char_height = 0;
    cursor_x = -1;
    cursor_y = -1;
    cursor_on = 1;

    resize(1024, 600);
    term_set_user_data( terminal, this );
    term_register_update( terminal, term_update );
    term_register_cursor( terminal, term_update_cursor );
    term_register_bell( terminal, term_bell );
    notifier = new QSocketNotifier( term_get_file_descriptor(terminal), QSocketNotifier::Read );
    exit_notifier = new QSocketNotifier( term_get_file_descriptor(terminal), QSocketNotifier::Exception );
    cursor_timer = new QTimer( this );
    QObject::connect(notifier, SIGNAL(activated(int)), this, SLOT(terminal_data()));
    QObject::connect(exit_notifier, SIGNAL(activated(int)), this, SLOT(terminate()));
    QObject::connect(cursor_timer, SIGNAL(timeout()), this, SLOT(blink_cursor()));
#ifdef __QNX__
#ifdef BPS_VERSION
    virtualkeyboard_show();
#else
    BlackBerry::Keyboard::instance().show();
#endif
#endif
    cursor_timer->start(BLINK_SPEED);
}

QTerm::~QTerm()
{
    delete notifier;
    delete exit_notifier;
    term_free( terminal );
}

void QTerm::term_bell(term_t handle)
{
#ifdef __QNX__
    char command[] = "msg::play_sound\ndat:json:{\"sound\":\"notification_general\"}";
    int f = open("/pps/services/multimedia/sound/control", O_RDWR);
    write(f, command, sizeof(command));
    ::close(f);
#else
    QApplication::beep();
#endif
}

void QTerm::term_update(term_t handle, int x, int y, int width, int height)
{
    QTerm *term = (QTerm *)term_get_user_data( handle );

    term->update( x * term->char_width, y * term->char_height,
                  width * term->char_width, height * term->char_height + term->char_descent );
}

void QTerm::term_update_cursor(term_t handle, int x, int y)
{
    QTerm *term = (QTerm *)term_get_user_data( handle );

    // Update old cursor location
    term->update( term->cursor_x * term->char_width,
                  term->cursor_y * term->char_height,
                  term->char_width, term->char_height );

    term->cursor_x = x;
    term->cursor_y = y;

    // Update new cursor location
    term->update( term->cursor_x * term->char_width,
                  term->cursor_y * term->char_height,
                  term->char_width, term->char_height );
}

void QTerm::terminal_data()
{
    if( !term_process_child( terminal ) ) {
        exit(0);
    }
}

void QTerm::terminate()
{
    exit(0);
}

void QTerm::blink_cursor()
{
    cursor_on ^= 1;
    update( cursor_x * char_width,
                  cursor_y * char_height,
                  char_width, char_height );
}

void QTerm::paintEvent(QPaintEvent *event)
{
    int i, j, color;
    int new_width;
    int new_height;
    const uint32_t **grid;
    const uint32_t **attribs;
    const uint32_t **colors;
    QPainter painter(this);
    QFont font;

    font.setStyleHint(QFont::TypeWriter);
    font.setFamily("Monospace");
    font.setFixedPitch(true);
    font.setKerning(false);
    painter.setBackgroundMode(Qt::TransparentMode);
    painter.setBrush(QColor(8, 0, 0));
    painter.setFont(font);

    // First erase the grid with its current dimensions
    painter.drawRect(event->rect());

    new_width = painter.fontMetrics().maxWidth();
    // Workaround for a bug in OSX - Dave reports that maxWidth returns 0,
    // when width of different characters returns the correct value
    if( new_width == 0 ) {
        new_width = painter.fontMetrics().width(QChar('X'));
    }
    new_height = painter.fontMetrics().lineSpacing();

    if( char_width != new_width
     || char_height != new_height ) {
        char_width = new_width;
        char_height = new_height;
        char_descent = painter.fontMetrics().descent();
#ifdef __QNX__
        {
            int kbd_height;
#ifdef BPS_VERSION
            virtualkeyboard_get_height( &kbd_height );
#else
            kbd_height = BlackBerry::Keyboard::instance().keyboardHeight();
#endif
            term_resize( terminal, contentsRect().width() / char_width, (contentsRect().height() - kbd_height) / char_height, 0 );
        }
#else
        term_resize( terminal, contentsRect().width() / char_width, contentsRect().height() / char_height, 0 );
#endif
        update( contentsRect() );
        return;
    }

    painter.setPen(QColor(255, 255, 255));
    painter.setBrush(QColor(255, 255, 255));
    grid = term_get_grid( terminal );
    attribs = term_get_attribs( terminal );
    colors = term_get_colours( terminal );
    for( i = 0; i < term_get_height( terminal ); i ++ ) {
        for( j = 0; j < term_get_width( terminal ); j ++ ) {
            if( cursor_on && j == cursor_x && i == cursor_y ) {
                painter.drawRect(j * char_width + 1, i * char_height + 1, char_width - 2, char_height - 2);
                painter.setPen(QColor(0, 0, 0));
                painter.drawText(j * char_width, (i + 1) * char_height - char_descent, QString( QChar( grid[ i ][ j ] ) ) );
                painter.setPen(QColor(255, 255, 255));
            } else {
                color = term_get_fg_color( attribs[ i ][ j ], colors[ i ][ j ] );
                painter.setPen(QColor((color >> 16) & 0xFF, (color >> 8) & 0xFF, color & 0xFF));
                painter.drawText(j * char_width, (i + 1) * char_height - char_descent, QString( QChar( grid[ i ][ j ] ) ) );
            }
        }
    }
}

void QTerm::keyPressEvent(QKeyEvent *event)
{
    switch(event->key()) {
        // FIXME These first two are a workaround for a bug in QT. Remove once it is fixed
        case Qt::Key_CapsLock:
        case Qt::Key_Shift:
            break;
        case Qt::Key_Up:
            term_send_special( terminal, TERM_KEY_UP );
            break;
        case Qt::Key_Down:
            term_send_special( terminal, TERM_KEY_DOWN );
            break;
        case Qt::Key_Right:
            term_send_special( terminal, TERM_KEY_RIGHT );
            break;
        case Qt::Key_Left:
            term_send_special( terminal, TERM_KEY_LEFT );
            break;
        default:
            term_send_data( terminal, event->text().toUtf8().constData(), event->text().count() );
            break;
    }
}

void QTerm::resizeEvent(QResizeEvent *event)
{
    if( char_width != 0 && char_height != 0 ) {
#ifdef __QNX__
        {
            int kbd_height;
#ifdef BPS_VERSION
            virtualkeyboard_get_height( &kbd_height );
#else
            kbd_height = BlackBerry::Keyboard::instance().keyboardHeight();
#endif
            term_resize( terminal, event->size().width() / char_width, (event->size().height() - kbd_height) / char_height, 0 );
        }
#else
        term_resize( terminal, event->size().width() / char_width, event->size().height() / char_height, 0 );
#endif
    }
}

int main(int argc, char *argv[])
{
    term_t terminal;

#ifdef __QNX__
#ifdef BPS_VERSION
    if( bps_initialize() != BPS_SUCCESS ) {
        fprintf(stderr, "Failed to initialize bps (%s)\n", strerror( errno ) );
        exit(1);
    }
#endif
#endif

    if( !term_create( &terminal ) ) {
        fprintf(stderr, "Failed to create terminal (%s)\n", strerror( errno ) );
        exit(1);
    }
    if( !term_begin( terminal, WIDTH, HEIGHT, 0 ) ) {
        fprintf(stderr, "Failed to begin terminal (%s)\n", strerror( errno ) );
        exit(1);
    }
    {
        QCoreApplication::addLibraryPath("app/native/lib");
        QApplication app(argc, argv);

        QTerm term(NULL, terminal);
        term.show();

        return app.exec();
    }
}
<commit_msg>Only resize to 1024x600 on qnx, since that's the playbook resolution. This needs to be fixed for colt<commit_after>#include <QPainter>
#include <QAbstractEventDispatcher>
#include <QApplication>
#include <QPaintEvent>
#include <QFontMetrics>
#include <qterm.h>
#include <stdio.h>
#include <QKeyEvent>
#include <QTimer>
#include <sys/select.h>
#include <errno.h>
#ifdef __QNX__
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <bps/bps.h>
#ifdef BPS_VERSION
#include <bps/virtualkeyboard.h>
#else
#include <bbsupport/Keyboard>
#include <bbsupport/Notification>
#endif
#endif

#define WIDTH    80
#define HEIGHT    17
#define BLINK_SPEED 1000

QTerm::QTerm(QWidget *parent) : QWidget(parent)
{
    term_create( &terminal );
    term_begin( terminal, WIDTH, HEIGHT, 0 );
    init();
}

QTerm::QTerm(QWidget *parent, term_t terminal) : QWidget(parent)
{
    this->terminal = terminal;
    init();
}

void QTerm::init()
{
    char_width = 0;
    char_height = 0;
    cursor_x = -1;
    cursor_y = -1;
    cursor_on = 1;

#ifdef __QNX__
    resize(1024, 600);
#endif
    term_set_user_data( terminal, this );
    term_register_update( terminal, term_update );
    term_register_cursor( terminal, term_update_cursor );
    term_register_bell( terminal, term_bell );
    notifier = new QSocketNotifier( term_get_file_descriptor(terminal), QSocketNotifier::Read );
    exit_notifier = new QSocketNotifier( term_get_file_descriptor(terminal), QSocketNotifier::Exception );
    cursor_timer = new QTimer( this );
    QObject::connect(notifier, SIGNAL(activated(int)), this, SLOT(terminal_data()));
    QObject::connect(exit_notifier, SIGNAL(activated(int)), this, SLOT(terminate()));
    QObject::connect(cursor_timer, SIGNAL(timeout()), this, SLOT(blink_cursor()));
#ifdef __QNX__
#ifdef BPS_VERSION
    virtualkeyboard_show();
#else
    BlackBerry::Keyboard::instance().show();
#endif
#endif
    cursor_timer->start(BLINK_SPEED);
}

QTerm::~QTerm()
{
    delete notifier;
    delete exit_notifier;
    term_free( terminal );
}

void QTerm::term_bell(term_t handle)
{
#ifdef __QNX__
    char command[] = "msg::play_sound\ndat:json:{\"sound\":\"notification_general\"}";
    int f = open("/pps/services/multimedia/sound/control", O_RDWR);
    write(f, command, sizeof(command));
    ::close(f);
#else
    QApplication::beep();
#endif
}

void QTerm::term_update(term_t handle, int x, int y, int width, int height)
{
    QTerm *term = (QTerm *)term_get_user_data( handle );

    term->update( x * term->char_width, y * term->char_height,
                  width * term->char_width, height * term->char_height + term->char_descent );
}

void QTerm::term_update_cursor(term_t handle, int x, int y)
{
    QTerm *term = (QTerm *)term_get_user_data( handle );

    // Update old cursor location
    term->update( term->cursor_x * term->char_width,
                  term->cursor_y * term->char_height,
                  term->char_width, term->char_height );

    term->cursor_x = x;
    term->cursor_y = y;

    // Update new cursor location
    term->update( term->cursor_x * term->char_width,
                  term->cursor_y * term->char_height,
                  term->char_width, term->char_height );
}

void QTerm::terminal_data()
{
    if( !term_process_child( terminal ) ) {
        exit(0);
    }
}

void QTerm::terminate()
{
    exit(0);
}

void QTerm::blink_cursor()
{
    cursor_on ^= 1;
    update( cursor_x * char_width,
                  cursor_y * char_height,
                  char_width, char_height );
}

void QTerm::paintEvent(QPaintEvent *event)
{
    int i, j, color;
    int new_width;
    int new_height;
    const uint32_t **grid;
    const uint32_t **attribs;
    const uint32_t **colors;
    QPainter painter(this);
    QFont font;

    font.setStyleHint(QFont::TypeWriter);
    font.setFamily("Monospace");
    font.setFixedPitch(true);
    font.setKerning(false);
    painter.setBackgroundMode(Qt::TransparentMode);
    painter.setBrush(QColor(8, 0, 0));
    painter.setFont(font);

    // First erase the grid with its current dimensions
    painter.drawRect(event->rect());

    new_width = painter.fontMetrics().maxWidth();
    // Workaround for a bug in OSX - Dave reports that maxWidth returns 0,
    // when width of different characters returns the correct value
    if( new_width == 0 ) {
        new_width = painter.fontMetrics().width(QChar('X'));
    }
    new_height = painter.fontMetrics().lineSpacing();

    if( char_width != new_width
     || char_height != new_height ) {
        char_width = new_width;
        char_height = new_height;
        char_descent = painter.fontMetrics().descent();
#ifdef __QNX__
        {
            int kbd_height;
#ifdef BPS_VERSION
            virtualkeyboard_get_height( &kbd_height );
#else
            kbd_height = BlackBerry::Keyboard::instance().keyboardHeight();
#endif
            term_resize( terminal, contentsRect().width() / char_width, (contentsRect().height() - kbd_height) / char_height, 0 );
        }
#else
        term_resize( terminal, contentsRect().width() / char_width, contentsRect().height() / char_height, 0 );
#endif
        update( contentsRect() );
        return;
    }

    painter.setPen(QColor(255, 255, 255));
    painter.setBrush(QColor(255, 255, 255));
    grid = term_get_grid( terminal );
    attribs = term_get_attribs( terminal );
    colors = term_get_colours( terminal );
    for( i = 0; i < term_get_height( terminal ); i ++ ) {
        for( j = 0; j < term_get_width( terminal ); j ++ ) {
            if( cursor_on && j == cursor_x && i == cursor_y ) {
                painter.drawRect(j * char_width + 1, i * char_height + 1, char_width - 2, char_height - 2);
                painter.setPen(QColor(0, 0, 0));
                painter.drawText(j * char_width, (i + 1) * char_height - char_descent, QString( QChar( grid[ i ][ j ] ) ) );
                painter.setPen(QColor(255, 255, 255));
            } else {
                color = term_get_fg_color( attribs[ i ][ j ], colors[ i ][ j ] );
                painter.setPen(QColor((color >> 16) & 0xFF, (color >> 8) & 0xFF, color & 0xFF));
                painter.drawText(j * char_width, (i + 1) * char_height - char_descent, QString( QChar( grid[ i ][ j ] ) ) );
            }
        }
    }
}

void QTerm::keyPressEvent(QKeyEvent *event)
{
    switch(event->key()) {
        // FIXME These first two are a workaround for a bug in QT. Remove once it is fixed
        case Qt::Key_CapsLock:
        case Qt::Key_Shift:
            break;
        case Qt::Key_Up:
            term_send_special( terminal, TERM_KEY_UP );
            break;
        case Qt::Key_Down:
            term_send_special( terminal, TERM_KEY_DOWN );
            break;
        case Qt::Key_Right:
            term_send_special( terminal, TERM_KEY_RIGHT );
            break;
        case Qt::Key_Left:
            term_send_special( terminal, TERM_KEY_LEFT );
            break;
        default:
            term_send_data( terminal, event->text().toUtf8().constData(), event->text().count() );
            break;
    }
}

void QTerm::resizeEvent(QResizeEvent *event)
{
    if( char_width != 0 && char_height != 0 ) {
#ifdef __QNX__
        {
            int kbd_height;
#ifdef BPS_VERSION
            virtualkeyboard_get_height( &kbd_height );
#else
            kbd_height = BlackBerry::Keyboard::instance().keyboardHeight();
#endif
            term_resize( terminal, event->size().width() / char_width, (event->size().height() - kbd_height) / char_height, 0 );
        }
#else
        term_resize( terminal, event->size().width() / char_width, event->size().height() / char_height, 0 );
#endif
    }
}

int main(int argc, char *argv[])
{
    term_t terminal;

#ifdef __QNX__
#ifdef BPS_VERSION
    if( bps_initialize() != BPS_SUCCESS ) {
        fprintf(stderr, "Failed to initialize bps (%s)\n", strerror( errno ) );
        exit(1);
    }
#endif
#endif

    if( !term_create( &terminal ) ) {
        fprintf(stderr, "Failed to create terminal (%s)\n", strerror( errno ) );
        exit(1);
    }
    if( !term_begin( terminal, WIDTH, HEIGHT, 0 ) ) {
        fprintf(stderr, "Failed to begin terminal (%s)\n", strerror( errno ) );
        exit(1);
    }
    {
        QCoreApplication::addLibraryPath("app/native/lib");
        QApplication app(argc, argv);

        QTerm term(NULL, terminal);
        term.show();

        return app.exec();
    }
}
<|endoftext|>


<commit_before>
#include "../HuboJmc.hpp"
#include "../HuboCanId.hpp"
#include "HuboState/State.hpp"
#include "HuboCmd/Aggregator.hpp"

using namespace HuboCan;

Hubo2PlusBasicJmc::Hubo2PlusBasicJmc()
{
    _startup = true;
}

void Hubo2PlusBasicJmc::update()
{
    if(NULL == _pump)
        return;

    _cycle_reset();

    if(_startup)
    {
        // TODO: handle any startup routines here
        // such as grabbing the first status reading for the JMC

        _startup = false;
    }

    if(_aux_commands.size() > 0)
    {
        std::cout << "AUX COMMAND" << std::endl;
        _process_auxiliary_commands();
        // If there are auxiliary commands that need to be handled, we put off requesting
        // encoder readings and sending position commands in order to reduce the load on
        // the CAN bus
    }
    else
    {
        _request_encoder_readings();
        _send_reference_commands();
    }
}

void Hubo2PlusBasicJmc::_cycle_reset()
{
    for(size_t i=0; i<joints.size(); ++i)
    {
        if(joints[i]->updated == false)
        {
            ++(joints[i]->dropped_count);
        }
    }
}

void Hubo2PlusBasicJmc::_request_encoder_readings()
{
    _frame.can_id   = CMD_BYTE;

    _frame.data[0]  = info.hardware_index;
    _frame.data[1]  = GET_ENCODER;
    _frame.data[2] = 0;

    _frame.can_dlc = 3;

    for(size_t i=0; i<joints.size(); ++i)
    {
        joints[i]->updated = false;
        ++joints[i]->expected_replies;
    }

    _pump->add_frame(_frame, info.can_channel, 1);
}

void Hubo2PlusBasicJmc::_send_reference_commands()
{
    for(size_t i=0; i<joints.size(); ++i)
    {
        hubo_joint_cmd_t& cmd = _agg->joint(joints[i]->info.software_index);
        if(cmd.mode == HUBO_CMD_RIGID)
        {
            _handle_rigid_reference_cmd();
            return; // All joints' reference commands get sent out with a single CAN frame
                    // so we quit as soon as a frame has been sent out
        }
    }
}

void Hubo2PlusBasicJmc::_handle_rigid_reference_cmd()
{
    if(joints.size() > 2)
    {
        std::cout << "Hubo2PlusBasicJmc named '" << info.name
                  << "' expected at most two joints, but instead has "
                  << joints.size() << std::endl;
        _pump->report_error();
        return;
    }

    can_frame_t frame; memset(&frame, 0, sizeof(frame));
    frame.can_id = REFERENCE_CMD + info.hardware_index;

    for(size_t i=0; i<joints.size(); ++i)
    {
        hubo_joint_cmd_t& cmd = _agg->joint(joints[i]->info.software_index);
        unsigned long reference = sign_convention_converter(
                    joints[i]->radian2encoder(cmd.position));

        for(size_t j=0; j<3; ++j)
        {
            frame.data[j + 3*i] = long_to_bytes(reference, j);
        }
        std::cout << joints[i]->info.name << ": " << cmd.position << " -> " << reference << std::endl;
    }
    frame.can_dlc = 6;

    _pump->add_frame(frame, info.can_channel);
}

unsigned long Hubo2PlusBasicJmc::sign_convention_converter(int encoder_value)
{
    if(encoder_value < 0)
        return (unsigned long)( ((-encoder_value)&0x7FFFFF) | (1<<23) );

    return (unsigned long)encoder_value;
}

bool Hubo2PlusBasicJmc::decode(const can_frame_t& frame, size_t channel)
{
    if( channel != info.can_channel )
        return false;

    if( frame.can_id - ENCODER_REPLY == info.hardware_index )
    {
        return _decode_encoder_reading(frame);
    }
    else if( frame.can_id - STATUS_REPORT == info.hardware_index )
    {
        return _decode_status_reading(frame);
    }

    return false;
}

bool Hubo2PlusBasicJmc::_decode_encoder_reading(const can_frame_t& frame)
{
    if(frame.can_dlc == 8)
    {
        for(size_t i=0; i < joints.size(); ++i)
        {
            int32_t encoder = 0;
            for(int j=3; j >= 0; --j)
            {
                encoder = (encoder << 8) + frame.data[j + i*4];
            }

            size_t joint_index = joints[i]->info.software_index;
            _state->joints[joint_index].position =
                        joints[i]->encoder2radian(encoder);

            // TODO: Decide if velocity should be computed here

            joints[i]->updated = true;
            ++joints[i]->received_replies;
        }
        return true;
    }
    return false;
}

bool Hubo2PlusBasicJmc::_decode_status_reading(const can_frame_t& frame)
{
    // TODO: Check can_dlc?

    for(size_t i=0; i<joints.size(); ++i)
    {
        size_t jnt = joints[i]->info.software_index;
        hubo_joint_status_t& status = _state->joints[jnt].status;

        uint8_t byte = frame.data[4*i+0];
        status.driver_on    = (byte>>0) & 0x01;
        status.control_on   = (byte>>1) & 0x01;
        status.control_mode = (byte>>2) & 0x01;
        status.limit_switch = (byte>>3) & 0x01;
        status.home_flag    = (byte>>4) & 0x0F;

        byte = frame.data[4*i+1];
        status.error.jam            = (byte>>0) & 0x01;
        status.error.pwm_saturated  = (byte>>1) & 0x01;
        status.error.big            = (byte>>2) & 0x01;
        status.error.encoder        = (byte>>3) & 0x01;
        status.error.driver_fault   = (byte>>4) & 0x01;
        status.error.motor_fail_0   = (byte>>5) & 0x01;
        status.error.motor_fail_1   = (byte>>6) & 0x01;

        byte = frame.data[4*i+2];
        status.error.min_position   = (byte>>0) & 0x01;
        status.error.max_position   = (byte>>1) & 0x01;
        status.error.velocity       = (byte>>2) & 0x01;
        status.error.acceleration   = (byte>>3) & 0x01;
        status.error.temperature    = (byte>>4) & 0x01;
    }

    return true;
}

void Hubo2PlusBasicJmc::_process_auxiliary_commands()
{
    // TODO: Consider only doing one per cycle?
    while(_aux_commands.size() > 0)
    {
        _handle_auxiliary_command(_aux_commands.back());
        _aux_commands.pop_back();
    }
}

void Hubo2PlusBasicJmc::_handle_auxiliary_command(const hubo_aux_cmd_t& cmd)
{
    switch(cmd.id)
    {
        case HOME_JOINT:
            _handle_home_joint(cmd); break;
        case HOME_ALL_JOINTS:
            _handle_home_all_joints(); break;

        default:
            std::cerr << "Unknown/Unsupported auxiliary command type: " << cmd.id << std::endl;
            break;
    }
}

void Hubo2PlusBasicJmc::_handle_home_joint(const hubo_aux_cmd_t& cmd)
{
    if(cmd.joint >= joints.size())
    {
        std::cerr << "Requested homing for invalid joint on " << info.name << " ("
                  << cmd.joint << ". Max joint size is " << joints.size() << std::endl;
        return;
    }
    std::cout << "Sending home command for joint " << joints[cmd.joint]->info.name
    << " (" << joints[cmd.joint]->info.software_index << ") " << " on board " << info.name
    << " (" << info.hardware_index << ")" << std::endl;

    memset(&_frame, 0, sizeof(_frame));

    _frame.can_id   = CMD_BYTE;

    _frame.data[0]  = info.hardware_index;
    _frame.data[1]  = GOTO_HOME;
    _frame.data[2]  = ((cmd.joint+1) << 4) | ( 0x01 << 1 );
    // Why do we add 1 to the joint index? This does not seem necessary according to
    // the CAN documentation. Does the firmware index the joints starting at 1 instead
    // of 0? According to experimental observation, IT DOES!

    // ( 0x01 << 1 ) is used to specify that we want the joint to home according to
    // the settings which are stored in the firmware and to ignore all remaining bytes
    // in the CAN frame.

    // All other bytes are ignored, so we will leave them as 0.
    _frame.can_dlc = 8;

    _pump->add_frame(_frame, info.can_channel);

    // TODO: Decide what other bookkeeping should be done when a joint gets homed.
}

void Hubo2PlusBasicJmc::_handle_home_all_joints()
{
    for(size_t i=0; i<joints.size(); ++i)
    {
        std::cout << "Sending home command for joint " << joints[i]->info.name
        << " (" << joints[i]->info.software_index << ") " << " \ton board " << info.name
        << " (" << info.hardware_index << ")" << std::endl;
    }

    memset(&_frame, 0, sizeof(_frame));

    _frame.can_id   = CMD_BYTE;

    _frame.data[0]  = info.hardware_index;
    _frame.data[1]  = GOTO_HOME;
    _frame.data[2]  = ( 0x0F << 4 ) | ( 0x01 << 1 );

    // ( 0x01 << 1 ) is used to specify that we want the joint to home according to
    // the settings which are stored in the firmware and to ignore all remaining bytes
    // in the CAN frame.

    // All other bytes are ignored, so we will leave them as 0.
    _frame.can_dlc = 8;

    _pump->add_frame(_frame, info.can_channel);

    // TODO: Decide what other bookkeeping should be done when a joint gets homed.
}

<commit_msg>debugging<commit_after>
#include "../HuboJmc.hpp"
#include "../HuboCanId.hpp"
#include "HuboState/State.hpp"
#include "HuboCmd/Aggregator.hpp"

using namespace HuboCan;

Hubo2PlusBasicJmc::Hubo2PlusBasicJmc()
{
    _startup = true;
}

void Hubo2PlusBasicJmc::update()
{
    if(NULL == _pump)
        return;

    _cycle_reset();

    if(_startup)
    {
        // TODO: handle any startup routines here
        // such as grabbing the first status reading for the JMC

        _startup = false;
    }

    if(_aux_commands.size() > 0)
    {
        std::cout << "AUX COMMAND" << std::endl;
        _process_auxiliary_commands();
        // If there are auxiliary commands that need to be handled, we put off requesting
        // encoder readings and sending position commands in order to reduce the load on
        // the CAN bus
    }
    else
    {
        _request_encoder_readings();
        _send_reference_commands();
    }
}

void Hubo2PlusBasicJmc::_cycle_reset()
{
    for(size_t i=0; i<joints.size(); ++i)
    {
        if(joints[i]->updated == false)
        {
            ++(joints[i]->dropped_count);
        }
    }
}

void Hubo2PlusBasicJmc::_request_encoder_readings()
{
    _frame.can_id   = CMD_BYTE;

    _frame.data[0]  = info.hardware_index;
    _frame.data[1]  = GET_ENCODER;
    _frame.data[2] = 0;

    _frame.can_dlc = 3;

    for(size_t i=0; i<joints.size(); ++i)
    {
        joints[i]->updated = false;
        ++joints[i]->expected_replies;
    }

    _pump->add_frame(_frame, info.can_channel, 1);
}

void Hubo2PlusBasicJmc::_send_reference_commands()
{
    for(size_t i=0; i<joints.size(); ++i)
    {
        hubo_joint_cmd_t& cmd = _agg->joint(joints[i]->info.software_index);
        if(cmd.mode == HUBO_CMD_RIGID)
        {
            _handle_rigid_reference_cmd();
            return; // All joints' reference commands get sent out with a single CAN frame
                    // so we quit as soon as a frame has been sent out
        }
    }
}

void Hubo2PlusBasicJmc::_handle_rigid_reference_cmd()
{
    if(joints.size() > 2)
    {
        std::cout << "Hubo2PlusBasicJmc named '" << info.name
                  << "' expected at most two joints, but instead has "
                  << joints.size() << std::endl;
        _pump->report_error();
        return;
    }

    can_frame_t frame; memset(&frame, 0, sizeof(frame));
    frame.can_id = REFERENCE_CMD + info.hardware_index;

    for(size_t i=0; i<joints.size(); ++i)
    {
        hubo_joint_cmd_t& cmd = _agg->joint(joints[i]->info.software_index);
        unsigned long reference = sign_convention_converter(
                    joints[i]->radian2encoder(cmd.position));

        for(size_t j=0; j<3; ++j)
        {
            frame.data[j + 3*i] = long_to_bytes(reference, j);
        }
        std::cout << joints[i]->info.name << ": " << cmd.position << " -> " << reference
                  << "\tlast:" << _state->joints[joints[i]->info.software_index].position << std::endl;
    }
    frame.can_dlc = 6;

    _pump->add_frame(frame, info.can_channel);
}

unsigned long Hubo2PlusBasicJmc::sign_convention_converter(int encoder_value)
{
    if(encoder_value < 0)
        return (unsigned long)( ((-encoder_value)&0x7FFFFF) | (1<<23) );

    return (unsigned long)encoder_value;
}

bool Hubo2PlusBasicJmc::decode(const can_frame_t& frame, size_t channel)
{
    if( channel != info.can_channel )
        return false;

    if( frame.can_id - ENCODER_REPLY == info.hardware_index )
    {
        return _decode_encoder_reading(frame);
    }
    else if( frame.can_id - STATUS_REPORT == info.hardware_index )
    {
        return _decode_status_reading(frame);
    }

    return false;
}

bool Hubo2PlusBasicJmc::_decode_encoder_reading(const can_frame_t& frame)
{
    if(frame.can_dlc == 8)
    {
        for(size_t i=0; i < joints.size(); ++i)
        {
            int32_t encoder = 0;
            for(int j=3; j >= 0; --j)
            {
                encoder = (encoder << 8) + frame.data[j + i*4];
            }

            size_t joint_index = joints[i]->info.software_index;
            _state->joints[joint_index].position =
                        joints[i]->encoder2radian(encoder);

            // TODO: Decide if velocity should be computed here

            joints[i]->updated = true;
            ++joints[i]->received_replies;
        }
        return true;
    }
    return false;
}

bool Hubo2PlusBasicJmc::_decode_status_reading(const can_frame_t& frame)
{
    // TODO: Check can_dlc?

    for(size_t i=0; i<joints.size(); ++i)
    {
        size_t jnt = joints[i]->info.software_index;
        hubo_joint_status_t& status = _state->joints[jnt].status;

        uint8_t byte = frame.data[4*i+0];
        status.driver_on    = (byte>>0) & 0x01;
        status.control_on   = (byte>>1) & 0x01;
        status.control_mode = (byte>>2) & 0x01;
        status.limit_switch = (byte>>3) & 0x01;
        status.home_flag    = (byte>>4) & 0x0F;

        byte = frame.data[4*i+1];
        status.error.jam            = (byte>>0) & 0x01;
        status.error.pwm_saturated  = (byte>>1) & 0x01;
        status.error.big            = (byte>>2) & 0x01;
        status.error.encoder        = (byte>>3) & 0x01;
        status.error.driver_fault   = (byte>>4) & 0x01;
        status.error.motor_fail_0   = (byte>>5) & 0x01;
        status.error.motor_fail_1   = (byte>>6) & 0x01;

        byte = frame.data[4*i+2];
        status.error.min_position   = (byte>>0) & 0x01;
        status.error.max_position   = (byte>>1) & 0x01;
        status.error.velocity       = (byte>>2) & 0x01;
        status.error.acceleration   = (byte>>3) & 0x01;
        status.error.temperature    = (byte>>4) & 0x01;
    }

    return true;
}

void Hubo2PlusBasicJmc::_process_auxiliary_commands()
{
    // TODO: Consider only doing one per cycle?
    while(_aux_commands.size() > 0)
    {
        _handle_auxiliary_command(_aux_commands.back());
        _aux_commands.pop_back();
    }
}

void Hubo2PlusBasicJmc::_handle_auxiliary_command(const hubo_aux_cmd_t& cmd)
{
    switch(cmd.id)
    {
        case HOME_JOINT:
            _handle_home_joint(cmd); break;
        case HOME_ALL_JOINTS:
            _handle_home_all_joints(); break;

        default:
            std::cerr << "Unknown/Unsupported auxiliary command type: " << cmd.id << std::endl;
            break;
    }
}

void Hubo2PlusBasicJmc::_handle_home_joint(const hubo_aux_cmd_t& cmd)
{
    if(cmd.joint >= joints.size())
    {
        std::cerr << "Requested homing for invalid joint on " << info.name << " ("
                  << cmd.joint << ". Max joint size is " << joints.size() << std::endl;
        return;
    }
    std::cout << "Sending home command for joint " << joints[cmd.joint]->info.name
    << " (" << joints[cmd.joint]->info.software_index << ") " << " on board " << info.name
    << " (" << info.hardware_index << ")" << std::endl;

    memset(&_frame, 0, sizeof(_frame));

    _frame.can_id   = CMD_BYTE;

    _frame.data[0]  = info.hardware_index;
    _frame.data[1]  = GOTO_HOME;
    _frame.data[2]  = ((cmd.joint+1) << 4) | ( 0x01 << 1 );
    // Why do we add 1 to the joint index? This does not seem necessary according to
    // the CAN documentation. Does the firmware index the joints starting at 1 instead
    // of 0? According to experimental observation, IT DOES!

    // ( 0x01 << 1 ) is used to specify that we want the joint to home according to
    // the settings which are stored in the firmware and to ignore all remaining bytes
    // in the CAN frame.

    // All other bytes are ignored, so we will leave them as 0.
    _frame.can_dlc = 8;

    _pump->add_frame(_frame, info.can_channel);

    // TODO: Decide what other bookkeeping should be done when a joint gets homed.
}

void Hubo2PlusBasicJmc::_handle_home_all_joints()
{
    for(size_t i=0; i<joints.size(); ++i)
    {
        std::cout << "Sending home command for joint " << joints[i]->info.name
        << " (" << joints[i]->info.software_index << ") " << " \ton board " << info.name
        << " (" << info.hardware_index << ")" << std::endl;
    }

    memset(&_frame, 0, sizeof(_frame));

    _frame.can_id   = CMD_BYTE;

    _frame.data[0]  = info.hardware_index;
    _frame.data[1]  = GOTO_HOME;
    _frame.data[2]  = ( 0x0F << 4 ) | ( 0x01 << 1 );

    // ( 0x01 << 1 ) is used to specify that we want the joint to home according to
    // the settings which are stored in the firmware and to ignore all remaining bytes
    // in the CAN frame.

    // All other bytes are ignored, so we will leave them as 0.
    _frame.can_dlc = 8;

    _pump->add_frame(_frame, info.can_channel);

    // TODO: Decide what other bookkeeping should be done when a joint gets homed.
}

<|endoftext|>


<commit_before>// Ylikuutio - A 3D game and simulation engine.
//
// Copyright (C) 2015-2019 Antti Nuortimo.
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Affero General Public License as
// published by the Free Software Foundation, either version 3 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Affero General Public License for more details.
//
// You should have received a copy of the GNU Affero General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.

#include "audio_master.hpp"
#include "code/ylikuutio/ontology/universe.hpp"

#include "SDL.h"

// Include standard headers
#include <iostream>      // std::cout, std::cin, std::cerr
#include <list>          // std::list
#include <stdint.h>      // uint32_t etc.
#include <string>        // std::string
#include <unordered_map> // std::unordered_map

namespace yli
{
    namespace audio
    {
        yli::audio::AudioMaster* AudioMaster::audio_master;

        AudioMaster::AudioMaster(yli::ontology::Universe* const universe)
        {
            // constructor.
            this->universe = universe;

            this->wav_pointer = nullptr;
            SDL_AtomicSet(&this->remaining_length, 0);

            this->current_playlist = ""; // no current playlist.
            this->loop = true;           // loop playlist.

            this->audio_spec.freq = 44100; // 44100 Hz.
            this->audio_spec.format = AUDIO_F32;
            this->audio_spec.channels = 2;
            this->audio_spec.silence = 0;        // dummy value.
            this->audio_spec.padding = 0;        // dummy value.
            this->audio_spec.samples = 4096;
            this->audio_spec.size = 0;           // dummy value.
            this->audio_spec.callback = yli::audio::AudioMaster::play_audio_callback;
            this->audio_spec.userdata = nullptr; // this is a pointer that is passed to the callback.

            this->audio_master = this;   // `this` is the `AudioMaster`. Do not create more than 1 `AudioMaster`!
        }

        AudioMaster::~AudioMaster()
        {
            // destructor.

            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                SDL_CloseAudioDevice(this->device);
            }
        }

        bool AudioMaster::load_and_play(const std::string& audio_file)
        {
            if (this->wav_buffer_pointer_map.count(audio_file) == 1)
            {
                // There is already a sound with that filename. It's not OK.
                return false;
            }

            // There's no sound with that filename loaded yet, so load it now.
            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                SDL_AudioSpec wav_spec = SDL_AudioSpec();
                uint32_t wav_length;
                uint8_t* wav_buffer;

                if (SDL_LoadWAV(audio_file.c_str(), &wav_spec, &wav_buffer, &wav_length) == NULL)
                {
                    // Loading the sound failed.
                    std::cerr << "Loading WAV file " << audio_file << " failed.\n";
                    return false;
                }

                // https://wiki.libsdl.org/SDL_OpenAudioDevice
                wav_spec.channels = 2;
                wav_spec.samples = 4096;
                wav_spec.callback = yli::audio::AudioMaster::play_audio_callback;

                this->device = SDL_OpenAudioDevice(NULL, 0, &wav_spec, &this->audio_spec, SDL_AUDIO_ALLOW_FORMAT_CHANGE);

                if (this->device == 0)
                {
                    SDL_Log("Failed to open audio: %s", SDL_GetError());
                }
                else
                {
                    if (this->audio_spec.format != wav_spec.format)
                    {
                        SDL_Log("Float32 audio format was not available.");
                    }
                }

                this->wav_pointer = wav_buffer;
                SDL_AtomicSet(&this->remaining_length, wav_length);
                this->wav_buffer_pointer_map[audio_file] = wav_buffer;
                SDL_PauseAudioDevice(this->device, 0); // start playing.
            }

            return true;
        }

        void AudioMaster::unload(const std::string& audio_file)
        {
            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                if (this->wav_buffer_pointer_map.count(audio_file) == 1)
                {
                    if (*current_playlist_sound_iterator == audio_file)
                    {
                        // Stop the sound before unloading it.
                        SDL_PauseAudioDevice(this->device, 1); // stop playing.
                    }

                    SDL_FreeWAV(this->wav_buffer_pointer_map[audio_file]);
                    this->wav_buffer_pointer_map.erase(audio_file);
                }
            }
        }

        void AudioMaster::add_to_playlist(const std::string& playlist, const std::string& audio_file)
        {
            // Playlist must have a name. Empty string is not accepted.
            if (!playlist.empty() && this->playlist_map.count(playlist) == 0)
            {
                this->playlist_map[playlist] = std::list<std::string>();
            }

            this->playlist_map[playlist].push_back(audio_file);
        }

        void AudioMaster::remove_from_playlist(const std::string& playlist, const std::string& audio_file)
        {
            if (this->playlist_map.count(playlist) == 1)
            {
                for (auto it = this->playlist_map[playlist].begin(); it != this->playlist_map[playlist].end(); )
                {
                    if (it->compare(audio_file) == 0)
                    {
                        this->playlist_map[playlist].erase(it--);
                    }

                    it++;
                }
            }
        }

        void AudioMaster::play_playlist(const std::string& playlist)
        {
            // This function starts playing the playlist "from the top".
            // Playlist must have a name. Empty string is not accepted.
            if (!playlist.empty() && this->playlist_map.count(playlist) == 1 && this->playlist_map.size() > 0)
            {
                this->current_playlist = playlist;
                this->current_playlist_sound_iterator = this->playlist_map[playlist].begin();
                this->load_and_play(*this->current_playlist_sound_iterator);
            }
        }

        void AudioMaster::update()
        {
            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                // This function checks if a playlist is running.
                // if yes, then if the the previous sound has ended,
                // its memory gets freed and a new sound gets started.
                if (this->current_playlist.size() > 0)
                {
                    if (this->wav_buffer_pointer_map.count(*this->current_playlist_sound_iterator) == 1)
                    {
                        // OK, there is a sound which might be playing, so let's check its status.
                        const int remaining_length = this->get_remaining_length();

                        if (remaining_length == 0)
                        {
                            SDL_PauseAudioDevice(this->device, 1); // stop playing.
                            this->unload(*this->current_playlist_sound_iterator);

                            // play the next sound.

                            if (++this->current_playlist_sound_iterator != this->playlist_map[this->current_playlist].end())
                            {
                                // Next sound.
                                this->load_and_play(*this->current_playlist_sound_iterator);
                            }
                            else
                            {
                                // The previous sound was the last sound.
                                if (this->loop)
                                {
                                    // Start from the first.
                                    this->current_playlist_sound_iterator = this->playlist_map[this->current_playlist].begin();
                                    this->load_and_play(*this->current_playlist_sound_iterator);
                                }
                                else
                                {
                                    this->current_playlist = ""; // no current playlist.
                                }
                            }
                        }
                    }
                }
            }
        }

        void AudioMaster::pause()
        {
            // TODO: implement pause!
        }

        void AudioMaster::continue_after_pause()
        {
            // TODO: implement continue after pause!
        }

        void AudioMaster::clear_playlist(const std::string& playlist)
        {
            // TODO: implement clear playlist!
        }

        void AudioMaster::erase_playlist(const std::string& playlist)
        {
            // TODO: implement erase playlist!
        }

        int AudioMaster::get_remaining_length()
        {
            // This function is not `const` due to use of `SDL_AtomicGet`.
            return SDL_AtomicGet(&this->remaining_length);
        }

        void AudioMaster::play_audio(void* userdata, uint8_t* stream, int length)
        {
            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                // TODO: mix audio with `SDL_MixAudioFormat`!
                // https://wiki.libsdl.org/SDL_MixAudioFormat
                int remaining_length = this->get_remaining_length();

                if (remaining_length == 0)
                {
                    // there's nothing to play.
                    return;
                }

                if (length > remaining_length)
                {
                    length = remaining_length;
                }

                SDL_memset(stream, 0, length);
                SDL_MixAudioFormat(stream, this->wav_pointer, AUDIO_F32, length, SDL_MIX_MAXVOLUME);
                this->wav_pointer += length;

                remaining_length -= length;
                SDL_AtomicSet(&this->remaining_length, remaining_length);
            }
        }

        void AudioMaster::play_audio_callback(void* userdata, uint8_t* stream, int length)
        {
            yli::audio::AudioMaster* audio_master = yli::audio::AudioMaster::audio_master;
            audio_master->play_audio(userdata, stream, length);
        }
    }
}
<commit_msg>`AudioMaster` constructor: initialize `device` member variable.<commit_after>// Ylikuutio - A 3D game and simulation engine.
//
// Copyright (C) 2015-2019 Antti Nuortimo.
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Affero General Public License as
// published by the Free Software Foundation, either version 3 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Affero General Public License for more details.
//
// You should have received a copy of the GNU Affero General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.

#include "audio_master.hpp"
#include "code/ylikuutio/ontology/universe.hpp"

#include "SDL.h"

// Include standard headers
#include <iostream>      // std::cout, std::cin, std::cerr
#include <list>          // std::list
#include <stdint.h>      // uint32_t etc.
#include <string>        // std::string
#include <unordered_map> // std::unordered_map

namespace yli
{
    namespace audio
    {
        yli::audio::AudioMaster* AudioMaster::audio_master;

        AudioMaster::AudioMaster(yli::ontology::Universe* const universe)
        {
            // constructor.
            this->universe = universe;

            this->wav_pointer = nullptr;
            SDL_AtomicSet(&this->remaining_length, 0);

            this->current_playlist = ""; // no current playlist.
            this->loop = true;           // loop playlist.

            this->audio_spec.freq = 44100; // 44100 Hz.
            this->audio_spec.format = AUDIO_F32;
            this->audio_spec.channels = 2;
            this->audio_spec.silence = 0;        // dummy value.
            this->audio_spec.padding = 0;        // dummy value.
            this->audio_spec.samples = 4096;
            this->audio_spec.size = 0;           // dummy value.
            this->audio_spec.callback = yli::audio::AudioMaster::play_audio_callback;
            this->audio_spec.userdata = nullptr; // this is a pointer that is passed to the callback.

            // initialize `device` member variable.
            SDL_AudioSpec wav_spec = SDL_AudioSpec();
            this->device = SDL_OpenAudioDevice(NULL, 0, &wav_spec, &this->audio_spec, SDL_AUDIO_ALLOW_FORMAT_CHANGE);
            SDL_PauseAudioDevice(this->device, 1); // stop playing.

            this->audio_master = this;   // `this` is the `AudioMaster`. Do not create more than 1 `AudioMaster`!
        }

        AudioMaster::~AudioMaster()
        {
            // destructor.

            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                SDL_CloseAudioDevice(this->device);
            }
        }

        bool AudioMaster::load_and_play(const std::string& audio_file)
        {
            if (this->wav_buffer_pointer_map.count(audio_file) == 1)
            {
                // There is already a sound with that filename. It's not OK.
                return false;
            }

            // There's no sound with that filename loaded yet, so load it now.
            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                SDL_AudioSpec wav_spec = SDL_AudioSpec();
                uint32_t wav_length;
                uint8_t* wav_buffer;

                if (SDL_LoadWAV(audio_file.c_str(), &wav_spec, &wav_buffer, &wav_length) == NULL)
                {
                    // Loading the sound failed.
                    std::cerr << "Loading WAV file " << audio_file << " failed.\n";
                    return false;
                }

                // https://wiki.libsdl.org/SDL_OpenAudioDevice
                wav_spec.channels = 2;
                wav_spec.samples = 4096;
                wav_spec.callback = yli::audio::AudioMaster::play_audio_callback;

                this->device = SDL_OpenAudioDevice(NULL, 0, &wav_spec, &this->audio_spec, SDL_AUDIO_ALLOW_FORMAT_CHANGE);

                if (this->device == 0)
                {
                    SDL_Log("Failed to open audio: %s", SDL_GetError());
                }
                else
                {
                    if (this->audio_spec.format != wav_spec.format)
                    {
                        SDL_Log("Float32 audio format was not available.");
                    }
                }

                this->wav_pointer = wav_buffer;
                SDL_AtomicSet(&this->remaining_length, wav_length);
                this->wav_buffer_pointer_map[audio_file] = wav_buffer;
                SDL_PauseAudioDevice(this->device, 0); // start playing.
            }

            return true;
        }

        void AudioMaster::unload(const std::string& audio_file)
        {
            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                if (this->wav_buffer_pointer_map.count(audio_file) == 1)
                {
                    if (*current_playlist_sound_iterator == audio_file)
                    {
                        // Stop the sound before unloading it.
                        SDL_PauseAudioDevice(this->device, 1); // stop playing.
                    }

                    SDL_FreeWAV(this->wav_buffer_pointer_map[audio_file]);
                    this->wav_buffer_pointer_map.erase(audio_file);
                }
            }
        }

        void AudioMaster::add_to_playlist(const std::string& playlist, const std::string& audio_file)
        {
            // Playlist must have a name. Empty string is not accepted.
            if (!playlist.empty() && this->playlist_map.count(playlist) == 0)
            {
                this->playlist_map[playlist] = std::list<std::string>();
            }

            this->playlist_map[playlist].push_back(audio_file);
        }

        void AudioMaster::remove_from_playlist(const std::string& playlist, const std::string& audio_file)
        {
            if (this->playlist_map.count(playlist) == 1)
            {
                for (auto it = this->playlist_map[playlist].begin(); it != this->playlist_map[playlist].end(); )
                {
                    if (it->compare(audio_file) == 0)
                    {
                        this->playlist_map[playlist].erase(it--);
                    }

                    it++;
                }
            }
        }

        void AudioMaster::play_playlist(const std::string& playlist)
        {
            // This function starts playing the playlist "from the top".
            // Playlist must have a name. Empty string is not accepted.
            if (!playlist.empty() && this->playlist_map.count(playlist) == 1 && this->playlist_map.size() > 0)
            {
                this->current_playlist = playlist;
                this->current_playlist_sound_iterator = this->playlist_map[playlist].begin();
                this->load_and_play(*this->current_playlist_sound_iterator);
            }
        }

        void AudioMaster::update()
        {
            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                // This function checks if a playlist is running.
                // if yes, then if the the previous sound has ended,
                // its memory gets freed and a new sound gets started.
                if (this->current_playlist.size() > 0)
                {
                    if (this->wav_buffer_pointer_map.count(*this->current_playlist_sound_iterator) == 1)
                    {
                        // OK, there is a sound which might be playing, so let's check its status.
                        const int remaining_length = this->get_remaining_length();

                        if (remaining_length == 0)
                        {
                            SDL_PauseAudioDevice(this->device, 1); // stop playing.
                            this->unload(*this->current_playlist_sound_iterator);

                            // play the next sound.

                            if (++this->current_playlist_sound_iterator != this->playlist_map[this->current_playlist].end())
                            {
                                // Next sound.
                                this->load_and_play(*this->current_playlist_sound_iterator);
                            }
                            else
                            {
                                // The previous sound was the last sound.
                                if (this->loop)
                                {
                                    // Start from the first.
                                    this->current_playlist_sound_iterator = this->playlist_map[this->current_playlist].begin();
                                    this->load_and_play(*this->current_playlist_sound_iterator);
                                }
                                else
                                {
                                    this->current_playlist = ""; // no current playlist.
                                }
                            }
                        }
                    }
                }
            }
        }

        void AudioMaster::pause()
        {
            // TODO: implement pause!
        }

        void AudioMaster::continue_after_pause()
        {
            // TODO: implement continue after pause!
        }

        void AudioMaster::clear_playlist(const std::string& playlist)
        {
            // TODO: implement clear playlist!
        }

        void AudioMaster::erase_playlist(const std::string& playlist)
        {
            // TODO: implement erase playlist!
        }

        int AudioMaster::get_remaining_length()
        {
            // This function is not `const` due to use of `SDL_AtomicGet`.
            return SDL_AtomicGet(&this->remaining_length);
        }

        void AudioMaster::play_audio(void* userdata, uint8_t* stream, int length)
        {
            if (this->universe != nullptr && !this->universe->get_is_headless())
            {
                // TODO: mix audio with `SDL_MixAudioFormat`!
                // https://wiki.libsdl.org/SDL_MixAudioFormat
                int remaining_length = this->get_remaining_length();

                if (remaining_length == 0)
                {
                    // there's nothing to play.
                    return;
                }

                if (length > remaining_length)
                {
                    length = remaining_length;
                }

                SDL_memset(stream, 0, length);
                SDL_MixAudioFormat(stream, this->wav_pointer, AUDIO_F32, length, SDL_MIX_MAXVOLUME);
                this->wav_pointer += length;

                remaining_length -= length;
                SDL_AtomicSet(&this->remaining_length, remaining_length);
            }
        }

        void AudioMaster::play_audio_callback(void* userdata, uint8_t* stream, int length)
        {
            yli::audio::AudioMaster* audio_master = yli::audio::AudioMaster::audio_master;
            audio_master->play_audio(userdata, stream, length);
        }
    }
}
<|endoftext|>


<commit_before>// Copyright Daniel Wallin 2006. Use, modification and distribution is
// subject to the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <libtorrent/torrent_handle.hpp>
#include <boost/python.hpp>
#include <libtorrent/bitfield.hpp>

using namespace boost::python;
using namespace libtorrent;

object pieces(torrent_status const& s)
{
    list result;

    for (bitfield::const_iterator i(s.pieces->begin()), e(s.pieces->end()); i != e; ++i)
        result.append(*i);

    return result;
}

void bind_torrent_status()
{
    scope status = class_<torrent_status>("torrent_status")
        .def_readonly("state", &torrent_status::state)
        .def_readonly("paused", &torrent_status::paused)
        .def_readonly("progress", &torrent_status::progress)
        .add_property(
            "next_announce"
          , make_getter(
                &torrent_status::next_announce, return_value_policy<return_by_value>()
            )
        )
        .add_property(
            "announce_interval"
          , make_getter(
                &torrent_status::announce_interval, return_value_policy<return_by_value>()
            )
        )
        .def_readonly("current_tracker", &torrent_status::current_tracker)
        .def_readonly("total_download", &torrent_status::total_download)
        .def_readonly("total_upload", &torrent_status::total_upload)
        .def_readonly("total_payload_download", &torrent_status::total_payload_download)
        .def_readonly("total_payload_upload", &torrent_status::total_payload_upload)
        .def_readonly("total_failed_bytes", &torrent_status::total_failed_bytes)
        .def_readonly("total_redundant_bytes", &torrent_status::total_redundant_bytes)
        .def_readonly("download_rate", &torrent_status::download_rate)
        .def_readonly("upload_rate", &torrent_status::upload_rate)
        .def_readonly("download_payload_rate", &torrent_status::download_payload_rate)
        .def_readonly("upload_payload_rate", &torrent_status::upload_payload_rate)
        .def_readonly("num_seeds", &torrent_status::num_seeds)
        .def_readonly("num_peers", &torrent_status::num_peers)
        .def_readonly("num_complete", &torrent_status::num_complete)
        .def_readonly("num_incomplete", &torrent_status::num_incomplete)
        .def_readonly("list_seeds", &torrent_status::list_seeds)
        .def_readonly("list_peers", &torrent_status::list_peers)
        .add_property("pieces", pieces)
        .def_readonly("num_pieces", &torrent_status::num_pieces)
        .def_readonly("total_done", &torrent_status::total_done)
        .def_readonly("total_wanted_done", &torrent_status::total_wanted_done)
        .def_readonly("total_wanted", &torrent_status::total_wanted)
        .def_readonly("distributed_copies", &torrent_status::distributed_copies)
        .def_readonly("block_size", &torrent_status::block_size)
        .def_readonly("num_uploads", &torrent_status::num_uploads)
        .def_readonly("num_connections", &torrent_status::num_connections)
        .def_readonly("uploads_limit", &torrent_status::uploads_limit)
        .def_readonly("connections_limit", &torrent_status::connections_limit)
        .def_readonly("storage_mode", &torrent_status::storage_mode)
        .def_readonly("up_bandwidth_queue", &torrent_status::up_bandwidth_queue)
        .def_readonly("down_bandwidth_queue", &torrent_status::down_bandwidth_queue)
        .def_readonly("all_time_upload", &torrent_status::all_time_upload)
        .def_readonly("all_time_download", &torrent_status::all_time_download)
        .def_readonly("active_time", &torrent_status::active_time)
        .def_readonly("seeding_time", &torrent_status::seeding_time)
        .def_readonly("seed_rank", &torrent_status::seed_rank)
        .def_readonly("last_scrape", &torrent_status::last_scrape)
        ;

    enum_<torrent_status::state_t>("states")
        .value("queued_for_checking", torrent_status::queued_for_checking)
        .value("checking_files", torrent_status::checking_files)
        .value("connecting_to_tracker", torrent_status::connecting_to_tracker)
        .value("downloading", torrent_status::downloading)
        .value("finished", torrent_status::finished)
        .value("seeding", torrent_status::seeding)
        .value("allocating", torrent_status::allocating)
        .export_values()
        ;
}

<commit_msg>Fix python bindings build<commit_after>// Copyright Daniel Wallin 2006. Use, modification and distribution is
// subject to the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <libtorrent/torrent_handle.hpp>
#include <boost/python.hpp>
#include <libtorrent/bitfield.hpp>

using namespace boost::python;
using namespace libtorrent;

object pieces(torrent_status const& s)
{
    list result;

    for (bitfield::const_iterator i(s.pieces.begin()), e(s.pieces.end()); i != e; ++i)
        result.append(*i);

    return result;
}

void bind_torrent_status()
{
    scope status = class_<torrent_status>("torrent_status")
        .def_readonly("state", &torrent_status::state)
        .def_readonly("paused", &torrent_status::paused)
        .def_readonly("progress", &torrent_status::progress)
        .add_property(
            "next_announce"
          , make_getter(
                &torrent_status::next_announce, return_value_policy<return_by_value>()
            )
        )
        .add_property(
            "announce_interval"
          , make_getter(
                &torrent_status::announce_interval, return_value_policy<return_by_value>()
            )
        )
        .def_readonly("current_tracker", &torrent_status::current_tracker)
        .def_readonly("total_download", &torrent_status::total_download)
        .def_readonly("total_upload", &torrent_status::total_upload)
        .def_readonly("total_payload_download", &torrent_status::total_payload_download)
        .def_readonly("total_payload_upload", &torrent_status::total_payload_upload)
        .def_readonly("total_failed_bytes", &torrent_status::total_failed_bytes)
        .def_readonly("total_redundant_bytes", &torrent_status::total_redundant_bytes)
        .def_readonly("download_rate", &torrent_status::download_rate)
        .def_readonly("upload_rate", &torrent_status::upload_rate)
        .def_readonly("download_payload_rate", &torrent_status::download_payload_rate)
        .def_readonly("upload_payload_rate", &torrent_status::upload_payload_rate)
        .def_readonly("num_seeds", &torrent_status::num_seeds)
        .def_readonly("num_peers", &torrent_status::num_peers)
        .def_readonly("num_complete", &torrent_status::num_complete)
        .def_readonly("num_incomplete", &torrent_status::num_incomplete)
        .def_readonly("list_seeds", &torrent_status::list_seeds)
        .def_readonly("list_peers", &torrent_status::list_peers)
        .add_property("pieces", pieces)
        .def_readonly("num_pieces", &torrent_status::num_pieces)
        .def_readonly("total_done", &torrent_status::total_done)
        .def_readonly("total_wanted_done", &torrent_status::total_wanted_done)
        .def_readonly("total_wanted", &torrent_status::total_wanted)
        .def_readonly("distributed_copies", &torrent_status::distributed_copies)
        .def_readonly("block_size", &torrent_status::block_size)
        .def_readonly("num_uploads", &torrent_status::num_uploads)
        .def_readonly("num_connections", &torrent_status::num_connections)
        .def_readonly("uploads_limit", &torrent_status::uploads_limit)
        .def_readonly("connections_limit", &torrent_status::connections_limit)
        .def_readonly("storage_mode", &torrent_status::storage_mode)
        .def_readonly("up_bandwidth_queue", &torrent_status::up_bandwidth_queue)
        .def_readonly("down_bandwidth_queue", &torrent_status::down_bandwidth_queue)
        .def_readonly("all_time_upload", &torrent_status::all_time_upload)
        .def_readonly("all_time_download", &torrent_status::all_time_download)
        .def_readonly("active_time", &torrent_status::active_time)
        .def_readonly("seeding_time", &torrent_status::seeding_time)
        .def_readonly("seed_rank", &torrent_status::seed_rank)
        .def_readonly("last_scrape", &torrent_status::last_scrape)
        ;

    enum_<torrent_status::state_t>("states")
        .value("queued_for_checking", torrent_status::queued_for_checking)
        .value("checking_files", torrent_status::checking_files)
        .value("connecting_to_tracker", torrent_status::connecting_to_tracker)
        .value("downloading", torrent_status::downloading)
        .value("finished", torrent_status::finished)
        .value("seeding", torrent_status::seeding)
        .value("allocating", torrent_status::allocating)
        .export_values()
        ;
}

<|endoftext|>