<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: taskcreatorsrv.cxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: rt $ $Date: 2006-10-30 08:11:12 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 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
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_framework.hxx"

#ifndef __FRAMEWORK_SERVICES_TASKCREATORSRV_HXX_
#include "services/taskcreatorsrv.hxx"
#endif

//_______________________________________________
// own includes

#ifndef __FRAMEWORK_HELPER_PERSISTENTWINDOWSTATE_HXX_
#include <helper/persistentwindowstate.hxx>
#endif

#ifndef __FRAMEWORK_THREADHELP_READGUARD_HXX_
#include <threadhelp/readguard.hxx>
#endif

#ifndef __FRAMEWORK_THREADHELP_WRITEGUARD_HXX_
#include <threadhelp/writeguard.hxx>
#endif

#ifndef __FRAMEWORK_TARGETS_H_
#include <targets.h>
#endif

#ifndef __FRAMEWORK_SERVICES_H_
#include <services.h>
#endif

//_______________________________________________
// interface includes

#ifndef _COM_SUN_STAR_FRAME_XFRAME_HPP_
#include <com/sun/star/frame/XFrame.hpp>
#endif

#ifndef _COM_SUN_STAR_FRAME_XCONTROLLER_HPP_
#include <com/sun/star/frame/XController.hpp>
#endif

#ifndef _COM_SUN_STAR_FRAME_XMODEL_HPP_
#include <com/sun/star/frame/XModel.hpp>
#endif

#ifndef _COM_SUN_STAR_FRAME_XDESKTOP_HPP_
#include <com/sun/star/frame/XDesktop.hpp>
#endif

#ifndef _COM_SUN_STAR_AWT_WINDOWDESCRIPTOR_HPP_
#include <com/sun/star/awt/WindowDescriptor.hpp>
#endif

#ifndef _COM_SUN_STAR_AWT_WINDOWATTRIBUTE_HPP_
#include <com/sun/star/awt/WindowAttribute.hpp>
#endif

#ifndef _COM_SUN_STAR_AWT_VCLWINDOWPEERATTRIBUTE_HPP_
#include <com/sun/star/awt/VclWindowPeerAttribute.hpp>
#endif

//_______________________________________________
// other includes

#ifndef INCLUDED_SVTOOLS_COLORCFG_HXX
#include <svtools/colorcfg.hxx>
#endif

#ifndef _SV_SVAPP_HXX
#include <vcl/svapp.hxx>
#endif

//_______________________________________________
// namespaces

#ifndef css
namespace css = ::com::sun::star;
#endif

namespace framework
{

//-----------------------------------------------
DEFINE_XINTERFACE_3(TaskCreatorService                                       ,
                    OWeakObject                                       ,
                    DIRECT_INTERFACE(css::lang::XTypeProvider        ),
                    DIRECT_INTERFACE(css::lang::XServiceInfo         ),
                    DIRECT_INTERFACE(css::lang::XSingleServiceFactory))

//-----------------------------------------------
DEFINE_XTYPEPROVIDER_3(TaskCreatorService                     ,
                       css::lang::XTypeProvider        ,
                       css::lang::XServiceInfo         ,
                       css::lang::XSingleServiceFactory)

//-----------------------------------------------
DEFINE_XSERVICEINFO_ONEINSTANCESERVICE(TaskCreatorService                       ,
                                       ::cppu::OWeakObject               ,
                                       SERVICENAME_TASKCREATOR           ,
                                       IMPLEMENTATIONNAME_FWK_TASKCREATOR)

//-----------------------------------------------
DEFINE_INIT_SERVICE(
                    TaskCreatorService,
                    {
                        /*Attention
                            I think we don't need any mutex or lock here ... because we are called by our own static method impl_createInstance()
                            to create a new instance of this class by our own supported service factory.
                            see macro DEFINE_XSERVICEINFO_MULTISERVICE and "impl_initService()" for further informations!
                        */
                    }
                   )

//-----------------------------------------------
TaskCreatorService::TaskCreatorService(const css::uno::Reference< css::lang::XMultiServiceFactory >& xSMGR)
    : ThreadHelpBase     (&Application::GetSolarMutex())
    , ::cppu::OWeakObject(                             )
    , m_xSMGR            (xSMGR                        )
{
}

//-----------------------------------------------
TaskCreatorService::~TaskCreatorService()
{
}

//-----------------------------------------------
css::uno::Reference< css::uno::XInterface > SAL_CALL TaskCreatorService::createInstance()
    throw(css::uno::Exception       ,
          css::uno::RuntimeException)
{
    return createInstanceWithArguments(css::uno::Sequence< css::uno::Any >());
}

//-----------------------------------------------
css::uno::Reference< css::uno::XInterface > SAL_CALL TaskCreatorService::createInstanceWithArguments(const css::uno::Sequence< css::uno::Any >& lArguments)
    throw(css::uno::Exception       ,
          css::uno::RuntimeException)
{
    static ::rtl::OUString PROP_TARGETNAME       = ::rtl::OUString::createFromAscii("TargetName");
    static ::rtl::OUString PROP_FRAME            = ::rtl::OUString::createFromAscii("Frame"     );
    static ::rtl::OUString PROP_VISIBLE          = ::rtl::OUString::createFromAscii("Visible"   );
    static ::rtl::OUString DEFAULTVAL_TARGETNAME = ::rtl::OUString::createFromAscii("_default"  );
    static sal_Bool        DEFAULTVAL_VISIBLE    = sal_False                                     ;

    ::comphelper::SequenceAsHashMap lArgs(lArguments);

    ::rtl::OUString                           sTarget  = lArgs.getUnpackedValueOrDefault(PROP_TARGETNAME, DEFAULTVAL_TARGETNAME );
    sal_Bool                                  bVisible = lArgs.getUnpackedValueOrDefault(PROP_VISIBLE   , DEFAULTVAL_VISIBLE);
    css::uno::Reference< css::frame::XFrame > xFrame   = lArgs.getUnpackedValueOrDefault(PROP_FRAME     , css::uno::Reference< css::frame::XFrame >());

    // Check incoming parameter. We don't allow special target names like e.g. "_blank"
    ::rtl::OUString sRightName = impl_filterNames(sTarget);

    /* SAFE { */
    ReadGuard aReadLock( m_aLock );
    css::uno::Reference< css::lang::XMultiServiceFactory > xSMGR = m_xSMGR;
    aReadLock.unlock();
    /* } SAFE */

    css::uno::Reference< css::frame::XFramesSupplier > xDesktop( xSMGR->createInstance(SERVICENAME_DESKTOP), css::uno::UNO_QUERY_THROW);
    css::uno::Reference< css::uno::XInterface >        xTask   ( implts_createSystemTask(xDesktop, sRightName, bVisible), css::uno::UNO_QUERY_THROW);
    return xTask;
}

/*-****************************************************************************************************//**
    @short      create a new task with a system window inside
    @descr      With this method you can create a new empty system task. We create the task and the container
                window inside of it. Created node will be a child of given parent - which can be the desktop only.

    @param      xDesktop
                    only the desktop can be the parent of such new created task frame
    @param      sName
                    the new name for this task (filtered!)
    @param      bVisible
                    used to set the state of frame container window after creation

    @return     A reference to the new created task or <NULL/> if it failed.

    @threadsafe yes
    @modified   16.05.2002 10:44, as96863
*//*-*****************************************************************************************************/
css::uno::Reference< css::frame::XFrame > TaskCreatorService::implts_createSystemTask( const css::uno::Reference< css::frame::XFramesSupplier >&   xDesktop ,
                                                                                const ::rtl::OUString&                                      sName    ,
                                                                                sal_Bool                                              bVisible )
{
    css::uno::Reference< css::frame::XFrame > xTask;

    // get toolkit to create task container window
    /* SAFE { */
    ReadGuard aReadLock( m_aLock );
    css::uno::Reference< css::lang::XMultiServiceFactory > xSMGR = m_xSMGR;
    aReadLock.unlock();
    /* } SAFE */
    css::uno::Reference< css::awt::XToolkit > xToolkit( xSMGR->createInstance( SERVICENAME_VCLTOOLKIT ), css::uno::UNO_QUERY_THROW);

    // describe window properties.
    css::awt::WindowDescriptor aDescriptor;
    aDescriptor.Type                =   css::awt::WindowClass_TOP                       ;
    aDescriptor.WindowServiceName   =   DECLARE_ASCII("window")                         ;
    aDescriptor.ParentIndex         =   -1                                              ;
    aDescriptor.Parent              =   css::uno::Reference< css::awt::XWindowPeer >()  ;
    aDescriptor.Bounds              =   css::awt::Rectangle(0,0,0,0)                    ;
    aDescriptor.WindowAttributes    =   css::awt::WindowAttribute::BORDER               |
                                        css::awt::WindowAttribute::MOVEABLE             |
                                        css::awt::WindowAttribute::SIZEABLE             |
                                        css::awt::WindowAttribute::CLOSEABLE            |
                                        css::awt::VclWindowPeerAttribute::CLIPCHILDREN  ;
    // create a new blank container window and get access to parent container to append new created task.
    css::uno::Reference< css::awt::XWindowPeer > xPeer      = xToolkit->createWindow( aDescriptor );
    css::uno::Reference< css::awt::XWindow >     xWindow    ( xPeer, css::uno::UNO_QUERY );
    xPeer->setBackground(::svtools::ColorConfig().GetColorValue(::svtools::APPBACKGROUND).nColor);
    css::uno::Reference< css::frame::XFrames >   xContainer = xDesktop->getFrames();
    if (
        ( xWindow.is()    ) &&
        ( xContainer.is() )
       )
    {
        // create new top level frame.
        xTask = css::uno::Reference< css::frame::XFrame >( xSMGR->createInstance( SERVICENAME_FRAME ), css::uno::UNO_QUERY );
        if (xTask.is())
        {
            // Set window on task.
            // Do it before you call other interface methods on task-object ...
            // because this object must be initialized before you can do such things.
            // Otherwise he throw an exception for UNINITIALIZED working mode!

            // Don't forget to create tree-bindings! use given parent as parent node of new task ...
            // ... and append it to his container.
            // (task member xParent will automaticly set by "append()" call!)

            xTask->initialize  ( xWindow );
            xTask->setName     ( sName   );
            xContainer->append ( xTask   );

            if (bVisible)
                xWindow->setVisible(bVisible);

            // Special feature: It's allowed for system tasks only - not for browser plugged ones!
            // We must create a special listener service and couple it with the new created task frame.
            // He will restore or save the window state of it ...
            // See used classes for further informations too.
            PersistentWindowState* pPersistentStateHandler = new PersistentWindowState(xSMGR);
            css::uno::Reference< css::lang::XInitialization > xInit(static_cast< ::cppu::OWeakObject* >(pPersistentStateHandler), css::uno::UNO_QUERY);
            // This will start listening at the task frame ... and then these two objects hold herself alive!
            // We can forget xInit without any problems.
            css::uno::Sequence< css::uno::Any > lInitData(1);
            lInitData[0] <<= xTask;
            xInit->initialize(lInitData);
        }
    }

    return xTask;
}

/*-****************************************************************************************************//**
    @short      decide which names are correct frame names
    @descr      Not all names are allowed as frame name. e.g. special targets like "_blank" are forbidden.
                They are used to force creation of new tasks and can make trouble during search off already
                existing ones.

    @attention  "_beamer" is a valid name - because:
                It exist one beamer for one task tree only.
                If he exist, we can find it - otherwhise he will be created by our task-frame!

    @param      sName
                    whished name by user

    @return     The given name of user if it is an allowed one - or an empty string if not.

    @threadsafe not neccessary
    @modified   16.05.2002 10:32, as96863
*//*-*****************************************************************************************************/
::rtl::OUString TaskCreatorService::impl_filterNames( const ::rtl::OUString& sName )
{
    ::rtl::OUString sFiltered( sName );
    if(
        ( sName == SPECIALTARGET_BLANK      )   ||
        ( sName == SPECIALTARGET_DEFAULT    )   ||
        ( sName == SPECIALTARGET_SELF       )   ||
        ( sName == SPECIALTARGET_PARENT     )   ||
        ( sName == SPECIALTARGET_TOP        )   ||
        ( sName == SPECIALTARGET_MENUBAR    )   ||
        ( sName == SPECIALTARGET_HELPAGENT  )
       )
    {
        sFiltered = ::rtl::OUString();
    }
    return sFiltered;
}

} // namespace framework
<commit_msg>INTEGRATION: CWS fwkdbdesign01 (1.3.56); FILE MERGED 2007/02/26 11:49:34 as 1.3.56.3: #i74541# improved service to create any type of frames 2007/02/19 10:58:57 as 1.3.56.2: #i74541# make framework ready for real sub frames 2007/02/14 14:26:23 as 1.3.56.1: #i74541# new API (and some small code changes) to support real sub frames greater level 1<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: taskcreatorsrv.cxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: ihi $ $Date: 2007-04-16 16:45:54 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 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
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_framework.hxx"

#ifndef __FRAMEWORK_SERVICES_TASKCREATORSRV_HXX_
#include "services/taskcreatorsrv.hxx"
#endif

//_______________________________________________
// own includes

#ifndef __FRAMEWORK_HELPER_PERSISTENTWINDOWSTATE_HXX_
#include <helper/persistentwindowstate.hxx>
#endif

#ifndef __FRAMEWORK_THREADHELP_READGUARD_HXX_
#include <threadhelp/readguard.hxx>
#endif

#ifndef __FRAMEWORK_THREADHELP_WRITEGUARD_HXX_
#include <threadhelp/writeguard.hxx>
#endif

#ifndef __FRAMEWORK_TARGETHELPER_HXX_
#include <loadenv/targethelper.hxx>
#endif

#ifndef __FRAMEWORK_SERVICES_H_
#include <services.h>
#endif

//_______________________________________________
// interface includes

#ifndef _COM_SUN_STAR_FRAME_XFRAME_HPP_
#include <com/sun/star/frame/XFrame.hpp>
#endif

#ifndef _COM_SUN_STAR_FRAME_XCONTROLLER_HPP_
#include <com/sun/star/frame/XController.hpp>
#endif

#ifndef _COM_SUN_STAR_FRAME_XMODEL_HPP_
#include <com/sun/star/frame/XModel.hpp>
#endif

#ifndef _COM_SUN_STAR_FRAME_XDESKTOP_HPP_
#include <com/sun/star/frame/XDesktop.hpp>
#endif

#ifndef _COM_SUN_STAR_AWT_XTOPWINDOW_HPP_
#include <com/sun/star/awt/XTopWindow.hpp>
#endif

#ifndef _COM_SUN_STAR_AWT_WINDOWDESCRIPTOR_HPP_
#include <com/sun/star/awt/WindowDescriptor.hpp>
#endif

#ifndef _COM_SUN_STAR_AWT_WINDOWATTRIBUTE_HPP_
#include <com/sun/star/awt/WindowAttribute.hpp>
#endif

#ifndef _COM_SUN_STAR_AWT_VCLWINDOWPEERATTRIBUTE_HPP_
#include <com/sun/star/awt/VclWindowPeerAttribute.hpp>
#endif

//_______________________________________________
// other includes

#ifndef INCLUDED_SVTOOLS_COLORCFG_HXX
#include <svtools/colorcfg.hxx>
#endif

#ifndef _SV_SVAPP_HXX
#include <vcl/svapp.hxx>
#endif

//_______________________________________________
// namespaces

namespace framework
{

//-----------------------------------------------
const ::rtl::OUString TaskCreatorService::ARGUMENT_PARENTFRAME                   = ::rtl::OUString::createFromAscii("ParentFrame"                   ); // XFrame
const ::rtl::OUString TaskCreatorService::ARGUMENT_FRAMENAME                     = ::rtl::OUString::createFromAscii("FrameName"                     ); // OUString
const ::rtl::OUString TaskCreatorService::ARGUMENT_MAKEVISIBLE                   = ::rtl::OUString::createFromAscii("MakeVisible"                   ); // sal_Bool
const ::rtl::OUString TaskCreatorService::ARGUMENT_CREATETOPWINDOW               = ::rtl::OUString::createFromAscii("CreateTopWindow"               ); // sal_Bool
const ::rtl::OUString TaskCreatorService::ARGUMENT_POSSIZE                       = ::rtl::OUString::createFromAscii("PosSize"                       ); // Rectangle
const ::rtl::OUString TaskCreatorService::ARGUMENT_CONTAINERWINDOW               = ::rtl::OUString::createFromAscii("ContainerWindow"               ); // XWindow
const ::rtl::OUString TaskCreatorService::ARGUMENT_SUPPORTPERSISTENTWINDOWSTATE  = ::rtl::OUString::createFromAscii("SupportPersistentWindowState"  ); // sal_Bool

//-----------------------------------------------
DEFINE_XINTERFACE_3(TaskCreatorService                                ,
                    OWeakObject                                       ,
                    DIRECT_INTERFACE(css::lang::XTypeProvider        ),
                    DIRECT_INTERFACE(css::lang::XServiceInfo         ),
                    DIRECT_INTERFACE(css::lang::XSingleServiceFactory))

//-----------------------------------------------
DEFINE_XTYPEPROVIDER_3(TaskCreatorService              ,
                       css::lang::XTypeProvider        ,
                       css::lang::XServiceInfo         ,
                       css::lang::XSingleServiceFactory)

//-----------------------------------------------
DEFINE_XSERVICEINFO_ONEINSTANCESERVICE(TaskCreatorService                ,
                                       ::cppu::OWeakObject               ,
                                       SERVICENAME_TASKCREATOR           ,
                                       IMPLEMENTATIONNAME_FWK_TASKCREATOR)

//-----------------------------------------------
DEFINE_INIT_SERVICE(
                    TaskCreatorService,
                    {
                        /*Attention
                            I think we don't need any mutex or lock here ... because we are called by our own static method impl_createInstance()
                            to create a new instance of this class by our own supported service factory.
                            see macro DEFINE_XSERVICEINFO_MULTISERVICE and "impl_initService()" for further informations!
                        */
                    }
                   )

//-----------------------------------------------
TaskCreatorService::TaskCreatorService(const css::uno::Reference< css::lang::XMultiServiceFactory >& xSMGR)
    : ThreadHelpBase     (&Application::GetSolarMutex())
    , ::cppu::OWeakObject(                             )
    , m_xSMGR            (xSMGR                        )
{
}

//-----------------------------------------------
TaskCreatorService::~TaskCreatorService()
{
}

//-----------------------------------------------
css::uno::Reference< css::uno::XInterface > SAL_CALL TaskCreatorService::createInstance()
    throw(css::uno::Exception       ,
          css::uno::RuntimeException)
{
    return createInstanceWithArguments(css::uno::Sequence< css::uno::Any >());
}

//-----------------------------------------------
css::uno::Reference< css::uno::XInterface > SAL_CALL TaskCreatorService::createInstanceWithArguments(const css::uno::Sequence< css::uno::Any >& lArguments)
    throw(css::uno::Exception       ,
          css::uno::RuntimeException)
{
    static ::rtl::OUString     DEFAULTVAL_FRAMENAME                     = ::rtl::OUString();
    static sal_Bool            DEFAULTVAL_MAKEVISIBLE                   = sal_False;
    static sal_Bool            DEFAULTVAL_CREATETOPWINDOW               = sal_True;
    static css::awt::Rectangle DEFAULTVAL_POSSIZE                       = css::awt::Rectangle(0, 0, 0, 0); // only possize=[0,0,0,0] triggers default handling of vcl !
    static sal_Bool            DEFAULTVAL_SUPPORTPERSSISTENTWINDOWSTATE = sal_False;

    ::comphelper::SequenceAsHashMap lArgs(lArguments);

    css::uno::Reference< css::frame::XFrame > xParentFrame                  = lArgs.getUnpackedValueOrDefault(TaskCreatorService::ARGUMENT_PARENTFRAME                  , css::uno::Reference< css::frame::XFrame >());
    ::rtl::OUString                           sFrameName                    = lArgs.getUnpackedValueOrDefault(TaskCreatorService::ARGUMENT_FRAMENAME                    , DEFAULTVAL_FRAMENAME                       );
    sal_Bool                                  bVisible                      = lArgs.getUnpackedValueOrDefault(TaskCreatorService::ARGUMENT_MAKEVISIBLE                  , DEFAULTVAL_MAKEVISIBLE                     );
    sal_Bool                                  bCreateTopWindow              = lArgs.getUnpackedValueOrDefault(TaskCreatorService::ARGUMENT_CREATETOPWINDOW              , DEFAULTVAL_CREATETOPWINDOW                 );
    css::awt::Rectangle                       aPosSize                      = lArgs.getUnpackedValueOrDefault(TaskCreatorService::ARGUMENT_POSSIZE                      , DEFAULTVAL_POSSIZE                         );
    css::uno::Reference< css::awt::XWindow >  xContainerWindow              = lArgs.getUnpackedValueOrDefault(TaskCreatorService::ARGUMENT_CONTAINERWINDOW              , css::uno::Reference< css::awt::XWindow >() );
    sal_Bool                                  bSupportPersistentWindowState = lArgs.getUnpackedValueOrDefault(TaskCreatorService::ARGUMENT_SUPPORTPERSISTENTWINDOWSTATE , DEFAULTVAL_SUPPORTPERSSISTENTWINDOWSTATE   );

    /* SAFE { */
    ReadGuard aReadLock( m_aLock );
    css::uno::Reference< css::lang::XMultiServiceFactory > xSMGR = m_xSMGR;
    aReadLock.unlock();
    /* } SAFE */

    // We use FrameName property to set it as API name of the new created frame later.
    // But those frame names must be different from the set of special target names as e.g. _blank, _self etcpp !
    ::rtl::OUString sRightName = impl_filterNames(sFrameName);

    // if no external frame window was given ... create a new one.
    if ( ! xContainerWindow.is())
    {
        css::uno::Reference< css::awt::XWindow > xParentWindow;
        if (xParentFrame.is())
            xParentWindow = xParentFrame->getContainerWindow();

        // Parent has no own window ...
        // So we have to create a top level window always !
        if ( ! xParentWindow.is())
            bCreateTopWindow = sal_True;

        xContainerWindow = implts_createContainerWindow(xParentWindow, aPosSize, bCreateTopWindow);
    }

    // create the new frame
    css::uno::Reference< css::frame::XFrame > xFrame = implts_createFrame(xParentFrame, xContainerWindow, sRightName);

    // special freature:
    // A special listener will restore pos/size states in case
    // a component was loaded into the frame first time.
    if (bSupportPersistentWindowState)
        implts_establishWindowStateListener(xFrame);

    // Make it visible directly here ...
    // if its required from outside.
    if (bVisible)
        xContainerWindow->setVisible(bVisible);

    return css::uno::Reference< css::uno::XInterface >(xFrame, css::uno::UNO_QUERY_THROW);
}

//-----------------------------------------------
css::uno::Reference< css::awt::XWindow > TaskCreatorService::implts_createContainerWindow( const css::uno::Reference< css::awt::XWindow >& xParentWindow ,
                                                                                const css::awt::Rectangle&                      aPosSize      ,
                                                                                sal_Bool                                  bTopWindow    )
{
    // SAFE  ->
    ReadGuard aReadLock( m_aLock );
    css::uno::Reference< css::lang::XMultiServiceFactory > xSMGR = m_xSMGR;
    aReadLock.unlock();
    // <- SAFE

    // get toolkit to create task container window
    css::uno::Reference< css::awt::XToolkit > xToolkit( xSMGR->createInstance( SERVICENAME_VCLTOOLKIT ), css::uno::UNO_QUERY_THROW);

    // Check if child frames can be created realy. We need at least a valid window at the parent frame ...
    css::uno::Reference< css::awt::XWindowPeer > xParentWindowPeer;
    if ( ! bTopWindow)
    {
        if ( ! xParentWindow.is())
            bTopWindow = sal_False;
        else
            xParentWindowPeer = css::uno::Reference< css::awt::XWindowPeer >(xParentWindow, css::uno::UNO_QUERY_THROW);
    }

    // describe window properties.
    css::awt::WindowDescriptor aDescriptor;
    if (bTopWindow)
    {
        aDescriptor.Type                =   css::awt::WindowClass_TOP                       ;
        aDescriptor.WindowServiceName   =   DECLARE_ASCII("window")                         ;
        aDescriptor.ParentIndex         =   -1                                              ;
        aDescriptor.Parent              =   css::uno::Reference< css::awt::XWindowPeer >()  ;
        aDescriptor.Bounds              =   aPosSize                                        ;
        aDescriptor.WindowAttributes    =   css::awt::WindowAttribute::BORDER               |
                                            css::awt::WindowAttribute::MOVEABLE             |
                                            css::awt::WindowAttribute::SIZEABLE             |
                                            css::awt::WindowAttribute::CLOSEABLE            |
                                            css::awt::VclWindowPeerAttribute::CLIPCHILDREN  ;
    }
    else
    {
        aDescriptor.Type                =   css::awt::WindowClass_TOP                       ;
        aDescriptor.WindowServiceName   =   DECLARE_ASCII("dockingwindow")                  ;
        aDescriptor.ParentIndex         =   1                                               ;
        aDescriptor.Parent              =   xParentWindowPeer                               ;
        aDescriptor.Bounds              =   aPosSize                                        ;
        aDescriptor.WindowAttributes    =   css::awt::VclWindowPeerAttribute::CLIPCHILDREN  ;
    }

    // create a new blank container window and get access to parent container to append new created task.
    css::uno::Reference< css::awt::XWindowPeer > xPeer      = xToolkit->createWindow( aDescriptor );
    css::uno::Reference< css::awt::XWindow >     xWindow    ( xPeer, css::uno::UNO_QUERY );
    if ( ! xWindow.is())
        throw css::uno::Exception(::rtl::OUString::createFromAscii("TaskCreator service was not able to create suitable frame window."),
                                  static_cast< ::cppu::OWeakObject* >(this));
    if (bTopWindow)
        xPeer->setBackground(::svtools::ColorConfig().GetColorValue(::svtools::APPBACKGROUND).nColor);
    else
        xPeer->setBackground(0xffffffff);

    return xWindow;
}

//-----------------------------------------------
css::uno::Reference< css::frame::XFrame > TaskCreatorService::implts_createFrame( const css::uno::Reference< css::frame::XFrame >& xParentFrame    ,
                                                                                const css::uno::Reference< css::awt::XWindow >&  xContainerWindow,
                                                                                const ::rtl::OUString&                           sName           )
{
    // SAFE  ->
    ReadGuard aReadLock( m_aLock );
    css::uno::Reference< css::lang::XMultiServiceFactory > xSMGR = m_xSMGR;
    aReadLock.unlock();
    // <- SAFE

    // create new frame.
    css::uno::Reference< css::frame::XFrame > xNewFrame( xSMGR->createInstance( SERVICENAME_FRAME ), css::uno::UNO_QUERY_THROW );

    // Set window on frame.
    // Do it before calling any other interface methods ...
    // The new created frame must be initialized before you can do anything else there.
    xNewFrame->initialize( xContainerWindow );

    // Put frame to the frame tree.
    // Note: The property creator/parent will be set on the new putted frame automaticly ... by the parent container.
    if (xParentFrame.is())
    {
        css::uno::Reference< css::frame::XFramesSupplier > xSupplier  (xParentFrame, css::uno::UNO_QUERY_THROW);
        css::uno::Reference< css::frame::XFrames >         xContainer = xSupplier->getFrames();
        xContainer->append( xNewFrame );
    }

    // Set it's API name (if there is one from outside)
    if (sName.getLength())
        xNewFrame->setName( sName );

    return xNewFrame;
}

//-----------------------------------------------
void TaskCreatorService::implts_establishWindowStateListener( const css::uno::Reference< css::frame::XFrame >& xFrame )
{
    // SAFE  ->
    ReadGuard aReadLock( m_aLock );
    css::uno::Reference< css::lang::XMultiServiceFactory > xSMGR = m_xSMGR;
    aReadLock.unlock();
    // <- SAFE

    // Special feature: It's allowed for frames using a top level window only!
    // We must create a special listener service and couple it with the new created task frame.
    // He will restore or save the window state of it ...
    // See used classes for further informations too.
    PersistentWindowState* pPersistentStateHandler = new PersistentWindowState(xSMGR);
    css::uno::Reference< css::lang::XInitialization > xInit(static_cast< ::cppu::OWeakObject* >(pPersistentStateHandler), css::uno::UNO_QUERY_THROW);

    // This will start listening at this frame ... and then these two objects hold herself alive!
    // We can forget xInit without any problems.
    css::uno::Sequence< css::uno::Any > lInitData(1);
    lInitData[0] <<= xFrame;
    xInit->initialize(lInitData);
}

//-----------------------------------------------
::rtl::OUString TaskCreatorService::impl_filterNames( const ::rtl::OUString& sName )
{
    ::rtl::OUString sFiltered;
    if (TargetHelper::isValidNameForFrame(sName))
        sFiltered = sName;
    return sFiltered;
}

} // namespace framework
<|endoftext|>


<commit_before>//===-- MCAsmParser.cpp - Abstract Asm Parser Interface -------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "llvm/MC/MCParser/MCAsmParser.h"
#include "llvm/ADT/Twine.h"
#include "llvm/MC/MCParser/MCAsmLexer.h"
#include "llvm/MC/MCParser/MCParsedAsmOperand.h"
#include "llvm/MC/MCParser/MCTargetAsmParser.h"
#include "llvm/Support/Debug.h"
#include "llvm/Support/SourceMgr.h"
#include "llvm/Support/raw_ostream.h"
using namespace llvm;

MCAsmParser::MCAsmParser()
    : TargetParser(nullptr), ShowParsedOperands(0), HadError(false), PendingErrors() {}

MCAsmParser::~MCAsmParser() {
}

void MCAsmParser::setTargetParser(MCTargetAsmParser &P) {
  assert(!TargetParser && "Target parser is already initialized!");
  TargetParser = &P;
  TargetParser->Initialize(*this);
}

const AsmToken &MCAsmParser::getTok() const {
  return getLexer().getTok();
}

bool MCAsmParser::parseTokenLoc(SMLoc &Loc) {
  Loc = getTok().getLoc();
  return false;
}

bool MCAsmParser::parseEOL(const Twine &Msg) {
  if (getTok().getKind() == AsmToken::Hash) {
    StringRef CommentStr = parseStringToEndOfStatement();
    getLexer().Lex();
    getLexer().UnLex(AsmToken(AsmToken::EndOfStatement, CommentStr));
  }
  if (getTok().getKind() != AsmToken::EndOfStatement)
    return Error(getTok().getLoc(), Msg);
  Lex();
  return false;
}

bool MCAsmParser::parseToken(AsmToken::TokenKind T, const Twine &Msg) {
  if (T == AsmToken::EndOfStatement)
    return parseEOL(Msg);
  if (getTok().getKind() != T)
    return Error(getTok().getLoc(), Msg);
  Lex();
  return false;
}

bool MCAsmParser::parseIntToken(int64_t &V, const Twine &Msg) {
  if (getTok().getKind() != AsmToken::Integer)
    return TokError(Msg);
  V = getTok().getIntVal();
  Lex();
  return false;
}

bool MCAsmParser::parseOptionalToken(AsmToken::TokenKind T, bool &Present) {
  Present = (getTok().getKind() == T);
  if (Present)
    Lex();
  return false;
}

bool MCAsmParser::check(bool P, const Twine &Msg) {
  return check(P, getTok().getLoc(), Msg);
}

bool MCAsmParser::check(bool P, SMLoc Loc, const Twine &Msg) {
  if (P)
    return Error(Loc, Msg);
  return false;
}

bool MCAsmParser::TokError(const Twine &Msg, SMRange Range) {
  return Error(getLexer().getLoc(), Msg, Range);
}

bool MCAsmParser::Error(SMLoc L, const Twine &Msg, SMRange Range) {
  HadError = true;

  MCPendingError PErr;
  PErr.Loc = L;
  Msg.toVector(PErr.Msg);
  PErr.Range = Range;
  PendingErrors.push_back(PErr);
  return true;
}

bool MCAsmParser::parseExpression(const MCExpr *&Res) {
  SMLoc L;
  return parseExpression(Res, L);
}

LLVM_DUMP_METHOD void MCParsedAsmOperand::dump() const {
  dbgs() << "  " << *this;
}
<commit_msg>Apply Clang-format to MCAsmParser.cpp NFC.<commit_after>//===-- MCAsmParser.cpp - Abstract Asm Parser Interface -------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "llvm/MC/MCParser/MCAsmParser.h"
#include "llvm/ADT/Twine.h"
#include "llvm/MC/MCParser/MCAsmLexer.h"
#include "llvm/MC/MCParser/MCParsedAsmOperand.h"
#include "llvm/MC/MCParser/MCTargetAsmParser.h"
#include "llvm/Support/Debug.h"
#include "llvm/Support/SourceMgr.h"
#include "llvm/Support/raw_ostream.h"
using namespace llvm;

MCAsmParser::MCAsmParser()
    : TargetParser(nullptr), ShowParsedOperands(0), HadError(false),
      PendingErrors() {}

MCAsmParser::~MCAsmParser() {
}

void MCAsmParser::setTargetParser(MCTargetAsmParser &P) {
  assert(!TargetParser && "Target parser is already initialized!");
  TargetParser = &P;
  TargetParser->Initialize(*this);
}

const AsmToken &MCAsmParser::getTok() const {
  return getLexer().getTok();
}

bool MCAsmParser::parseTokenLoc(SMLoc &Loc) {
  Loc = getTok().getLoc();
  return false;
}

bool MCAsmParser::parseEOL(const Twine &Msg) {
  if (getTok().getKind() == AsmToken::Hash) {
    StringRef CommentStr = parseStringToEndOfStatement();
    getLexer().Lex();
    getLexer().UnLex(AsmToken(AsmToken::EndOfStatement, CommentStr));
  }
  if (getTok().getKind() != AsmToken::EndOfStatement)
    return Error(getTok().getLoc(), Msg);
  Lex();
  return false;
}

bool MCAsmParser::parseToken(AsmToken::TokenKind T, const Twine &Msg) {
  if (T == AsmToken::EndOfStatement)
    return parseEOL(Msg);
  if (getTok().getKind() != T)
    return Error(getTok().getLoc(), Msg);
  Lex();
  return false;
}

bool MCAsmParser::parseIntToken(int64_t &V, const Twine &Msg) {
  if (getTok().getKind() != AsmToken::Integer)
    return TokError(Msg);
  V = getTok().getIntVal();
  Lex();
  return false;
}

bool MCAsmParser::parseOptionalToken(AsmToken::TokenKind T, bool &Present) {
  Present = (getTok().getKind() == T);
  if (Present)
    Lex();
  return false;
}

bool MCAsmParser::check(bool P, const Twine &Msg) {
  return check(P, getTok().getLoc(), Msg);
}

bool MCAsmParser::check(bool P, SMLoc Loc, const Twine &Msg) {
  if (P)
    return Error(Loc, Msg);
  return false;
}

bool MCAsmParser::TokError(const Twine &Msg, SMRange Range) {
  return Error(getLexer().getLoc(), Msg, Range);
}

bool MCAsmParser::Error(SMLoc L, const Twine &Msg, SMRange Range) {
  HadError = true;

  MCPendingError PErr;
  PErr.Loc = L;
  Msg.toVector(PErr.Msg);
  PErr.Range = Range;
  PendingErrors.push_back(PErr);
  return true;
}

bool MCAsmParser::parseExpression(const MCExpr *&Res) {
  SMLoc L;
  return parseExpression(Res, L);
}

LLVM_DUMP_METHOD void MCParsedAsmOperand::dump() const {
  dbgs() << "  " << *this;
}
<|endoftext|>


<commit_before>// Lidar Pass trough filter
// TODO: get the urdf model from LCM:

#include <stdio.h>
#include <inttypes.h>
#include <iostream>
#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/vector.hpp>

#include <lcm/lcm-cpp.hpp>

#include <bot_frames/bot_frames.h>
#include <bot_param/param_client.h>
#include <bot_frames_cpp/bot_frames_cpp.hpp>
#include <bot_lcmgl_client/lcmgl.h>

#include <collision/collision.h>
#include <collision/collision_detector.h>
#include <collision/collision_object_gfe.h>
#include <collision/collision_object_point_cloud.h>

#include <pointcloud_tools/pointcloud_vis.hpp> // visualize pt clds
#include <pointcloud_tools/pointcloud_lcm.hpp> // unpack lidar to xyz
#include "lcmtypes/bot_core.hpp"
#include <ConciseArgs>

using namespace std;
using namespace drc;
using namespace Eigen;
using namespace collision;
using namespace boost::assign; // bring 'operator+()' into scope

class Pass{
  public:
    Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_,
         std::string lidar_channel_, std::string urdf_fname_);

    ~Pass(){
    }
  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    bool verbose_;
    std::string urdf_fname_, lidar_channel_;

    void lidar_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg);
    void robot_state_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::robot_state_t* msg);

    Collision_Object_GFE* collision_object_gfe_;
    Collision_Object_Point_Cloud* collision_object_point_cloud_;
    Collision_Detector* collision_detector_;
    int n_collision_points_;

    void DoCollisionCheck(int64_t current_utime);

    BotParam* botparam_;
    BotFrames* botframes_;
    bot::frames* frames_cpp_;
    bot_lcmgl_t* lcmgl_;

    pointcloud_vis* pc_vis_;
    pointcloud_lcm* pc_lcm_;
    int vis_counter_; // used for visualization
    int printf_counter_; // used for terminal feedback

    // Last robot state: this is used as the collision gfe/robot
    drc::robot_state_t last_rstate_;
    bool init_rstate_;
    // Scan as pointcloud in local frame: this is used as the collision points
    pcl::PointCloud<PointXYZRGB>::Ptr scan_cloud_s2l_;

    // Output filter lidar:
    bot_core::planar_lidar_t lidar_msgout_;
};

Pass::Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_,
         std::string lidar_channel_, std::string urdf_fname_):
    lcm_(lcm_), verbose_(verbose_),
    lidar_channel_(lidar_channel_), urdf_fname_(urdf_fname_){
  botparam_ = bot_param_new_from_server(lcm_->getUnderlyingLCM(), 0);
  botframes_= bot_frames_get_global(lcm_->getUnderlyingLCM(), botparam_);

  lcmgl_= bot_lcmgl_init(lcm_->getUnderlyingLCM(), "lidar-pt");
  lcm_->subscribe( lidar_channel_ ,&Pass::lidar_handler,this);
  lcm_->subscribe("EST_ROBOT_STATE",&Pass::robot_state_handler,this);

  // Vis Config:
  pc_vis_ = new pointcloud_vis( lcm_->getUnderlyingLCM() );
  // obj: id name type reset
  // pts: id name type reset objcoll usergb rgb
  pc_vis_->obj_cfg_list.push_back( obj_cfg(60000,"Pose - Laser",5,0) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(60001,"Cloud - Laser"         ,1,0, 60000,1, {0.0, 0.0, 1.0} ));
  pc_vis_->obj_cfg_list.push_back( obj_cfg(1000,"Pose - Null",5,0) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(1001,"Cloud - Null"           ,1,0, 1000,1, { 0.0, 1.0, 0.0} ));
  vis_counter_ =0;
  printf_counter_ =0;

  // TODO: get the urdf model from LCM:
  collision_object_gfe_ = new Collision_Object_GFE( "collision-object-gfe", urdf_fname_ );
  n_collision_points_ = 1000;
  collision_object_point_cloud_ = new Collision_Object_Point_Cloud( "collision-object-point-cloud", n_collision_points_ );
  // create the collision detector
  collision_detector_ = new Collision_Detector();
  // add the two collision objects to the collision detector with different groups and filters (to prevent checking of self collisions)
  collision_detector_->add_collision_object( collision_object_gfe_, COLLISION_DETECTOR_GROUP_1, COLLISION_DETECTOR_GROUP_2 );
  collision_detector_->add_collision_object( collision_object_point_cloud_, COLLISION_DETECTOR_GROUP_2, COLLISION_DETECTOR_GROUP_1 );
  init_rstate_ =false;

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan_cloud_s2l__ptr (new pcl::PointCloud<pcl::PointXYZRGB> ());
  scan_cloud_s2l_ = scan_cloud_s2l__ptr;
  cout << "Finished setting up\n";
}

void Pass::DoCollisionCheck(int64_t current_utime ){
  // create a std::vector< Eigen::Vector3f > of points
  vector< Vector3f > points;
  int which=2; // 0 random points in a box, 1 a line, 2 the actual lidar returns
  if (which==0){
    for( unsigned int i = 0; i < 500; i++ ){
      Vector3f point(last_rstate_.origin_position.translation.x +  -1.0 + 2.0 * ( double )( rand() % 1000 ) / 1000.0,
                    last_rstate_.origin_position.translation.y + -1.0 + 2.0 * ( double )( rand() % 1000 ) / 1000.0,
                    last_rstate_.origin_position.translation.z +  -1.0 + 2.0 * ( double )( rand() % 1000 ) / 1000.0 );
      points.push_back( point );
    }
  }else if(which==1){
    for( unsigned int i = 0; i < 500; i++ ){
      Vector3f point(-0.0, -1 + 0.005*i,-0.2);
      points.push_back( point );
    }
  }else{
    for( unsigned int i = 0; i < scan_cloud_s2l_->points.size() ; i++ ){
      Vector3f point(scan_cloud_s2l_->points[i].x, scan_cloud_s2l_->points[i].y, scan_cloud_s2l_->points[i].z );
      points.push_back( point );
    }
  }

  // set the state of the collision objects
  collision_object_gfe_->set( last_rstate_ );
  //cout << "gfe obj size: " << collision_object_gfe_->bt_collision_objects().size() << "\n";
  collision_object_point_cloud_->set( points );
  // get the vector of collisions by running collision detection
  vector< Collision > collisions = collision_detector_->get_collisions();

  // display the collisions (debugging purposes only)
  // for( unsigned int j = 0; j < collisions.size(); j++ ){
  //   cout << "collisions[" << j << "]: " << collisions[ j ] << endl;
  // }

  vector< Vector3f > free_points;
  vector< Vector3f > colliding_points;
  vector< unsigned int > filtered_point_indices;
  for( unsigned int j = 0; j < collisions.size(); j++ ){
    filtered_point_indices.push_back( atoi( collisions[ j ].second_id().c_str() ) );
  }

  for( unsigned int j = 0; j < points.size(); j++ ){
    bool point_filtered = false;
    for( unsigned int k = 0; k < filtered_point_indices.size(); k++ ){
      if( j == filtered_point_indices[ k ] ){
        point_filtered = true;
        // Set filtered returns to max range
        lidar_msgout_.ranges[j] = 35.0;
      }
    }
    if( point_filtered ){
      colliding_points.push_back( points[ j ] );
    } else {
      free_points.push_back( points[ j ] );
    }
  }

  if (verbose_){
    cout << current_utime << " | total returns "<< scan_cloud_s2l_->points.size()
        << " | colliding " << colliding_points.size() << " | free " << free_points.size() << endl;
  }
  // Output channel is incoming channel appended with FREE
  lcm_->publish( (lidar_channel_ + "_FREE") , &lidar_msgout_);

  if (verbose_){
    bot_lcmgl_point_size(lcmgl_, 4.5f);
    bot_lcmgl_color3f(lcmgl_, 0, 1, 0);
    for (int i = 0; i < free_points.size(); ++i) {
      bot_lcmgl_begin(lcmgl_, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl_, free_points[i][0], free_points[i][1], free_points[i][2]);
      bot_lcmgl_end(lcmgl_);
    }

    bot_lcmgl_point_size(lcmgl_, 10.5f);
    bot_lcmgl_color3f(lcmgl_, 1, 0, 0);
    for (int i = 0; i < colliding_points.size(); ++i) {
      bot_lcmgl_begin(lcmgl_, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl_, colliding_points[i][0], colliding_points[i][1], colliding_points[i][2]);
      bot_lcmgl_end(lcmgl_);
    }
    bot_lcmgl_switch_buffer(lcmgl_);
  }
}

void Pass::lidar_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg){
  if (!init_rstate_){
    cout << "have laser but no robot state, refusing to publish\n";
    return;
  }
  // TODO: check if the rstate is stale

  // A counter for visualization:
  vis_counter_++;
  if (vis_counter_ >=1){ // set this to 1 to only see the last return
    vis_counter_=0;
  }
  int64_t pose_id=vis_counter_;
  // int64_t pose_id=msg->utime;

  // 0. Make Local copy to later output
  lidar_msgout_ = *msg;

  // 1. Convert scan into simple XY point cloud:
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
  double maxRange = 29.7;
  double validBeamAngles[] ={-10,10}; // consider everything
  convertLidar(msg->ranges, msg->nranges, msg->rad0,
      msg->radstep, scan_cloud, maxRange,
      validBeamAngles[0], validBeamAngles[1]);

  // 2. Project the scan into local frame:
  Eigen::Isometry3d scan_to_local;
  frames_cpp_->get_trans_with_utime( botframes_ ,  "ROTATING_SCAN", "local", msg->utime, scan_to_local);
  Eigen::Isometry3f pose_f = Isometry_d2f(scan_to_local);
  Eigen::Quaternionf pose_quat(pose_f.rotation());
  pcl::transformPointCloud (*scan_cloud, *scan_cloud_s2l_,
      pose_f.translation(), pose_quat);

  if (verbose_){
    // Plot original scan in sensor frame:
    Isometry3dTime scan_to_localT = Isometry3dTime(pose_id, scan_to_local);
    pc_vis_->pose_to_lcm_from_list(60000, scan_to_localT);
    pc_vis_->ptcld_to_lcm_from_list(60001, *scan_cloud, pose_id, pose_id);

    // Plot scan in local frame:
    Eigen::Isometry3d null_pose;
    null_pose.setIdentity();
    Isometry3dTime null_poseT = Isometry3dTime(pose_id, null_pose);
    pc_vis_->pose_to_lcm_from_list(1000, null_poseT);
    pc_vis_->ptcld_to_lcm_from_list(1001, *scan_cloud_s2l_, pose_id, pose_id);
  }

  if (scan_cloud_s2l_->points.size() > n_collision_points_){
    std::cout << "more points in scan ("<<scan_cloud_s2l_->points.size() <<") than reserved in collision detector ("<< n_collision_points_ << ")"
              << "\nincrease detector size to match\n";
    exit(-1);
  }

  DoCollisionCheck(msg->utime);
  if (printf_counter_%80 ==0){
    cout << "Filtering: " << lidar_channel_ << " "  << msg->utime << "\n";
  }
  printf_counter_++;

}

void Pass::robot_state_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::robot_state_t* msg){
  last_rstate_= *msg;
  init_rstate_=true;
}

int main( int argc, char** argv ){
  ConciseArgs parser(argc, argv, "lidar-passthrough");
  bool verbose=FALSE;
  string lidar_channel="ROTATING_SCAN";
  string urdf_fname="/home/mfallon/drc/software/build/models/mit_gazebo_models/mit_robot/model.urdf"; // replace by lcm based delivery of urdf string
  parser.add(verbose, "v", "verbose", "Verbosity");
  parser.add(lidar_channel, "l", "lidar_channel", "Incoming LIDAR channel");
  parser.add(urdf_fname, "u", "urdf_fname", "URDF filename [temp]");
  parser.parse();
  cout << verbose << " is verbose\n";
  cout << lidar_channel << " is lidar_channel\n";
  cout << urdf_fname << " is urdf_fname\n";

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  Pass app(lcm,verbose,lidar_channel,urdf_fname);
  cout << "Ready to filter lidar points" << endl << "============================" << endl;
  while(0 == lcm->handle());
  return 0;
}
<commit_msg><commit_after>// Lidar Pass trough filter
// TODO: get the urdf model from LCM:

#include <stdio.h>
#include <inttypes.h>
#include <iostream>
#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/vector.hpp>

#include <lcm/lcm-cpp.hpp>

#include <bot_frames/bot_frames.h>
#include <bot_param/param_client.h>
#include <bot_frames_cpp/bot_frames_cpp.hpp>
#include <bot_lcmgl_client/lcmgl.h>

#include <collision/collision.h>
#include <collision/collision_detector.h>
#include <collision/collision_object_gfe.h>
#include <collision/collision_object_point_cloud.h>

#include <pointcloud_tools/pointcloud_vis.hpp> // visualize pt clds
#include <pointcloud_tools/pointcloud_lcm.hpp> // unpack lidar to xyz
#include "lcmtypes/bot_core.hpp"
#include <ConciseArgs>

using namespace std;
using namespace drc;
using namespace Eigen;
using namespace collision;
using namespace boost::assign; // bring 'operator+()' into scope

class Pass{
  public:
    Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_,
         std::string lidar_channel_);

    ~Pass(){
    }
  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    bool verbose_;
    std::string urdf_xml_string_, lidar_channel_;

    void urdfHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::robot_urdf_t* msg);
    void lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg);
    void robotStateHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::robot_state_t* msg);

    Collision_Object_GFE* collision_object_gfe_;
    Collision_Object_Point_Cloud* collision_object_point_cloud_;
    Collision_Detector* collision_detector_;
    int n_collision_points_;

    void DoCollisionCheck(int64_t current_utime);

    BotParam* botparam_;
    BotFrames* botframes_;
    bot::frames* frames_cpp_;
    bot_lcmgl_t* lcmgl_;

    pointcloud_vis* pc_vis_;
    pointcloud_lcm* pc_lcm_;
    int vis_counter_; // used for visualization
    int printf_counter_; // used for terminal feedback

    bool urdf_parsed_;
    bool urdf_subscription_on_;
    lcm::Subscription *urdf_subscription_; //valid as long as urdf_parsed_ == false
    // Last robot state: this is used as the collision gfe/robot
    drc::robot_state_t last_rstate_;
    bool init_rstate_;
    // Scan as pointcloud in local frame: this is used as the collision points
    pcl::PointCloud<PointXYZRGB>::Ptr scan_cloud_s2l_;

    // Output filter lidar:
    bot_core::planar_lidar_t lidar_msgout_;
};

Pass::Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_,
         std::string lidar_channel_):
    lcm_(lcm_), verbose_(verbose_),
    lidar_channel_(lidar_channel_),urdf_parsed_(false){
  botparam_ = bot_param_new_from_server(lcm_->getUnderlyingLCM(), 0);
  botframes_= bot_frames_get_global(lcm_->getUnderlyingLCM(), botparam_);

  lcmgl_= bot_lcmgl_init(lcm_->getUnderlyingLCM(), "lidar-pt");
  lcm_->subscribe( lidar_channel_ ,&Pass::lidarHandler,this);
  lcm_->subscribe("EST_ROBOT_STATE",&Pass::robotStateHandler,this);
  urdf_subscription_ = lcm_->subscribe("ROBOT_MODEL", &Pass::urdfHandler,this);
  urdf_subscription_on_ = true;

  // Vis Config:
  pc_vis_ = new pointcloud_vis( lcm_->getUnderlyingLCM() );
  // obj: id name type reset
  // pts: id name type reset objcoll usergb rgb
  pc_vis_->obj_cfg_list.push_back( obj_cfg(60000,"Pose - Laser",5,0) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(60001,"Cloud - Laser"         ,1,0, 60000,1, {0.0, 0.0, 1.0} ));
  pc_vis_->obj_cfg_list.push_back( obj_cfg(1000,"Pose - Null",5,0) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(1001,"Cloud - Null"           ,1,0, 1000,1, { 0.0, 1.0, 0.0} ));
  vis_counter_ =0;
  printf_counter_ =0;

  init_rstate_ =false;

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan_cloud_s2l__ptr (new pcl::PointCloud<pcl::PointXYZRGB> ());
  scan_cloud_s2l_ = scan_cloud_s2l__ptr;
  cout << "Finished setting up\n";
}

void Pass::DoCollisionCheck(int64_t current_utime ){
  // create a std::vector< Eigen::Vector3f > of points
  vector< Vector3f > points;
  int which=2; // 0 random points in a box, 1 a line, 2 the actual lidar returns
  if (which==0){
    for( unsigned int i = 0; i < 500; i++ ){
      Vector3f point(last_rstate_.origin_position.translation.x +  -1.0 + 2.0 * ( double )( rand() % 1000 ) / 1000.0,
                    last_rstate_.origin_position.translation.y + -1.0 + 2.0 * ( double )( rand() % 1000 ) / 1000.0,
                    last_rstate_.origin_position.translation.z +  -1.0 + 2.0 * ( double )( rand() % 1000 ) / 1000.0 );
      points.push_back( point );
    }
  }else if(which==1){
    for( unsigned int i = 0; i < 500; i++ ){
      Vector3f point(-0.0, -1 + 0.005*i,-0.2);
      points.push_back( point );
    }
  }else{
    for( unsigned int i = 0; i < scan_cloud_s2l_->points.size() ; i++ ){
      Vector3f point(scan_cloud_s2l_->points[i].x, scan_cloud_s2l_->points[i].y, scan_cloud_s2l_->points[i].z );
      points.push_back( point );
    }
  }

  // set the state of the collision objects
  collision_object_gfe_->set( last_rstate_ );
  //cout << "gfe obj size: " << collision_object_gfe_->bt_collision_objects().size() << "\n";
  collision_object_point_cloud_->set( points );
  // get the vector of collisions by running collision detection
  vector< Collision > collisions = collision_detector_->get_collisions();

  // display the collisions (debugging purposes only)
  // for( unsigned int j = 0; j < collisions.size(); j++ ){
  //   cout << "collisions[" << j << "]: " << collisions[ j ] << endl;
  // }

  vector< Vector3f > free_points;
  vector< Vector3f > colliding_points;
  vector< unsigned int > filtered_point_indices;
  for( unsigned int j = 0; j < collisions.size(); j++ ){
    filtered_point_indices.push_back( atoi( collisions[ j ].second_id().c_str() ) );
  }

  for( unsigned int j = 0; j < points.size(); j++ ){
    bool point_filtered = false;
    for( unsigned int k = 0; k < filtered_point_indices.size(); k++ ){
      if( j == filtered_point_indices[ k ] ){
        point_filtered = true;
        // Set filtered returns to max range
        lidar_msgout_.ranges[j] = 35.0;
      }
    }
    if( point_filtered ){
      colliding_points.push_back( points[ j ] );
    } else {
      free_points.push_back( points[ j ] );
    }
  }

  if (verbose_){
    cout << current_utime << " | total returns "<< scan_cloud_s2l_->points.size()
        << " | colliding " << colliding_points.size() << " | free " << free_points.size() << endl;
  }
  // Output channel is incoming channel appended with FREE
  lcm_->publish( (lidar_channel_ + "_FREE") , &lidar_msgout_);

  if (verbose_){
    bot_lcmgl_point_size(lcmgl_, 4.5f);
    bot_lcmgl_color3f(lcmgl_, 0, 1, 0);
    for (int i = 0; i < free_points.size(); ++i) {
      bot_lcmgl_begin(lcmgl_, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl_, free_points[i][0], free_points[i][1], free_points[i][2]);
      bot_lcmgl_end(lcmgl_);
    }

    bot_lcmgl_point_size(lcmgl_, 10.5f);
    bot_lcmgl_color3f(lcmgl_, 1, 0, 0);
    for (int i = 0; i < colliding_points.size(); ++i) {
      bot_lcmgl_begin(lcmgl_, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl_, colliding_points[i][0], colliding_points[i][1], colliding_points[i][2]);
      bot_lcmgl_end(lcmgl_);
    }
    bot_lcmgl_switch_buffer(lcmgl_);
  }
}

void Pass::lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg){
  if (!init_rstate_){
    cout << "have laser but no robot state, refusing to publish\n";
    return;
  }
  // TODO: check if the rstate is stale

  // A counter for visualization:
  vis_counter_++;
  if (vis_counter_ >=1){ // set this to 1 to only see the last return
    vis_counter_=0;
  }
  int64_t pose_id=vis_counter_;
  // int64_t pose_id=msg->utime;

  // 0. Make Local copy to later output
  lidar_msgout_ = *msg;

  // 1. Convert scan into simple XY point cloud:
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
  double maxRange = 29.7;
  double validBeamAngles[] ={-10,10}; // consider everything
  convertLidar(msg->ranges, msg->nranges, msg->rad0,
      msg->radstep, scan_cloud, maxRange,
      validBeamAngles[0], validBeamAngles[1]);

  // 2. Project the scan into local frame:
  Eigen::Isometry3d scan_to_local;
  frames_cpp_->get_trans_with_utime( botframes_ ,  "ROTATING_SCAN", "local", msg->utime, scan_to_local);
  Eigen::Isometry3f pose_f = Isometry_d2f(scan_to_local);
  Eigen::Quaternionf pose_quat(pose_f.rotation());
  pcl::transformPointCloud (*scan_cloud, *scan_cloud_s2l_,
      pose_f.translation(), pose_quat);

  if (verbose_){
    // Plot original scan in sensor frame:
    Isometry3dTime scan_to_localT = Isometry3dTime(pose_id, scan_to_local);
    pc_vis_->pose_to_lcm_from_list(60000, scan_to_localT);
    pc_vis_->ptcld_to_lcm_from_list(60001, *scan_cloud, pose_id, pose_id);

    // Plot scan in local frame:
    Eigen::Isometry3d null_pose;
    null_pose.setIdentity();
    Isometry3dTime null_poseT = Isometry3dTime(pose_id, null_pose);
    pc_vis_->pose_to_lcm_from_list(1000, null_poseT);
    pc_vis_->ptcld_to_lcm_from_list(1001, *scan_cloud_s2l_, pose_id, pose_id);
  }

  if (scan_cloud_s2l_->points.size() > n_collision_points_){
    std::cout << "more points in scan ("<<scan_cloud_s2l_->points.size() <<") than reserved in collision detector ("<< n_collision_points_ << ")"
              << "\nincrease detector size to match\n";
    exit(-1);
  }

  DoCollisionCheck(msg->utime);
  if (printf_counter_%80 ==0){
    cout << "Filtering: " << lidar_channel_ << " "  << msg->utime << "\n";
  }
  printf_counter_++;
}

void Pass::urdfHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::robot_urdf_t* msg){
  if(urdf_parsed_ ==false){
    cout<< "URDF received"<< endl;
    // Received robot urdf string. Store it internally and get all available joints.
    urdf_xml_string_ = msg->urdf_xml_string;

    // TODO: get the urdf model from LCM:
    cout << "c\n";
    collision_object_gfe_ = new Collision_Object_GFE( "collision-object-gfe", urdf_xml_string_ );
    cout << "d\n";
    n_collision_points_ = 1000;
    collision_object_point_cloud_ = new Collision_Object_Point_Cloud( "collision-object-point-cloud", n_collision_points_ );
    // create the collision detector
    collision_detector_ = new Collision_Detector();
    // add the two collision objects to the collision detector with different groups and filters (to prevent checking of self collisions)
    collision_detector_->add_collision_object( collision_object_gfe_, COLLISION_DETECTOR_GROUP_1, COLLISION_DETECTOR_GROUP_2 );
    collision_detector_->add_collision_object( collision_object_point_cloud_, COLLISION_DETECTOR_GROUP_2, COLLISION_DETECTOR_GROUP_1 );

    lcm_->unsubscribe(urdf_subscription_);
    urdf_parsed_ = true;
  }
}

void Pass::robotStateHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::robot_state_t* msg){
  if (!urdf_parsed_){      return;    }
  if(urdf_subscription_on_){
    cout << "robotStateHandler: unsubscribing from urdf" << endl;
    lcm_->unsubscribe(urdf_subscription_); //unsubscribe from urdf messages
    urdf_subscription_on_ =  false;
  }

  last_rstate_= *msg;
  init_rstate_=true;
}

int main( int argc, char** argv ){
  ConciseArgs parser(argc, argv, "lidar-passthrough");
  bool verbose=FALSE;
  string lidar_channel="ROTATING_SCAN";
  parser.add(verbose, "v", "verbose", "Verbosity");
  parser.add(lidar_channel, "l", "lidar_channel", "Incoming LIDAR channel");
  parser.parse();
  cout << verbose << " is verbose\n";
  cout << lidar_channel << " is lidar_channel\n";

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  Pass app(lcm,verbose,lidar_channel);
  cout << "Ready to filter lidar points" << endl << "============================" << endl;
  while(0 == lcm->handle());
  return 0;
}
<|endoftext|>


<commit_before>#include "Tests.h"
#include "IziAssert.h"

#include <memory>
#include <cstdlib>
#include <ctime>
#include <limits>
#include <vector>
#include <algorithm>
#include <string.h>

using namespace nsTests;
using namespace std;

namespace
{
    int rand(int min, int max)
    {
        return min + (std::rand() % (max - min + 1));
    }

    class TestClass
    {
    public:
        TestClass(int a = 0, const string& name = "ALAIN CASALOL") noexcept : m_a(a), m_name(name) { }

        bool operator ==(const TestClass& c) const noexcept
        {
            return m_a == c.m_a && m_name == c.m_name;
        }

        int getA() const noexcept
        {
            return m_a;
        }

        string getName() const noexcept
        {
            return m_name;
        }

    private:
        int m_a;
        string m_name;
    };

    std::ostream &operator<<(std::ostream &os, TestClass const &p) {
        return os << "PERS. " << p.getName() << ". " << p.getA();
    }

    template<typename T>
    class ValueProvider
    {
    public:
        vector<T> operator ()(int /*valueCount*/ = 0)
        {
            // typeid(T).name() only gives mangled class name, not very clear but still a good indication.
            throw runtime_error(string("Value provider for type ") + typeid(T).name() + " does not exist.");
        }
    };

    template<typename T>
    class ValueProvider<T*>
    {
    public:
        vector<T*> operator ()(int /*valueCount*/ = 0)
        {
            vector<T> array = ValueProvider<T>()();
            vector<T*> ptrArray;
            ptrArray.reserve(array.size());

            for (T x : array)
                ptrArray.push_back(new T(x));

            return ptrArray;
        }
    };

    template<typename T>
    class ValueProvider<shared_ptr<T>>
    {
    public:
        vector<shared_ptr<T>> operator ()(int/* valueCount*/ = 0)
        {
            vector<T> array = ValueProvider<T>()();
            vector<shared_ptr<T>> ptrArray;
            ptrArray.reserve(array.size());

            for (T x : array)
                ptrArray.push_back(make_shared<T>(x));

            return ptrArray;
        }
    };

    template<>
    class ValueProvider<int>
    {
    public:
        vector<int> operator ()(int valueCount = 0) noexcept
        {
            int arraySize = valueCount ? valueCount : rand(10, 100);
            cout << "TProv " << valueCount << " " << arraySize << endl;
            vector<int> array(arraySize);

            array[0] = 0;
            array[1] = std::numeric_limits<int>::max();
            array[2] = std::numeric_limits<int>::min();

            for (int i = 3; i < arraySize; ++i)
                array[i] = std::rand();

            return array;
        }
    };

    template<>
    class ValueProvider<TestClass>
    {
    public:
        vector<TestClass> operator()(int valueCount = 0) noexcept
        {
            int arraySize = valueCount ? valueCount : rand(10, 100);
            vector<TestClass> array(arraySize);

            array[0] = TestClass(4654, "MARQUE LA PORTE");

            for (int i = 1; i < arraySize; ++i)
            {
                string name;
                for (int j = 0; j < rand(10, 20); ++j)
                    name += static_cast<char>(rand(66, 89));

                array[i] = TestClass(rand(0, 30), name);
            }

            return array;
        }
    };

    template<typename T>
    void CreateEmptyList() noexcept
    {
        CTestedList<T> list;

        IZI_ASSERT(list.size() == 0);
        IZI_ASSERT(list.empty());
        IZI_ASSERT(list.begin() == list.end());
    }

    template<typename T>
    void CreateSizedList() noexcept
    {
        typename CTestedList<T>::size_type listSize = rand(1, 10);

        CTestedList<T> list(listSize);

        IZI_ASSERT(list.size() == listSize);
        IZI_ASSERT(!list.empty());
        IZI_ASSERT(list.begin() != list.end());

        for (T x : list)
            IZI_ASSERT(x == T());
    }

    template<typename T>
    void CreateSizeListWithValue(T value) noexcept
    {
        typename CTestedList<T>::size_type listSize = rand(1, 10);

        CTestedList<T> list(listSize, value);

        IZI_ASSERT(list.size() == listSize);
        IZI_ASSERT(!list.empty());
        IZI_ASSERT(list.begin() != list.end());

        for (T x : list)
            IZI_ASSERT(x == value)
    }

    template<typename T>
    void CreateListByExplicitCopy() noexcept
    {
        typename CTestedList<T>::size_type listSize = rand(1, 10000);

        CTestedList<T> referenceList;
        for (T x : ValueProvider<T>()(listSize))
            referenceList.push_back(x);

        CTestedList<T> copiedList(referenceList);

        IZI_ASSERT(referenceList.size() == copiedList.size());

        while (!referenceList.empty())
        {
            IZI_ASSERT(referenceList.front() == copiedList.front());
            referenceList.pop_front();
            copiedList.pop_front();
        }
    }

    template<typename T>
    void CreateListByImplicitCopy() noexcept
    {
        typename CTestedList<T>::size_type listSize = rand(1, 10000);

        CTestedList<T> referenceList;
        for (T x : ValueProvider<T>()(listSize))
            referenceList.push_back(x);

        CTestedList<T> copiedList = referenceList;

        IZI_ASSERT(referenceList.size() == copiedList.size());

        while (!referenceList.empty())
        {
            IZI_ASSERT(referenceList.front() == copiedList.front());
            referenceList.pop_front();
            copiedList.pop_front();
        }
    }

    template<typename T>
    void Iterate() noexcept
    {
        const typename CTestedList<T>::size_type listSize = 20;

        CTestedList<T> list;
        vector<T> t = ValueProvider<T>()(listSize);
        vector<T> originalData(t.begin(), t.begin() + listSize / 2);
        vector<T> newData(t.begin() + listSize / 2, t.end());

        for (T x : originalData)
            list.push_back(x);

        typename CTestedList<T>::iterator begin = list.begin();
        typename CTestedList<T>::iterator end = list.end();

        for (; begin != end; ++begin)
        {
            int i = distance(list.begin(), begin);
            IZI_ASSERT(originalData[i] == *begin);
            *begin = newData[i];
            IZI_ASSERT(newData[i] == *begin);
        }

        IZI_ASSERT(begin == end);
    }

    template<typename T>
    void ConstIterate() noexcept
    {
        const typename CTestedList<T>::size_type listSize = 20;

        CTestedList<T> list;
        vector<T> t = ValueProvider<T>()(listSize);
        for (T x : t)
            list.push_back(x);

        typename CTestedList<T>::const_iterator cbegin = list.cbegin();
        typename CTestedList<T>::const_iterator cend = list.cend();

        for (; cbegin != cend; ++cbegin)
            IZI_ASSERT(t[distance(list.cbegin(), cbegin)] == *cbegin);

        IZI_ASSERT(cbegin == cend);
    }

    template<typename T>
    void ReverseIterate() noexcept
    {
        const typename CTestedList<T>::size_type listSize = 20;

        CTestedList<T> list;
        vector<T> t = ValueProvider<T>()(listSize);
        vector<T> originalData(t.begin(), t.begin() + listSize / 2);
        vector<T> newData(t.begin() + listSize / 2, t.end());

        cout << listSize << " " << t.size() << " " << originalData.size() << " " << newData.size() << endl;

        for (T x : originalData)
            list.push_back(x);

        reverse(originalData.begin(), originalData.end());
        reverse(newData.begin(), newData.end());

        typename CTestedList<T>::reverse_iterator rbegin = list.rbegin();
        typename CTestedList<T>::reverse_iterator rend = list.rend();

        for (; rbegin != rend; ++rbegin)
        {
            int i = distance(list.rbegin(), rbegin);
            IZI_ASSERT(originalData[i] == *rbegin);
            *rbegin = newData[i];
            IZI_ASSERT(newData[i] == *rbegin);
        }

        IZI_ASSERT(rbegin == rend);
    }

    template<typename T>
    void ReverseConstIterate() noexcept
    {const typename CTestedList<T>::size_type listSize = 20;

        CTestedList<T> list;
        vector<T> t = ValueProvider<T>()(listSize);
        for (T x : t)
            list.push_back(x);

        reverse(t.begin(), t.end());

        typename CTestedList<T>::const_reverse_iterator crbegin = list.crbegin();
        typename CTestedList<T>::const_reverse_iterator crend = list.crend();

        for (; crbegin != crend; ++crbegin)
            IZI_ASSERT(t[distance(list.crbegin(), crbegin)] == *crbegin);

        IZI_ASSERT(crbegin == crend);
    }

    template <typename T>
    void RunTemplatedTests() noexcept
    {
        CreateEmptyList<T>();
        CreateSizedList<T>();

        for (T x : ValueProvider<T>()(5))
            CreateSizeListWithValue<T>(x);

        CreateListByExplicitCopy<T>();
        CreateListByImplicitCopy<T>();

        Iterate<T>();
        ConstIterate<T>();
        ReverseIterate<T>();
        ReverseConstIterate<T>();
    }
}

void CTests::RunTests() noexcept
{
    cout << "Starting tests..." << endl << endl;

    srand(time(NULL));
    IZI_CALLTEST(RunTemplatedTests<int>());
    IZI_CALLTEST(RunTemplatedTests<int*>());
    IZI_CALLTEST(RunTemplatedTests<shared_ptr<int>>());

    IZI_CALLTEST(RunTemplatedTests<TestClass>());
    IZI_CALLTEST(RunTemplatedTests<TestClass*>());
    IZI_CALLTEST(RunTemplatedTests<shared_ptr<TestClass>>());

    cout << "Tests done..." << endl << endl;
}
<commit_msg>Warning fix<commit_after>#include "Tests.h"
#include "IziAssert.h"

#include <memory>
#include <cstdlib>
#include <ctime>
#include <limits>
#include <vector>
#include <algorithm>
#include <string.h>

using namespace nsTests;
using namespace std;

namespace
{
    int rand(int min, int max)
    {
        return min + (std::rand() % (max - min + 1));
    }

    class TestClass
    {
    public:
        TestClass(int a = 0, const string& name = "ALAIN CASALOL") noexcept : m_a(a), m_name(name) { }

        bool operator ==(const TestClass& c) const noexcept
        {
            return m_a == c.m_a && m_name == c.m_name;
        }

        int getA() const noexcept
        {
            return m_a;
        }

        string getName() const noexcept
        {
            return m_name;
        }

    private:
        int m_a;
        string m_name;
    };

//    std::ostream &operator<<(std::ostream &os, TestClass const &p) {
//        return os << "PERS. " << p.getName() << ". " << p.getA();
//    }

    template<typename T>
    class ValueProvider
    {
    public:
        vector<T> operator ()(int /*valueCount*/ = 0)
        {
            // typeid(T).name() only gives mangled class name, not very clear but still a good indication.
            throw runtime_error(string("Value provider for type ") + typeid(T).name() + " does not exist.");
        }
    };

    template<typename T>
    class ValueProvider<T*>
    {
    public:
        vector<T*> operator ()(int /*valueCount*/ = 0)
        {
            vector<T> array = ValueProvider<T>()();
            vector<T*> ptrArray;
            ptrArray.reserve(array.size());

            for (T x : array)
                ptrArray.push_back(new T(x));

            return ptrArray;
        }
    };

    template<typename T>
    class ValueProvider<shared_ptr<T>>
    {
    public:
        vector<shared_ptr<T>> operator ()(int/* valueCount*/ = 0)
        {
            vector<T> array = ValueProvider<T>()();
            vector<shared_ptr<T>> ptrArray;
            ptrArray.reserve(array.size());

            for (T x : array)
                ptrArray.push_back(make_shared<T>(x));

            return ptrArray;
        }
    };

    template<>
    class ValueProvider<int>
    {
    public:
        vector<int> operator ()(int valueCount = 0) noexcept
        {
            int arraySize = valueCount ? valueCount : rand(10, 100);
            cout << "TProv " << valueCount << " " << arraySize << endl;
            vector<int> array(arraySize);

            array[0] = 0;
            array[1] = std::numeric_limits<int>::max();
            array[2] = std::numeric_limits<int>::min();

            for (int i = 3; i < arraySize; ++i)
                array[i] = std::rand();

            return array;
        }
    };

    template<>
    class ValueProvider<TestClass>
    {
    public:
        vector<TestClass> operator()(int valueCount = 0) noexcept
        {
            int arraySize = valueCount ? valueCount : rand(10, 100);
            vector<TestClass> array(arraySize);

            array[0] = TestClass(4654, "MARQUE LA PORTE");

            for (int i = 1; i < arraySize; ++i)
            {
                string name;
                for (int j = 0; j < rand(10, 20); ++j)
                    name += static_cast<char>(rand(66, 89));

                array[i] = TestClass(rand(0, 30), name);
            }

            return array;
        }
    };

    template<typename T>
    void CreateEmptyList() noexcept
    {
        CTestedList<T> list;

        IZI_ASSERT(list.size() == 0);
        IZI_ASSERT(list.empty());
        IZI_ASSERT(list.begin() == list.end());
    }

    template<typename T>
    void CreateSizedList() noexcept
    {
        typename CTestedList<T>::size_type listSize = rand(1, 10);

        CTestedList<T> list(listSize);

        IZI_ASSERT(list.size() == listSize);
        IZI_ASSERT(!list.empty());
        IZI_ASSERT(list.begin() != list.end());

        for (T x : list)
            IZI_ASSERT(x == T());
    }

    template<typename T>
    void CreateSizeListWithValue(T value) noexcept
    {
        typename CTestedList<T>::size_type listSize = rand(1, 10);

        CTestedList<T> list(listSize, value);

        IZI_ASSERT(list.size() == listSize);
        IZI_ASSERT(!list.empty());
        IZI_ASSERT(list.begin() != list.end());

        for (T x : list)
            IZI_ASSERT(x == value)
    }

    template<typename T>
    void CreateListByExplicitCopy() noexcept
    {
        typename CTestedList<T>::size_type listSize = rand(1, 10000);

        CTestedList<T> referenceList;
        for (T x : ValueProvider<T>()(listSize))
            referenceList.push_back(x);

        CTestedList<T> copiedList(referenceList);

        IZI_ASSERT(referenceList.size() == copiedList.size());

        while (!referenceList.empty())
        {
            IZI_ASSERT(referenceList.front() == copiedList.front());
            referenceList.pop_front();
            copiedList.pop_front();
        }
    }

    template<typename T>
    void CreateListByImplicitCopy() noexcept
    {
        typename CTestedList<T>::size_type listSize = rand(1, 10000);

        CTestedList<T> referenceList;
        for (T x : ValueProvider<T>()(listSize))
            referenceList.push_back(x);

        CTestedList<T> copiedList = referenceList;

        IZI_ASSERT(referenceList.size() == copiedList.size());

        while (!referenceList.empty())
        {
            IZI_ASSERT(referenceList.front() == copiedList.front());
            referenceList.pop_front();
            copiedList.pop_front();
        }
    }

    template<typename T>
    void Iterate() noexcept
    {
        const typename CTestedList<T>::size_type listSize = 20;

        CTestedList<T> list;
        vector<T> t = ValueProvider<T>()(listSize);
        vector<T> originalData(t.begin(), t.begin() + listSize / 2);
        vector<T> newData(t.begin() + listSize / 2, t.end());

        for (T x : originalData)
            list.push_back(x);

        typename CTestedList<T>::iterator begin = list.begin();
        typename CTestedList<T>::iterator end = list.end();

        for (; begin != end; ++begin)
        {
            int i = distance(list.begin(), begin);
            IZI_ASSERT(originalData[i] == *begin);
            *begin = newData[i];
            IZI_ASSERT(newData[i] == *begin);
        }

        IZI_ASSERT(begin == end);
    }

    template<typename T>
    void ConstIterate() noexcept
    {
        const typename CTestedList<T>::size_type listSize = 20;

        CTestedList<T> list;
        vector<T> t = ValueProvider<T>()(listSize);
        for (T x : t)
            list.push_back(x);

        typename CTestedList<T>::const_iterator cbegin = list.cbegin();
        typename CTestedList<T>::const_iterator cend = list.cend();

        for (; cbegin != cend; ++cbegin)
            IZI_ASSERT(t[distance(list.cbegin(), cbegin)] == *cbegin);

        IZI_ASSERT(cbegin == cend);
    }

    template<typename T>
    void ReverseIterate() noexcept
    {
        const typename CTestedList<T>::size_type listSize = 20;

        CTestedList<T> list;
        vector<T> t = ValueProvider<T>()(listSize);
        vector<T> originalData(t.begin(), t.begin() + listSize / 2);
        vector<T> newData(t.begin() + listSize / 2, t.end());

        cout << listSize << " " << t.size() << " " << originalData.size() << " " << newData.size() << endl;

        for (T x : originalData)
            list.push_back(x);

        reverse(originalData.begin(), originalData.end());
        reverse(newData.begin(), newData.end());

        typename CTestedList<T>::reverse_iterator rbegin = list.rbegin();
        typename CTestedList<T>::reverse_iterator rend = list.rend();

        for (; rbegin != rend; ++rbegin)
        {
            int i = distance(list.rbegin(), rbegin);
            IZI_ASSERT(originalData[i] == *rbegin);
            *rbegin = newData[i];
            IZI_ASSERT(newData[i] == *rbegin);
        }

        IZI_ASSERT(rbegin == rend);
    }

    template<typename T>
    void ReverseConstIterate() noexcept
    {const typename CTestedList<T>::size_type listSize = 20;

        CTestedList<T> list;
        vector<T> t = ValueProvider<T>()(listSize);
        for (T x : t)
            list.push_back(x);

        reverse(t.begin(), t.end());

        typename CTestedList<T>::const_reverse_iterator crbegin = list.crbegin();
        typename CTestedList<T>::const_reverse_iterator crend = list.crend();

        for (; crbegin != crend; ++crbegin)
            IZI_ASSERT(t[distance(list.crbegin(), crbegin)] == *crbegin);

        IZI_ASSERT(crbegin == crend);
    }

    template <typename T>
    void RunTemplatedTests() noexcept
    {
        CreateEmptyList<T>();
        CreateSizedList<T>();

        for (T x : ValueProvider<T>()(5))
            CreateSizeListWithValue<T>(x);

        CreateListByExplicitCopy<T>();
        CreateListByImplicitCopy<T>();

        Iterate<T>();
        ConstIterate<T>();
        ReverseIterate<T>();
        ReverseConstIterate<T>();
    }
}

void CTests::RunTests() noexcept
{
    cout << "Starting tests..." << endl << endl;

    srand(time(NULL));
    IZI_CALLTEST(RunTemplatedTests<int>());
    IZI_CALLTEST(RunTemplatedTests<int*>());
    IZI_CALLTEST(RunTemplatedTests<shared_ptr<int>>());

    IZI_CALLTEST(RunTemplatedTests<TestClass>());
    IZI_CALLTEST(RunTemplatedTests<TestClass*>());
    IZI_CALLTEST(RunTemplatedTests<shared_ptr<TestClass>>());

    cout << "Tests done..." << endl << endl;
}
<|endoftext|>


<commit_before>/* ---------------------------------------------------------------------------
** This software is in the public domain, furnished "as is", without technical
** support, and with no warranty, express or implied, as to its usefulness for
** any purpose.
**
** PValueModuleScorer.cpp
** This class scores a gene based on an empirical p-value.
**
** Author: kca
** -------------------------------------------------------------------------*/

#include "PValueModuleScorer.h"
#include "coreroutines.h"
#include <algorithm>
#include <math.h>
#include <ctime>

void shuffleGroups(const std::vector<int> groupSizes, std::vector<int>& allIndices, TIndicesGroups& shuffledGroups) {
  std::random_shuffle(allIndices.begin(), allIndices.end());
  std::vector<int>::const_iterator it(allIndices.begin());

  for (auto size : groupSizes) {
    std::vector<int> newGroup;
    for (int i = 0; i < size && it != allIndices.end(); ++i, it++) {
      int item(*it);
      newGroup.push_back(item);
    }
    shuffledGroups.push_back(newGroup);
  }
}

void pullOutSubMatrix(const float* const similarities, const int width, std::vector<int>& indices, float* const newMatrix, std::map<int, int>& newIndexMap) {
  // Sort indices from least to greatest
  std::sort(indices.begin(), indices.end());
  const int n = indices.size();
  float* wcurr = newMatrix;

  // Pull out submatrix
  for (auto i : indices) {
    const float* curr = similarities + width * i;
    int col = 0;
    for (auto j : indices) {
      wcurr[col++] = curr[j];
    }
    wcurr += n;
  }

  // Map new indices
  int ii = 0;
  for (auto i : indices) {
    newIndexMap[i] = ii++;
  }
}

float calculateZScore(const std::vector<float>& nullScores, const float actualScore) {
  float zscore = (actualScore - mean(nullScores)) / sqrtf(variance(nullScores));
  return zscore;
}

bool PValueModuleScorer::ScoreModule(const float* const similarities, const int width, const TIndicesGroups& groups, TScoreMap& scores) const
{
  // Seed the random number generator.
  std::srand(std::time(0));

  std::map<int, std::vector<float> > allScores;
  int gind = 1;

  int n = 0;
  for (auto const &g : groups) {
    n += g.size();
  }

  // Allocate buffer
  float* const subMatrix = (float*)malloc(sizeof(float) * n * n);
  if (subMatrix == 0) {
    std::cerr << "Could not allocate buffer of " << sizeof(float) * n * n << " bytes" << std::endl;
    exit(-1);
  }

  for (auto const &g : groups) {
    std::vector<int> otherIndices;
    for (int i = 0; i < width; ++i) {
      otherIndices.push_back(i);
    }
    for (auto const i : g) {
      auto it = std::find(otherIndices.begin(), otherIndices.end(), i);
      otherIndices.erase(it);
    }
    TIndicesGroups otherGroups(groups);
    auto it = std::find(otherGroups.begin(), otherGroups.end(), g);
    otherGroups.erase(it);
    std::vector<int> groupSizes;
    for (auto const &o : otherGroups) {
      groupSizes.push_back(o.size());
    }

    printf("Calculating p-values for locus %d / %d\n", gind++, (int)groups.size());
    for (int i = 0; i < mNumIterations; ++i) {
      //if (i % 1000 == 0) printf("Iteration %d complete.\n", i);
      TIndicesGroups shuffledGroups;
      shuffleGroups(groupSizes, otherIndices, shuffledGroups);
      shuffledGroups.push_back(g);

      // Make temporary matrix-- this is good for cache locality. Much faster.
      std::vector<int> flattenedIndices;
      flattenGroups(shuffledGroups, flattenedIndices);
      std::map<int, int> indexMap;
      pullOutSubMatrix(similarities, width, flattenedIndices, subMatrix, indexMap);

      // Map new indices
      TIndicesGroups newGroups;
      mapGroupsToIndices(shuffledGroups, indexMap, newGroups);

      TScoreMap temp;
      //mScorer->ScoreModule(similarities, width, shuffledGroups, temp);
      mScorer->ScoreModule(subMatrix, n, newGroups, temp);
      for (auto const i : g) {
	allScores[i].push_back(temp[indexMap[i]]);
      }
    }
  }

  TScoreMap real;
  mScorer->ScoreModule(similarities, width, groups, real);
  for (auto const &item : allScores) {
    float myScore(real[item.first]);
    int numBetter(0);

    for (auto const i : item.second) {
      if (i > myScore) {
    	numBetter++;
      }
    }
    scores[item.first] = ((float)numBetter / mNumIterations);
    //scores[item.first] = calculateZScore(item.second, myScore);
  }

  free(subMatrix);
  return true;
}

bool pvalCompare(const std::pair<int, float>& firstElem, const std::pair<int, float>& secondElem) {
  return firstElem.second < secondElem.second;
}

bool zscoreCompare(const std::pair<int, float>& firstElem, const std::pair<int, float>& secondElem) {
  return firstElem.second > secondElem.second;
}

void calculatePValues(const TScoreMap& scores, TScoreMap& pvalScores) {
  for (auto const &e : scores) {
    pvalScores[e.first] = 1.0f - phi(e.second);
  }
}

void benjaminiHochbergCorrection(const TScoreMap& pvals, TScoreMap& pvalAdjusted) {
  std::vector<std::pair<int, float> > pairs;
  sortMapByVal(pvals, pairs, pvalCompare);

  int n(pairs.size());
  int k = 1;
  for (auto const &e : pairs) {
    pvalAdjusted[e.first] = std::min(1.0f, e.second * (float)n / (float)k);
    k++;
  }
}

void bonferroniCorrection(const TScoreMap& pvals, TScoreMap& pvalAdjusted) {
  std::vector<std::pair<int, float> > pairs;
  sortMapByVal(pvals, pairs, pvalCompare);

  int n(pairs.size());
  for (auto const &e : pairs) {
    pvalAdjusted[e.first] = std::min(1.0f, e.second * (float)n);
  }
}

void PValueModuleScorer::BriefSummary(TScoreMap& scores, TReverseIndexMap& rmap, std::ostream& out) const
{
  std::vector<std::pair<int, float> > pairs;
  //sortMapByVal(scores, pairs, zscoreCompare);
  sortMapByVal(scores, pairs, pvalCompare);
  //TScoreMap pvals, pvalsAdjusted;
  TScoreMap pvalsAdjusted;
  //calculatePValues(scores, pvals);
  //benjaminiHochbergCorrection(pvals, pvalsAdjusted);
  //bonferroniCorrection(pvals, pvalsAdjusted);
  bonferroniCorrection(scores, pvalsAdjusted);

  out << std::endl << "Top 10 genes" << std::endl << "Gene\tZ score\tP value\tAdj. p value" << std::endl << "----------------" << std::endl;
  int i = 0;
  for (auto it = pairs.begin(); it != pairs.end() && i < 10; it++, i++) {
    //out << rmap[it->first] << "\t" << it->second << "\t" << pvals[it->first] << "\t" << pvalsAdjusted[it->first] << std::endl;
    out << rmap[it->first] << "\t" << it->second << "\t" << scores[it->first] << "\t" << pvalsAdjusted[it->first] << std::endl;
  }
}

void PValueModuleScorer::LongSummary(TScoreMap& scores, TReverseIndexMap& rmap, const TIndicesGroups& groups, std::ostream& out) const
{
  TScoreMap pvalsAdjusted;
  //TScoreMap pvals, pvalsAdjusted;
  //calculatePValues(scores, pvals);
  //benjaminiHochbergCorrection(pvals, pvalsAdjusted);
  //bonferroniCorrection(pvals, pvalsAdjusted);
  bonferroniCorrection(scores, pvalsAdjusted);

  int l = 1;
  for (auto const &g : groups) {
    out << "----------------" << std::endl;
    out << "Locus " << l << std::endl;
    out << "Gene\tZ score\tP value\tAdj. p value" << std::endl;
    out << "----------------" << std::endl;
    TScoreMap locusScores;
    for (auto const &e : g) {
      locusScores[e] = scores[e];
    }
    std::vector<std::pair<int, float> > pairs;
    //sortMapByVal(locusScores, pairs, zscoreCompare);
    sortMapByVal(locusScores, pairs, pvalCompare);
    for (auto const &e : pairs) {
      //out << rmap[e.first] << "\t" << e.second << "\t" << pvals[e.first] << "\t" << pvalsAdjusted[e.first] << std::endl;
      out << rmap[e.first] << "\t" << e.second << "\t" << scores[e.first] << "\t" << pvalsAdjusted[e.first] << std::endl;
    }
    l++;
  }

}
<commit_msg>For posterity.<commit_after>/* ---------------------------------------------------------------------------
** This software is in the public domain, furnished "as is", without technical
** support, and with no warranty, express or implied, as to its usefulness for
** any purpose.
**
** PValueModuleScorer.cpp
** This class scores a gene based on an empirical p-value.
**
** Author: kca
** -------------------------------------------------------------------------*/

#include "PValueModuleScorer.h"
#include "coreroutines.h"
#include <algorithm>
#include <math.h>
#include <ctime>

void shuffleGroups(const std::vector<int> groupSizes, std::vector<int>& allIndices, TIndicesGroups& shuffledGroups) {
  std::random_shuffle(allIndices.begin(), allIndices.end());
  std::vector<int>::const_iterator it(allIndices.begin());

  for (auto size : groupSizes) {
    std::vector<int> newGroup;
    for (int i = 0; i < size && it != allIndices.end(); ++i, it++) {
      int item(*it);
      newGroup.push_back(item);
    }
    shuffledGroups.push_back(newGroup);
  }
}

void pullOutSubMatrix(const float* const similarities, const int width, std::vector<int>& indices, float* const newMatrix, std::map<int, int>& newIndexMap) {
  // Sort indices from least to greatest
  std::sort(indices.begin(), indices.end());
  const int n = indices.size();
  float* wcurr = newMatrix;

  // Pull out submatrix
  for (auto i : indices) {
    const float* curr = similarities + width * i;
    int col = 0;
    for (auto j : indices) {
      wcurr[col++] = curr[j];
    }
    wcurr += n;
  }

  // Map new indices
  int ii = 0;
  for (auto i : indices) {
    newIndexMap[i] = ii++;
  }
}

float calculateZScore(const std::vector<float>& nullScores, const float actualScore) {
  float zscore = (actualScore - mean(nullScores)) / sqrtf(variance(nullScores));
  return zscore;
}

bool PValueModuleScorer::ScoreModule(const float* const similarities, const int width, const TIndicesGroups& groups, TScoreMap& scores) const
{
  // Seed the random number generator.
  std::srand(std::time(0));

  std::map<int, std::vector<float> > allScores;
  int gind = 1;

  int n = 0;
  for (auto const &g : groups) {
    n += g.size();
  }

  // Allocate buffer
  float* const subMatrix = (float*)malloc(sizeof(float) * n * n);
  if (subMatrix == 0) {
    std::cerr << "Could not allocate buffer of " << sizeof(float) * n * n << " bytes" << std::endl;
    exit(-1);
  }

  for (auto const &g : groups) {
    std::vector<int> otherIndices;
    for (int i = 0; i < width; ++i) {
      otherIndices.push_back(i);
    }
    for (auto const i : g) {
      auto it = std::find(otherIndices.begin(), otherIndices.end(), i);
      otherIndices.erase(it);
    }
    TIndicesGroups otherGroups(groups);
    auto it = std::find(otherGroups.begin(), otherGroups.end(), g);
    otherGroups.erase(it);
    std::vector<int> groupSizes;
    for (auto const &o : otherGroups) {
      groupSizes.push_back(o.size());
    }

    printf("Calculating p-values for locus %d / %d\n", gind++, (int)groups.size());

    // ** Pseudo-randomly choose guys in loci. **
    // [1] Randomly choose starting point
    //int startingPoint = rand() % otherIndices.size();
    for (int i = 0; i < mNumIterations; ++i) {
      //if (i % 1000 == 0) printf("Iteration %d complete.\n", i);
      TIndicesGroups shuffledGroups;
      shuffleGroups(groupSizes, otherIndices, shuffledGroups);
      shuffledGroups.push_back(g);
      // int curr = startingPoint;
      // for (auto const s : groupSizes) {
      // 	std::vector<int> newGroup;
      // 	for (int v = 0; v < s; ++v) {
      // 	  const int me = curr % otherIndices.size();
      // 	  newGroup.push_back(otherIndices[me]);
      // 	  curr++;
      // 	}
      // 	shuffledGroups.push_back(newGroup);
      // }
      // shuffledGroups.push_back(g);
      // startingPoint += (otherIndices.size() - 19);
      // startingPoint %= otherIndices.size();

      // Make temporary matrix-- this is good for cache locality. Much faster.
      std::vector<int> flattenedIndices;
      flattenGroups(shuffledGroups, flattenedIndices);
      std::map<int, int> indexMap;
      pullOutSubMatrix(similarities, width, flattenedIndices, subMatrix, indexMap);

      // Map new indices
      TIndicesGroups newGroups;
      mapGroupsToIndices(shuffledGroups, indexMap, newGroups);

      TScoreMap temp;
      //mScorer->ScoreModule(similarities, width, shuffledGroups, temp);
      mScorer->ScoreModule(subMatrix, n, newGroups, temp);
      for (auto const i : g) {
	allScores[i].push_back(temp[indexMap[i]]);
      }
    }
  }

  TScoreMap real;
  mScorer->ScoreModule(similarities, width, groups, real);
  for (auto const &item : allScores) {
    float myScore(real[item.first]);
    // int numBetter(0);
    // for (auto const i : item.second) {
    //   if (i > myScore) {
    // 	numBetter++;
    //   }
    // }
    // scores[item.first] = ((float)numBetter / mNumIterations);
    scores[item.first] = calculateZScore(item.second, myScore);
  }

  free(subMatrix);
  return true;
}

bool pvalCompare(const std::pair<int, float>& firstElem, const std::pair<int, float>& secondElem) {
  return firstElem.second < secondElem.second;
}

bool zscoreCompare(const std::pair<int, float>& firstElem, const std::pair<int, float>& secondElem) {
  return firstElem.second > secondElem.second;
}

void calculatePValues(const TScoreMap& scores, TScoreMap& pvalScores) {
  for (auto const &e : scores) {
    pvalScores[e.first] = 1.0f - phi(e.second);
  }
}

void benjaminiHochbergCorrection(const TScoreMap& pvals, TScoreMap& pvalAdjusted) {
  std::vector<std::pair<int, float> > pairs;
  sortMapByVal(pvals, pairs, pvalCompare);

  int n(pairs.size());
  int k = 1;
  for (auto const &e : pairs) {
    pvalAdjusted[e.first] = std::min(1.0f, e.second * (float)n / (float)k);
    k++;
  }
}

void bonferroniCorrection(const TScoreMap& pvals, TScoreMap& pvalAdjusted) {
  std::vector<std::pair<int, float> > pairs;
  sortMapByVal(pvals, pairs, pvalCompare);

  int n(pairs.size());
  for (auto const &e : pairs) {
    pvalAdjusted[e.first] = std::min(1.0f, e.second * (float)n);
  }
}

void PValueModuleScorer::BriefSummary(TScoreMap& scores, TReverseIndexMap& rmap, std::ostream& out) const
{
  std::vector<std::pair<int, float> > pairs;
  sortMapByVal(scores, pairs, zscoreCompare);
  //sortMapByVal(scores, pairs, pvalCompare);
  TScoreMap pvals, pvalsAdjusted;
  //TScoreMap pvalsAdjusted;
  calculatePValues(scores, pvals);
  //benjaminiHochbergCorrection(pvals, pvalsAdjusted);
  bonferroniCorrection(pvals, pvalsAdjusted);
  //bonferroniCorrection(scores, pvalsAdjusted);

  out << std::endl << "Top 10 genes" << std::endl << "Gene\tZ score\tP value\tAdj. p value" << std::endl << "----------------" << std::endl;
  int i = 0;
  for (auto it = pairs.begin(); it != pairs.end() && i < 10; it++, i++) {
    out << rmap[it->first] << "\t" << it->second << "\t" << pvals[it->first] << "\t" << pvalsAdjusted[it->first] << std::endl;
    //out << rmap[it->first] << "\t" << it->second << "\t" << scores[it->first] << "\t" << pvalsAdjusted[it->first] << std::endl;
  }
}

void PValueModuleScorer::LongSummary(TScoreMap& scores, TReverseIndexMap& rmap, const TIndicesGroups& groups, std::ostream& out) const
{
  //TScoreMap pvalsAdjusted;
  TScoreMap pvals, pvalsAdjusted;
  calculatePValues(scores, pvals);
  //benjaminiHochbergCorrection(pvals, pvalsAdjusted);
  bonferroniCorrection(pvals, pvalsAdjusted);
  //bonferroniCorrection(scores, pvalsAdjusted);

  int l = 1;
  for (auto const &g : groups) {
    out << "----------------" << std::endl;
    out << "Locus " << l << std::endl;
    out << "Gene\tZ score\tP value\tAdj. p value" << std::endl;
    out << "----------------" << std::endl;
    TScoreMap locusScores;
    for (auto const &e : g) {
      locusScores[e] = scores[e];
    }
    std::vector<std::pair<int, float> > pairs;
    //sortMapByVal(locusScores, pairs, zscoreCompare);
    sortMapByVal(locusScores, pairs, pvalCompare);
    for (auto const &e : pairs) {
      out << rmap[e.first] << "\t" << e.second << "\t" << pvals[e.first] << "\t" << pvalsAdjusted[e.first] << std::endl;
      //out << rmap[e.first] << "\t" << e.second << "\t" << scores[e.first] << "\t" << pvalsAdjusted[e.first] << std::endl;
    }
    l++;
  }

}
<|endoftext|>