<commit_before>/*
    Copyright (C) 2008  Paul Richards.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "stdafx.h"
#include "Externals.h"

namespace {
    const std::string lines = std::string(50, '-') + "\n";
}

class ProffyException : public std::exception
{
public:
    ProffyException(const std::string& reason) :
        fReason(reason)
    {
    }

    /// std::exception
    virtual const char* what () const throw ()
    {
        return fReason.c_str();
    }

private:
    const std::string fReason;
};

void Assert(
    const bool v,
    const std::string& function,
    const std::string& file,
    const int lineNumber)
{
    if (v == false) {
        std::ostringstream message;
        message << "Assertion failure: " << function << " (" << file << ", line " << lineNumber << ")";
        throw ProffyException(message.str());
    }
}

#define ASSERT(x) Assert((x), __FUNCTION__, __FILE__, __LINE__);

class DebugEventCallbacks : public DebugBaseEventCallbacks
{
public:
    virtual ~DebugEventCallbacks()
    {
    }

    virtual ULONG __stdcall AddRef(void)
    {
        return 1;
    }

    virtual ULONG __stdcall Release(void)
    {
        return 1;
    }

    virtual HRESULT __stdcall GetInterestMask(ULONG* mask)
    {
        *mask =
            DEBUG_EVENT_BREAKPOINT |
            DEBUG_EVENT_EXCEPTION |
            DEBUG_EVENT_CREATE_THREAD |
            DEBUG_EVENT_EXIT_THREAD |
            DEBUG_EVENT_CREATE_PROCESS |
            DEBUG_EVENT_EXIT_PROCESS |
            DEBUG_EVENT_LOAD_MODULE |
            DEBUG_EVENT_UNLOAD_MODULE |
            DEBUG_EVENT_SYSTEM_ERROR |
            DEBUG_EVENT_SESSION_STATUS |
            DEBUG_EVENT_CHANGE_DEBUGGEE_STATE |
            DEBUG_EVENT_CHANGE_ENGINE_STATE |
            DEBUG_EVENT_CHANGE_SYMBOL_STATE;
        return S_OK;
    }

    virtual HRESULT __stdcall ChangeEngineState(
        ULONG flags,
        ULONG64 /*argument*/)
    {
        //if (flags & DEBUG_CES_CURRENT_THREAD) {
        //    std::cout << __FUNCTION__ << ": The current thread has changed, which implies that the current target and current process might also have changed.\n";
        //}
        if (flags & DEBUG_CES_EFFECTIVE_PROCESSOR) {
            std::cout << __FUNCTION__ << ": The effective processor has changed.\n";
        }
        if (flags & DEBUG_CES_BREAKPOINTS) {
            std::cout << __FUNCTION__ << ": One or more breakpoints have changed.\n";
        }
        if (flags & DEBUG_CES_CODE_LEVEL) {
            std::cout << __FUNCTION__ << ": The code interpretation level has changed.\n";
        }
        //if (flags & DEBUG_CES_EXECUTION_STATUS) {
        //    std::cout << __FUNCTION__ << ": The execution status has changed.\n";
        //}
        if (flags & DEBUG_CES_ENGINE_OPTIONS) {
            std::cout << __FUNCTION__ << ": The engine options have changed.\n";
        }
        if (flags & DEBUG_CES_LOG_FILE) {
            std::cout << __FUNCTION__ << ": The log file has been opened or closed.\n";
        }
        if (flags & DEBUG_CES_RADIX) {
            std::cout << __FUNCTION__ << ": The default radix has changed.\n";
        }
        if (flags & DEBUG_CES_EVENT_FILTERS) {
            std::cout << __FUNCTION__ << ": The event filters have changed.\n";
        }
        if (flags & DEBUG_CES_PROCESS_OPTIONS) {
            std::cout << __FUNCTION__ << ": The process options for the current process have changed.\n";
        }
        if (flags & DEBUG_CES_EXTENSIONS) {
            std::cout << __FUNCTION__ << ": Extension DLLs have been loaded or unloaded. (For more information, see Loading Debugger Extension DLLs.)\n";
        }
        if (flags & DEBUG_CES_SYSTEMS) {
            std::cout << __FUNCTION__ << ": A target has been added or removed.\n";
        }
        if (flags & DEBUG_CES_ASSEMBLY_OPTIONS) {
            std::cout << __FUNCTION__ << ": The assemble options have changed.\n";
        }
        if (flags & DEBUG_CES_EXPRESSION_SYNTAX) {
            std::cout << __FUNCTION__ << ": The default expression syntax has changed.\n";
        }
        if (flags & DEBUG_CES_TEXT_REPLACEMENTS) {
            std::cout << __FUNCTION__ << ": Text replacements have changed.\n";
        }

        return S_OK;
    }

    virtual HRESULT __stdcall Breakpoint(IDebugBreakpoint* /*breakpoint*/)
    {
        std::cout << __FUNCTION__ << "\n";
        return S_OK;
    }
};

class DebugOutputCallbacks : public IDebugOutputCallbacks
{
public:
    virtual ~DebugOutputCallbacks()
    {
    }

    virtual HRESULT __stdcall QueryInterface(REFIID interfaceId, PVOID* result)
    {
        *result = NULL;

        if (IsEqualIID(interfaceId, __uuidof(IUnknown)) ||
            IsEqualIID(interfaceId, __uuidof(IDebugEventCallbacks))) {
            *result = this;
            AddRef();
            return S_OK;
        } else {
            return E_NOINTERFACE;
        }
    }

    virtual ULONG __stdcall AddRef(void)
    {
        return 1;
    }

    virtual ULONG __stdcall Release(void)
    {
        return 1;
    }

    virtual HRESULT __stdcall Output(ULONG /*mask*/, PCSTR text)
    {
        std::cout << text;
        return S_OK;
    }
};

int main(void)
{
    try {
        // Start something which we will attempt to profile.
        STARTUPINFO startupInfo = {0};
        PROCESS_INFORMATION processInformation = {0};
        BOOL processStarted = ::CreateProcess(
            L"../Debug/SampleTarget.exe",
            NULL,
            NULL,
            NULL,
            FALSE,
            CREATE_NEW_CONSOLE,
            NULL,
            NULL,
            &startupInfo,
            &processInformation);
        if (processStarted != TRUE) {
            std::ostringstream message;
            message << ::GetLastError();
            throw ProffyException(message.str());
        }

        // Initialize COM, no idea if this is necessary.
        HRESULT result = S_OK;
        result = ::CoInitialize(0);
        assert(result == S_OK);

        // Get the IDebugClient to start.
        IDebugClient* debugClient = NULL;
        result = ::DebugCreate(
            __uuidof(IDebugClient),
            reinterpret_cast<void**>(&debugClient));
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Set our output callbacks.
        DebugOutputCallbacks debugOutputCallbacks;
        result = debugClient->SetOutputCallbacks(&debugOutputCallbacks);
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Set our event callbacks.
        DebugEventCallbacks debugEventCallbacks;
        result = debugClient->SetEventCallbacks(&debugEventCallbacks);
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Get the IDebugControl.
        IDebugControl* debugControl = NULL;
        result = debugClient->QueryInterface(
            __uuidof(IDebugControl),
            reinterpret_cast<void**>(&debugControl));
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Get the IDebugSystemObjects.
        IDebugSystemObjects* debugSystemObjects = NULL;
        result = debugClient->QueryInterface(
            __uuidof(IDebugSystemObjects),
            reinterpret_cast<void**>(&debugSystemObjects));
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Done getting and setting, so now attach.
        result = debugClient->AttachProcess(
            0,
            processInformation.dwProcessId,
            0);
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Verify we have attached to what we think we have.
        ULONG debugeeTypeClass;
        ULONG debugeeTypeQualifier;
        result = debugControl->GetDebuggeeType(&debugeeTypeClass, &debugeeTypeQualifier);
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);
        ASSERT(debugeeTypeClass == DEBUG_CLASS_USER_WINDOWS);
        ASSERT(debugeeTypeQualifier == DEBUG_USER_WINDOWS_PROCESS);

        while (true) {
            std::cout << lines << "Waiting..\n" << lines;
            result = debugControl->WaitForEvent(0, 1000);
            ASSERT(result == S_FALSE);
            std::cout << lines << "Done Waiting..\n" << lines;

            ULONG executionStatus;
            result = debugControl->GetExecutionStatus(&executionStatus);
            ASSERT(result == S_OK);
            ASSERT(debugClient->FlushCallbacks() == S_OK);
            std::cout << "ExecutionStatus: " << executionStatus << "\n";

            //ULONG numberThreads;
            //result = debugSystemObjects->GetNumberThreads(&numberThreads);
            //ASSERT(result == S_OK);
            //ASSERT(debugClient->FlushCallbacks() == S_OK);
            //std::cout << "NumberThreads: " << numberThreads << "\n";

            ASSERT(executionStatus == DEBUG_STATUS_BREAK);

            //result = debugControl->OutputStackTrace(
            //    DEBUG_OUTCTL_THIS_CLIENT,
            //    NULL,
            //    10,
            //    DEBUG_STACK_FRAME_NUMBERS);
            //ASSERT(result == S_OK);
            //ASSERT(debugClient->FlushCallbacks() == S_OK);

            std::cout << "Sleeping..\n";
            ::Sleep(5000);
            std::cout << "Done sleeping..\n";
        }

        return EXIT_SUCCESS;
    } catch (const std::exception& ex) {
        std::cerr << typeid(ex).name() << ": " << ex.what() << "\n";
    }
    return EXIT_FAILURE;
}
<commit_msg>Successful printing of stack trace and dumping thread count!<commit_after>/*
    Copyright (C) 2008  Paul Richards.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "stdafx.h"
#include "Externals.h"

namespace {
    const std::string lines = std::string(50, '-') + "\n";
}

class ProffyException : public std::exception
{
public:
    ProffyException(const std::string& reason) :
        fReason(reason)
    {
    }

    /// std::exception
    virtual const char* what () const throw ()
    {
        return fReason.c_str();
    }

private:
    const std::string fReason;
};

void Assert(
    const bool v,
    const std::string& function,
    const std::string& file,
    const int lineNumber)
{
    if (v == false) {
        ::DebugBreak();
        std::ostringstream message;
        message << "Assertion failure: " << function << " (" << file << ", line " << lineNumber << ")";
        throw ProffyException(message.str());
    }
}

#define ASSERT(x) Assert((x), __FUNCTION__, __FILE__, __LINE__);

class DebugEventCallbacks : public DebugBaseEventCallbacks
{
public:
    virtual ~DebugEventCallbacks()
    {
    }

    virtual ULONG __stdcall AddRef(void)
    {
        return 1;
    }

    virtual ULONG __stdcall Release(void)
    {
        return 1;
    }

    virtual HRESULT __stdcall GetInterestMask(ULONG* mask)
    {
        *mask =
            DEBUG_EVENT_BREAKPOINT |
            DEBUG_EVENT_EXCEPTION |
            DEBUG_EVENT_CREATE_THREAD |
            DEBUG_EVENT_EXIT_THREAD |
            DEBUG_EVENT_CREATE_PROCESS |
            DEBUG_EVENT_EXIT_PROCESS |
            DEBUG_EVENT_LOAD_MODULE |
            DEBUG_EVENT_UNLOAD_MODULE |
            DEBUG_EVENT_SYSTEM_ERROR |
            DEBUG_EVENT_SESSION_STATUS |
            DEBUG_EVENT_CHANGE_DEBUGGEE_STATE |
            DEBUG_EVENT_CHANGE_ENGINE_STATE |
            DEBUG_EVENT_CHANGE_SYMBOL_STATE;
        return S_OK;
    }

    virtual HRESULT __stdcall ChangeEngineState(
        ULONG flags,
        ULONG64 /*argument*/)
    {
        //if (flags & DEBUG_CES_CURRENT_THREAD) {
        //    std::cout << __FUNCTION__ << ": The current thread has changed, which implies that the current target and current process might also have changed.\n";
        //}
        if (flags & DEBUG_CES_EFFECTIVE_PROCESSOR) {
            std::cout << __FUNCTION__ << ": The effective processor has changed.\n";
        }
        if (flags & DEBUG_CES_BREAKPOINTS) {
            std::cout << __FUNCTION__ << ": One or more breakpoints have changed.\n";
        }
        if (flags & DEBUG_CES_CODE_LEVEL) {
            std::cout << __FUNCTION__ << ": The code interpretation level has changed.\n";
        }
        //if (flags & DEBUG_CES_EXECUTION_STATUS) {
        //    std::cout << __FUNCTION__ << ": The execution status has changed.\n";
        //}
        if (flags & DEBUG_CES_ENGINE_OPTIONS) {
            std::cout << __FUNCTION__ << ": The engine options have changed.\n";
        }
        if (flags & DEBUG_CES_LOG_FILE) {
            std::cout << __FUNCTION__ << ": The log file has been opened or closed.\n";
        }
        if (flags & DEBUG_CES_RADIX) {
            std::cout << __FUNCTION__ << ": The default radix has changed.\n";
        }
        if (flags & DEBUG_CES_EVENT_FILTERS) {
            std::cout << __FUNCTION__ << ": The event filters have changed.\n";
        }
        if (flags & DEBUG_CES_PROCESS_OPTIONS) {
            std::cout << __FUNCTION__ << ": The process options for the current process have changed.\n";
        }
        if (flags & DEBUG_CES_EXTENSIONS) {
            std::cout << __FUNCTION__ << ": Extension DLLs have been loaded or unloaded. (For more information, see Loading Debugger Extension DLLs.)\n";
        }
        if (flags & DEBUG_CES_SYSTEMS) {
            std::cout << __FUNCTION__ << ": A target has been added or removed.\n";
        }
        if (flags & DEBUG_CES_ASSEMBLY_OPTIONS) {
            std::cout << __FUNCTION__ << ": The assemble options have changed.\n";
        }
        if (flags & DEBUG_CES_EXPRESSION_SYNTAX) {
            std::cout << __FUNCTION__ << ": The default expression syntax has changed.\n";
        }
        if (flags & DEBUG_CES_TEXT_REPLACEMENTS) {
            std::cout << __FUNCTION__ << ": Text replacements have changed.\n";
        }

        return S_OK;
    }

    virtual HRESULT __stdcall Breakpoint(IDebugBreakpoint* /*breakpoint*/)
    {
        std::cout << __FUNCTION__ << "\n";
        return S_OK;
    }
};

class DebugOutputCallbacks : public IDebugOutputCallbacks
{
public:
    virtual ~DebugOutputCallbacks()
    {
    }

    virtual HRESULT __stdcall QueryInterface(REFIID interfaceId, PVOID* result)
    {
        *result = NULL;

        if (IsEqualIID(interfaceId, __uuidof(IUnknown)) ||
            IsEqualIID(interfaceId, __uuidof(IDebugEventCallbacks))) {
            *result = this;
            AddRef();
            return S_OK;
        } else {
            return E_NOINTERFACE;
        }
    }

    virtual ULONG __stdcall AddRef(void)
    {
        return 1;
    }

    virtual ULONG __stdcall Release(void)
    {
        return 1;
    }

    virtual HRESULT __stdcall Output(ULONG /*mask*/, PCSTR text)
    {
        std::cout << text;
        return S_OK;
    }
};

int main(void)
{
    try {
        // Start something which we will attempt to profile.
        STARTUPINFO startupInfo = {0};
        PROCESS_INFORMATION processInformation = {0};
        BOOL processStarted = ::CreateProcess(
            L"../Debug/SampleTarget.exe",
            NULL,
            NULL,
            NULL,
            FALSE,
            CREATE_NEW_CONSOLE,
            NULL,
            NULL,
            &startupInfo,
            &processInformation);
        if (processStarted != TRUE) {
            std::ostringstream message;
            message << ::GetLastError();
            throw ProffyException(message.str());
        }

        // Initialize COM, no idea if this is necessary.
        HRESULT result = S_OK;
        result = ::CoInitialize(0);
        assert(result == S_OK);

        // Get the IDebugClient to start.
        IDebugClient* debugClient = NULL;
        result = ::DebugCreate(
            __uuidof(IDebugClient),
            reinterpret_cast<void**>(&debugClient));
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Set our output callbacks.
        DebugOutputCallbacks debugOutputCallbacks;
        result = debugClient->SetOutputCallbacks(&debugOutputCallbacks);
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Set our event callbacks.
        DebugEventCallbacks debugEventCallbacks;
        result = debugClient->SetEventCallbacks(&debugEventCallbacks);
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Get the IDebugControl.
        IDebugControl* debugControl = NULL;
        result = debugClient->QueryInterface(
            __uuidof(IDebugControl),
            reinterpret_cast<void**>(&debugControl));
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Get the IDebugSystemObjects.
        IDebugSystemObjects* debugSystemObjects = NULL;
        result = debugClient->QueryInterface(
            __uuidof(IDebugSystemObjects),
            reinterpret_cast<void**>(&debugSystemObjects));
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Done getting and setting, so now attach.
        result = debugClient->AttachProcess(
            0,
            processInformation.dwProcessId,
            DEBUG_ATTACH_NONINVASIVE);
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);

        // Verify we have attached to what we think we have.
        ULONG debugeeTypeClass;
        ULONG debugeeTypeQualifier;
        result = debugControl->GetDebuggeeType(&debugeeTypeClass, &debugeeTypeQualifier);
        ASSERT(result == S_OK);
        ASSERT(debugClient->FlushCallbacks() == S_OK);
        ASSERT(debugeeTypeClass == DEBUG_CLASS_USER_WINDOWS);
        ASSERT(debugeeTypeQualifier == DEBUG_USER_WINDOWS_PROCESS);

        while (true) {
            std::cout << lines << "Waiting..\n" << lines;
            result = debugControl->WaitForEvent(0, 3000);
            ASSERT(result == S_OK);
            std::cout << lines << "Done Waiting..\n" << lines;

            ULONG executionStatus;
            result = debugControl->GetExecutionStatus(&executionStatus);
            ASSERT(result == S_OK);
            ASSERT(debugClient->FlushCallbacks() == S_OK);
            std::cout << "ExecutionStatus: " << executionStatus << "\n";

            ULONG numberThreads;
            result = debugSystemObjects->GetNumberThreads(&numberThreads);
            ASSERT(result == S_OK);
            ASSERT(debugClient->FlushCallbacks() == S_OK);
            std::cout << "NumberThreads: " << numberThreads << "\n";

            ASSERT(executionStatus == DEBUG_STATUS_BREAK);

            result = debugControl->OutputStackTrace(
                DEBUG_OUTCTL_THIS_CLIENT,
                NULL,
                10,
                DEBUG_STACK_FRAME_NUMBERS);
            ASSERT(result == S_OK);
            ASSERT(debugClient->FlushCallbacks() == S_OK);

            std::cout << "Sleeping..\n";
            ::Sleep(5000);
            std::cout << "Done sleeping..\n";
            break;
        }

        return EXIT_SUCCESS;
    } catch (const std::exception& ex) {
        std::cerr << typeid(ex).name() << ": " << ex.what() << "\n";
    }
    return EXIT_FAILURE;
}
<|endoftext|>


<commit_before>/**
 * @author  Akram Hussein
 * Copyright (C) 2012  Akram Hussein
 *                     Imperial College London
 **/

#include <sys/stat.h>
#include <iostream>
#include <limits>

#include <pangolin/pangolin.h>
#include <pangolin/video.h>
#include <pangolin/video/firewire.h>
#include <pangolin/display_internal.h>

#include <boost/thread.hpp>

using namespace pangolin;
using namespace std;

int main( int argc, char* argv[] )
{
    /*-----------------------------------------------------------------------
     *  SETUP SOURCE
     *-----------------------------------------------------------------------*/

    FirewireVideo video = FirewireVideo();
    video.LoadConfig();
    video.SetHDRRegister(false);
    video.SetMetaDataFlags( META_ALL_AND_ABS );
    video.CreateShutterMaps();
    video.SetAllFeaturesAuto();
    //video.Stop();
    //video.FlushDMABuffer();//remove spurious frames
    //video.PrintCameraReport();

    unsigned char* img = new unsigned char[video.SizeBytes()];

    /*-----------------------------------------------------------------------
     *  GUI
     *-----------------------------------------------------------------------*/

    VideoPixelFormat vid_fmt = VideoFormatFromString(video.PixFormat());
    const unsigned w = video.Width();
    const unsigned h = video.Height();

    // Create Glut window
    const int panel_width = 200;
    double scale = 1.25;
    pangolin::CreateGlutWindowAndBind("SimpleHDR", w*scale + panel_width, h*scale);

    // Create viewport for video with fixed aspect
    View& d_panel = pangolin::CreatePanel("ui.")
    .SetBounds(0.0, 1.0, 0.0, Attach::Pix(panel_width));

    // Create viewport for video with fixed aspect
    View& vVideo = Display("Video")
    .SetBounds(0.0, 1.0, Attach::Pix(panel_width), 1.0)
    .SetAspect((float)w/h);

    // OpenGl Texture for video frame
    GlTexture texVideo(w,h,GL_RGBA8);

    /*-----------------------------------------------------------------------
     *  KEYBOARD SHORCUTS
     *-----------------------------------------------------------------------*/

    pangolin::RegisterKeyPressCallback( 'h', SetVarFunctor<bool>("ui.HDR Mode", true));                 // hdr mode
    pangolin::RegisterKeyPressCallback( 'n', SetVarFunctor<bool>("ui.HDR Mode", false));                // normal mode
    pangolin::RegisterKeyPressCallback( 32 , SetVarFunctor<bool>("ui.Record", true));                   // grab multiple frames in ppm & jpeg
    pangolin::RegisterKeyPressCallback( 'c', SetVarFunctor<bool>("ui.Capture Frame", true));            // grab single frame in ppm & jpeg
    pangolin::RegisterKeyPressCallback( 'm', SetVarFunctor<bool>("ui.Manual Camera Settings", true));   // manual on
    pangolin::RegisterKeyPressCallback( 'a', SetVarFunctor<bool>("ui.Manual Camera Settings", false));  // manual off
    pangolin::RegisterKeyPressCallback( 'e', SetVarFunctor<bool>("ui.Automatic Exposure Control", true));  // AEC on
    pangolin::RegisterKeyPressCallback( 'f', SetVarFunctor<bool>("ui.Get Response Function", true));    // get response function

    /*-----------------------------------------------------------------------
     *  CONTROL PANEL
     *-----------------------------------------------------------------------*/

    // camera brand/model info
    static Var<string> vendor("ui.Vendor", video.GetCameraVendor());
    static Var<string> camera("ui.Camera", video.GetCameraModel());

    // capture options
    static Var<bool> record("ui.Record",false,false);
    static Var<bool> hdr("ui.HDR Mode",false,true);
    static Var<bool> AEC("ui.Automatic Exposure Control",false,true);

    // recording info
    static Var<int> recorded_frames("ui.Recorded Frames", 0);
    static Var<int> recorded_time("ui.Recorded (secs)", 0);

    // single frame
    static Var<bool> capture("ui.Capture Frame",false,false);
    static Var<bool> capture_hdr("ui.Capture HDR Frame",false,false);

    // manual control
    static Var<bool> manual("ui.Manual Camera Settings",false,true);

    // camera settings
    static Var<float> shutter("ui.Shutter (s)", video.GetFeatureValue(DC1394_FEATURE_SHUTTER),
                               video.GetFeatureValueMin(DC1394_FEATURE_SHUTTER),
                               video.GetFeatureValueMax(DC1394_FEATURE_SHUTTER), true);

    static Var<float> exposure("ui.Exposure (EV)", video.GetFeatureValue(DC1394_FEATURE_EXPOSURE),
                                video.GetFeatureValueMin(DC1394_FEATURE_EXPOSURE),
                                video.GetFeatureValueMax(DC1394_FEATURE_EXPOSURE),false);

    static Var<float> brightness("ui.Brightness (%)", video.GetFeatureValue(DC1394_FEATURE_BRIGHTNESS),
                                 video.GetFeatureValueMin(DC1394_FEATURE_BRIGHTNESS),
                                 video.GetFeatureValueMax(DC1394_FEATURE_BRIGHTNESS),false);

    static Var<float> gain("ui.Gain (dB)", video.GetFeatureValue(DC1394_FEATURE_GAIN),
                           video.GetFeatureValueMin(DC1394_FEATURE_GAIN),
                           video.GetFeatureValueMax(DC1394_FEATURE_GAIN),false);

    static Var<float> gamma("ui.Gamma", video.GetFeatureValue(DC1394_FEATURE_GAMMA),
                            video.GetFeatureValueMin(DC1394_FEATURE_GAMMA),
                            video.GetFeatureValueMax(DC1394_FEATURE_GAMMA),false);

    static Var<float> saturation("ui.Saturation (%)", video.GetFeatureValue(DC1394_FEATURE_SATURATION),
                                 video.GetFeatureValueMin(DC1394_FEATURE_SATURATION),
                                 video.GetFeatureValueMax(DC1394_FEATURE_SATURATION),false);

    static Var<float> hue("ui.Hue (deg)", video.GetFeatureValue(DC1394_FEATURE_HUE),
                        video.GetFeatureValueMin(DC1394_FEATURE_HUE),
                        video.GetFeatureValueMax(DC1394_FEATURE_HUE),false);

    static Var<int> sharpness("ui.Sharpness",video.GetFeatureQuant(DC1394_FEATURE_SHARPNESS),
                              video.GetFeatureQuantMin(DC1394_FEATURE_SHARPNESS),
                              video.GetFeatureQuantMax(DC1394_FEATURE_SHARPNESS),false);

    static Var<int> whitebalance_B_U("ui.White Balance (Blue/U)",video.GetWhiteBalanceBlueU(),
                                     video.GetFeatureQuantMin(DC1394_FEATURE_WHITE_BALANCE),
                                     video.GetFeatureQuantMax(DC1394_FEATURE_WHITE_BALANCE),false);

    static Var<int> whitebalance_R_V("ui.White Balance (Red/V)",video.GetWhiteBalanceRedV(),
                                     video.GetFeatureQuantMin(DC1394_FEATURE_WHITE_BALANCE),
                                     video.GetFeatureQuantMax(DC1394_FEATURE_WHITE_BALANCE),false);

    static Var<bool> response_function("ui.Get Response Function",false,false);

    /*-----------------------------------------------------------------------
     *  CAPTURE LOOP
     *-----------------------------------------------------------------------*/

    bool save = false;
    bool under_over = true;
    time_t start, end;
    uint32_t hdr_shutter[3];

    // loop until quit (e.g ESC key)
    for(int frame_number = 0; !pangolin::ShouldQuit(); ++frame_number)
    {
        // Screen refresh
        if(pangolin::HasResized())
            DisplayBase().ActivateScissorAndClear();

        glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

        /*-----------------------------------------------------------------------
         *  CONTROL LOGIC
         *-----------------------------------------------------------------------*/

        if( pangolin::Pushed(response_function) ){ video.GetResponseFunction(); }

        // HDR MODE

        // checks if hdr mode has been switched and sets register on
        if(pangolin::Pushed(hdr.var->meta_gui_changed)){

            if( hdr ){

                video.SetFeatureAuto(DC1394_FEATURE_SHUTTER);
                float EV = video.GetFeatureValue(DC1394_FEATURE_EXPOSURE);

                cout << "[HDR]: HDR mode bracket range set at: " << endl;
                for (int i = -1; i <= 1 ; i++){
                    video.SetFeatureValue(DC1394_FEATURE_EXPOSURE, EV+i);
                    sleep(1);
                    cout << i << ": " << EV+i << " EV" << endl;
                    hdr_shutter[i+1] = video.GetFeatureQuant(DC1394_FEATURE_SHUTTER);
                }

                // set shutter values
                video.SetHDRShutterFlags(hdr_shutter[0], hdr_shutter[0], hdr_shutter[1], hdr_shutter[2]);
                video.SetHDRRegister(true);

            } else {
                video.SetHDRRegister(false);
            }
        }

        // switch under over flag
        under_over = under_over ? false : true ;

        // shutter settings when AEC mode activated
        if(pangolin::Pushed(AEC.var->meta_gui_changed)){

            if (hdr && AEC){
                cout << "[AEC]: AEC enabled" << endl;
                if(under_over){
                    hdr_shutter[0] = video.AEC(img, video.GetShutterMapAbs(hdr_shutter[0]), under_over);
                } else {
                    hdr_shutter[2] = video.AEC(img, video.GetShutterMapAbs(hdr_shutter[2]), under_over);
                }
                video.SetHDRShutterFlags(hdr_shutter[0], hdr_shutter[0], hdr_shutter[1], hdr_shutter[2]);
            } else {
                cout << "[AEC]: AEC disabled" << endl;
            }

        }
        // MANUAL SETTINGS

        if ( manual && !hdr ) {

            /*
             * exposure & shutter inter-linked, therefore need special logic control
             *  if: exposure val is changed, shutter val is changed (camera works out new value)
             *  else: shutter val is set to gui value
             */

            if(pangolin::Pushed(exposure.var->meta_gui_changed)){
                video.SetFeatureValue(DC1394_FEATURE_EXPOSURE, exposure);
                video.SetFeatureAuto(DC1394_FEATURE_SHUTTER);
                shutter.operator=(video.GetFeatureValue(DC1394_FEATURE_SHUTTER));
            }
            else{
                video.SetFeatureValue(DC1394_FEATURE_SHUTTER, shutter);
            }

        }

        if ( manual ){
            video.SetFeatureValue(DC1394_FEATURE_BRIGHTNESS, brightness);
            video.SetFeatureValue(DC1394_FEATURE_GAIN, gain);
            video.SetFeatureValue(DC1394_FEATURE_GAMMA, gamma);
            video.SetFeatureValue(DC1394_FEATURE_SATURATION, saturation);
            video.SetFeatureValue(DC1394_FEATURE_HUE, hue);
            video.SetFeatureQuant(DC1394_FEATURE_SHARPNESS, sharpness);
            video.SetWhiteBalance(whitebalance_B_U, whitebalance_R_V);
        }

        // resets all features to automatic mode and update trackbars
        else if( !manual && !hdr ) {

            // set auto mode for all features and reset gamma and hue
            video.SetAllFeaturesAuto();
            video.ResetBrightness();
            video.ResetHue();
            video.ResetGamma();

            // update all trackbars upon switching back to automatic mode
            exposure.operator=(video.GetFeatureValue(DC1394_FEATURE_EXPOSURE));
            shutter.operator=(video.GetFeatureValue(DC1394_FEATURE_SHUTTER));
            brightness.operator=(video.GetFeatureValue(DC1394_FEATURE_BRIGHTNESS));
            gain.operator=(video.GetFeatureValue(DC1394_FEATURE_GAIN));
            gamma.operator=(video.GetFeatureValue(DC1394_FEATURE_GAMMA));
            saturation.operator=(video.GetFeatureValue(DC1394_FEATURE_SATURATION));
            hue.operator=(video.GetFeatureValue(DC1394_FEATURE_HUE));
            sharpness.operator=(video.GetFeatureQuant(DC1394_FEATURE_SHARPNESS));
            whitebalance_B_U.operator=(video.GetWhiteBalanceBlueU());
            whitebalance_R_V.operator=(video.GetWhiteBalanceRedV());

        }

        if( pangolin::Pushed(capture) ) { video.SaveSingleFrame(img); }

        if( pangolin::Pushed(capture_hdr) ){

            // see if response function has already been generated
            if (!video.CheckResponseFunction()) {
                cout << "[HDR]: No response function found, generating one" << endl;
                video.GetResponseFunction();
            }

            video.SetFeatureAuto(DC1394_FEATURE_SHUTTER);
            float EV = video.GetFeatureValue(DC1394_FEATURE_EXPOSURE);
            for (int i = -1; i <= 1 ; i++){
                video.SetFeatureValue(DC1394_FEATURE_EXPOSURE, EV+i);
                sleep(1);
                cout << "[HDR]: image " << i+1 << " @ " << EV+i << "EV" << endl;
                hdr_shutter[i+1] = video.GetFeatureQuant(DC1394_FEATURE_SHUTTER);
            }

            cout << "[HDR]: EV range calibrated" << endl;

            // capture HDR images
            video.CaptureHDRFrame(img, 3, hdr_shutter);

        }

        /*-----------------------------------------------------------------------
         *  Save options
         *-----------------------------------------------------------------------*/

        // start/stop recording
        if ( save && pangolin::Pushed(record) ){

            save = false; // set save flag to false

            char time_stamp[32];
            char command[128];
            char *format;

            // set output video format from config or if not loaded, to default (mpeg)
            if (!video.CheckConfigLoaded()){
                format = (char *) video.GetConfigValue("VIDEO_FORMAT").c_str();
            } else {
                format = (char *) "mpeg" ;
            }
            // get time stamp for file name
            video.GetTimeStamp(time_stamp);

            if(hdr){

                cout << "[VIDEO]: Processing HDR video" << endl;

            } else {

                cout << "[VIDEO]: Processing video" << endl;

                // create command string: convert video, remove files and then echo completed - should be thread safe this way
                sprintf(command, "convert -quality 100 ./video/ppm/*.ppm ./video/%s.%s \
                        && rm -rf ./video/ppm/  \
                        && echo '[VIDEO]: Video saved'",
                        time_stamp, format);

            }

            // run video conversion in seperate thread (may take a while so lets us continue)
            boost::thread(system, command);

        }

        if ( !save && pangolin::Pushed(record) ){
            cout << "[VIDEO]: Recording video" << endl;
            save = true;
            time (&start); // get current time
            frame_number = 0;
            recorded_frames.operator=(frame_number);
            recorded_time.operator=(0);
        }

        // save mode
        if ( save && hdr ){
            video.RecordFramesOneShot(frame_number, img, true, hdr);
            recorded_frames.operator=(frame_number);
            time (&end);
            recorded_time.operator=(difftime(end, start));
        }
        else if ( save ){
            video.RecordFrames(frame_number, img, true, false, hdr);
            recorded_frames.operator=(frame_number);
            time (&end);
            recorded_time.operator=(difftime(end, start));
        }
        else if ( hdr ){
            video.GrabOneShot(img);
        }
        else{
            video.GrabOneShot(img);
        }

        // refresh screen
        texVideo.Upload(img, vid_fmt.channels==1 ? GL_LUMINANCE:GL_RGB, GL_UNSIGNED_BYTE);
        // Activate video viewport and render texture
        vVideo.ActivateScissorAndClear();
        texVideo.RenderToViewportFlipY();
        // Swap back buffer with front and process window events via GLUT
        d_panel.Render();
        pangolin::FinishGlutFrame();

    }

    delete[] img;

    return 0;
}
<commit_msg>good working copy: known issues - first frame of HDR image capture is not correct<commit_after>/**
 * @author  Akram Hussein
 * Copyright (C) 2012  Akram Hussein
 *                     Imperial College London
 **/

#include <sys/stat.h>
#include <iostream>
#include <limits>

#include <pangolin/pangolin.h>
#include <pangolin/video.h>
#include <pangolin/video/firewire.h>
#include <pangolin/display_internal.h>

#include <boost/thread.hpp>

using namespace pangolin;
using namespace std;

int main( int argc, char* argv[] )
{
    /*-----------------------------------------------------------------------
     *  SETUP SOURCE
     *-----------------------------------------------------------------------*/

    FirewireVideo video = FirewireVideo();
    video.LoadConfig();
    video.SetHDRRegister(false);
    video.SetMetaDataFlags( META_ALL_AND_ABS );
    video.CreateShutterMaps();
    video.SetAllFeaturesAuto();
    //video.Stop();
    //video.FlushDMABuffer();//remove spurious frames
    //video.PrintCameraReport();

    unsigned char* img = new unsigned char[video.SizeBytes()];

    /*-----------------------------------------------------------------------
     *  GUI
     *-----------------------------------------------------------------------*/

    VideoPixelFormat vid_fmt = VideoFormatFromString(video.PixFormat());
    const unsigned w = video.Width();
    const unsigned h = video.Height();

    // Create Glut window
    const int panel_width = 200;
    double scale = 1.25;
    pangolin::CreateGlutWindowAndBind("SimpleHDR", w*scale + panel_width, h*scale);

    // Create viewport for video with fixed aspect
    View& d_panel = pangolin::CreatePanel("ui.")
    .SetBounds(0.0, 1.0, 0.0, Attach::Pix(panel_width));

    // Create viewport for video with fixed aspect
    View& vVideo = Display("Video")
    .SetBounds(0.0, 1.0, Attach::Pix(panel_width), 1.0)
    .SetAspect((float)w/h);

    // OpenGl Texture for video frame
    GlTexture texVideo(w,h,GL_RGBA8);

    /*-----------------------------------------------------------------------
     *  KEYBOARD SHORCUTS
     *-----------------------------------------------------------------------*/

    pangolin::RegisterKeyPressCallback( 'h', SetVarFunctor<bool>("ui.HDR Mode", true));                 // hdr mode
    pangolin::RegisterKeyPressCallback( 'n', SetVarFunctor<bool>("ui.HDR Mode", false));                // normal mode
    pangolin::RegisterKeyPressCallback( 32 , SetVarFunctor<bool>("ui.Record", true));                   // grab multiple frames in ppm & jpeg
    pangolin::RegisterKeyPressCallback( 'c', SetVarFunctor<bool>("ui.Capture Frame", true));            // grab single frame in ppm & jpeg
    pangolin::RegisterKeyPressCallback( 'm', SetVarFunctor<bool>("ui.Manual Camera Settings", true));   // manual on
    pangolin::RegisterKeyPressCallback( 'a', SetVarFunctor<bool>("ui.Manual Camera Settings", false));  // manual off
    pangolin::RegisterKeyPressCallback( 'e', SetVarFunctor<bool>("ui.Automatic Exposure Control", true));  // AEC on
    pangolin::RegisterKeyPressCallback( 'f', SetVarFunctor<bool>("ui.Get Response Function", true));    // get response function

    /*-----------------------------------------------------------------------
     *  CONTROL PANEL
     *-----------------------------------------------------------------------*/

    // camera brand/model info
    static Var<string> vendor("ui.Vendor", video.GetCameraVendor());
    static Var<string> camera("ui.Camera", video.GetCameraModel());

    // capture options
    static Var<bool> record("ui.Record",false,false);
    static Var<bool> hdr("ui.HDR Mode",false,true);
    static Var<bool> AEC("ui.Automatic Exposure Control",false,true);

    // recording info
    static Var<int> recorded_frames("ui.Recorded Frames", 0);
    static Var<int> recorded_time("ui.Recorded (secs)", 0);

    // single frame
    static Var<bool> capture("ui.Capture Frame",false,false);
    static Var<bool> capture_hdr("ui.Capture HDR Frame",false,false);

    // manual control
    static Var<bool> manual("ui.Manual Camera Settings",false,true);

    // camera settings
    static Var<float> shutter("ui.Shutter (s)", video.GetFeatureValue(DC1394_FEATURE_SHUTTER),
                               video.GetFeatureValueMin(DC1394_FEATURE_SHUTTER),
                               video.GetFeatureValueMax(DC1394_FEATURE_SHUTTER), true);

    static Var<float> exposure("ui.Exposure (EV)", video.GetFeatureValue(DC1394_FEATURE_EXPOSURE),
                                video.GetFeatureValueMin(DC1394_FEATURE_EXPOSURE),
                                video.GetFeatureValueMax(DC1394_FEATURE_EXPOSURE),false);

    static Var<float> brightness("ui.Brightness (%)", video.GetFeatureValue(DC1394_FEATURE_BRIGHTNESS),
                                 video.GetFeatureValueMin(DC1394_FEATURE_BRIGHTNESS),
                                 video.GetFeatureValueMax(DC1394_FEATURE_BRIGHTNESS),false);

    static Var<float> gain("ui.Gain (dB)", video.GetFeatureValue(DC1394_FEATURE_GAIN),
                           video.GetFeatureValueMin(DC1394_FEATURE_GAIN),
                           video.GetFeatureValueMax(DC1394_FEATURE_GAIN),false);

    static Var<float> gamma("ui.Gamma", video.GetFeatureValue(DC1394_FEATURE_GAMMA),
                            video.GetFeatureValueMin(DC1394_FEATURE_GAMMA),
                            video.GetFeatureValueMax(DC1394_FEATURE_GAMMA),false);

    static Var<float> saturation("ui.Saturation (%)", video.GetFeatureValue(DC1394_FEATURE_SATURATION),
                                 video.GetFeatureValueMin(DC1394_FEATURE_SATURATION),
                                 video.GetFeatureValueMax(DC1394_FEATURE_SATURATION),false);

    static Var<float> hue("ui.Hue (deg)", video.GetFeatureValue(DC1394_FEATURE_HUE),
                        video.GetFeatureValueMin(DC1394_FEATURE_HUE),
                        video.GetFeatureValueMax(DC1394_FEATURE_HUE),false);

    static Var<int> sharpness("ui.Sharpness",video.GetFeatureQuant(DC1394_FEATURE_SHARPNESS),
                              video.GetFeatureQuantMin(DC1394_FEATURE_SHARPNESS),
                              video.GetFeatureQuantMax(DC1394_FEATURE_SHARPNESS),false);

    static Var<int> whitebalance_B_U("ui.White Balance (Blue/U)",video.GetWhiteBalanceBlueU(),
                                     video.GetFeatureQuantMin(DC1394_FEATURE_WHITE_BALANCE),
                                     video.GetFeatureQuantMax(DC1394_FEATURE_WHITE_BALANCE),false);

    static Var<int> whitebalance_R_V("ui.White Balance (Red/V)",video.GetWhiteBalanceRedV(),
                                     video.GetFeatureQuantMin(DC1394_FEATURE_WHITE_BALANCE),
                                     video.GetFeatureQuantMax(DC1394_FEATURE_WHITE_BALANCE),false);

    static Var<bool> response_function("ui.Get Response Function",false,false);

    /*-----------------------------------------------------------------------
     *  CAPTURE LOOP
     *-----------------------------------------------------------------------*/

    bool save = false;
    bool under_over = true;
    time_t start, end;
    uint32_t hdr_shutter[3];

    // loop until quit (e.g ESC key)
    for(int frame_number = 0; !pangolin::ShouldQuit(); ++frame_number)
    {
        // Screen refresh
        if(pangolin::HasResized())
            DisplayBase().ActivateScissorAndClear();

        glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

        /*-----------------------------------------------------------------------
         *  CONTROL LOGIC
         *-----------------------------------------------------------------------*/

        if( pangolin::Pushed(response_function) ){ video.GetResponseFunction(); }

        // HDR MODE

        // checks if hdr mode has been switched and sets register on
        if(pangolin::Pushed(hdr.var->meta_gui_changed)){

            if( hdr ){

                video.SetFeatureAuto(DC1394_FEATURE_SHUTTER);
                float EV = video.GetFeatureValue(DC1394_FEATURE_EXPOSURE);

                cout << "[HDR]: HDR mode bracket range set at: " << endl;
                for (int i = -1; i <= 1 ; i++){
                    video.SetFeatureValue(DC1394_FEATURE_EXPOSURE, EV+i);
                    sleep(1);
                    cout << i << ": " << EV+i << " EV" << endl;
                    hdr_shutter[i+1] = video.GetFeatureQuant(DC1394_FEATURE_SHUTTER);
                }

                // set shutter values
                video.SetHDRShutterFlags(hdr_shutter[0], hdr_shutter[0], hdr_shutter[1], hdr_shutter[2]);
                video.SetHDRRegister(true);

            } else {
                video.SetHDRRegister(false);
            }
        }

        // switch under over flag
        under_over = under_over ? false : true ;

        // shutter settings when AEC mode activated
        if(pangolin::Pushed(AEC.var->meta_gui_changed)){

            if (hdr && AEC){
                cout << "[AEC]: AEC enabled" << endl;
                if(under_over){
                    hdr_shutter[0] = video.AEC(img, video.GetShutterMapAbs(hdr_shutter[0]), under_over);
                } else {
                    hdr_shutter[2] = video.AEC(img, video.GetShutterMapAbs(hdr_shutter[2]), under_over);
                }
                video.SetHDRShutterFlags(hdr_shutter[0], hdr_shutter[0], hdr_shutter[1], hdr_shutter[2]);
            } else {
                cout << "[AEC]: AEC disabled" << endl;
            }

        }
        // MANUAL SETTINGS

        if ( manual && !hdr ) {

            /*
             * exposure & shutter inter-linked, therefore need special logic control
             *  if: exposure val is changed, shutter val is changed (camera works out new value)
             *  else: shutter val is set to gui value
             */

            if(pangolin::Pushed(exposure.var->meta_gui_changed)){
                video.SetFeatureValue(DC1394_FEATURE_EXPOSURE, exposure);
                video.SetFeatureAuto(DC1394_FEATURE_SHUTTER);
                shutter.operator=(video.GetFeatureValue(DC1394_FEATURE_SHUTTER));
            }
            else{
                video.SetFeatureValue(DC1394_FEATURE_SHUTTER, shutter);
            }

        }

        if ( manual ){
            video.SetFeatureValue(DC1394_FEATURE_BRIGHTNESS, brightness);
            video.SetFeatureValue(DC1394_FEATURE_GAIN, gain);
            video.SetFeatureValue(DC1394_FEATURE_GAMMA, gamma);
            video.SetFeatureValue(DC1394_FEATURE_SATURATION, saturation);
            video.SetFeatureValue(DC1394_FEATURE_HUE, hue);
            video.SetFeatureQuant(DC1394_FEATURE_SHARPNESS, sharpness);
            video.SetWhiteBalance(whitebalance_B_U, whitebalance_R_V);
        }

        // resets all features to automatic mode and update trackbars
        else if( !manual && !hdr ) {

            // set auto mode for all features and reset gamma and hue
            video.SetAllFeaturesAuto();
            video.ResetBrightness();
            video.ResetHue();
            video.ResetGamma();

            // update all trackbars upon switching back to automatic mode
            exposure.operator=(video.GetFeatureValue(DC1394_FEATURE_EXPOSURE));
            shutter.operator=(video.GetFeatureValue(DC1394_FEATURE_SHUTTER));
            brightness.operator=(video.GetFeatureValue(DC1394_FEATURE_BRIGHTNESS));
            gain.operator=(video.GetFeatureValue(DC1394_FEATURE_GAIN));
            gamma.operator=(video.GetFeatureValue(DC1394_FEATURE_GAMMA));
            saturation.operator=(video.GetFeatureValue(DC1394_FEATURE_SATURATION));
            hue.operator=(video.GetFeatureValue(DC1394_FEATURE_HUE));
            sharpness.operator=(video.GetFeatureQuant(DC1394_FEATURE_SHARPNESS));
            whitebalance_B_U.operator=(video.GetWhiteBalanceBlueU());
            whitebalance_R_V.operator=(video.GetWhiteBalanceRedV());

        }

        if( pangolin::Pushed(capture) ) { video.SaveSingleFrame(img); }

        if( pangolin::Pushed(capture_hdr) ){

            // see if response function has already been generated
            if (!video.CheckResponseFunction()) {
                cout << "[HDR]: No response function found, generating one" << endl;
                video.GetResponseFunction();
            }

            video.SetFeatureAuto(DC1394_FEATURE_SHUTTER);
            float EV = video.GetFeatureValue(DC1394_FEATURE_EXPOSURE);
            for (int i = -1; i <= 1 ; i++){
                video.SetFeatureValue(DC1394_FEATURE_EXPOSURE, EV+i);
                sleep(1);
                cout << "[HDR]: image " << i+1 << " @ " << EV+i << "EV" << endl;
                hdr_shutter[i+1] = video.GetFeatureQuant(DC1394_FEATURE_SHUTTER);
            }

            cout << "[HDR]: EV range calibrated" << endl;

            // capture HDR images
            video.CaptureHDRFrame(img, 3, hdr_shutter);

        }

        /*-----------------------------------------------------------------------
         *  Save options
         *-----------------------------------------------------------------------*/

        // start/stop recording
        if ( save && pangolin::Pushed(record) ){

            save = false; // set save flag to false

            char time_stamp[32];
            char command[128];
            char *format;

            // set output video format from config or if not loaded, to default (mpeg)
            if (!video.CheckConfigLoaded()){
                format = (char *) video.GetConfigValue("VIDEO_FORMAT").c_str();
            } else {
                format = (char *) "mpeg" ;
            }
            // get time stamp for file name
            video.GetTimeStamp(time_stamp);

            if(hdr){

                cout << "[VIDEO]: Processing HDR video" << endl;

            } else {

                cout << "[VIDEO]: Processing video" << endl;

                // create command string: convert video, remove files and then echo completed - should be thread safe this way
                sprintf(command, "convert -quality 100 ./video/ppm/*.ppm ./video/%s.%s \
                        && rm -rf ./video/ppm/  \
                        && echo '[VIDEO]: Video saved'",
                        time_stamp, format);

            }

            // run video conversion in seperate thread (may take a while so lets us continue)
            boost::thread(system, command);

        }

        if ( !save && pangolin::Pushed(record) ){
            cout << "[VIDEO]: Recording video" << endl;
            save = true;
            time (&start); // get current time
            frame_number = 0;
            recorded_frames.operator=(frame_number);
            recorded_time.operator=(0);
        }

        // save mode
        if ( save && hdr ){
            video.RecordFramesOneShot(frame_number, img, true, hdr);
            recorded_frames.operator=(frame_number);
            time (&end);
            recorded_time.operator=(difftime(end, start));
        }
        else if ( save ){
            video.RecordFrames(frame_number, img, true, false, hdr);
            recorded_frames.operator=(frame_number);
            time (&end);
            recorded_time.operator=(difftime(end, start));
        }
        else if ( hdr ){
            video.GrabOneShot(img);
        }
        else{
            video.GrabOneShot(img);
        }

        // refresh screen
        texVideo.Upload(img, vid_fmt.channels==1 ? GL_LUMINANCE:GL_RGB, GL_UNSIGNED_BYTE);
        // Activate video viewport and render texture
        vVideo.ActivateScissorAndClear();
        texVideo.RenderToViewportFlipY();
        // Swap back buffer with front and process window events via GLUT
        d_panel.Render();
        pangolin::FinishGlutFrame();

    }

    delete[] img;

    return 0;
}
<|endoftext|>


<commit_before>/*******************************************************************************
 *    ALMA - Atacama Large Millimiter Array
 *    (c) European Southern Observatory, 2002
 *    Copyright by ESO (in the framework of the ALMA collaboration)
 *    and Cosylab 2002, All rights reserved
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License as published by the Free Software Foundation; either
 *    version 2.1 of the License, or (at your option) any later version.
 *
 *    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
 *
 *
 * "@(#) "
 *
 * who       when        what
 * --------  ----------  ----------------------------------------------
 * javarias  May 7, 2010  	 created
 */
#define ENABLE_LOG4CPP_MACROS
#include "loggingACEMACROS.h"

static void testStaticLoggingWithAudience()
{
    STATIC_LOG(Logging::BaseLog::LM_INFO, __PRETTY_FUNCTION__,
            "Testing Static Log");
    STATIC_LOG_TO_DEVELOPER(LM_INFO, "STATIC_LOG_TO_DEVELOPER");
    STATIC_LOG_TO_OPERATOR(LM_INFO, "STATIC_LOG_TO_OPERATOR");
    STATIC_LOG_TO_SCIENCE(LM_INFO, "STATIC_LOG_TO_SCIENCE");
    STATIC_LOG_TO_SCILOG(LM_INFO, "STATIC_LOG_TO_SCILOG");
}

void testAutoTraceFunc()
{
	 AUTO_TRACE("testAutoTraceFunc");
}

int main (int argc, char * argv[])
{
	char *tooLong_p = "1234567890123456789012345678901234567890123456789012345678901234567890123456789012345678901234567890";

	AUTO_TRACE("someFunc");

	testAutoTraceFunc();

    ACS_SHORT_LOG((LM_INFO, "%s a %s b %s c %s d %s e %s f %s g %s h %s i %s j %s k Should never see this...\n",
    		tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p));

    ACS_LOG(LM_RUNTIME_CONTEXT, "main",
	    (LM_INFO, "Test of formatted log 1 - %s",
	     "This is a string parameter"));

    ACS_LOG( LM_SOURCE_INFO, "main",
	    (LM_INFO, "Test of formatted log 2 - %s",
	     "This is a string parameter"));

    ACS_LOG( LM_FULL_INFO, "main",
	    (LM_INFO, "Test of formatted log 3 - %s",
	     "This is a string parameter"));

	//Test Levels
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_TRACE, "Test of LM_TRACE log"));

	ACS_LOG( LM_FULL_INFO, "main",
			(LM_ERROR, "Test of LM_ERROR log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_DELOUSE, "Test of LM_DELOUSE log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_DEBUG, "Test of LM_DEBUG log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_INFO, "Test of LM_INFO log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_NOTICE, "Test of LM_NOTICE log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_WARNING, "Test of LM_WARNING log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_CRITICAL, "Test of LM_CRITICAL log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_ALERT, "Test of LM_ALERT log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_EMERGENCY, "Test of LM_EMERGENCY log"));

	    //Test audience macros
//	    LOG_TO_AUDIENCE_WITH_LOGGER(log4cpp::Priority::INFO,
//	            "Test of LOG_TO_AUDIENCE_WITH_LOGGER log",
//	            log_audience::OPERATOR, myLoggerSmartPtr);
//	    LOG_TO_DEVELOPER(log4cpp::Priority::INFO, "Test of LOG_TO_DEVELOPER log");
//	    LOG_TO_DEVELOPER_WITH_LOGGER(log4cpp::Priority::INFO,
//	            "Test of LOG_TO_DEVELOPER_WITH_LOGGER",
//	            myLoggerSmartPtr);
//	    LOG_TO_OPERATOR(log4cpp::Priority::INFO, "Test of LOG_TO_OPERATOR log");
//	    LOG_TO_OPERATOR_WITH_LOGGER(log4cpp::Priority::INFO, "Test of LOG_TO_OPERATOR_WITH_LOGGER",
//	            myLoggerSmartPtr);
	    LOG_TO_SCIENCE(LM_INFO, "Test of LOG_TO_SCIENCE log");
	    LOG_TO_SCILOG(LM_INFO, "Test of LOG_TO_SCILOG log");

	    testStaticLoggingWithAudience();
	return 0;
}
<commit_msg>Updated Log4cpp test<commit_after>/*******************************************************************************
 *    ALMA - Atacama Large Millimiter Array
 *    (c) European Southern Observatory, 2002
 *    Copyright by ESO (in the framework of the ALMA collaboration)
 *    and Cosylab 2002, All rights reserved
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License as published by the Free Software Foundation; either
 *    version 2.1 of the License, or (at your option) any later version.
 *
 *    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
 *
 *
 * "@(#) "
 *
 * who       when        what
 * --------  ----------  ----------------------------------------------
 * javarias  May 7, 2010  	 created
 */
#define ENABLE_LOG4CPP_MACROS
#include "loggingACEMACROS.h"

static void testStaticLoggingWithAudience()
{
    STATIC_LOG(Logging::BaseLog::LM_INFO, __PRETTY_FUNCTION__,
            "Testing Static Log");
    STATIC_LOG_TO_DEVELOPER(LM_INFO, "STATIC_LOG_TO_DEVELOPER");
    STATIC_LOG_TO_OPERATOR(LM_INFO, "STATIC_LOG_TO_OPERATOR");
    STATIC_LOG_TO_SCIENCE(LM_INFO, "STATIC_LOG_TO_SCIENCE");
    STATIC_LOG_TO_SCILOG(LM_INFO, "STATIC_LOG_TO_SCILOG");
}

void testAutoTraceFunc()
{
	 AUTO_TRACE("testAutoTraceFunc");
}

int main (int argc, char * argv[])
{
//#ifdef ENABLE_LOG4CPP_MACROS
//	LOGGER_FACTORY->enableSyslogAppender();
//#endif
	char *tooLong_p = "1234567890123456789012345678901234567890123456789012345678901234567890123456789012345678901234567890";

	AUTO_TRACE("someFunc");

	testAutoTraceFunc();

    ACS_SHORT_LOG((LM_INFO, "%s a %s b %s c %s d %s e %s f %s g %s h %s i %s j %s k Should never see this...\n",
    		tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p,
		   tooLong_p));

    ACS_LOG(LM_RUNTIME_CONTEXT, "main",
	    (LM_INFO, "Test of formatted log 1 - %s",
	     "This is a string parameter"));

    ACS_LOG( LM_SOURCE_INFO, "main",
	    (LM_INFO, "Test of formatted log 2 - %s",
	     "This is a string parameter"));

    ACS_LOG( LM_FULL_INFO, "main",
	    (LM_INFO, "Test of formatted log 3 - %s",
	     "This is a string parameter"));

	//Test Levels
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_TRACE, "Test of LM_TRACE log"));

	ACS_LOG( LM_FULL_INFO, "main",
			(LM_ERROR, "Test of LM_ERROR log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_DELOUSE, "Test of LM_DELOUSE log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_DEBUG, "Test of LM_DEBUG log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_INFO, "Test of LM_INFO log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_NOTICE, "Test of LM_NOTICE log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_WARNING, "Test of LM_WARNING log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_CRITICAL, "Test of LM_CRITICAL log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_ALERT, "Test of LM_ALERT log"));
	ACS_LOG( LM_FULL_INFO, "main",
			(LM_EMERGENCY, "Test of LM_EMERGENCY log"));

	    //Test audience macros
//	    LOG_TO_AUDIENCE_WITH_LOGGER(log4cpp::Priority::INFO,
//	            "Test of LOG_TO_AUDIENCE_WITH_LOGGER log",
//	            log_audience::OPERATOR, myLoggerSmartPtr);
//	    LOG_TO_DEVELOPER(log4cpp::Priority::INFO, "Test of LOG_TO_DEVELOPER log");
//	    LOG_TO_DEVELOPER_WITH_LOGGER(log4cpp::Priority::INFO,
//	            "Test of LOG_TO_DEVELOPER_WITH_LOGGER",
//	            myLoggerSmartPtr);
//	    LOG_TO_OPERATOR(log4cpp::Priority::INFO, "Test of LOG_TO_OPERATOR log");
//	    LOG_TO_OPERATOR_WITH_LOGGER(log4cpp::Priority::INFO, "Test of LOG_TO_OPERATOR_WITH_LOGGER",
//	            myLoggerSmartPtr);
	    LOG_TO_SCIENCE(LM_INFO, "Test of LOG_TO_SCIENCE log");
	    LOG_TO_SCILOG(LM_INFO, "Test of LOG_TO_SCILOG log");

	    testStaticLoggingWithAudience();
	return 0;
}
<|endoftext|>


<commit_before>#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
#include <iostream>
#include <string.h>
using namespace std;

int select_operation(int a, int b, char op) {//Функция выбора оберации
	switch (op) {
	case '+':
		return a + b;
	case '-':
		return a - b;
	case '*':
		return a * b;
	case '/':
		if (b == 0)
			throw - 1;
		return a / b;
	default:
		return 0;
	}
}

int expr(char*& str);
int _expr(char*& str, int left_operand);
int term(char*& str);
int _term(char*& str, int left_operand);
int prim(char*& str);
int number(char*& str);

void deleteSpace(char* src)
{
	char* dst = src;
	while ((*dst = *src++))
		if (*dst != ' ')
			dst++;
}

int main(int argc, char* argv[]) {
 	if (argc != 2) { //Количество аргументов командной строки не равно 2
		cout << "INVALID ARGUMENTS" << endl;
		return 0;
	}

	deleteSpace(argv[1]);
	for (int i = 0; argv[1][i] != '\0'; ++i) {
		if ((argv[1][i] == '+' || argv[1][i] == '-' || argv[1][i] == '*' || argv[1][i] == '/')
			&& (argv[1][i] != '-' && !(isdigit(argv[1][i + 1])))) {
			cout << "INVALID EXPRESSION!" << endl;
			return 0;
		}
		if (!isdigit(argv[1][i]))
			if (argv[1][i] != '+' && argv[1][i] != '-' && argv[1][i] != '*' && argv[1][i] != '/') {
				cout << "INVALID EXPRESSION!" << endl;
				return 0;
		}
	}
	cout << expr(argv[1]) << endl;
	return 0;
}

/*Далее под операндом будем понимать произведение | частное |число
* под выражением будем понимать сумму | разность операндов
*/
int expr(char*& str) {
	int left_operand = term(str); //Левый операнд выражения
	if (*str == '+' || *str == '-') { //Если он не единственный
		return _expr(str, left_operand);
	}
	return left_operand; //Если он единственный
}

int _expr(char*& str, int left_operand) {
	char operation = *str; //Операция между правым и левым операндом
	++str;
	int right_operand = term(str); //Получаем правый операнд
	if (*str == '\0') {	//Выражение закончилось, возвращаем ответ
		return select_operation(left_operand, right_operand, operation);;
	}
	else { //В выражении есть другие операнды
		return  _expr(str, select_operation(left_operand, right_operand, operation));
	}
}

int term(char*& str) {
	int left_part = prim(str); //Вычисление левой части операнда
	if (*str == '*' || *str == '/') {//Если операнд - произведение | частное
		return _term(str, left_part);
	}
	return left_part; //Если операнд - число
}

int _term(char*& str, int left_part) {//Вычисление операнда - левую часть уже вычислили
	char operation = *str;//Операция между частями операнда
	if (operation == '+' || operation == '-') //Если + | - вычисление операнда закончилось
		throw left_part; //Выкидываем результат
	++str;
	int right_part = prim(str); //Вычисляем правую часть
	if (*str == '\0') { //Это был последний операнд
		try {
			//вычисляем значение операнда
			return select_operation(left_part, right_part, operation);

		}
		catch (int a) {
			if (a == -1) { //Спецификация - деление на нуль
				//в ответе получаем мусор
				cout << "DIVISION BY ZERO!" << endl;
			}
		}
	}
	else { //это был не последний операнд
		int right_term; //Вычисляем дальше операнды справа
		try {
			right_term = _term(str, select_operation(left_part, right_part, operation));
		}
		catch (int a) {
			if (a == -1) { //Опять деление на нуль
				cout << "DIVISION BY ZERO!" << endl;
			}
			return a;	// Ловим то, что выкинули, если встретили + | -
		}
		return right_term; //возвращаем значение
	}
	return 0;
}

int prim(char*& str) { //Получаем число
	if (*str == '-') { //унарный минус
		++str;
		return -number(str);
	}
	return number(str);
}

int number(char*& str) {
	char* str_num = (char*)malloc(sizeof(char));
	int index = 0;
	while (*str >= '0' && *str <= '9') { //Читаем символы до первого сивола операции
		str_num[index] = *str;	//записываем их в строку
		++index;
		str_num = (char*)realloc(str_num, (index + 1)*sizeof(char));
		str++;
	}
	str_num[index] = '\0';
	int num = atoi(str_num); //Преобразуем char* в int
	free(str_num);
	return num; //возвращаем цифру
}
<commit_msg>Update calc.cpp<commit_after>#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
#include <iostream>
#include <string.h>
using namespace std;

int select_operation(int a, int b, char op) {//Функция выбора оберации
	switch (op) {
	case '+':
		return a + b;
	case '-':
		return a - b;
	case '*':
		return a * b;
	case '/':
		if (b == 0)
			throw - 1;
		return a / b;
	default:
		return 0;
	}
}

int expr(char*& str);
int _expr(char*& str, int left_operand);
int term(char*& str);
int _term(char*& str, int left_operand);
int prim(char*& str);
int number(char*& str);

void deleteSpace(char* src)
{
	char* dst = src;
	while ((*dst = *src++))
		if (*dst != ' ')
			dst++;
}

int main(int argc, char* argv[]) {
 	if (argc != 2) { //Количество аргументов командной строки не равно 2
		cout << "INVALID ARGUMENTS" << endl;
		return 1;
	}

	deleteSpace(argv[1]);
	for (int i = 0; argv[1][i] != '\0'; ++i) {
		if ((argv[1][i] == '+' || argv[1][i] == '-' || argv[1][i] == '*' || argv[1][i] == '/'))
			if (argv[1][i+1] != '-' && !(isdigit(argv[1][i + 1]))) {
			cout << "INVALID EXPRESSION!" << endl;
			return 1;
		}
		if (!isdigit(argv[1][i]))
			if (argv[1][i] != '+' && argv[1][i] != '-' && argv[1][i] != '*' && argv[1][i] != '/') {
				cout << "INVALID EXPRESSION!" << endl;
				return 1;
		}
	}
	try {
		cout << expr(argv[1]) << endl;
	}
	catch (const char* msg) {
		cout << msg << endl;
		return 1;
	}
	return 0;
}

/*Далее под операндом будем понимать произведение | частное |число
* под выражением будем понимать сумму | разность операндов
*/
int expr(char*& str) {
	int left_operand = term(str); //Левый операнд выражения
	if (*str == '+' || *str == '-') { //Если он не единственный
		return _expr(str, left_operand);
	}
	return left_operand; //Если он единственный
}

int _expr(char*& str, int left_operand) {
	char operation = *str; //Операция между правым и левым операндом
	++str;
	int right_operand = term(str); //Получаем правый операнд
	if (*str == '\0') {	//Выражение закончилось, возвращаем ответ
		return select_operation(left_operand, right_operand, operation);;
	}
	else { //В выражении есть другие операнды
		return  _expr(str, select_operation(left_operand, right_operand, operation));
	}
}

int term(char*& str) {
	int left_part = prim(str); //Вычисление левой части операнда
	if (*str == '*' || *str == '/') {//Если операнд - произведение | частное
		return _term(str, left_part);
	}
	return left_part; //Если операнд - число
}

int _term(char*& str, int left_part) {//Вычисление операнда - левую часть уже вычислили
	char operation = *str;//Операция между частями операнда
	if (operation == '+' || operation == '-') //Если + | - вычисление операнда закончилось
		throw left_part; //Выкидываем результат
	++str;
	int right_part = prim(str); //Вычисляем правую часть
	if (*str == '\0') { //Это был последний операнд
		try {
			//вычисляем значение операнда
			return select_operation(left_part, right_part, operation);

		}
		catch (int a) {
			if (a == -1) { //Спецификация - деление на нуль
				//в ответе получаем мусор
				throw "DIVISION BY ZERO!";
			}
		}
	}
	else { //это был не последний операнд
		int right_term; //Вычисляем дальше операнды справа
		try {
			right_term = _term(str, select_operation(left_part, right_part, operation));
		}
		catch (int a) {
			if (a == -1) { //Опять деление на нуль
				throw "DIVISION BY ZERO!";
			}
			return a;	// Ловим то, что выкинули, если встретили + | -
		}
		return right_term; //возвращаем значение
	}
	return 0;
}

int prim(char*& str) { //Получаем число
	if (*str == '-') { //унарный минус
		++str;
		return -number(str);
	}
	return number(str);
}

int number(char*& str) {
	char* str_num = (char*)malloc(sizeof(char));
	int index = 0;
	while (*str >= '0' && *str <= '9') { //Читаем символы до первого сивола операции
		str_num[index] = *str;	//записываем их в строку
		++index;
		str_num = (char*)realloc(str_num, (index + 1)*sizeof(char));
		str++;
	}
	str_num[index] = '\0';
	int num = atoi(str_num); //Преобразуем char* в int
	free(str_num);
	return num; //возвращаем цифру
}
<|endoftext|>


<commit_before>#include <ros/ros.h>

//ROS libraries
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

//ROS messages
#include <std_msgs/UInt8.h>
#include <sensor_msgs/Range.h>

using namespace std;

//Globals
double collisionDistance = 0.4; //meters the ultrasonic detectors will flag obstacles
string publishedName;
char host[128];

//Publishers
ros::Publisher obstaclePublish;

//Callback handlers
void sonarHandler(const sensor_msgs::Range::ConstPtr& sonarLeft, const sensor_msgs::Range::ConstPtr& sonarCenter, const sensor_msgs::Range::ConstPtr& sonarRight);

int main(int argc, char** argv) {
    gethostname(host, sizeof (host));
    string hostname(host);

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "! Obstacle module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No name selected. Default is: " << publishedName << endl;
    }

    ros::init(argc, argv, (publishedName + "_OBSTACLE"));
    ros::NodeHandle oNH;

    obstaclePublish = oNH.advertise<std_msgs::UInt8>((publishedName + "/obstacle"), 10);

    message_filters::Subscriber<sensor_msgs::Range> sonarLeftSubscriber(oNH, (publishedName + "/sonarLeft"), 10);
    message_filters::Subscriber<sensor_msgs::Range> sonarCenterSubscriber(oNH, (publishedName + "/sonarCenter"), 10);
    message_filters::Subscriber<sensor_msgs::Range> sonarRightSubscriber(oNH, (publishedName + "/sonarRight"), 10);
    message_filters::TimeSynchronizer<sensor_msgs::Range, sensor_msgs::Range, sensor_msgs::Range> sonarSync(sonarLeftSubscriber, sonarCenterSubscriber, sonarRightSubscriber, 10);
	sonarSync.registerCallback(boost::bind(&sonarHandler, _1, _2, _3));

    ros::spin();

    return EXIT_SUCCESS;
}

void sonarHandler(const sensor_msgs::Range::ConstPtr& sonarLeft, const sensor_msgs::Range::ConstPtr& sonarCenter, const sensor_msgs::Range::ConstPtr& sonarRight) {
	std_msgs::UInt8 obstacleMode;

	if ((sonarLeft->range > collisionDistance) && (sonarCenter->range > collisionDistance) && (sonarRight->range > collisionDistance)) {
		obstacleMode.data = 0; //no collision
	}
	else if ((sonarLeft->range > collisionDistance) && (sonarRight->range < collisionDistance)) {
		obstacleMode.data = 1; //collision on right side
	}
	else {
		obstacleMode.data = 2; //collision in front or on left side
	}

        obstaclePublish.publish(obstacleMode);
}

<commit_msg>Bug fix: change to approximate time synchronizer in obstacle node<commit_after>#include <ros/ros.h>

//ROS libraries
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

//ROS messages
#include <std_msgs/UInt8.h>
#include <sensor_msgs/Range.h>

using namespace std;

//Globals
double collisionDistance = 0.4; //meters the ultrasonic detectors will flag obstacles
string publishedName;
char host[128];

//Publishers
ros::Publisher obstaclePublish;

//Callback handlers
void sonarHandler(const sensor_msgs::Range::ConstPtr& sonarLeft, const sensor_msgs::Range::ConstPtr& sonarCenter, const sensor_msgs::Range::ConstPtr& sonarRight);

int main(int argc, char** argv) {
    gethostname(host, sizeof (host));
    string hostname(host);

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "! Obstacle module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No name selected. Default is: " << publishedName << endl;
    }

    ros::init(argc, argv, (publishedName + "_OBSTACLE"));
    ros::NodeHandle oNH;

    obstaclePublish = oNH.advertise<std_msgs::UInt8>((publishedName + "/obstacle"), 10);

    message_filters::Subscriber<sensor_msgs::Range> sonarLeftSubscriber(oNH, (publishedName + "/sonarLeft"), 10);
    message_filters::Subscriber<sensor_msgs::Range> sonarCenterSubscriber(oNH, (publishedName + "/sonarCenter"), 10);
    message_filters::Subscriber<sensor_msgs::Range> sonarRightSubscriber(oNH, (publishedName + "/sonarRight"), 10);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Range, sensor_msgs::Range, sensor_msgs::Range> sonarSyncPolicy;

    message_filters::Synchronizer<sonarSyncPolicy> sonarSync(sonarSyncPolicy(10), sonarLeftSubscriber, sonarCenterSubscriber, sonarRightSubscriber);
    sonarSync.registerCallback(boost::bind(&sonarHandler, _1, _2, _3));

    ros::spin();

    return EXIT_SUCCESS;
}

void sonarHandler(const sensor_msgs::Range::ConstPtr& sonarLeft, const sensor_msgs::Range::ConstPtr& sonarCenter, const sensor_msgs::Range::ConstPtr& sonarRight) {
	std_msgs::UInt8 obstacleMode;

	if ((sonarLeft->range > collisionDistance) && (sonarCenter->range > collisionDistance) && (sonarRight->range > collisionDistance)) {
		obstacleMode.data = 0; //no collision
	}
	else if ((sonarLeft->range > collisionDistance) && (sonarRight->range < collisionDistance)) {
		obstacleMode.data = 1; //collision on right side
	}
	else {
		obstacleMode.data = 2; //collision in front or on left side
	}

        obstaclePublish.publish(obstacleMode);
}

<|endoftext|>


<commit_before>/*
Copyright (c) 2014, Dilyan Rusev
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

* Neither the name of the {organization} nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "stdafx.h"

#ifdef WIN32

#include "Game.h"
#include"Board.h"
#include <cassert>
#include <algorithm>

using namespace std;
using Microsoft::WRL::ComPtr;

struct Game::Impl {
	Impl(Game* parent);
	~Impl();

	void CreateAppWindow();
	void InitGraphicsSystems();

	void RunMainLoop();
	void Initialize();

	static LRESULT CALLBACK GameProc(HWND window, UINT message, WPARAM wparam, LPARAM lparam);
	LRESULT OnDestroy();
	LRESULT OnClose();
	LRESULT OnPaint();
	LRESULT OnKeyDown(int vk);
	LRESULT OnKeyUp(int vk);

	void Update(float ms);
	void Render();
	void CreateDeviceResources();
	void CleanDeviceResources();

	ComPtr<ID2D1Bitmap> LoadImage(const wchar_t* fileName) const;

	const wchar_t* APP_TITLE;
	const wchar_t* APP_CLASS;
	const int WINDOW_WIDTH;
	const int WINDOW_HEIGHT;
	const int BLOCK_WIDTH;
	const int BLOCK_HEIGHT;

	Game* m_parent;
	RECT m_clientRect;
	bool m_isClassRegistered;
	HWND m_window;
	HINSTANCE m_instance;
	LARGE_INTEGER m_perfFrequency;
	Board m_board;
	D2D1_POINT_2F m_boardPos;
	ComPtr<ID2D1Factory> m_drawFactory;
	ComPtr<IDWriteFactory> m_writeFactory;
	ComPtr<IWICImagingFactory> m_imageFactory;
	ComPtr<ID2D1HwndRenderTarget> m_renderTarget;
	ComPtr<ID2D1Brush> m_testBrush;
	ComPtr<ID2D1Bitmap> m_terimonoBitmaps[Count_Tetrimonos];
};

Game::Impl::Impl(Game* parent)
		: APP_TITLE(L"Falling Blocks")
		, APP_CLASS(L"FallingBlocks")
		, WINDOW_WIDTH(1280)
		, WINDOW_HEIGHT(720)
		, BLOCK_WIDTH(35)
		, BLOCK_HEIGHT(35)
		, m_parent(parent)
		, m_isClassRegistered(false)
		, m_window(nullptr)
		, m_instance(nullptr) {
}

Game::Impl::~Impl() {
	if (m_isClassRegistered && m_instance) {
		::UnregisterClass(APP_CLASS, m_instance);
	}
}

Game::Game()
		: m_impl(new Impl(this)) {
}

Game::~Game() {
}

void Game::Initialize() {
	m_impl->Initialize();
}

void Game::Impl::Initialize() {
	m_instance = static_cast<HINSTANCE>(::GetModuleHandle(nullptr));

	assert(::QueryPerformanceFrequency(&m_perfFrequency));

	CreateAppWindow();
	InitGraphicsSystems();
	CreateDeviceResources();
}

void Game::Impl::InitGraphicsSystems() {
	HRESULT hr = ::CoCreateInstance(CLSID_WICImagingFactory1, nullptr,
		CLSCTX_INPROC_SERVER, __uuidof(m_imageFactory),
		reinterpret_cast<void**>(m_imageFactory.GetAddressOf()));
	assert(SUCCEEDED(hr));

	hr = ::DWriteCreateFactory(DWRITE_FACTORY_TYPE_SHARED, __uuidof(m_writeFactory),
		reinterpret_cast<IUnknown**>(m_writeFactory.GetAddressOf()));
	assert(SUCCEEDED(hr));

	hr = ::D2D1CreateFactory(D2D1_FACTORY_TYPE_SINGLE_THREADED, IID_PPV_ARGS(&m_drawFactory));
	assert(SUCCEEDED(hr));
}

void Game::Impl::CreateAppWindow() {
	assert(m_instance != nullptr);

	WNDCLASSEX wc;
	wc.cbSize = sizeof(wc);
	wc.cbClsExtra = 0;
	wc.cbWndExtra = 0;
	wc.hbrBackground = nullptr;
	wc.hCursor = ::LoadCursor(nullptr, IDC_ARROW);
	wc.hIcon = ::LoadIcon(nullptr, IDI_APPLICATION);
	wc.hIconSm = ::LoadIcon(nullptr, IDI_APPLICATION);
	wc.hInstance = m_instance;
	wc.lpfnWndProc = &GameProc;
	wc.lpszClassName = APP_CLASS;
	wc.lpszMenuName = nullptr;
	wc.style = 0;

	assert(::RegisterClassEx(&wc));
	m_isClassRegistered = true;

	DWORD windowStyle = WS_CAPTION | WS_MINIMIZEBOX | WS_SYSMENU;
	DWORD exStyle = 0;
	RECT windowSize = { 0, 0, WINDOW_WIDTH, WINDOW_HEIGHT };
	assert(::AdjustWindowRectEx(&windowSize, windowStyle, FALSE, exStyle));

	int width = windowSize.right - windowSize.left;
	int height = windowSize.bottom - windowSize.top;
	int x = (::GetSystemMetrics(SM_CXSCREEN) - width) / 2;
	int y = (::GetSystemMetrics(SM_CYSCREEN) - height) / 2;

	m_window = ::CreateWindowExW(exStyle, APP_CLASS, APP_TITLE,
		windowStyle, x, y, width, height, HWND_DESKTOP, nullptr, m_instance,
		reinterpret_cast<void*>(m_parent));
	assert(m_window != nullptr);

	m_clientRect.left = 0;
	m_clientRect.right = WINDOW_WIDTH;
	m_clientRect.top = 0;
	m_clientRect.bottom = WINDOW_HEIGHT;

	m_boardPos.x = (WINDOW_WIDTH - m_board.WIDTH * BLOCK_WIDTH) / 2.0f;
	m_boardPos.y = (WINDOW_HEIGHT - m_board.HEIGHT * BLOCK_HEIGHT) / 2.0f;
}

void Game::RunMainLoop() {
	m_impl->RunMainLoop();
}

void Game::Impl::RunMainLoop() {
	assert(m_instance);
	assert(m_window);

	::ShowWindow(m_window, SW_SHOW);
	::UpdateWindow(m_window);

	MSG msg;
	msg.message = WM_NULL;

	LARGE_INTEGER before, now;
	float elapsedInMs;

	::QueryPerformanceCounter(&before);
	while (msg.message != WM_QUIT) {
		while (::PeekMessage(&msg, nullptr, 0, 0, PM_REMOVE)) {
			::TranslateMessage(&msg);
			::DispatchMessage(&msg);
		}

		::QueryPerformanceCounter(&now);
		elapsedInMs = static_cast<float>((now.QuadPart - before.QuadPart) * 1000 / m_perfFrequency.QuadPart);

		Update(elapsedInMs);
		Render();
		before = now;
	}
}

void Game::Impl::Update(float ms) {
	m_board.Update(ms);
}

void Game::Impl::Render() {
	m_renderTarget->BeginDraw();
	m_renderTarget->Clear(D2D1::ColorF(D2D1::ColorF::AliceBlue));

	D2D1_RECT_F dest;
	for (int y = 0; y < m_board.HEIGHT; y++) {
		for (int x = 0; x < m_board.WIDTH; x++) {
			auto t = m_board.GetAt(x, y);
			auto& bitmap = m_terimonoBitmaps[t];
			dest.left = m_boardPos.x + static_cast<float>(x * BLOCK_WIDTH);
			dest.top = m_boardPos.y + static_cast<float>(y * BLOCK_HEIGHT);
			dest.right = dest.left + BLOCK_WIDTH;
			dest.bottom = dest.top + BLOCK_HEIGHT;

			m_renderTarget->DrawBitmap(bitmap.Get(), &dest, 1.0f, D2D1_BITMAP_INTERPOLATION_MODE_LINEAR);
		}
	}

	D2D1_POINT_2F nextOffset = D2D1::Point2F(m_boardPos.x + m_board.WIDTH * BLOCK_WIDTH + 30, m_boardPos.y);
	for (int y = 0; y < m_board.MAX_TETRIMONO_HEIGHT; y++) {
		for (int x = 0; x < m_board.MAX_TETRIMONO_WIDTH; x++) {
			auto t = m_board.GetNextAt(x, y);
			auto& bitmap = m_terimonoBitmaps[t];
			dest.left = nextOffset.x + static_cast<float>(x * BLOCK_WIDTH);
			dest.top = nextOffset.y + static_cast<float>(y * BLOCK_HEIGHT);
			dest.right = dest.left + BLOCK_WIDTH;
			dest.bottom = dest.top + BLOCK_HEIGHT;

			m_renderTarget->DrawBitmap(bitmap.Get(), &dest, 1.0f, D2D1_BITMAP_INTERPOLATION_MODE_LINEAR);
		}
	}

	if (m_renderTarget->EndDraw() == D2DERR_RECREATE_TARGET) {
		CleanDeviceResources();
		CreateDeviceResources();
	}
}

void Game::Impl::CreateDeviceResources() {
	HRESULT hr;
	hr = m_drawFactory->CreateHwndRenderTarget(
		D2D1::RenderTargetProperties(D2D1_RENDER_TARGET_TYPE_DEFAULT, D2D1::PixelFormat(), 96.0f, 96.0f),
		D2D1::HwndRenderTargetProperties(m_window, D2D1::SizeU(WINDOW_WIDTH, WINDOW_HEIGHT)),
		m_renderTarget.ReleaseAndGetAddressOf());
	assert(SUCCEEDED(hr));

	Microsoft::WRL::ComPtr<ID2D1SolidColorBrush> solidBrush;
	assert(SUCCEEDED(
		m_renderTarget->CreateSolidColorBrush(D2D1::ColorF(D2D1::ColorF::Red), &solidBrush)
		));
	solidBrush.As(&m_testBrush);

	m_terimonoBitmaps[Tetrimono_Empty] = LoadImage(L"Assets/block_empty.png");
	m_terimonoBitmaps[Tetrimono_I] = LoadImage(L"Assets/block_blue.png");
	m_terimonoBitmaps[Tetrimono_J] = LoadImage(L"Assets/block_cyan.png");
	m_terimonoBitmaps[Tetrimono_L] = LoadImage(L"Assets/block_green.png");
	m_terimonoBitmaps[Tetrimono_O] = LoadImage(L"Assets/block_orange.png");
	m_terimonoBitmaps[Tetrimono_S] = LoadImage(L"Assets/block_red.png");
	m_terimonoBitmaps[Tetrimono_T] = LoadImage(L"Assets/block_violet.png");
	m_terimonoBitmaps[Tetrimono_Z] = LoadImage(L"Assets/block_yellow.png");
}

ComPtr<ID2D1Bitmap> Game::Impl::LoadImage(const wchar_t* fileName) const {
	ComPtr<IWICBitmapDecoder> decoder;
	assert(SUCCEEDED(
		m_imageFactory->CreateDecoderFromFilename(fileName, nullptr, GENERIC_READ,
			WICDecodeMetadataCacheOnDemand, &decoder)
	));

	ComPtr<IWICBitmapFrameDecode> frame;
	assert(SUCCEEDED( decoder->GetFrame(0, &frame) ));

	ComPtr<IWICFormatConverter> formatConverter;
	assert(SUCCEEDED( m_imageFactory->CreateFormatConverter(&formatConverter) ));

	assert(SUCCEEDED(
		formatConverter->Initialize(frame.Get(), GUID_WICPixelFormat32bppPBGRA,
			WICBitmapDitherTypeNone, nullptr, 0.0f, WICBitmapPaletteTypeCustom)
	));

	ComPtr<ID2D1Bitmap> bitmap;
	assert(SUCCEEDED(
		m_renderTarget->CreateBitmapFromWicBitmap(formatConverter.Get(), &bitmap)
	));

	return bitmap;
}

void Game::Impl::CleanDeviceResources() {
	m_testBrush = nullptr;
	m_renderTarget = nullptr;
}

LRESULT Game::Impl::GameProc(HWND window, UINT message, WPARAM wparam, LPARAM lparam) {
	if (message == WM_CREATE) {
		CREATESTRUCT* createData = reinterpret_cast<CREATESTRUCT*>(lparam);
		Game* game = reinterpret_cast<Game*>(createData->lpCreateParams);

		::SetWindowLongPtr(window, GWLP_USERDATA,
#ifdef _WIN64
			reinterpret_cast<LONG_PTR>(game)
#else
			reinterpret_cast<LONG>(game)
#endif
			);
		return 0;
	} else {
		Game* game = reinterpret_cast<Game*>(::GetWindowLongPtr(window, GWLP_USERDATA));
		std::unique_ptr<Game::Impl>& impl = game->m_impl;

		switch (message) {
		case WM_PAINT:
			return impl->OnPaint();
		case WM_CLOSE:
			return impl->OnClose();
		case WM_DESTROY:
			return impl->OnDestroy();
		case WM_KEYDOWN:
			return impl->OnKeyDown(static_cast<int>(wparam));
		case WM_KEYUP:
			return impl->OnKeyUp(static_cast<int>(wparam));
		default:
			return ::DefWindowProc(window, message, wparam, lparam);
		}
	}
}

LRESULT Game::Impl::OnKeyDown(int vk) {
	switch (vk) {
	case VK_LEFT:
		m_board.MoveCurrent(-1, 0); break;
	case VK_RIGHT:
		m_board.MoveCurrent(1, 0); break;
	case VK_UP:
		m_board.Rotate(RotateDirection_AntiClockwize); break;
	case VK_DOWN:
		m_board.Rotate(RotateDirection_Clockwize); break;
	case VK_SPACE:
		m_board.FallDown(); break;
	case VK_ESCAPE:
		::PostQuitMessage(0); break;
	}
	return 0;
}

LRESULT Game::Impl::OnKeyUp(int vk) {
	UNREFERENCED_PARAMETER(vk);
	return 0;
}

LRESULT Game::Impl::OnDestroy() {
	::PostQuitMessage(0);
	return 0;
}

LRESULT Game::Impl::OnClose() {
	::DestroyWindow(m_window);
	return 0;
}

LRESULT Game::Impl::OnPaint() {
	::ValidateRect(m_window, &m_clientRect);
	return 0;
}

#endif<commit_msg>remove unnecessary break;<commit_after>/*
Copyright (c) 2014, Dilyan Rusev
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

* Neither the name of the {organization} nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "stdafx.h"

#ifdef WIN32

#include "Game.h"
#include"Board.h"
#include <cassert>
#include <algorithm>

using namespace std;
using Microsoft::WRL::ComPtr;

struct Game::Impl {
	Impl(Game* parent);
	~Impl();

	void CreateAppWindow();
	void InitGraphicsSystems();

	void RunMainLoop();
	void Initialize();

	static LRESULT CALLBACK GameProc(HWND window, UINT message, WPARAM wparam, LPARAM lparam);
	LRESULT OnDestroy();
	LRESULT OnClose();
	LRESULT OnPaint();
	LRESULT OnKeyDown(int vk);
	LRESULT OnKeyUp(int vk);

	void Update(float ms);
	void Render();
	void CreateDeviceResources();
	void CleanDeviceResources();

	ComPtr<ID2D1Bitmap> LoadImage(const wchar_t* fileName) const;

	const wchar_t* APP_TITLE;
	const wchar_t* APP_CLASS;
	const int WINDOW_WIDTH;
	const int WINDOW_HEIGHT;
	const int BLOCK_WIDTH;
	const int BLOCK_HEIGHT;

	Game* m_parent;
	RECT m_clientRect;
	bool m_isClassRegistered;
	HWND m_window;
	HINSTANCE m_instance;
	LARGE_INTEGER m_perfFrequency;
	Board m_board;
	D2D1_POINT_2F m_boardPos;
	ComPtr<ID2D1Factory> m_drawFactory;
	ComPtr<IDWriteFactory> m_writeFactory;
	ComPtr<IWICImagingFactory> m_imageFactory;
	ComPtr<ID2D1HwndRenderTarget> m_renderTarget;
	ComPtr<ID2D1Brush> m_testBrush;
	ComPtr<ID2D1Bitmap> m_terimonoBitmaps[Count_Tetrimonos];
};

Game::Impl::Impl(Game* parent)
		: APP_TITLE(L"Falling Blocks")
		, APP_CLASS(L"FallingBlocks")
		, WINDOW_WIDTH(1280)
		, WINDOW_HEIGHT(720)
		, BLOCK_WIDTH(35)
		, BLOCK_HEIGHT(35)
		, m_parent(parent)
		, m_isClassRegistered(false)
		, m_window(nullptr)
		, m_instance(nullptr) {
}

Game::Impl::~Impl() {
	if (m_isClassRegistered && m_instance) {
		::UnregisterClass(APP_CLASS, m_instance);
	}
}

Game::Game()
		: m_impl(new Impl(this)) {
}

Game::~Game() {
}

void Game::Initialize() {
	m_impl->Initialize();
}

void Game::Impl::Initialize() {
	m_instance = static_cast<HINSTANCE>(::GetModuleHandle(nullptr));

	assert(::QueryPerformanceFrequency(&m_perfFrequency));

	CreateAppWindow();
	InitGraphicsSystems();
	CreateDeviceResources();
}

void Game::Impl::InitGraphicsSystems() {
	HRESULT hr = ::CoCreateInstance(CLSID_WICImagingFactory1, nullptr,
		CLSCTX_INPROC_SERVER, __uuidof(m_imageFactory),
		reinterpret_cast<void**>(m_imageFactory.GetAddressOf()));
	assert(SUCCEEDED(hr));

	hr = ::DWriteCreateFactory(DWRITE_FACTORY_TYPE_SHARED, __uuidof(m_writeFactory),
		reinterpret_cast<IUnknown**>(m_writeFactory.GetAddressOf()));
	assert(SUCCEEDED(hr));

	hr = ::D2D1CreateFactory(D2D1_FACTORY_TYPE_SINGLE_THREADED, IID_PPV_ARGS(&m_drawFactory));
	assert(SUCCEEDED(hr));
}

void Game::Impl::CreateAppWindow() {
	assert(m_instance != nullptr);

	WNDCLASSEX wc;
	wc.cbSize = sizeof(wc);
	wc.cbClsExtra = 0;
	wc.cbWndExtra = 0;
	wc.hbrBackground = nullptr;
	wc.hCursor = ::LoadCursor(nullptr, IDC_ARROW);
	wc.hIcon = ::LoadIcon(nullptr, IDI_APPLICATION);
	wc.hIconSm = ::LoadIcon(nullptr, IDI_APPLICATION);
	wc.hInstance = m_instance;
	wc.lpfnWndProc = &GameProc;
	wc.lpszClassName = APP_CLASS;
	wc.lpszMenuName = nullptr;
	wc.style = 0;

	assert(::RegisterClassEx(&wc));
	m_isClassRegistered = true;

	DWORD windowStyle = WS_CAPTION | WS_MINIMIZEBOX | WS_SYSMENU;
	DWORD exStyle = 0;
	RECT windowSize = { 0, 0, WINDOW_WIDTH, WINDOW_HEIGHT };
	assert(::AdjustWindowRectEx(&windowSize, windowStyle, FALSE, exStyle));

	int width = windowSize.right - windowSize.left;
	int height = windowSize.bottom - windowSize.top;
	int x = (::GetSystemMetrics(SM_CXSCREEN) - width) / 2;
	int y = (::GetSystemMetrics(SM_CYSCREEN) - height) / 2;

	m_window = ::CreateWindowExW(exStyle, APP_CLASS, APP_TITLE,
		windowStyle, x, y, width, height, HWND_DESKTOP, nullptr, m_instance,
		reinterpret_cast<void*>(m_parent));
	assert(m_window != nullptr);

	m_clientRect.left = 0;
	m_clientRect.right = WINDOW_WIDTH;
	m_clientRect.top = 0;
	m_clientRect.bottom = WINDOW_HEIGHT;

	m_boardPos.x = (WINDOW_WIDTH - m_board.WIDTH * BLOCK_WIDTH) / 2.0f;
	m_boardPos.y = (WINDOW_HEIGHT - m_board.HEIGHT * BLOCK_HEIGHT) / 2.0f;
}

void Game::RunMainLoop() {
	m_impl->RunMainLoop();
}

void Game::Impl::RunMainLoop() {
	assert(m_instance);
	assert(m_window);

	::ShowWindow(m_window, SW_SHOW);
	::UpdateWindow(m_window);

	MSG msg;
	msg.message = WM_NULL;

	LARGE_INTEGER before, now;
	float elapsedInMs;

	::QueryPerformanceCounter(&before);
	while (msg.message != WM_QUIT) {
		while (::PeekMessage(&msg, nullptr, 0, 0, PM_REMOVE)) {
			::TranslateMessage(&msg);
			::DispatchMessage(&msg);
		}

		::QueryPerformanceCounter(&now);
		elapsedInMs = static_cast<float>((now.QuadPart - before.QuadPart) * 1000 / m_perfFrequency.QuadPart);

		Update(elapsedInMs);
		Render();
		before = now;
	}
}

void Game::Impl::Update(float ms) {
	m_board.Update(ms);
}

void Game::Impl::Render() {
	m_renderTarget->BeginDraw();
	m_renderTarget->Clear(D2D1::ColorF(D2D1::ColorF::AliceBlue));

	D2D1_RECT_F dest;
	for (int y = 0; y < m_board.HEIGHT; y++) {
		for (int x = 0; x < m_board.WIDTH; x++) {
			auto t = m_board.GetAt(x, y);
			auto& bitmap = m_terimonoBitmaps[t];
			dest.left = m_boardPos.x + static_cast<float>(x * BLOCK_WIDTH);
			dest.top = m_boardPos.y + static_cast<float>(y * BLOCK_HEIGHT);
			dest.right = dest.left + BLOCK_WIDTH;
			dest.bottom = dest.top + BLOCK_HEIGHT;

			m_renderTarget->DrawBitmap(bitmap.Get(), &dest, 1.0f, D2D1_BITMAP_INTERPOLATION_MODE_LINEAR);
		}
	}

	D2D1_POINT_2F nextOffset = D2D1::Point2F(m_boardPos.x + m_board.WIDTH * BLOCK_WIDTH + 30, m_boardPos.y);
	for (int y = 0; y < m_board.MAX_TETRIMONO_HEIGHT; y++) {
		for (int x = 0; x < m_board.MAX_TETRIMONO_WIDTH; x++) {
			auto t = m_board.GetNextAt(x, y);
			auto& bitmap = m_terimonoBitmaps[t];
			dest.left = nextOffset.x + static_cast<float>(x * BLOCK_WIDTH);
			dest.top = nextOffset.y + static_cast<float>(y * BLOCK_HEIGHT);
			dest.right = dest.left + BLOCK_WIDTH;
			dest.bottom = dest.top + BLOCK_HEIGHT;

			m_renderTarget->DrawBitmap(bitmap.Get(), &dest, 1.0f, D2D1_BITMAP_INTERPOLATION_MODE_LINEAR);
		}
	}

	if (m_renderTarget->EndDraw() == D2DERR_RECREATE_TARGET) {
		CleanDeviceResources();
		CreateDeviceResources();
	}
}

void Game::Impl::CreateDeviceResources() {
	HRESULT hr;
	hr = m_drawFactory->CreateHwndRenderTarget(
		D2D1::RenderTargetProperties(D2D1_RENDER_TARGET_TYPE_DEFAULT, D2D1::PixelFormat(), 96.0f, 96.0f),
		D2D1::HwndRenderTargetProperties(m_window, D2D1::SizeU(WINDOW_WIDTH, WINDOW_HEIGHT)),
		m_renderTarget.ReleaseAndGetAddressOf());
	assert(SUCCEEDED(hr));

	Microsoft::WRL::ComPtr<ID2D1SolidColorBrush> solidBrush;
	assert(SUCCEEDED(
		m_renderTarget->CreateSolidColorBrush(D2D1::ColorF(D2D1::ColorF::Red), &solidBrush)
		));
	solidBrush.As(&m_testBrush);

	m_terimonoBitmaps[Tetrimono_Empty] = LoadImage(L"Assets/block_empty.png");
	m_terimonoBitmaps[Tetrimono_I] = LoadImage(L"Assets/block_blue.png");
	m_terimonoBitmaps[Tetrimono_J] = LoadImage(L"Assets/block_cyan.png");
	m_terimonoBitmaps[Tetrimono_L] = LoadImage(L"Assets/block_green.png");
	m_terimonoBitmaps[Tetrimono_O] = LoadImage(L"Assets/block_orange.png");
	m_terimonoBitmaps[Tetrimono_S] = LoadImage(L"Assets/block_red.png");
	m_terimonoBitmaps[Tetrimono_T] = LoadImage(L"Assets/block_violet.png");
	m_terimonoBitmaps[Tetrimono_Z] = LoadImage(L"Assets/block_yellow.png");
}

ComPtr<ID2D1Bitmap> Game::Impl::LoadImage(const wchar_t* fileName) const {
	ComPtr<IWICBitmapDecoder> decoder;
	assert(SUCCEEDED(
		m_imageFactory->CreateDecoderFromFilename(fileName, nullptr, GENERIC_READ,
			WICDecodeMetadataCacheOnDemand, &decoder)
	));

	ComPtr<IWICBitmapFrameDecode> frame;
	assert(SUCCEEDED( decoder->GetFrame(0, &frame) ));

	ComPtr<IWICFormatConverter> formatConverter;
	assert(SUCCEEDED( m_imageFactory->CreateFormatConverter(&formatConverter) ));

	assert(SUCCEEDED(
		formatConverter->Initialize(frame.Get(), GUID_WICPixelFormat32bppPBGRA,
			WICBitmapDitherTypeNone, nullptr, 0.0f, WICBitmapPaletteTypeCustom)
	));

	ComPtr<ID2D1Bitmap> bitmap;
	assert(SUCCEEDED(
		m_renderTarget->CreateBitmapFromWicBitmap(formatConverter.Get(), &bitmap)
	));

	return bitmap;
}

void Game::Impl::CleanDeviceResources() {
	m_testBrush = nullptr;
	m_renderTarget = nullptr;
}

LRESULT Game::Impl::GameProc(HWND window, UINT message, WPARAM wparam, LPARAM lparam) {
	if (message == WM_CREATE) {
		CREATESTRUCT* createData = reinterpret_cast<CREATESTRUCT*>(lparam);
		Game* game = reinterpret_cast<Game*>(createData->lpCreateParams);

		::SetWindowLongPtr(window, GWLP_USERDATA,
#ifdef _WIN64
			reinterpret_cast<LONG_PTR>(game)
#else
			reinterpret_cast<LONG>(game)
#endif
			);
		return 0;
	} else {
		Game* game = reinterpret_cast<Game*>(::GetWindowLongPtr(window, GWLP_USERDATA));
		std::unique_ptr<Game::Impl>& impl = game->m_impl;

		switch (message) {
		case WM_PAINT:
			return impl->OnPaint();
		case WM_CLOSE:
			return impl->OnClose();
		case WM_DESTROY:
			return impl->OnDestroy();
		case WM_KEYDOWN:
			return impl->OnKeyDown(static_cast<int>(wparam));
		case WM_KEYUP:
			return impl->OnKeyUp(static_cast<int>(wparam));
		default:
			return ::DefWindowProc(window, message, wparam, lparam);
		}
	}
}

LRESULT Game::Impl::OnKeyDown(int vk) {
	switch (vk) {
	case VK_LEFT:
		m_board.MoveCurrent(-1, 0); break;
	case VK_RIGHT:
		m_board.MoveCurrent(1, 0); break;
	case VK_UP:
		m_board.Rotate(RotateDirection_Clockwize); break;
	case VK_DOWN:
		m_board.MoveCurrent(0, 1); break;
		 break;
	case VK_SPACE:
		m_board.FallDown(); break;
	case VK_ESCAPE:
		::PostQuitMessage(0); break;
	}
	return 0;
}

LRESULT Game::Impl::OnKeyUp(int vk) {
	UNREFERENCED_PARAMETER(vk);
	return 0;
}

LRESULT Game::Impl::OnDestroy() {
	::PostQuitMessage(0);
	return 0;
}

LRESULT Game::Impl::OnClose() {
	::DestroyWindow(m_window);
	return 0;
}

LRESULT Game::Impl::OnPaint() {
	::ValidateRect(m_window, &m_clientRect);
	return 0;
}

#endif<|endoftext|>