<commit_before>/*************************************************************************
 *
 *  $RCSfile: fuline.cxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: hr $ $Date: 2004-02-04 10:08:49 $
 *
 *  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): _______________________________________
 *
 *
 ************************************************************************/

#pragma hdrstop

#include "fuline.hxx"

#include <svx/svxids.hrc>
#ifndef _SVX_TAB_LINE_HXX //autogen
#include <svx/tabline.hxx>
#endif
#ifndef _XENUM_HXX //autogen
#include <svx/xenum.hxx>
#endif
#ifndef _SV_MSGBOX_HXX //autogen
#include <vcl/msgbox.hxx>
#endif
#ifndef _SFXINTITEM_HXX //autogen
#include <svtools/intitem.hxx>
#endif
#ifndef _SFXSTRITEM_HXX //autogen
#include <svtools/stritem.hxx>
#endif
#ifndef _SFXREQUEST_HXX //autogen
#include <sfx2/request.hxx>
#endif
#ifndef _XDEF_HXX //autogen
#include <svx/xdef.hxx>
#endif
#ifndef _SFX_BINDINGS_HXX //autogen
#include <sfx2/bindings.hxx>
#endif
#ifndef _SFXVIEWFRM_HXX
#include <sfx2/viewfrm.hxx>
#endif

#ifndef SD_VIEW_SHELL_HXX
#include "ViewShell.hxx"
#endif
#ifndef SD_VIEW_HXX
#include "View.hxx"
#endif
#ifndef SD_WINDOW_SHELL_HXX
#include "Window.hxx"
#endif
#include "drawdoc.hxx"
#include "app.hrc"
#include <svx/svxdlg.hxx> //CHINA001
#include <svx/dialogs.hrc> //CHINA001

namespace sd {

TYPEINIT1( FuLine, FuPoor );

/*************************************************************************
|*
|* Konstruktor
|*
\************************************************************************/

FuLine::FuLine (
    ViewShell* pViewSh,
    ::sd::Window* pWin,
    ::sd::View* pView,
    SdDrawDocument* pDoc,
    SfxRequest& rReq)
    : FuPoor(pViewSh, pWin, pView, pDoc, rReq)
{
    BOOL        bHasMarked = pView->HasMarkedObj();

    const SfxItemSet* pArgs = rReq.GetArgs();

    if( !pArgs )
    {
        // erst einmal alle eingabeparameter fuer den dialog retten
        SfxItemSet aInputAttr( pDoc->GetPool() );
        pView->GetAttributes( aInputAttr );

        const XLineStyleItem &rILineStyleItem = (const XLineStyleItem &) aInputAttr.Get (XATTR_LINESTYLE);
        XLineStyle           eILineStyle      = rILineStyleItem.GetValue ();

        const XLineDashItem  &rILineDashItem  = (const XLineDashItem &) aInputAttr.Get (XATTR_LINEDASH);
        const XDash          &rIDash          = rILineDashItem.GetValue ();

        const XLineWidthItem &rILineWidthItem = (const XLineWidthItem &) aInputAttr.Get (XATTR_LINEWIDTH);
        long                 nILineWidth      = rILineWidthItem.GetValue ();

        const XLineColorItem &rILineColorItem = (const XLineColorItem &) aInputAttr.Get (XATTR_LINECOLOR);
        const Color          &rIColor         = rILineColorItem.GetValue ();

        const SdrObject* pObj = NULL;
        const SdrMarkList& rMarkList = pView->GetMarkList();
        if( rMarkList.GetMarkCount() == 1 )
            pObj = rMarkList.GetMark(0)->GetObj();

        SfxItemSet* pNewAttr = new SfxItemSet( pDoc->GetPool() );
        pView->GetAttributes( *pNewAttr );

        //CHINA001 SvxLineTabDialog* pDlg = new SvxLineTabDialog( NULL, pNewAttr, pDoc, pObj, bHasMarked );
        SvxAbstractDialogFactory* pFact = SvxAbstractDialogFactory::Create();
        DBG_ASSERT(pFact, "Dialogdiet Factory fail!");//CHINA001
        SfxAbstractTabDialog * pDlg = pFact->CreateSvxLineTabDialog(NULL,
                    pNewAttr,
                    pDoc,
                ResId(RID_SVXDLG_LINE),
                pObj,
                bHasMarked);
        DBG_ASSERT(pDlg, "Dialogdiet fail!");//CHINA001
        if ( pDlg->Execute() == RET_OK )
        {
            // die ausgabeparameter des dialogs bestimmen
            SfxItemSet           *pOutputAttr     = (SfxItemSet *) pDlg->GetOutputItemSet();

            const XLineStyleItem &rOLineStyleItem = (const XLineStyleItem &) pOutputAttr->Get (XATTR_LINESTYLE);
            XLineStyle           eOLineStyle      = rOLineStyleItem.GetValue ();

            const XLineDashItem  &rOLineDashItem  = (const XLineDashItem &) pOutputAttr->Get (XATTR_LINEDASH);
            const XDash          &rODash          = rOLineDashItem.GetValue ();

            const XLineWidthItem &rOLineWidthItem = (const XLineWidthItem &) pOutputAttr->Get (XATTR_LINEWIDTH);
            long                 nOLineWidth      = rOLineWidthItem.GetValue ();

            const XLineColorItem &rOLineColorItem = (const XLineColorItem &) pOutputAttr->Get (XATTR_LINECOLOR);
            const Color          &rOColor         = rOLineColorItem.GetValue ();

            pView->SetAttributes (*(pDlg->GetOutputItemSet ()));
        }

        // Attribute wurden geaendert, Listboxes in Objectbars muessen aktualisiert werden
        static USHORT SidArray[] = {
                        SID_ATTR_LINE_STYLE,
                        SID_ATTR_LINE_DASH,
                        SID_ATTR_LINE_WIDTH,
                        SID_ATTR_LINE_COLOR,
                        0 };

        pViewShell->GetViewFrame()->GetBindings().Invalidate( SidArray );

        delete pDlg;
        delete pNewAttr;
    }

    rReq.Ignore ();
}

} // end of namespace sd
<commit_msg>INTEGRATION: CWS aw013 (1.4.120); FILE MERGED 2004/06/24 09:33:10 aw 1.4.120.1: #i29181#<commit_after>/*************************************************************************
 *
 *  $RCSfile: fuline.cxx,v $
 *
 *  $Revision: 1.5 $
 *
 *  last change: $Author: rt $ $Date: 2004-07-12 15:03:16 $
 *
 *  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): _______________________________________
 *
 *
 ************************************************************************/

#pragma hdrstop

#include "fuline.hxx"

#include <svx/svxids.hrc>
#ifndef _SVX_TAB_LINE_HXX //autogen
#include <svx/tabline.hxx>
#endif
#ifndef _XENUM_HXX //autogen
#include <svx/xenum.hxx>
#endif
#ifndef _SV_MSGBOX_HXX //autogen
#include <vcl/msgbox.hxx>
#endif
#ifndef _SFXINTITEM_HXX //autogen
#include <svtools/intitem.hxx>
#endif
#ifndef _SFXSTRITEM_HXX //autogen
#include <svtools/stritem.hxx>
#endif
#ifndef _SFXREQUEST_HXX //autogen
#include <sfx2/request.hxx>
#endif
#ifndef _XDEF_HXX //autogen
#include <svx/xdef.hxx>
#endif
#ifndef _SFX_BINDINGS_HXX //autogen
#include <sfx2/bindings.hxx>
#endif
#ifndef _SFXVIEWFRM_HXX
#include <sfx2/viewfrm.hxx>
#endif

#ifndef SD_VIEW_SHELL_HXX
#include "ViewShell.hxx"
#endif
#ifndef SD_VIEW_HXX
#include "View.hxx"
#endif
#ifndef SD_WINDOW_SHELL_HXX
#include "Window.hxx"
#endif
#include "drawdoc.hxx"
#include "app.hrc"
#include <svx/svxdlg.hxx> //CHINA001
#include <svx/dialogs.hrc> //CHINA001

namespace sd {

TYPEINIT1( FuLine, FuPoor );

/*************************************************************************
|*
|* Konstruktor
|*
\************************************************************************/

FuLine::FuLine (
    ViewShell* pViewSh,
    ::sd::Window* pWin,
    ::sd::View* pView,
    SdDrawDocument* pDoc,
    SfxRequest& rReq)
    : FuPoor(pViewSh, pWin, pView, pDoc, rReq)
{
    BOOL        bHasMarked = pView->AreObjectsMarked();

    const SfxItemSet* pArgs = rReq.GetArgs();

    if( !pArgs )
    {
        // erst einmal alle eingabeparameter fuer den dialog retten
        SfxItemSet aInputAttr( pDoc->GetPool() );
        pView->GetAttributes( aInputAttr );

        const XLineStyleItem &rILineStyleItem = (const XLineStyleItem &) aInputAttr.Get (XATTR_LINESTYLE);
        XLineStyle           eILineStyle      = rILineStyleItem.GetValue ();

        const XLineDashItem  &rILineDashItem  = (const XLineDashItem &) aInputAttr.Get (XATTR_LINEDASH);
        const XDash          &rIDash          = rILineDashItem.GetValue ();

        const XLineWidthItem &rILineWidthItem = (const XLineWidthItem &) aInputAttr.Get (XATTR_LINEWIDTH);
        long                 nILineWidth      = rILineWidthItem.GetValue ();

        const XLineColorItem &rILineColorItem = (const XLineColorItem &) aInputAttr.Get (XATTR_LINECOLOR);
        const Color          &rIColor         = rILineColorItem.GetValue ();

        const SdrObject* pObj = NULL;
        const SdrMarkList& rMarkList = pView->GetMarkedObjectList();
        if( rMarkList.GetMarkCount() == 1 )
            pObj = rMarkList.GetMark(0)->GetObj();

        SfxItemSet* pNewAttr = new SfxItemSet( pDoc->GetPool() );
        pView->GetAttributes( *pNewAttr );

        //CHINA001 SvxLineTabDialog* pDlg = new SvxLineTabDialog( NULL, pNewAttr, pDoc, pObj, bHasMarked );
        SvxAbstractDialogFactory* pFact = SvxAbstractDialogFactory::Create();
        DBG_ASSERT(pFact, "Dialogdiet Factory fail!");//CHINA001
        SfxAbstractTabDialog * pDlg = pFact->CreateSvxLineTabDialog(NULL,
                    pNewAttr,
                    pDoc,
                ResId(RID_SVXDLG_LINE),
                pObj,
                bHasMarked);
        DBG_ASSERT(pDlg, "Dialogdiet fail!");//CHINA001
        if ( pDlg->Execute() == RET_OK )
        {
            // die ausgabeparameter des dialogs bestimmen
            SfxItemSet           *pOutputAttr     = (SfxItemSet *) pDlg->GetOutputItemSet();

            const XLineStyleItem &rOLineStyleItem = (const XLineStyleItem &) pOutputAttr->Get (XATTR_LINESTYLE);
            XLineStyle           eOLineStyle      = rOLineStyleItem.GetValue ();

            const XLineDashItem  &rOLineDashItem  = (const XLineDashItem &) pOutputAttr->Get (XATTR_LINEDASH);
            const XDash          &rODash          = rOLineDashItem.GetValue ();

            const XLineWidthItem &rOLineWidthItem = (const XLineWidthItem &) pOutputAttr->Get (XATTR_LINEWIDTH);
            long                 nOLineWidth      = rOLineWidthItem.GetValue ();

            const XLineColorItem &rOLineColorItem = (const XLineColorItem &) pOutputAttr->Get (XATTR_LINECOLOR);
            const Color          &rOColor         = rOLineColorItem.GetValue ();

            pView->SetAttributes (*(pDlg->GetOutputItemSet ()));
        }

        // Attribute wurden geaendert, Listboxes in Objectbars muessen aktualisiert werden
        static USHORT SidArray[] = {
                        SID_ATTR_LINE_STYLE,
                        SID_ATTR_LINE_DASH,
                        SID_ATTR_LINE_WIDTH,
                        SID_ATTR_LINE_COLOR,
                        0 };

        pViewShell->GetViewFrame()->GetBindings().Invalidate( SidArray );

        delete pDlg;
        delete pNewAttr;
    }

    rReq.Ignore ();
}

} // end of namespace sd
<|endoftext|>


<commit_before>/**
 * @file main.cpp
 *
 * @brief File containing main Q-Learning algorithm and all necessary associated functions to implementing this routine.
 *
 * @author Machine Learning Team 2015-2016
 * @date March, 2016
 */

#include <cmath>
#include <cstdlib>
#include <ctime>
#include <string>
#include <sstream>
#include <fstream>
#include "StateSpace.h"
#include "PriorityQueue.h"
#include "State.h"
#include "encoder.h"
#include "CreateModule.h"

/**
* @brief Gives a std::string representation of a primitive type.
*
* @remark Can be used to print priority queue contents.
* @param x Primitive type such as int, double, long ...
* @return std::string conversion of param x
*/
template<typename T> std::string to_string(T x) {
	return static_cast<std::ostringstream&>((std::ostringstream() << std::dec << x)).str();
}

/**
 * @brief Analog to temperature variable in Boltzmann Distribution, computes 'evolution variable' of system.
 *
 * This function computes a normal distribution over the parameterised number of loop iterations in order
 * to provide a function which explores the state space dilligently initially which then decays off to the
 * optimal solution after a large number of loop iterations.
 *
 * The normal distribution used by this function is:
 *
 * \f[ a e^{-\frac{bt^2}{c}} + \epsilon \f]
 *
 * where a, b and c are scaling coefficients (see method body) and \f$\epsilon\f$ is some small offset.
 *
 * @param t Number of loop iterations.
 * @return Value of normal distribution at t loop iterations.
 */
double probabilityFluxDensityCoeffieicent(unsigned long t);

//function to select next action

/**
 * @brief Selects an action to perform based on experience, iterations and probabilities.
 *
 * @param a_queue A priority queue instance storing integral types with double type priorities,
 *		  represents the queue of possible actions with pre-initialised priority levels.
 * @param iterations Number of loop iterations completed.
 * @return integer corresponding to chosen action
 */
int selectAction(PriorityQueue<int, double>& a_queue, unsigned long iterations);

/**
 * @brief Updates the utility (Q-value) of the system.
 *
 * Utility (Q-Value) of the system is updated via the Temporal Difference Learning Rule given by the following equation,
 *
 * \f[ Q_{i+1} (s,a) = Q_i (s,a) + \alpha [R(s') + \gamma \underset{a}{max} Q(s',a) - Q_i (s,a)] \f]
 *
 * where Q represents the utility of a state-action pair, \f$ \alpha \f$ is the learning rate, \f$ \gamma \f$ is the discount
 * factor and \f$ max_a (Q) \f$ yields the action-maximum of the Q space. The learning rate should be set to a low value for
 * highly stochastic, asymmetric systems or a high value for a lowly stochastic, symmetric system - this value lies between 0 and 1.
 * The discount factor (also in the interval [0,1]) represents the time considerations of the learning algorithm, lower values indicate
 * "living in the moment", whilst higher values indicate "planning for the future".
 *
 * @param space Reference to StateSpace object
 * @param action Integer code to previous performed action
 * @param new_state Reference to State instance giving the new system state
 * @param old_state Reference to State instance giving the old system state
 * @param alpha Learning rate of temporal difference learning algorithm (in the interval [0,1])
 * @param gamma Discount factor applied to q-learning equation (in the interval [0,1])
 */
void updateQ(StateSpace & space, int action, State & new_state, State & old_state, double alpha, double gamma);

/**
 * @brief Program launcher!
 *
 * Contains all initialisations including setting up the StateSpace and connecting to robot via a proxy. Holds main
 * program loop to perform algorithm continuously using calls to selectAction and updateQ sequentially to update
 * utilities and choose most optimal actions to perform based on position and velocity.
 *
 * @return Program exit code
 */
int main() {
	// STUFF WE DONT UNDERSTAND, AND DONT NEED TO
	//__________________________________________________________________________________________
	//__________________________________________________________________________________________
	// Libraries to load
	std::string bodyLibName = "bodyinfo";
	std::string movementLibName = "movementtools";

	// Name of camera module in library
	std::string bodyModuleName = "BodyInfo";
	std::string movementModuleName = "MovementTools";

	// Set broker name, ip and port, finding first available port from 54000
	const std::string brokerName = "MotionTimingBroker";
	int brokerPort = qi::os::findAvailablePort(54000);
	const std::string brokerIp = "0.0.0.0";

	// Default parent port and ip
	int pport = 9559;
	std::string pip = "127.0.0.1";

	// Need this for SOAP serialisation of floats to work
	setlocale(LC_NUMERIC, "C");

	// Create a broker
	boost::shared_ptr<AL::ALBroker> broker;
	try {
		broker = AL::ALBroker::createBroker(
			brokerName,
			brokerIp,
			brokerPort,
			pip,
			pport,
			0);
	}
	// Throw error and quit if a broker could not be created
	catch (...) {
		std::cerr << "Failed to connect broker to: "
			<< pip
			<< ":"
			<< pport
			<< std::endl;
		AL::ALBrokerManager::getInstance()->killAllBroker();
		AL::ALBrokerManager::kill();

		return 1;
	}

	// Add the broker to NAOqi
	AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock());
	AL::ALBrokerManager::getInstance()->addBroker(broker);

	CreateModule(movementLibName, movementModuleName, broker, false, true);
	CreateModule(bodyLibName, bodyModuleName, broker, false, true);

	AL::ALProxy bodyInfoProxy(bodyModuleName, pip, pport);
	AL::ALProxy movementToolsProxy(movementModuleName, pip, pport);
	AL::ALMotionProxy motion(pip, pport);
	//__________________________________________________________________________________________
	//__________________________________________________________________________________________
	//END OF STUFF WE DONT UNDERSTAND, BREATHE NOW

	//learning factor
	const double alpha = 0.8;
	//discount factor
	const double gamma = 0.5;

	//seed rng
	std::srand(static_cast<unsigned int>(std::time(NULL)));

	int action_forwards = FORWARD;
	int action_backwards = BACKWARD;
	int chosen_action = action_forwards;

	//create a priority queue to copy to all the state space priority queues
	PriorityQueue<int, double> initiator_queue(MAX);
	initiator_queue.enqueueWithPriority(action_forwards, 0);
	initiator_queue.enqueueWithPriority(action_backwards, 0);

	//create encoder
	Encoder encoder;
	encoder.Calibrate();

	//pause briefly to allow the robot to be given a push if desired
	qi::os::msleep(5000);

	//create the state space
	StateSpace space(100, 50, M_PI * 0.25, 1.0, initiator_queue);

	//state objects
	State current_state(0, 0, FORWARD);
	State old_state(0, 0, FORWARD);

	std::cout << "Initialisation complete!" << std::endl;
	for( unsigned long i = 0; i<500 ;++i ) {
		// set current state angle to angle received from encoder
		// and set current state velocity to difference in new and
		// old state angles over some time difference
		current_state.theta = M_PI * (encoder.GetAngle()) / 180;
		current_state.theta_dot = (current_state.theta - old_state.theta) / 700; //Needs actual time
		current_state.robot_state = static_cast<ROBOT_STATE>(chosen_action);

		std::cout << "Angle of current state: " << current_state.theta << std::endl;

		// call updateQ function with state space, old and current states
		// and learning rate, discount factor
		updateQ(space, chosen_action, old_state, current_state, alpha, gamma);
		std::cout << "Update Q completed!" << std::endl;
		// set old_state to current_state
		old_state = current_state;

		// determine chosen_action for current state
		chosen_action = selectAction(space[current_state], i);
		std::cout << "Select Action completed!" << std::endl;
		// depending upon chosen action, call robot movement tools proxy with either
		// swingForwards or swingBackwards commands.
		(chosen_action) ? movementToolsProxy.callVoid("swingForwards") : movementToolsProxy.callVoid("swingBackwards");
	}

//	std::ofstream output("output.txt");
//	output<<space;
//	output.close();

	return 1;
}

double probabilityFluxDensityCoeffieicent(unsigned long t) {
	return 100.0*std::exp((-8.0*t*t) / (2600.0*2600.0)) + 0.1;//0.1 is an offset
}

int selectAction(PriorityQueue<int, double>& a_queue, unsigned long iterations) {

	typedef std::vector<std::pair<int, double> > VecPair ;

	//turn priority queue into a vector of pairs
	VecPair vec = a_queue.saveOrderedQueueAsVector();

    //sum for partition function
	double sum = 0.0;

	// calculate partition function by iterating over action-values
	for (VecPair::iterator iter = vec.begin(), end = vec.end(); iter < end; ++iter) {
		sum += std::exp((iter->second) / probabilityFluxDensityCoeffieicent(iterations));
	}

	// compute Boltzmann factors for action-values and enqueue to vec
	for (VecPair::iterator iter = vec.begin(); iter < vec.end(); ++iter) {
		iter->second = std::exp(iter->second / probabilityFluxDensityCoeffieicent(iterations)) / sum;
	}

	// calculate cumulative probability distribution
	for (VecPair::iterator iter = vec.begin(), end = vec.end(); iter < end; ++iter) {
        //second member of pair becomes addition of its current value
        //and that of the index before it
        	if (iter != vec.begin())
			iter->second += (iter-1)->second;
	}

	//generate RN between 0 and 1
	double rand_num = static_cast<double>(rand()) / RAND_MAX;

	// choose action based on random number relation to priorities within action queue
	for (VecPair::iterator iter = vec.begin(), end = vec.end(); iter < end; ++iter) {
		if (rand_num < iter->second)
			return iter->first;
	}

	return -1; //note that this line should never be reached
}
int selectAction(PriorityQueue<int, double>& a_queue, unsigned long iterations) {
	double rand_num = static_cast<double>(rand()) / RAND_MAX);
	if(rand_num < epsilon){
		return a_queue.peekFront()->first;
	}

	rand_num = static_cast<double>(rand()) / RAND_MAX);
	return a_queue[ std::round( rand_num*(a_queue.getSize()-1) ) ];
}

void updateQ(StateSpace & space, int action, State & new_state, State & old_state, double alpha, double gamma) {
	//oldQ value reference
	double oldQ = space[old_state].search(action).second;

	//reward given to current state
	double R = new_state.getReward();

	//optimal Q value for new state i.e. first element
	double maxQ = space[new_state].peekFront().second;

	//new Q value determined by Q learning algorithm
	double newQ = oldQ + alpha * (R + (gamma * maxQ) - oldQ);

	// change priority of action to new Q value
	space[old_state].changePriority(action, newQ);
}
/* OLD SELECT ACTION
int selectAction(const PriorityQueue<int, double>& a_queue, unsigned long iterations) {
	/*
	// queue to store action values
	PriorityQueue<int, double> actionQueue(MAX);
	double sum = 0.0;
	// calculate partition function by iterating over action-values
	for (PriorityQueue<int, double>::const_iterator iter = a_queue.begin(), end = a_queue.end(); iter < end; ++iter) {
		sum += std::exp((iter->second) / temperature(iterations));
	}
	// compute Boltzmann factors for action-values and enqueue to actionQueue
	for (PriorityQueue<int, double>::const_iterator iter = a_queue.begin(); iter < a_queue.end(); ++iter) {
		double priority = std::exp(iter.operator*().second / temperature(iterations)) / sum;
		actionQueue.enqueueWithPriority(iter.operator*().first, priority);
	}
	// calculate cumulative probability distribution
	for (PriorityQueue<int, double>::const_iterator it1 = actionQueue.begin()++, it2 = actionQueue.begin(), end = actionQueue.end(); it1 < end; ++it1, ++it2) {
		// change priority of it1->first data item in actionQueue to
		// sum of priorities of it1 and it2 items
		actionQueue.changePriority(it1->first, it1->second + it2->second);
	}
	//generate RN between 0 and 1
	double rand_num = static_cast<double>(rand()) / RAND_MAX;
	// choose action based on random number relation to priorities within action queue
	for (PriorityQueue<int, double>::const_iterator iter = actionQueue.begin(), end = actionQueue.end(); iter < end; ++iter) {
		if (rand_num < iter->second)
			return iter->first;
	}

	return -1; //note that this line should never be reached

	double rand_num = static_cast<double>(rand()) / RAND_MAX;

	return (rand_num > 0.5)?0:1;
}
*/
<commit_msg>added file i/o for saving StateSpace instance<commit_after>/**
 * @file main.cpp
 *
 * @brief File containing main Q-Learning algorithm and all necessary associated functions to implementing this routine.
 *
 * @author Machine Learning Team 2015-2016
 * @date March, 2016
 */

#include <cmath>
#include <cstdlib>
#include <ctime>
#include <string>
#include <sstream>
#include <fstream>
#include "StateSpace.h"
#include "PriorityQueue.h"
#include "State.h"
#include "encoder.h"
#include "CreateModule.h"

/**
* @brief Gives a std::string representation of a primitive type.
*
* @remark Can be used to print priority queue contents.
* @param x Primitive type such as int, double, long ...
* @return std::string conversion of param x
*/
template<typename T> std::string to_string(T x) {
	return static_cast<std::ostringstream&>((std::ostringstream() << std::dec << x)).str();
}

/**
 * @brief Determines whether a file exists / is accessible
 *
 * @param filename File to check for
 * @return true if file exists, false if not or is inaccessible
 */
bool fileExists(const char* filename) {
	std::ifstream file(filename);
	return file.good();
}

/**
 * @brief Analog to temperature variable in Boltzmann Distribution, computes 'evolution variable' of system.
 *
 * This function computes a normal distribution over the parameterised number of loop iterations in order
 * to provide a function which explores the state space dilligently initially which then decays off to the
 * optimal solution after a large number of loop iterations.
 *
 * The normal distribution used by this function is:
 *
 * \f[ a e^{-\frac{bt^2}{c}} + \epsilon \f]
 *
 * where a, b and c are scaling coefficients (see method body) and \f$\epsilon\f$ is some small offset.
 *
 * @param t Number of loop iterations.
 * @return Value of normal distribution at t loop iterations.
 */
double probabilityFluxDensityCoeffieicent(unsigned long t);

//function to select next action

/**
 * @brief Selects an action to perform based on experience, iterations and probabilities.
 *
 * @param a_queue A priority queue instance storing integral types with double type priorities,
 *		  represents the queue of possible actions with pre-initialised priority levels.
 * @param iterations Number of loop iterations completed.
 * @return integer corresponding to chosen action
 */
int selectAction(PriorityQueue<int, double>& a_queue, unsigned long iterations);

/**
 * @brief Updates the utility (Q-value) of the system.
 *
 * Utility (Q-Value) of the system is updated via the Temporal Difference Learning Rule given by the following equation,
 *
 * \f[ Q_{i+1} (s,a) = Q_i (s,a) + \alpha [R(s') + \gamma \underset{a}{max} Q(s',a) - Q_i (s,a)] \f]
 *
 * where Q represents the utility of a state-action pair, \f$ \alpha \f$ is the learning rate, \f$ \gamma \f$ is the discount
 * factor and \f$ max_a (Q) \f$ yields the action-maximum of the Q space. The learning rate should be set to a low value for
 * highly stochastic, asymmetric systems or a high value for a lowly stochastic, symmetric system - this value lies between 0 and 1.
 * The discount factor (also in the interval [0,1]) represents the time considerations of the learning algorithm, lower values indicate
 * "living in the moment", whilst higher values indicate "planning for the future".
 *
 * @param space Reference to StateSpace object
 * @param action Integer code to previous performed action
 * @param new_state Reference to State instance giving the new system state
 * @param old_state Reference to State instance giving the old system state
 * @param alpha Learning rate of temporal difference learning algorithm (in the interval [0,1])
 * @param gamma Discount factor applied to q-learning equation (in the interval [0,1])
 */
void updateQ(StateSpace & space, int action, State & new_state, State & old_state, double alpha, double gamma);

/**
 * @brief Program launcher!
 *
 * Contains all initialisations including setting up the StateSpace and connecting to robot via a proxy. Holds main
 * program loop to perform algorithm continuously using calls to selectAction and updateQ sequentially to update
 * utilities and choose most optimal actions to perform based on position and velocity.
 *
 * @return Program exit code
 */
int main() {
	// STUFF WE DONT UNDERSTAND, AND DONT NEED TO
	//__________________________________________________________________________________________
	//__________________________________________________________________________________________
	// Libraries to load
	std::string bodyLibName = "bodyinfo";
	std::string movementLibName = "movementtools";

	// Name of camera module in library
	std::string bodyModuleName = "BodyInfo";
	std::string movementModuleName = "MovementTools";

	// Set broker name, ip and port, finding first available port from 54000
	const std::string brokerName = "MotionTimingBroker";
	int brokerPort = qi::os::findAvailablePort(54000);
	const std::string brokerIp = "0.0.0.0";

	// Default parent port and ip
	int pport = 9559;
	std::string pip = "127.0.0.1";

	// Need this for SOAP serialisation of floats to work
	setlocale(LC_NUMERIC, "C");

	// Create a broker
	boost::shared_ptr<AL::ALBroker> broker;
	try {
		broker = AL::ALBroker::createBroker(
			brokerName,
			brokerIp,
			brokerPort,
			pip,
			pport,
			0);
	}
	// Throw error and quit if a broker could not be created
	catch (...) {
		std::cerr << "Failed to connect broker to: "
			<< pip
			<< ":"
			<< pport
			<< std::endl;
		AL::ALBrokerManager::getInstance()->killAllBroker();
		AL::ALBrokerManager::kill();

		return 1;
	}

	// Add the broker to NAOqi
	AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock());
	AL::ALBrokerManager::getInstance()->addBroker(broker);

	CreateModule(movementLibName, movementModuleName, broker, false, true);
	CreateModule(bodyLibName, bodyModuleName, broker, false, true);

	AL::ALProxy bodyInfoProxy(bodyModuleName, pip, pport);
	AL::ALProxy movementToolsProxy(movementModuleName, pip, pport);
	AL::ALMotionProxy motion(pip, pport);
	//__________________________________________________________________________________________
	//__________________________________________________________________________________________
	//END OF STUFF WE DONT UNDERSTAND, BREATHE NOW

	//learning factor
	const double alpha = 0.8;
	//discount factor
	const double gamma = 0.5;

	//seed rng
	std::srand(static_cast<unsigned int>(std::time(NULL)));

	int action_forwards = FORWARD;
	int action_backwards = BACKWARD;
	int chosen_action = action_forwards;

	//create a priority queue to copy to all the state space priority queues
	PriorityQueue<int, double> initiator_queue(MAX);
	initiator_queue.enqueueWithPriority(action_forwards, 0);
	initiator_queue.enqueueWithPriority(action_backwards, 0);

	//create encoder
	Encoder encoder;
	encoder.Calibrate();

	//pause briefly to allow the robot to be given a push if desired
	qi::os::msleep(5000);

	//create the state space
	StateSpace space(100, 50, M_PI * 0.25, 1.0, initiator_queue);

	const char* filename = "output.txt";

	// if file containing previous state space data exists
	// get handle to this file and stream contents to space object
	if (fileExists(filename)) {
		std::ifstream inputFile(filename);
		std::string firstFileLine;
		std::getline(inputFile, firstFileLine);
		// if file is not empty, save file data to state space object
		if (!firstFileLine.empty())
			inputFile >> space;
	}

	//state objects
	State current_state(0, 0, FORWARD);
	State old_state(0, 0, FORWARD);

	std::cout << "Initialisation complete!" << std::endl;
	for( unsigned long i = 0; i<500 ;++i ) {
		// set current state angle to angle received from encoder
		// and set current state velocity to difference in new and
		// old state angles over some time difference
		current_state.theta = M_PI * (encoder.GetAngle()) / 180;
		current_state.theta_dot = (current_state.theta - old_state.theta) / 700; //Needs actual time
		current_state.robot_state = static_cast<ROBOT_STATE>(chosen_action);

		std::cout << "Angle of current state: " << current_state.theta << std::endl;

		// call updateQ function with state space, old and current states
		// and learning rate, discount factor
		updateQ(space, chosen_action, old_state, current_state, alpha, gamma);
		std::cout << "Update Q completed!" << std::endl;
		// set old_state to current_state
		old_state = current_state;

		// determine chosen_action for current state
		chosen_action = selectAction(space[current_state], i);
		std::cout << "Select Action completed!" << std::endl;
		// depending upon chosen action, call robot movement tools proxy with either
		// swingForwards or swingBackwards commands.
		(chosen_action) ? movementToolsProxy.callVoid("swingForwards") : movementToolsProxy.callVoid("swingBackwards");
	}

	std::ofstream output(filename);
	output<<space;
	output.close();

	return 1;
}

double probabilityFluxDensityCoeffieicent(unsigned long t) {
	return 100.0*std::exp((-8.0*t*t) / (2600.0*2600.0)) + 0.1;//0.1 is an offset
}

int selectAction(PriorityQueue<int, double>& a_queue, unsigned long iterations) {

	typedef std::vector<std::pair<int, double> > VecPair ;

	//turn priority queue into a vector of pairs
	VecPair vec = a_queue.saveOrderedQueueAsVector();

    //sum for partition function
	double sum = 0.0;

	// calculate partition function by iterating over action-values
	for (VecPair::iterator iter = vec.begin(), end = vec.end(); iter < end; ++iter) {
		sum += std::exp((iter->second) / probabilityFluxDensityCoeffieicent(iterations));
	}

	// compute Boltzmann factors for action-values and enqueue to vec
	for (VecPair::iterator iter = vec.begin(); iter < vec.end(); ++iter) {
		iter->second = std::exp(iter->second / probabilityFluxDensityCoeffieicent(iterations)) / sum;
	}

	// calculate cumulative probability distribution
	for (VecPair::iterator iter = vec.begin(), end = vec.end(); iter < end; ++iter) {
        //second member of pair becomes addition of its current value
        //and that of the index before it
        	if (iter != vec.begin())
			iter->second += (iter-1)->second;
	}

	//generate RN between 0 and 1
	double rand_num = static_cast<double>(rand()) / RAND_MAX;

	// choose action based on random number relation to priorities within action queue
	for (VecPair::iterator iter = vec.begin(), end = vec.end(); iter < end; ++iter) {
		if (rand_num < iter->second)
			return iter->first;
	}

	return -1; //note that this line should never be reached
}
int selectAction(PriorityQueue<int, double>& a_queue, unsigned long iterations) {
	double rand_num = static_cast<double>(rand()) / RAND_MAX);
	if(rand_num < epsilon){
		return a_queue.peekFront()->first;
	}

	rand_num = static_cast<double>(rand()) / RAND_MAX);
	return a_queue[ std::round( rand_num*(a_queue.getSize()-1) ) ];
}

void updateQ(StateSpace & space, int action, State & new_state, State & old_state, double alpha, double gamma) {
	//oldQ value reference
	double oldQ = space[old_state].search(action).second;

	//reward given to current state
	double R = new_state.getReward();

	//optimal Q value for new state i.e. first element
	double maxQ = space[new_state].peekFront().second;

	//new Q value determined by Q learning algorithm
	double newQ = oldQ + alpha * (R + (gamma * maxQ) - oldQ);

	// change priority of action to new Q value
	space[old_state].changePriority(action, newQ);
}
/* OLD SELECT ACTION
int selectAction(const PriorityQueue<int, double>& a_queue, unsigned long iterations) {
	/*
	// queue to store action values
	PriorityQueue<int, double> actionQueue(MAX);
	double sum = 0.0;
	// calculate partition function by iterating over action-values
	for (PriorityQueue<int, double>::const_iterator iter = a_queue.begin(), end = a_queue.end(); iter < end; ++iter) {
		sum += std::exp((iter->second) / temperature(iterations));
	}
	// compute Boltzmann factors for action-values and enqueue to actionQueue
	for (PriorityQueue<int, double>::const_iterator iter = a_queue.begin(); iter < a_queue.end(); ++iter) {
		double priority = std::exp(iter.operator*().second / temperature(iterations)) / sum;
		actionQueue.enqueueWithPriority(iter.operator*().first, priority);
	}
	// calculate cumulative probability distribution
	for (PriorityQueue<int, double>::const_iterator it1 = actionQueue.begin()++, it2 = actionQueue.begin(), end = actionQueue.end(); it1 < end; ++it1, ++it2) {
		// change priority of it1->first data item in actionQueue to
		// sum of priorities of it1 and it2 items
		actionQueue.changePriority(it1->first, it1->second + it2->second);
	}
	//generate RN between 0 and 1
	double rand_num = static_cast<double>(rand()) / RAND_MAX;
	// choose action based on random number relation to priorities within action queue
	for (PriorityQueue<int, double>::const_iterator iter = actionQueue.begin(), end = actionQueue.end(); iter < end; ++iter) {
		if (rand_num < iter->second)
			return iter->first;
	}

	return -1; //note that this line should never be reached

	double rand_num = static_cast<double>(rand()) / RAND_MAX;

	return (rand_num > 0.5)?0:1;
}
*/
<|endoftext|>


<commit_before>#include <cmath>
#include <cstdlib>
#include <ctime>
#include <string>
#include <sstream>
#include "StateSpace.h"
#include "PriorityQueue.h"
#include "State.h"
#include "encoder.h"
#include "CreateModule.h"

/**
 * @brief Gives a std::string representation of a primitive type
 *
 * @param x Primitive type such as int, double, long
 * @return std::string conversion of param x
 */
template<typename T> std::string to_string(T x) {
	return static_cast<std::ostringstream&>((std::ostringstream() << std::dec << x)).str();
}

//function to calculate a temperature for the select action function as a function of time
double temperature();

//function to select next action
int selectAction(PriorityQueue<int,double>& a_queue);

//function to update a q value
void updateQ(StateSpace & space, int action, State & new_state, State & old_state, double alpha, double gamma);

int main()
{
	// STUFF WE DONT UNDERSTAND, AND DONT NEED TO
	//__________________________________________________________________________________________
	//__________________________________________________________________________________________
	// Libraries to load
	std::string bodyLibName = "bodyinfo";
	std::string movementLibName = "movementtools";

	// Name of camera module in library
	std::string bodyModuleName = "BodyInfo";
	std::string movementModuleName = "MovementTools";

	// Set broker name, ip and port, finding first available port from 54000
	const std::string brokerName = "MotionTimingBroker";
	int brokerPort = qi::os::findAvailablePort(54000);
	const std::string brokerIp = "0.0.0.0";

	// Default parent port and ip
	int pport = 9559;
	std::string pip = "127.0.0.1";

	// Need this for SOAP serialisation of floats to work
	setlocale(LC_NUMERIC, "C");

	// Create a broker
	boost::shared_ptr<AL::ALBroker> broker;
	try
	{
		broker = AL::ALBroker::createBroker(
				brokerName,
				brokerIp,
				brokerPort,
				pip,
				pport,
				0);
	}
	// Throw error and quit if a broker could not be created
	catch(...)
	{
		std::cerr << "Failed to connect broker to: "
			  << pip
			  << ":"
			  << pport
			  << std::endl;
		AL::ALBrokerManager::getInstance()->killAllBroker();
		AL::ALBrokerManager::kill();

		return 1;
	}

	// Add the broker to NAOqi
	AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock());
	AL::ALBrokerManager::getInstance()->addBroker(broker);

	CreateModule(movementLibName, movementModuleName, broker, false, true);
	CreateModule(bodyLibName, bodyModuleName, broker, false, true);

	AL::ALProxy bodyInfoProxy(bodyModuleName, pip, pport);
	AL::ALProxy movementToolsProxy(movementModuleName, pip, pport);
	AL::ALMotionProxy motion(pip, pport);
	//__________________________________________________________________________________________
	//__________________________________________________________________________________________
	//END OF STUFF WE DONT UNDERSTAND, BREATHE NOW

	//learning factor
	const double alpha=0.5;
	//discount factor
	const double gamma=0.5;

	//seed rng
	std::srand(std::time(NULL));

	int action_forwards = FORWARD;
	int action_backwards = BACKWARD;
	int chosen_action = action_forwards;

	//create a priority queue to copy to all the state space priority queues
	PriorityQueue<int,double> initiator_queue(MAX);
	initiator_queue.enqueueWithPriority(action_forwards,0);
	initiator_queue.enqueueWithPriority(action_backwards,0);

	//create encoder
	Encoder encoder;
	encoder.Calibrate();

	//create the state space
	StateSpace space(100,50,initiator_queue);
	space.setAngleBins(100);
	space.setVelocityBins(50);

	//state objects
	State current_state(0,0,FORWARD);
	State old_state(0,0,FORWARD);

	while(true)
	{
		current_state.theta= M_PI * (encoder.GetAngle())/180;
		current_state.theta_dot=(current_state.theta - old_state.theta)/700; //Needs actual time
		current_state.robot_state=static_cast<ROBOT_STATE>(chosen_action);

		updateQ(space, chosen_action, old_state, current_state, alpha, gamma);

		old_state=current_state;

		chosen_action=selectAction(space[current_state]);

		(chosen_action)?movementToolsProxy.callVoid("swingForwards"):movementToolsProxy.callVoid("swingBackwards");
	}

	return 1;
}

double temperature()
{
	return static_cast<double>(std::time(NULL));
}

//function is fed with a priority queue of action-values
//generates Boltzmann distribution of these action-values
//and selects an action based on probabilities
int selectAction(PriorityQueue<int,double>& a_queue)
{
	typedef PriorityQueue<int,double> PQ;
	typedef std::vector< std::pair<int, double> > Vec_Pair;
	typedef std::pair<int, double> Pair;

	double sum= 0;
	size_t i = 0;

	size_t size = a_queue.getSize();
	Vec_Pair action_vec(size);

	//Calculate partition function by iterating over action-values
	for(PQ::const_iterator iter = a_queue.begin(),end=a_queue.end(); iter < end; ++iter)
	{
		sum += std::exp((iter->second)/temperature());
	}
	//Calculate boltzmann factors for action-values
	for(Vec_Pair::iterator it = action_vec.begin(),end=action_vec.end(); it < end; ++it)
	{
	    it->first = a_queue[i].first;
	    it->second = std::exp(a_queue[i].second /temperature()) / sum;
	    ++i;
	}

	//calculate cumulative probability distribution
	for(Vec_Pair::iterator it1 = action_vec.begin()++,it2 = action_vec.begin(),end=action_vec.end(); it1 < end; ++it1,++it2)
	{
	    it1->second += it2->second;
	}

	//generate RN between 0 and 1
	double rand_num = static_cast<double>(rand())/ RAND_MAX;

	//select action based on probability
	for(Vec_Pair::iterator it = action_vec.begin(),end=action_vec.end(); it < end; ++it)
	{
		//if RN falls within cumulative probability bin return the corresponding action
		if(rand_num < it->second)return it->first;
	}

	return -1; //note that this line should never be reached
}

void updateQ(StateSpace & space, int action, State & new_state, State & old_state, double alpha, double gamma)
{
    //oldQ value reference
    double oldQ = space[old_state].search(action).second;

    //reward given to current state
    double R = new_state.getReward();

    //optimal Q value for new state i.e. first element
    double maxQ = space[new_state].peekFront().second;

    //new Q value determined by Q learning algorithm
    double newQ = oldQ + alpha * (R + (gamma * maxQ) - oldQ);

    // change priority of action to new Q value
    space[old_state].changePriority(action, newQ);
}

/**
* @brief Selects an action to perform based on probabilities.
*
* @param a_queue A priority queue instance storing integral types with double type priorities,
represents the queue of possible actions with pre-initialised priority levels.
* @return integer corresponding to chosen action
*/
int selectActionAlt(const PriorityQueue<int, double>& a_queue) {

	// queue to store action values
	PriorityQueue<int, double> actionQueue(MAX);

	double sum = 0.0;

	// calculate partition function by iterating over action-values
	for (PriorityQueue<int, double>::const_iterator iter = a_queue.begin(), end = a_queue.end(); iter < end; ++iter) {
		sum += std::exp((iter->second) / temperature());
	}

	// compute Boltzmann factors for action-values and enqueue to actionQueue
	for (PriorityQueue<int, double>::const_iterator iter = a_queue.begin(); iter < a_queue.end(); ++iter) {
		double priority = std::exp(iter.operator*().second / temperature()) / sum;
		actionQueue.enqueueWithPriority(iter.operator*().first, priority);
	}

	// calculate cumulative probability distribution
	for (PriorityQueue<int, double>::const_iterator it1 = actionQueue.begin()++, it2 = actionQueue.begin(), end = actionQueue.end(); it1 < end; ++it1, ++it2) {
		// change priority of it1->first data item in actionQueue to
		// sum of priorities of it1 and it2 items
		actionQueue.changePriority(it1->first, it1->second + it2->second);
	}

	//generate RN between 0 and 1
	double rand_num = static_cast<double>(rand()) / RAND_MAX;
	std::cout << rand_num << std::endl;
	// choose action based on random number relation to priorities within action queue
	for (PriorityQueue<int, double>::const_iterator iter = actionQueue.begin(), end = actionQueue.end(); iter < end; ++iter) {
		if (rand_num < iter->second)
			return iter->first;
	}

	return -1; //note that this line should never be reached
}
<commit_msg>Removed redundant code and doxygenated some functions<commit_after>#include <cmath>
#include <cstdlib>
#include <ctime>
#include <string>
#include <sstream>
#include "StateSpace.h"
#include "PriorityQueue.h"
#include "State.h"
#include "encoder.h"
#include "CreateModule.h"

/**
 * @brief Gives a std::string representation of a primitive type
 *
 * @param x Primitive type such as int, double, long
 * @return std::string conversion of param x
 */
template<typename T> std::string to_string(T x) {
	return static_cast<std::ostringstream&>((std::ostringstream() << std::dec << x)).str();
}

//function to calculate a temperature for the select action function as a function of time
double temperature();

//function to select next action
int selectAction(PriorityQueue<int,double>& a_queue);

//function to update a q value
void updateQ(StateSpace & space, int action, State & new_state, State & old_state, double alpha, double gamma);

int main()
{
	// STUFF WE DONT UNDERSTAND, AND DONT NEED TO
	//__________________________________________________________________________________________
	//__________________________________________________________________________________________
	// Libraries to load
	std::string bodyLibName = "bodyinfo";
	std::string movementLibName = "movementtools";

	// Name of camera module in library
	std::string bodyModuleName = "BodyInfo";
	std::string movementModuleName = "MovementTools";

	// Set broker name, ip and port, finding first available port from 54000
	const std::string brokerName = "MotionTimingBroker";
	int brokerPort = qi::os::findAvailablePort(54000);
	const std::string brokerIp = "0.0.0.0";

	// Default parent port and ip
	int pport = 9559;
	std::string pip = "127.0.0.1";

	// Need this for SOAP serialisation of floats to work
	setlocale(LC_NUMERIC, "C");

	// Create a broker
	boost::shared_ptr<AL::ALBroker> broker;
	try
	{
		broker = AL::ALBroker::createBroker(
				brokerName,
				brokerIp,
				brokerPort,
				pip,
				pport,
				0);
	}
	// Throw error and quit if a broker could not be created
	catch(...)
	{
		std::cerr << "Failed to connect broker to: "
			  << pip
			  << ":"
			  << pport
			  << std::endl;
		AL::ALBrokerManager::getInstance()->killAllBroker();
		AL::ALBrokerManager::kill();

		return 1;
	}

	// Add the broker to NAOqi
	AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock());
	AL::ALBrokerManager::getInstance()->addBroker(broker);

	CreateModule(movementLibName, movementModuleName, broker, false, true);
	CreateModule(bodyLibName, bodyModuleName, broker, false, true);

	AL::ALProxy bodyInfoProxy(bodyModuleName, pip, pport);
	AL::ALProxy movementToolsProxy(movementModuleName, pip, pport);
	AL::ALMotionProxy motion(pip, pport);
	//__________________________________________________________________________________________
	//__________________________________________________________________________________________
	//END OF STUFF WE DONT UNDERSTAND, BREATHE NOW

	//learning factor
	const double alpha=0.5;
	//discount factor
	const double gamma=0.5;

	//seed rng
	std::srand(std::time(NULL));

	int action_forwards = FORWARD;
	int action_backwards = BACKWARD;
	int chosen_action = action_forwards;

	//create a priority queue to copy to all the state space priority queues
	PriorityQueue<int,double> initiator_queue(MAX);
	initiator_queue.enqueueWithPriority(action_forwards,0);
	initiator_queue.enqueueWithPriority(action_backwards,0);

	//create encoder
	Encoder encoder;
	encoder.Calibrate();

	//create the state space
	StateSpace space(100,50,initiator_queue);
	space.setAngleBins(100);
	space.setVelocityBins(50);

	//state objects
	State current_state(0,0,FORWARD);
	State old_state(0,0,FORWARD);

	while(true)
	{
		current_state.theta= M_PI * (encoder.GetAngle())/180;
		current_state.theta_dot=(current_state.theta - old_state.theta)/700; //Needs actual time
		current_state.robot_state=static_cast<ROBOT_STATE>(chosen_action);

		updateQ(space, chosen_action, old_state, current_state, alpha, gamma);

		old_state=current_state;

		chosen_action=selectAction(space[current_state]);

		(chosen_action)?movementToolsProxy.callVoid("swingForwards"):movementToolsProxy.callVoid("swingBackwards");
	}

	return 1;
}

/**
 * @brief Analog to temperature variable in Boltzmann Distribution.
 *
 * @return current system time in milliseconds
 */
double temperature()
{
	return static_cast<double>(std::time(NULL));
}

/**
 * @brief Selects an action to perform based on probabilities.
 *
 * @param a_queue A priority queue instance storing integral types with double type priorities,
 *		  represents the queue of possible actions with pre-initialised priority levels.
 * @return integer corresponding to chosen action
 */
int selectAction(const PriorityQueue<int,double>& a_queue)
{
	typedef PriorityQueue<int,double> PQ;
	typedef std::vector< std::pair<int, double> > Vec_Pair;
	typedef std::pair<int, double> Pair;

	double sum= 0;
	size_t i = 0;

	size_t size = a_queue.getSize();
	Vec_Pair action_vec(size);

	//Calculate partition function by iterating over action-values
	for(PQ::const_iterator iter = a_queue.begin(),end=a_queue.end(); iter < end; ++iter)
	{
		sum += std::exp((iter->second)/temperature());
	}
	//Calculate boltzmann factors for action-values
	for(Vec_Pair::iterator it = action_vec.begin(),end=action_vec.end(); it < end; ++it)
	{
	    it->first = a_queue[i].first;
	    it->second = std::exp(a_queue[i].second /temperature()) / sum;
	    ++i;
	}

	//calculate cumulative probability distribution
	for(Vec_Pair::iterator it1 = action_vec.begin()++,it2 = action_vec.begin(),end=action_vec.end(); it1 < end; ++it1,++it2)
	{
	    it1->second += it2->second;
	}

	//generate RN between 0 and 1
	double rand_num = static_cast<double>(rand())/ RAND_MAX;

	//select action based on probability
	for(Vec_Pair::iterator it = action_vec.begin(),end=action_vec.end(); it < end; ++it)
	{
		//if RN falls within cumulative probability bin return the corresponding action
		if(rand_num < it->second)return it->first;
	}

	return -1; //note that this line should never be reached
}

/**
 * @brief Updates the utility (Q-value) of the system
 *
 * @param space Reference to StateSpace object
 * @param new_state Reference to State instance giving the new system state
 * @param old_state Reference to State instance giving the old system state
 * @param alpha Learning rate of temporal difference learning algorithm
 * @param gamma Discount factor applied to q-learning equation
 */
void updateQ(StateSpace & space, int action, State & new_state, State & old_state, double alpha, double gamma)
{
    //oldQ value reference
    double oldQ = space[old_state].search(action).second;

    //reward given to current state
    double R = new_state.getReward();

    //optimal Q value for new state i.e. first element
    double maxQ = space[new_state].peekFront().second;

    //new Q value determined by Q learning algorithm
    double newQ = oldQ + alpha * (R + (gamma * maxQ) - oldQ);

    // change priority of action to new Q value
    space[old_state].changePriority(action, newQ);
}

/**
 * @brief Selects an action to perform based on probabilities.
 *
 * @param a_queue A priority queue instance storing integral types with double type priorities,
 *		  represents the queue of possible actions with pre-initialised priority levels.
 * @return integer corresponding to chosen action
 */
int selectActionAlt(const PriorityQueue<int, double>& a_queue) {

	// queue to store action values
	PriorityQueue<int, double> actionQueue(MAX);

	double sum = 0.0;

	// calculate partition function by iterating over action-values
	for (PriorityQueue<int, double>::const_iterator iter = a_queue.begin(), end = a_queue.end(); iter < end; ++iter) {
		sum += std::exp((iter->second) / temperature());
	}

	// compute Boltzmann factors for action-values and enqueue to actionQueue
	for (PriorityQueue<int, double>::const_iterator iter = a_queue.begin(); iter < a_queue.end(); ++iter) {
		double priority = std::exp(iter.operator*().second / temperature()) / sum;
		actionQueue.enqueueWithPriority(iter.operator*().first, priority);
	}

	// calculate cumulative probability distribution
	for (PriorityQueue<int, double>::const_iterator it1 = actionQueue.begin()++, it2 = actionQueue.begin(), end = actionQueue.end(); it1 < end; ++it1, ++it2) {
		// change priority of it1->first data item in actionQueue to
		// sum of priorities of it1 and it2 items
		actionQueue.changePriority(it1->first, it1->second + it2->second);
	}

	//generate RN between 0 and 1
	double rand_num = static_cast<double>(rand()) / RAND_MAX;

	// choose action based on random number relation to priorities within action queue
	for (PriorityQueue<int, double>::const_iterator iter = actionQueue.begin(), end = actionQueue.end(); iter < end; ++iter) {
		if (rand_num < iter->second)
			return iter->first;
	}

	return -1; //note that this line should never be reached
}
<|endoftext|>


<commit_before>#include "DTE.h"
#include "URC.h"

void DTE::debugPrint(const char message[], bool returnChar) {
	if(debug) {
		Serial.print(message);
		if(returnChar) Serial.println();
	}
}

void DTE::debugPrint(const __FlashStringHelper *message, bool returnChar) {
	char buffer[strlen_P((const char *)message)+1];
	strcpy_P(buffer, (const char *)message);
	debugPrint(buffer, returnChar);
}

bool DTE::atReIssueLastCommand(void) {
  const __FlashStringHelper *command = F("A/\r");

	clearReceivedBuffer();
  if(!ATCommand(command)) return false;
	if(!ATResponseOk()) return false;
	return true;
}

bool DTE::atSetCommandEchoMode(bool echo) {
  const __FlashStringHelper *command = F("ATE%d&W\r");
	char buffer[8]; // "ATEX&W\r"

	sprintf_P(buffer, (const char *)command, echo?1:0);

	clearReceivedBuffer();
  if(!ATCommand(buffer)) return false;
	if(!ATResponseOk()) return false;
  this->echo = echo;
	return true;
}

bool DTE::atSetLocalDataFlowControl(void) {
	const __FlashStringHelper *command = F("AT+IFC?\r");
	const __FlashStringHelper *response = F("+IFC: ");
  struct FlowControl flowControl;

  flowControl = this->flowControl;
	clearReceivedBuffer();
  if(!ATCommand(command)) return false;
	if(!ATResponseContain(response)) return false;
	char *pointer = strstr_P(getResponse(), (const char *)response) + strlen_P((const char *)response);
  char *str = strtok(pointer, ",");
  for (size_t i = 0; i < 2 && str != NULL; i++) {
    if (i == 0) flowControl.dce = str[0] - '0';
    if (i == 1) flowControl.dte = str[0] - '0';
    str = strtok(NULL, ",");
  }
  if(!ATResponseOk()) return false;
	this->flowControl = flowControl;
	return true;
}

bool DTE::atSetLocalDataFlowControl(unsigned char dce, unsigned char dte) {
  const __FlashStringHelper *command = F("AT+IFC=%d,%d;&W\r");
	char buffer[15]; // "AT+IFC=X,X;&W\r"

	sprintf_P(buffer, (const char *)command, dce, dte);

	clearReceivedBuffer();
  if(!ATCommand(buffer)) return false;
	if(!ATResponseOk()) return false;
  flowControl.dce = dce;
  flowControl.dte = dte;
	return true;
}

bool DTE::atSetFixedLocalRate(void) {
	const __FlashStringHelper *command = F("AT+IPR?\r");
	const __FlashStringHelper *response = F("+IPR: ");
  long baudrate = 0;

	clearReceivedBuffer();
  if(!ATCommand(command)) return false;
	if(!ATResponseContain(response)) return false;
	char *str = strstr_P(getResponse(), (const char *)response) + strlen_P((const char *)response);
  baudrate = atoi(str);
  if(!ATResponseOk()) return false;
	this->baudrate = baudrate;
	return true;
}

bool DTE::atSetFixedLocalRate(long baudrate) {
  const __FlashStringHelper *command = F("AT+IPR=%ld\r");
	char buffer[15]; // "AT+IPR=XXXXXX\r"

	sprintf_P(buffer, (const char *)command, baudrate);

	clearReceivedBuffer();
  if(!ATCommand(buffer)) return false;
	if(!ATResponseOk()) return false;
  this->baudrate = baudrate;
	return true;
}

/* DTE Class */
DTE::DTE(HardwareSerial &hardwareSerial, int powerPin, bool debug) {
	this->hardwareSerial = &hardwareSerial;
	this->softwareSerial = NULL;
	this->powerPin = powerPin;
	this->debug = debug;
	this->response[0] = '\0';
	this->echo = true;
	this->flowControl = (struct FlowControl) { 0, true, 0, true };
	this->baudrate = -1;
	this->powerDown = true;

	pinMode(this->powerPin, OUTPUT);
	digitalWrite(this->powerPin, LOW);
}

DTE::DTE(SoftwareSerial &softwareSerial, int powerPin, bool debug) {
	this->hardwareSerial = NULL;
	this->softwareSerial = &softwareSerial;
	this->powerPin = powerPin;
	this->debug = debug;
	this->response[0] = '\0';
	this->echo = true;
	this->flowControl = (struct FlowControl) { 0, true, 0, true };
	this->baudrate = -1;
	this->powerDown = true;

	pinMode(this->powerPin, OUTPUT);
	digitalWrite(this->powerPin, LOW);
}

int DTE::available(void) {
	if(hardwareSerial) return hardwareSerial->available();
	if(softwareSerial) return softwareSerial->available();
	return -1;
}

bool DTE::isListening(void) {
	if(softwareSerial) return softwareSerial->isListening();
	return false;
}

bool DTE::listen(void) {
	if(softwareSerial) return softwareSerial->listen();
	return false;
}

size_t DTE::write(const char str[]) {
	if(hardwareSerial) return hardwareSerial->write(str);
	if(softwareSerial) return softwareSerial->write(str);
	return 0;
}

size_t DTE::readBytes(char buffer[], size_t length) {
	if(hardwareSerial) return hardwareSerial->readBytes(buffer, length);
	if(softwareSerial) return softwareSerial->readBytes(buffer, length);
	return 0;
}

void DTE::togglePower(void) {
	debugPrint(F("Toggle Power"), true);

	digitalWrite(powerPin, HIGH);
	delay(1200);
	digitalWrite(powerPin, LOW);
	powerDown = false;
	flowControl = (struct FlowControl) { 0, true, 0, true };
	while(ATResponse(3000)) {
		if(isResponseEqual(F("RDY"))) {
			powerDown = false;
			Urc.resetUnsolicitedResultCode();
			if(AT()) return;
		}
		if(isResponseEqual(F("NORMAL POWER DOWN"))) {
			powerDown = true;
			delay(1000);
			return;
		}
		else {
			unsolicitedResultCode();
		}
	}
	if (AT()) {
		powerDown = false;
		Urc.resetUnsolicitedResultCode();
		return;
	}
	else {
		powerDown = true;
		delay(1000);
		return;
	}
}

void DTE::clearReceivedBuffer(void) {
	debugPrint(F("clearReceivedBuffer"), true);
  setFlowControlStatusDce(true);
  unsigned long t = millis();
	if(hardwareSerial) {
    while(hardwareSerial->available() == 0 && millis() - t < 50);
		if(hardwareSerial->available() == 0) {
		  setFlowControlStatusDce(false);
			return;
		}
	}
	if(softwareSerial) {
    while(softwareSerial->available() == 0 && millis() - t < 50);
		if(softwareSerial->available() == 0) {
		  setFlowControlStatusDce(false);
			return;
		}
	}
	while (ATResponse(50)) {
		if(isResponseEqual("RDY")) echo = true;
		else unsolicitedResultCode();
	}
  setFlowControlStatusDce(false);
}

bool DTE::AT(void) {
  const __FlashStringHelper *command = F("AT\r");

	powerDown = false;
	echo = false;
	ATCommand(command);
	while(true) {
		if(!ATResponse()) {
			powerDown = true;
			break;
		}
		if(isResponseEqual(command)) echo = true;
		else if(isResponseOk()) return true;
		else unsolicitedResultCode();
	}
  powerDown = true;
	return false;
}

bool DTE::ATCommand(const char at[]) {
	debugPrint("Command: ");
	debugPrint(at, true);

  setFlowControlStatusDce(false);
	if(hardwareSerial) hardwareSerial->print(at);
	if(softwareSerial) softwareSerial->print(at);
	if(echo) {
    if(strlen(at) > (sizeof(response)-2)) {
      char atEcho[strlen(at) + 3];
      while (true) {
        if (!ATResponse(atEcho, sizeof(atEcho))) return false;
        if (strstr(atEcho, "ERROR") != NULL) return false;
        if (strcmp(atEcho, at) == 0) break;
        else Urc.unsolicitedResultCode(atEcho);
      }
    }
    else if(!ATResponseEqual(at)) return false;
  }
	return true;
}

bool DTE::ATCommand(const __FlashStringHelper *at) {
	char buffer[strlen_P((const char *)at)+1];
	strcpy_P(buffer, (const char *)at);
	return ATCommand(buffer);
}

bool DTE::ATResponse(char buffer[], size_t bufferSize, unsigned long timeout) {
	if(powerDown) {
		debugPrint(F("Power Down"), true);
		return false;
	}

  setFlowControlStatusDce(true);

	unsigned long t = millis();
	while(true) {
		if(hardwareSerial) {
			if (hardwareSerial->available() > 0) break;
		}
		if(softwareSerial) {
			if (softwareSerial->available() > 0) break;
		}
		if(millis() - t > timeout) {
			debugPrint(F("No response"), true);
			return false;
		}
	}

	t = millis();
	unsigned int i = 0;
	while(true) {
		if(hardwareSerial) {
			while (hardwareSerial->available() > 0) {
				buffer[i++] = hardwareSerial->read();
				t = millis();
        if(i >= 2) {
    			if(buffer[i-2] == '\r' && buffer[i-1] == '\n') break;
        }
			}
		}
		if(softwareSerial) {
			while (softwareSerial->available() > 0) {
				buffer[i++] = softwareSerial->read();
				t = millis();
        if(i >= 2) {
    			if(buffer[i-2] == '\r' && buffer[i-1] == '\n') break;
        }
			}
		}
		if(i >= 2) {
			if(buffer[i-2] == '\r' && buffer[i-1] == '\n') {
				buffer[i-2] = '\0';
				if(strlen(buffer) > 0) break;
				else i = 0;
			}
		}
		if(millis() - t > 50 || i >= (bufferSize-1)) {
			buffer[i] = '\0';
			break;
		}
	}
	debugPrint(F("Response: "));
	debugPrint(buffer, true);
	return true;
}

bool DTE::ATResponse(unsigned long timeout) {
  return ATResponse(response, sizeof(response), timeout);
}

bool DTE::ATResponseEqual(const char expected[], unsigned long timeout) {
	while(true) {
		if(!ATResponse(timeout)) return false;
		if(isResponseContain(F("ERROR"))) return false;
		if(isResponseEqual(expected)) break;
		if(isResponseEqual(F("RDY"))) echo = true;
		else unsolicitedResultCode();
	}
	return true;
}

bool DTE::ATResponseEqual(const __FlashStringHelper *expected, unsigned long timeout) {
	char buffer[strlen_P((const char *)expected)+1];
	strcpy_P(buffer, (const char *)expected);
	return ATResponseEqual(buffer, timeout);
}

bool DTE::ATResponseContain(const char expected[], unsigned long timeout) {
	while(true) {
		if(!ATResponse(timeout)) return false;
		if(isResponseContain(F("ERROR"))) return false;
		if(isResponseContain(expected)) break;
		if(isResponseEqual(F("RDY"))) echo = true;
		else unsolicitedResultCode();
	}
	return true;
}

bool DTE::ATResponseContain(const __FlashStringHelper *expected, unsigned long timeout) {
	char buffer[strlen_P((const char *)expected)+1];
	strcpy_P(buffer, (const char *)expected);
	return ATResponseContain(buffer, timeout);
}

bool DTE::ATResponseOk(unsigned long timeout) {
	if(!ATResponseEqual(F("OK"), timeout)) return false;
	if(available()) clearReceivedBuffer();
	setFlowControlStatusDce(false);
	return true;
}

bool DTE::isResponseEqual(const char expected[]) {
	if(strcmp(response, expected) != 0) return false;
	return true;
}

bool DTE::isResponseEqual(const __FlashStringHelper *expected) {
	char buffer[strlen_P((const char *)expected)+1];
	strcpy_P(buffer, (const char *)expected);
	return isResponseEqual(buffer);
}

bool DTE::isResponseContain(const char expected[]) {
	if(strstr(response, expected) == NULL) return false;
	return true;
}

bool DTE::isResponseContain(const __FlashStringHelper *expected) {
	char buffer[strlen_P((const char *)expected)+1];
	strcpy_P(buffer, (const char *)expected);
	return isResponseContain(buffer);
}

bool DTE::isResponseOk(void) {
  if(!isResponseContain(F("OK"))) return false;
  if(available()) clearReceivedBuffer();
  setFlowControlStatusDce(false);
	return true;
}

bool DTE::unsolicitedResultCode(void) {
	debugPrint(F("URC: "));
	debugPrint(response, true);
  return Urc.unsolicitedResultCode(response);
}

bool DTE::setEcho(bool echo) {
	if (this->echo == echo) return true;
	if (!atSetCommandEchoMode(echo)) return false;
	return true;
}

struct FlowControl DTE::getFlowControl(void) {
  if(!atSetLocalDataFlowControl()) return  (struct FlowControl) { 0, true, 0, true };
  return this->flowControl;
}

bool DTE::setFlowControl(unsigned char dce, unsigned char dte) {
  if(!atSetLocalDataFlowControl(dce, dte)) return false;
  if(flowControl.dce == 1) setFlowControlStatusDce(false);
  return true;
}

bool DTE::setFlowControlStatusDce(bool on) {
  if (flowControl.dce != 1) return false;
  if (flowControl.dceOn == on) return true;
  if (hardwareSerial) hardwareSerial->write(on?17:19);
  if (softwareSerial) softwareSerial->write(on?17:19);
  flowControl.dceOn = on;
  return true;
}

long DTE::getBaudrate(void) {
  if(!atSetLocalDataFlowControl()) return 0;
  return baudrate;
}

bool DTE::powerReset(void) {
	togglePower();
	while (powerDown) {
		togglePower();
	}
	powerDown = false;
	if(isEcho())
		setEcho(false);
	if(getFlowControl().dce == 0)
  	setFlowControl(1, 0);
	setFlowControlStatusDce(false);
	return true;
}
<commit_msg>Move initialization to togglePower<commit_after>#include "DTE.h"
#include "URC.h"

void DTE::debugPrint(const char message[], bool returnChar) {
	if(debug) {
		Serial.print(message);
		if(returnChar) Serial.println();
	}
}

void DTE::debugPrint(const __FlashStringHelper *message, bool returnChar) {
	char buffer[strlen_P((const char *)message)+1];
	strcpy_P(buffer, (const char *)message);
	debugPrint(buffer, returnChar);
}

bool DTE::atReIssueLastCommand(void) {
  const __FlashStringHelper *command = F("A/\r");

	clearReceivedBuffer();
  if(!ATCommand(command)) return false;
	if(!ATResponseOk()) return false;
	return true;
}

bool DTE::atSetCommandEchoMode(bool echo) {
  const __FlashStringHelper *command = F("ATE%d&W\r");
	char buffer[8]; // "ATEX&W\r"

	sprintf_P(buffer, (const char *)command, echo?1:0);

	clearReceivedBuffer();
  if(!ATCommand(buffer)) return false;
	if(!ATResponseOk()) return false;
  this->echo = echo;
	return true;
}

bool DTE::atSetLocalDataFlowControl(void) {
	const __FlashStringHelper *command = F("AT+IFC?\r");
	const __FlashStringHelper *response = F("+IFC: ");
  struct FlowControl flowControl;

  flowControl = this->flowControl;
	clearReceivedBuffer();
  if(!ATCommand(command)) return false;
	if(!ATResponseContain(response)) return false;
	char *pointer = strstr_P(getResponse(), (const char *)response) + strlen_P((const char *)response);
  char *str = strtok(pointer, ",");
  for (size_t i = 0; i < 2 && str != NULL; i++) {
    if (i == 0) flowControl.dce = str[0] - '0';
    if (i == 1) flowControl.dte = str[0] - '0';
    str = strtok(NULL, ",");
  }
  if(!ATResponseOk()) return false;
	this->flowControl = flowControl;
	return true;
}

bool DTE::atSetLocalDataFlowControl(unsigned char dce, unsigned char dte) {
  const __FlashStringHelper *command = F("AT+IFC=%d,%d;&W\r");
	char buffer[15]; // "AT+IFC=X,X;&W\r"

	sprintf_P(buffer, (const char *)command, dce, dte);

	clearReceivedBuffer();
  if(!ATCommand(buffer)) return false;
	if(!ATResponseOk()) return false;
  flowControl.dce = dce;
  flowControl.dte = dte;
	return true;
}

bool DTE::atSetFixedLocalRate(void) {
	const __FlashStringHelper *command = F("AT+IPR?\r");
	const __FlashStringHelper *response = F("+IPR: ");
  long baudrate = 0;

	clearReceivedBuffer();
  if(!ATCommand(command)) return false;
	if(!ATResponseContain(response)) return false;
	char *str = strstr_P(getResponse(), (const char *)response) + strlen_P((const char *)response);
  baudrate = atoi(str);
  if(!ATResponseOk()) return false;
	this->baudrate = baudrate;
	return true;
}

bool DTE::atSetFixedLocalRate(long baudrate) {
  const __FlashStringHelper *command = F("AT+IPR=%ld\r");
	char buffer[15]; // "AT+IPR=XXXXXX\r"

	sprintf_P(buffer, (const char *)command, baudrate);

	clearReceivedBuffer();
  if(!ATCommand(buffer)) return false;
	if(!ATResponseOk()) return false;
  this->baudrate = baudrate;
	return true;
}

/* DTE Class */
DTE::DTE(HardwareSerial &hardwareSerial, int powerPin, bool debug) {
	this->hardwareSerial = &hardwareSerial;
	this->softwareSerial = NULL;
	this->powerPin = powerPin;
	this->debug = debug;
	this->response[0] = '\0';
	this->echo = true;
	this->flowControl = (struct FlowControl) { 0, true, 0, true };
	this->baudrate = -1;
	this->powerDown = true;

	pinMode(this->powerPin, OUTPUT);
	digitalWrite(this->powerPin, LOW);
}

DTE::DTE(SoftwareSerial &softwareSerial, int powerPin, bool debug) {
	this->hardwareSerial = NULL;
	this->softwareSerial = &softwareSerial;
	this->powerPin = powerPin;
	this->debug = debug;
	this->response[0] = '\0';
	this->echo = true;
	this->flowControl = (struct FlowControl) { 0, true, 0, true };
	this->baudrate = -1;
	this->powerDown = true;

	pinMode(this->powerPin, OUTPUT);
	digitalWrite(this->powerPin, LOW);
}

int DTE::available(void) {
	if(hardwareSerial) return hardwareSerial->available();
	if(softwareSerial) return softwareSerial->available();
	return -1;
}

bool DTE::isListening(void) {
	if(softwareSerial) return softwareSerial->isListening();
	return false;
}

bool DTE::listen(void) {
	if(softwareSerial) return softwareSerial->listen();
	return false;
}

size_t DTE::write(const char str[]) {
	if(hardwareSerial) return hardwareSerial->write(str);
	if(softwareSerial) return softwareSerial->write(str);
	return 0;
}

size_t DTE::readBytes(char buffer[], size_t length) {
	if(hardwareSerial) return hardwareSerial->readBytes(buffer, length);
	if(softwareSerial) return softwareSerial->readBytes(buffer, length);
	return 0;
}

void DTE::togglePower(void) {
	debugPrint(F("Toggle Power"), true);

	digitalWrite(powerPin, HIGH);
	delay(1200);
	digitalWrite(powerPin, LOW);
	powerDown = false;
	flowControl = (struct FlowControl) { 0, true, 0, true };
	while(ATResponse(3000)) {
		if(isResponseEqual(F("RDY"))) {
			powerDown = false;
			Urc.resetUnsolicitedResultCode();
			if(AT()) return;
		}
		if(isResponseEqual(F("NORMAL POWER DOWN"))) {
			powerDown = true;
			delay(1000);
			return;
		}
		else {
			unsolicitedResultCode();
		}
	}
	if (AT()) {
		powerDown = false;
		Urc.resetUnsolicitedResultCode();
		if(isEcho())
			setEcho(false);
		if(getFlowControl().dce == 0)
	  	setFlowControl(1, 0);
		setFlowControlStatusDce(false);
		return;
	}
	else {
		powerDown = true;
		delay(1000);
		return;
	}
}

void DTE::clearReceivedBuffer(void) {
	debugPrint(F("clearReceivedBuffer"), true);
  setFlowControlStatusDce(true);
  unsigned long t = millis();
	if(hardwareSerial) {
    while(hardwareSerial->available() == 0 && millis() - t < 50);
		if(hardwareSerial->available() == 0) {
		  setFlowControlStatusDce(false);
			return;
		}
	}
	if(softwareSerial) {
    while(softwareSerial->available() == 0 && millis() - t < 50);
		if(softwareSerial->available() == 0) {
		  setFlowControlStatusDce(false);
			return;
		}
	}
	while (ATResponse(50)) {
		if(isResponseEqual("RDY")) echo = true;
		else unsolicitedResultCode();
	}
  setFlowControlStatusDce(false);
}

bool DTE::AT(void) {
  const __FlashStringHelper *command = F("AT\r");

	powerDown = false;
	echo = false;
	ATCommand(command);
	while(true) {
		if(!ATResponse()) {
			powerDown = true;
			break;
		}
		if(isResponseEqual(command)) echo = true;
		else if(isResponseOk()) return true;
		else unsolicitedResultCode();
	}
  powerDown = true;
	return false;
}

bool DTE::ATCommand(const char at[]) {
	debugPrint("Command: ");
	debugPrint(at, true);

  setFlowControlStatusDce(false);
	if(hardwareSerial) hardwareSerial->print(at);
	if(softwareSerial) softwareSerial->print(at);
	if(echo) {
    if(strlen(at) > (sizeof(response)-2)) {
      char atEcho[strlen(at) + 3];
      while (true) {
        if (!ATResponse(atEcho, sizeof(atEcho))) return false;
        if (strstr(atEcho, "ERROR") != NULL) return false;
        if (strcmp(atEcho, at) == 0) break;
        else Urc.unsolicitedResultCode(atEcho);
      }
    }
    else if(!ATResponseEqual(at)) return false;
  }
	return true;
}

bool DTE::ATCommand(const __FlashStringHelper *at) {
	char buffer[strlen_P((const char *)at)+1];
	strcpy_P(buffer, (const char *)at);
	return ATCommand(buffer);
}

bool DTE::ATResponse(char buffer[], size_t bufferSize, unsigned long timeout) {
	if(powerDown) {
		debugPrint(F("Power Down"), true);
		return false;
	}

  setFlowControlStatusDce(true);

	unsigned long t = millis();
	while(true) {
		if(hardwareSerial) {
			if (hardwareSerial->available() > 0) break;
		}
		if(softwareSerial) {
			if (softwareSerial->available() > 0) break;
		}
		if(millis() - t > timeout) {
			debugPrint(F("No response"), true);
			return false;
		}
	}

	t = millis();
	unsigned int i = 0;
	while(true) {
		if(hardwareSerial) {
			while (hardwareSerial->available() > 0) {
				buffer[i++] = hardwareSerial->read();
				t = millis();
        if(i >= 2) {
    			if(buffer[i-2] == '\r' && buffer[i-1] == '\n') break;
        }
			}
		}
		if(softwareSerial) {
			while (softwareSerial->available() > 0) {
				buffer[i++] = softwareSerial->read();
				t = millis();
        if(i >= 2) {
    			if(buffer[i-2] == '\r' && buffer[i-1] == '\n') break;
        }
			}
		}
		if(i >= 2) {
			if(buffer[i-2] == '\r' && buffer[i-1] == '\n') {
				buffer[i-2] = '\0';
				if(strlen(buffer) > 0) break;
				else i = 0;
			}
		}
		if(millis() - t > 50 || i >= (bufferSize-1)) {
			buffer[i] = '\0';
			break;
		}
	}
	debugPrint(F("Response: "));
	debugPrint(buffer, true);
	return true;
}

bool DTE::ATResponse(unsigned long timeout) {
  return ATResponse(response, sizeof(response), timeout);
}

bool DTE::ATResponseEqual(const char expected[], unsigned long timeout) {
	while(true) {
		if(!ATResponse(timeout)) return false;
		if(isResponseContain(F("ERROR"))) return false;
		if(isResponseEqual(expected)) break;
		if(isResponseEqual(F("RDY"))) echo = true;
		else unsolicitedResultCode();
	}
	return true;
}

bool DTE::ATResponseEqual(const __FlashStringHelper *expected, unsigned long timeout) {
	char buffer[strlen_P((const char *)expected)+1];
	strcpy_P(buffer, (const char *)expected);
	return ATResponseEqual(buffer, timeout);
}

bool DTE::ATResponseContain(const char expected[], unsigned long timeout) {
	while(true) {
		if(!ATResponse(timeout)) return false;
		if(isResponseContain(F("ERROR"))) return false;
		if(isResponseContain(expected)) break;
		if(isResponseEqual(F("RDY"))) echo = true;
		else unsolicitedResultCode();
	}
	return true;
}

bool DTE::ATResponseContain(const __FlashStringHelper *expected, unsigned long timeout) {
	char buffer[strlen_P((const char *)expected)+1];
	strcpy_P(buffer, (const char *)expected);
	return ATResponseContain(buffer, timeout);
}

bool DTE::ATResponseOk(unsigned long timeout) {
	if(!ATResponseEqual(F("OK"), timeout)) return false;
	if(available()) clearReceivedBuffer();
	setFlowControlStatusDce(false);
	return true;
}

bool DTE::isResponseEqual(const char expected[]) {
	if(strcmp(response, expected) != 0) return false;
	return true;
}

bool DTE::isResponseEqual(const __FlashStringHelper *expected) {
	char buffer[strlen_P((const char *)expected)+1];
	strcpy_P(buffer, (const char *)expected);
	return isResponseEqual(buffer);
}

bool DTE::isResponseContain(const char expected[]) {
	if(strstr(response, expected) == NULL) return false;
	return true;
}

bool DTE::isResponseContain(const __FlashStringHelper *expected) {
	char buffer[strlen_P((const char *)expected)+1];
	strcpy_P(buffer, (const char *)expected);
	return isResponseContain(buffer);
}

bool DTE::isResponseOk(void) {
  if(!isResponseContain(F("OK"))) return false;
  if(available()) clearReceivedBuffer();
  setFlowControlStatusDce(false);
	return true;
}

bool DTE::unsolicitedResultCode(void) {
	debugPrint(F("URC: "));
	debugPrint(response, true);
  return Urc.unsolicitedResultCode(response);
}

bool DTE::setEcho(bool echo) {
	if (this->echo == echo) return true;
	if (!atSetCommandEchoMode(echo)) return false;
	return true;
}

struct FlowControl DTE::getFlowControl(void) {
  if(!atSetLocalDataFlowControl()) return  (struct FlowControl) { 0, true, 0, true };
  return this->flowControl;
}

bool DTE::setFlowControl(unsigned char dce, unsigned char dte) {
  if(!atSetLocalDataFlowControl(dce, dte)) return false;
  if(flowControl.dce == 1) setFlowControlStatusDce(false);
  return true;
}

bool DTE::setFlowControlStatusDce(bool on) {
  if (flowControl.dce != 1) return false;
  if (flowControl.dceOn == on) return true;
  if (hardwareSerial) hardwareSerial->write(on?17:19);
  if (softwareSerial) softwareSerial->write(on?17:19);
  flowControl.dceOn = on;
  return true;
}

long DTE::getBaudrate(void) {
  if(!atSetLocalDataFlowControl()) return 0;
  return baudrate;
}

bool DTE::powerReset(void) {
	togglePower();
	while (powerDown) {
		togglePower();
	}
	return true;
}
<|endoftext|>


<commit_before>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile$
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "mitkInteractor.h"
#include <mitkDataTreeNode.h>
#include <mitkDisplayPositionEvent.h>
#include <mitkPositionEvent.h>
#include <mitkGeometry3D.h>
#include <mitkAction.h>
#include <mitkStatusBar.h>
#include <vtkCamera.h>
#include <vtkRenderer.h>
#include <mitkOpenGLRenderer.h>
#include "mitkInteractionConst.h"
#include <vtkTransform.h>
#include <itkVector.h>
#include <mitkModeOperation.h>

mitk::Interactor::Interactor(const char * type, DataTreeNode* dataTreeNode)
: mitk::StateMachine(type), m_DataTreeNode(dataTreeNode), m_Mode(SMDESELECTED)
{
  if (m_DataTreeNode != NULL)
    m_DataTreeNode->SetInteractor(this);
}

mitk::Interactor::SMMode mitk::Interactor::GetMode() const
{
  return m_Mode;
}

bool mitk::Interactor::IsNotSelected() const
{
  return (m_Mode==SMDESELECTED);
}

bool mitk::Interactor::IsSelected() const
{
  return (m_Mode!=SMDESELECTED);
}

void mitk::Interactor::CreateModeOperation(ModeType mode)
{
  mitk::ModeOperation* doOp = new mitk::ModeOperation(OpMODECHANGE, mode);
	if (m_UndoEnabled)
	{
    mitk::ModeOperation* undoOp = new mitk::ModeOperation(OpMODECHANGE, this->GetMode());
		OperationEvent *operationEvent = new OperationEvent(this, doOp, undoOp);
		m_UndoController->SetOperationEvent(operationEvent);
	}
	this->ExecuteOperation(doOp);
}

bool mitk::Interactor::ExecuteAction(Action* action, mitk::StateEvent const* stateEvent)
{

  switch (action->GetActionId())
  {
    case AcMODEDESELECT:
      {
        this->CreateModeOperation(SMDESELECTED);
        return true;
      }
    case AcMODESELECT:
      {
        this->CreateModeOperation(SMSELECTED);
        return true;
      }
    case AcMODESUBSELECT:
      {
        mitk::StatusBar::DisplayText("Error! in XML-Interaction: an simple Interactor can not set in sub selected", 1102);
        return false;
      }
    default:
      {
        itkWarningMacro("Message from Interactor.cpp: Action could not be understood!");
      }
  }

  return false;
}

//##Documentation
//## change if derived class can handle it better!
float mitk::Interactor::CalculateJurisdiction(StateEvent const* stateEvent) const
{
  float returnvalue = 0.0;
  //if it is a key event that can be handled in the current state, then return 0.5
  mitk::DisplayPositionEvent const  *disPosEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());

  //Key event handling:
  if (disPosEvent == NULL)
  {
    //check, if the current state has a transition waiting for that key event.
    if (this->GetCurrentState()->GetTransition(stateEvent->GetId())!=NULL)
    {
      return 0.5;
    }
    else
    {
      return 0;
    }
  }

  //Mouse event handling:
  //on MouseMove do nothing! reimplement if needed differently
  if (stateEvent->GetEvent()->GetType() == mitk::Type_MouseMove)
  {
    return 0;
  }

  //compute the center of the data taken care of
  mitk::BoundingBox *bBox = const_cast<mitk::BoundingBox*>(m_DataTreeNode->GetData()->GetGeometry()->GetBoundingBox());
  if (bBox == NULL)
    return 0;

  mitk::PositionEvent const  *posEvent = dynamic_cast <const mitk::PositionEvent *> (stateEvent->GetEvent());
	if (posEvent == NULL) //2D information from a 3D window
  {
    //get camera and calculate the distance between the center of this boundingbox and the camera
    mitk::OpenGLRenderer* oglRenderer = dynamic_cast<mitk::OpenGLRenderer*>(stateEvent->GetEvent()->GetSender());
    if (oglRenderer == NULL)
      return 0;

    vtkCamera* camera = oglRenderer->GetVtkRenderer()->GetActiveCamera();
    if (camera == NULL)
    {
      assert(disPosEvent->GetSender()!=NULL);
      return 0;
    }
    float normal[3];
    camera->GetViewPlaneNormal(normal);

    mitk::BoundingBox::PointType center,n;
    vtk2itk(n, normal);
    returnvalue = center.SquaredEuclideanDistanceTo( n );
    //map between 0.5 and 1
  }
  else//3D information from a 2D window.
  {
    mitk::Point3D worldPoint = posEvent->GetWorldPosition();
    float p[3];
    itk2vtk(worldPoint, p);
    //transforming the Worldposition to local coordinatesystem
    m_DataTreeNode->GetData()->GetGeometry()->GetVtkTransform()->GetInverse()->TransformPoint(p, p);
    vtk2itk(p, worldPoint);

    //distance between center and point
    mitk::BoundingBox::PointType center = bBox->GetCenter();
    returnvalue = worldPoint.EuclideanDistanceTo(center);

    //now compared to size of boundingbox to get between 0 and 1;
    returnvalue = returnvalue/( (bBox->GetMaximum().EuclideanDistanceTo(bBox->GetMinimum() ) ) );

    //safety: if by now returnvalue is not in 0 and 1, then return 1!
    if (returnvalue>1 ||returnvalue<0)
      return 0;

    //shall be 1 if short length to center
    returnvalue = 1 - returnvalue;

    //check if the given position lies inside the data-object
    if (bBox->IsInside(worldPoint))
    {
      //mapped between 0,5 and 1
      returnvalue = 0.5 + (returnvalue / 2);
    }
    else
    {
      //set it in range between 0 and 0.5
      returnvalue = returnvalue / 2;
    }
  }
  return returnvalue;
}

void mitk::Interactor::ExecuteOperation(Operation* operation)
{
	switch (operation->GetOperationType())
	{
	case OpMODECHANGE:
    {
      ModeOperation *modeOp = dynamic_cast<ModeOperation*>(operation);
      if (modeOp)
      {
         m_Mode = modeOp->GetMode();
      }
    }
		break;
  default:
    Superclass::ExecuteOperation(operation);
  }
}

<commit_msg>FIX: removed tabs<commit_after>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile$
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "mitkInteractor.h"
#include <mitkDataTreeNode.h>
#include <mitkDisplayPositionEvent.h>
#include <mitkPositionEvent.h>
#include <mitkGeometry3D.h>
#include <mitkAction.h>
#include <mitkStatusBar.h>
#include <vtkCamera.h>
#include <vtkRenderer.h>
#include <mitkOpenGLRenderer.h>
#include "mitkInteractionConst.h"
#include <vtkTransform.h>
#include <itkVector.h>
#include <mitkModeOperation.h>

mitk::Interactor::Interactor(const char * type, DataTreeNode* dataTreeNode)
: mitk::StateMachine(type), m_DataTreeNode(dataTreeNode), m_Mode(SMDESELECTED)
{
  if (m_DataTreeNode != NULL)
    m_DataTreeNode->SetInteractor(this);
}

mitk::Interactor::SMMode mitk::Interactor::GetMode() const
{
  return m_Mode;
}

bool mitk::Interactor::IsNotSelected() const
{
  return (m_Mode==SMDESELECTED);
}

bool mitk::Interactor::IsSelected() const
{
  return (m_Mode!=SMDESELECTED);
}

void mitk::Interactor::CreateModeOperation(ModeType mode)
{
  mitk::ModeOperation* doOp = new mitk::ModeOperation(OpMODECHANGE, mode);
  if (m_UndoEnabled)
  {
    mitk::ModeOperation* undoOp = new mitk::ModeOperation(OpMODECHANGE, this->GetMode());
    OperationEvent *operationEvent = new OperationEvent(this, doOp, undoOp);
    m_UndoController->SetOperationEvent(operationEvent);
  }
  this->ExecuteOperation(doOp);
}

bool mitk::Interactor::ExecuteAction(Action* action, mitk::StateEvent const* stateEvent)
{

  switch (action->GetActionId())
  {
  case AcMODEDESELECT:
    {
      this->CreateModeOperation(SMDESELECTED);
      return true;
    }
  case AcMODESELECT:
    {
      this->CreateModeOperation(SMSELECTED);
      return true;
    }
  case AcMODESUBSELECT:
    {
      mitk::StatusBar::DisplayText("Error! in XML-Interaction: an simple Interactor can not set in sub selected", 1102);
      return false;
    }
  default:
    {
      itkWarningMacro("Message from Interactor.cpp: Action could not be understood!");
    }
  }

  return false;
}

//##Documentation
//## change if derived class can handle it better!
float mitk::Interactor::CalculateJurisdiction(StateEvent const* stateEvent) const
{
  float returnvalue = 0.0;
  //if it is a key event that can be handled in the current state, then return 0.5
  mitk::DisplayPositionEvent const  *disPosEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());

  //Key event handling:
  if (disPosEvent == NULL)
  {
    //check, if the current state has a transition waiting for that key event.
    if (this->GetCurrentState()->GetTransition(stateEvent->GetId())!=NULL)
    {
      return 0.5;
    }
    else
    {
      return 0;
    }
  }

  //Mouse event handling:
  //on MouseMove do nothing! reimplement if needed differently
  if (stateEvent->GetEvent()->GetType() == mitk::Type_MouseMove)
  {
    return 0;
  }

  //compute the center of the data taken care of
  mitk::BoundingBox *bBox = const_cast<mitk::BoundingBox*>(m_DataTreeNode->GetData()->GetGeometry()->GetBoundingBox());
  if (bBox == NULL)
    return 0;

  mitk::PositionEvent const  *posEvent = dynamic_cast <const mitk::PositionEvent *> (stateEvent->GetEvent());
  if (posEvent == NULL) //2D information from a 3D window
  {
    //get camera and calculate the distance between the center of this boundingbox and the camera
    mitk::OpenGLRenderer* oglRenderer = dynamic_cast<mitk::OpenGLRenderer*>(stateEvent->GetEvent()->GetSender());
    if (oglRenderer == NULL)
      return 0;

    vtkCamera* camera = oglRenderer->GetVtkRenderer()->GetActiveCamera();
    if (camera == NULL)
    {
      assert(disPosEvent->GetSender()!=NULL);
      return 0;
    }
    float normal[3];
    camera->GetViewPlaneNormal(normal);

    mitk::BoundingBox::PointType center,n;
    vtk2itk(n, normal);
    returnvalue = center.SquaredEuclideanDistanceTo( n );
    //map between 0.5 and 1
  }
  else//3D information from a 2D window.
  {
    mitk::Point3D worldPoint = posEvent->GetWorldPosition();
    float p[3];
    itk2vtk(worldPoint, p);
    //transforming the Worldposition to local coordinatesystem
    m_DataTreeNode->GetData()->GetGeometry()->GetVtkTransform()->GetInverse()->TransformPoint(p, p);
    vtk2itk(p, worldPoint);

    //distance between center and point
    mitk::BoundingBox::PointType center = bBox->GetCenter();
    returnvalue = worldPoint.EuclideanDistanceTo(center);

    //now compared to size of boundingbox to get between 0 and 1;
    returnvalue = returnvalue/( (bBox->GetMaximum().EuclideanDistanceTo(bBox->GetMinimum() ) ) );

    //safety: if by now returnvalue is not in 0 and 1, then return 1!
    if (returnvalue>1 ||returnvalue<0)
      return 0;

    //shall be 1 if short length to center
    returnvalue = 1 - returnvalue;

    //check if the given position lies inside the data-object
    if (bBox->IsInside(worldPoint))
    {
      //mapped between 0,5 and 1
      returnvalue = 0.5 + (returnvalue / 2);
    }
    else
    {
      //set it in range between 0 and 0.5
      returnvalue = returnvalue / 2;
    }
  }
  return returnvalue;
}

void mitk::Interactor::ExecuteOperation(Operation* operation)
{
  switch (operation->GetOperationType())
  {
  case OpMODECHANGE:
    {
      ModeOperation *modeOp = dynamic_cast<ModeOperation*>(operation);
      if (modeOp)
      {
        m_Mode = modeOp->GetMode();
      }
    }
    break;
  default:
    Superclass::ExecuteOperation(operation);
  }
}
<|endoftext|>