// Copyright 2020 The Defold Foundation
// Licensed under the Defold License version 1.0 (the "License"); you may not use
// this file except in compliance with the License.
//
// You may obtain a copy of the License, together with FAQs at
// https://www.defold.com/license
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.

#include <stdint.h>
#include <dlib/hashtable.h>
#include <dlib/index_pool.h>
#include <dlib/log.h>
#include <dlib/math.h>
#include <dlib/profile.h>

#include "sound.h"
#include "sound_codec.h"
#include "sound_private.h"

#include <math.h>
#include <cfloat>

/**
 * Defold simple sound system
 * NOTE: Must units is in frames, i.e a sample in time with N channels
 *
 */
namespace dmSound
{
    using namespace Vectormath::Aos;

    #define SOUND_MAX_MIX_CHANNELS (2)
    #define SOUND_OUTBUFFER_COUNT (6)
    #define SOUND_MAX_SPEED (5)

    // TODO: How many bits?
    const uint32_t RESAMPLE_FRACTION_BITS = 31;

    const dmhash_t MASTER_GROUP_HASH = dmHashString64("master");
    const uint32_t MAX_GROUPS = 32;
    const uint32_t GROUP_MEMORY_BUFFER_COUNT = 64;

    /**
     * Value with memory for "ramping" of values. See also struct Ramp below.
     */
    struct Value
    {
        inline void Reset(float value)
        {
            // NOTE: We always ramp from zero to x. Otherwise we can
            // get initial clicks when playing sounds
            m_Prev = 0.0f;
            m_Current = value;
            m_Next = value;
        }

        inline void Set(float value, bool reset)
        {
            if (reset) {
                Reset(value);
            } else {
               m_Next = value;
            }
        }

        inline bool IsZero()
        {
            // Let's do the check in integer space instead (16 instructions -> 6)
            const uint32_t* pa = (const uint32_t*)&m_Prev;
            const uint32_t* pb = (const uint32_t*)&m_Current;
            const uint32_t* pc = (const uint32_t*)&m_Next;
            return (*pa | *pb | *pc ) == 0;
        }

        /**
         * Set previous value to current and current to next, i.e. "sample"
         */
        inline void Step()
        {
            m_Prev = m_Current;
            m_Current = m_Next;
        }

        Value()
        {
            Reset(1.0f);
        }

        float m_Prev;
        float m_Current;
        float m_Next;
    };

    /**
     * Helper for calculating ramps
     */
    struct Ramp
    {
        float m_From, m_To, m_TotalSamplesRecip;

        Ramp(const Value* value, uint32_t buffer, uint32_t total_buffers, uint32_t total_samples)
        {
            float ramp_length = (value->m_Current - value->m_Prev) / total_buffers;
            m_From = value->m_Prev + ramp_length * buffer;
            m_To = m_From + ramp_length;
            m_TotalSamplesRecip = 1.0f / total_samples;
        }

        inline float GetValue(int i) const
        {
            float mix = i * m_TotalSamplesRecip;
            return m_From + mix * (m_To - m_From);
        }
    };

    /**
     * Context with data for mixing N buffers, i.e. during update
     */
    struct MixContext
    {
        MixContext(uint32_t current_buffer, uint32_t total_buffers)
        {
            m_CurrentBuffer = current_buffer;
            m_TotalBuffers = total_buffers;
        }
        // Current buffer index
        uint32_t m_CurrentBuffer;
        // How many buffers to mix
        uint32_t m_TotalBuffers;
    };

    Ramp GetRamp(const MixContext* mix_context, const Value* value, uint32_t total_samples)
    {
        Ramp ramp(value, mix_context->m_CurrentBuffer, mix_context->m_TotalBuffers, total_samples);
        return ramp;
    }

    struct SoundData
    {
        dmhash_t      m_NameHash;
        void*         m_Data;
        int           m_Size;
        // Index in m_SoundData
        uint16_t      m_Index;
        SoundDataType m_Type;
    };

    struct SoundInstance
    {
        dmSoundCodec::HDecoder m_Decoder;
        void*       m_Frames;
        dmhash_t    m_Group;

        Value       m_Gain;     // default: 1.0f
        Value       m_Pan;      // 0 = -45deg left, 1 = 45 deg right
        float       m_Speed;    // 1.0 = normal speed, 0.5 = half speed, 2.0 = double speed
        uint32_t    m_FrameCount;
        uint64_t    m_FrameFraction;

        uint16_t    m_Index;
        uint16_t    m_SoundDataIndex;
        uint8_t     m_Looping : 1;
        uint8_t     m_EndOfStream : 1;
        uint8_t     m_Playing : 1;
        uint8_t     : 5;
    };

    struct SoundGroup
    {
        dmhash_t m_NameHash;
        Value    m_Gain;
        float*   m_MixBuffer;
        float    m_SumSquaredMemory[SOUND_MAX_MIX_CHANNELS * GROUP_MEMORY_BUFFER_COUNT];
        float    m_PeakMemorySq[SOUND_MAX_MIX_CHANNELS * GROUP_MEMORY_BUFFER_COUNT];
        int      m_NextMemorySlot;
    };

    struct SoundSystem
    {
        dmSoundCodec::HCodecContext   m_CodecContext;
        DeviceType*                   m_DeviceType;
        HDevice                       m_Device;

        dmArray<SoundInstance>  m_Instances;
        dmIndexPool16           m_InstancesPool;

        dmArray<SoundData>      m_SoundData;
        dmIndexPool16           m_SoundDataPool;

        dmHashTable<dmhash_t, int> m_GroupMap;
        SoundGroup              m_Groups[MAX_GROUPS];

        Stats                   m_Stats;

        uint32_t                m_MixRate;
        uint32_t                m_FrameCount;
        uint32_t                m_PlayCounter;

        int16_t*                m_OutBuffers[SOUND_OUTBUFFER_COUNT];
        uint16_t                m_NextOutBuffer;

        bool                    m_IsDeviceStarted;
        bool                    m_IsPhoneCallActive;
        bool                    m_HasWindowFocus;
    };

    SoundSystem* g_SoundSystem = 0;

    DeviceType* g_FirstDevice = 0;

    void SetDefaultInitializeParams(InitializeParams* params)
    {
        memset(params, 0, sizeof(InitializeParams));
        params->m_OutputDevice = "default";
        params->m_MasterGain = 1.0f;
        params->m_MaxSoundData = 128;
        params->m_MaxSources = 16;
        params->m_MaxBuffers = 32;
        params->m_BufferSize = 12 * 4096;
        params->m_FrameCount = 768;
        params->m_MaxInstances = 256;
    }

    Result RegisterDevice(struct DeviceType* device)
    {
        device->m_Next = g_FirstDevice;
        g_FirstDevice = device;
        return RESULT_OK;
    }

    static Result OpenDevice(const char* name, const OpenDeviceParams* params, DeviceType** device_type, HDevice* device)
    {
        DeviceType* d = g_FirstDevice;
        while (d) {
            if (strcmp(d->m_Name, name) == 0) {
                *device_type = d;
                return d->m_Open(params, device);
            }
            d = d->m_Next;
        }

        return RESULT_DEVICE_NOT_FOUND;
    }

    static int GetOrCreateGroup(const char* group_name)
    {
        dmhash_t group_hash = dmHashString64(group_name);

        SoundSystem* sound = g_SoundSystem;
        if (sound->m_GroupMap.Full()) {
            return -1;
        }

        if (sound->m_GroupMap.Get(group_hash)) {
            return *sound->m_GroupMap.Get(group_hash);
        }

        uint32_t index = sound->m_GroupMap.Size();
        SoundGroup* group = &sound->m_Groups[index];
        group->m_NameHash = group_hash;
        group->m_Gain.Reset(1.0f);
        size_t mix_buffer_size = sound->m_FrameCount * sizeof(float) * SOUND_MAX_MIX_CHANNELS;
        group->m_MixBuffer = (float*) malloc(mix_buffer_size);
        memset(group->m_MixBuffer, 0, mix_buffer_size);
        sound->m_GroupMap.Put(group_hash, index);
        return index;
    }

    Result Initialize(dmConfigFile::HConfig config, const InitializeParams* params)
    {
        Result r = PlatformInitialize(config, params);
        if (r != RESULT_OK) {
            return r;
        }

        HDevice device;
        OpenDeviceParams device_params;
        // TODO: m_BufferCount configurable?
        device_params.m_BufferCount = SOUND_OUTBUFFER_COUNT;
        device_params.m_FrameCount = params->m_FrameCount;
        DeviceType* device_type;
        r = OpenDevice(params->m_OutputDevice, &device_params, &device_type, &device);
        if (r != RESULT_OK) {
            dmLogError("Failed to Open device '%s'", params->m_OutputDevice);
            return r;
        }

        DeviceInfo device_info;
        device_type->m_DeviceInfo(device, &device_info);

        float master_gain = params->m_MasterGain;

        g_SoundSystem = new SoundSystem();
        SoundSystem* sound = g_SoundSystem;
        sound->m_IsDeviceStarted = false;
        sound->m_IsPhoneCallActive = false;
        sound->m_HasWindowFocus = true; // Assume we startup with the window focused
        sound->m_DeviceType = device_type;
        sound->m_Device = device;
        dmSoundCodec::NewCodecContextParams codec_params;
        codec_params.m_MaxDecoders = params->m_MaxInstances;
        sound->m_CodecContext = dmSoundCodec::New(&codec_params);

        uint32_t max_sound_data = params->m_MaxSoundData;
        uint32_t max_buffers = params->m_MaxBuffers;
        uint32_t max_sources = params->m_MaxSources;
        uint32_t max_instances = params->m_MaxInstances;

        if (config)
        {
            master_gain = dmConfigFile::GetFloat(config, "sound.gain", 1.0f);
            max_sound_data = (uint32_t) dmConfigFile::GetInt(config, "sound.max_sound_data", (int32_t) max_sound_data);
            max_buffers = (uint32_t) dmConfigFile::GetInt(config, "sound.max_sound_buffers", (int32_t) max_buffers);
            max_sources = (uint32_t) dmConfigFile::GetInt(config, "sound.max_sound_sources", (int32_t) max_sources);
            max_instances = (uint32_t) dmConfigFile::GetInt(config, "sound.max_sound_instances", (int32_t) max_instances);
        }

        sound->m_Instances.SetCapacity(max_instances);
        sound->m_Instances.SetSize(max_instances);
        sound->m_InstancesPool.SetCapacity(max_instances);
        for (uint32_t i = 0; i < max_instances; ++i)
        {
            SoundInstance* instance = &sound->m_Instances[i];
            memset(instance, 0, sizeof(*instance));
            instance->m_Index = 0xffff;
            instance->m_SoundDataIndex = 0xffff;
            // NOTE: +1 for "over-fetch" when up-sampling
            // NOTE: and x SOUND_MAX_SPEED for potential pitch range
            instance->m_Frames = malloc((params->m_FrameCount * SOUND_MAX_SPEED + 1) * sizeof(int16_t) * SOUND_MAX_MIX_CHANNELS);
            instance->m_FrameCount = 0;
            instance->m_Speed = 1.0f;
        }

        sound->m_SoundData.SetCapacity(max_sound_data);
        sound->m_SoundData.SetSize(max_sound_data);
        sound->m_SoundDataPool.SetCapacity(max_sound_data);
        for (uint32_t i = 0; i < max_sound_data; ++i)
        {
            sound->m_SoundData[i].m_Index = 0xffff;
        }

        sound->m_MixRate = device_info.m_MixRate;
        sound->m_FrameCount = params->m_FrameCount;
        for (int i = 0; i < SOUND_OUTBUFFER_COUNT; ++i) {
            sound->m_OutBuffers[i] = (int16_t*) malloc(params->m_FrameCount * sizeof(int16_t) * SOUND_MAX_MIX_CHANNELS);
        }
        sound->m_NextOutBuffer = 0;

        memset(&g_SoundSystem->m_Stats, 0, sizeof(g_SoundSystem->m_Stats));
        sound->m_GroupMap.SetCapacity(MAX_GROUPS * 2 + 1, MAX_GROUPS);
        for (uint32_t i = 0; i < MAX_GROUPS; ++i) {
            memset(&sound->m_Groups[i], 0, sizeof(SoundGroup));
        }

        int master_index = GetOrCreateGroup("master");
        SoundGroup* master = &sound->m_Groups[master_index];
        master->m_Gain.Reset(master_gain);

        return RESULT_OK;
    }

    Result Finalize()
    {
        PlatformFinalize();

        Result result = RESULT_OK;

        if (g_SoundSystem)
        {
            SoundSystem* sound = g_SoundSystem;
            dmSoundCodec::Delete(sound->m_CodecContext);

            for (uint32_t i = 0; i < sound->m_Instances.Size(); ++i)
            {
                SoundInstance* instance = &sound->m_Instances[i];
                instance->m_Index = 0xffff;
                instance->m_SoundDataIndex = 0xffff;
                free(instance->m_Frames);
                memset(instance, 0, sizeof(*instance));
            }

            for (int i = 0; i < SOUND_OUTBUFFER_COUNT; ++i) {
                free((void*) sound->m_OutBuffers[i]);
            }

            for (uint32_t i = 0; i < MAX_GROUPS; i++) {
                SoundGroup* g = &sound->m_Groups[i];
                if (g->m_MixBuffer) {
                    free((void*) g->m_MixBuffer);
                }
            }

            sound->m_DeviceType->m_Close(sound->m_Device);

            delete sound;
            g_SoundSystem = 0;
        }

        return result;
    }

    void GetStats(Stats* stats)
    {
        *stats = g_SoundSystem->m_Stats;
    }

    static inline const char* GetSoundName(SoundSystem* sound, SoundInstance* instance)
    {
        dmhash_t hash = sound->m_SoundData[instance->m_SoundDataIndex].m_NameHash;
        return dmHashReverseSafe64(hash);
    }

    Result NewSoundData(const void* sound_buffer, uint32_t sound_buffer_size, SoundDataType type, HSoundData* sound_data, dmhash_t name)
    {
        SoundSystem* sound = g_SoundSystem;

        if (sound->m_SoundDataPool.Remaining() == 0)
        {
            *sound_data = 0;
            dmLogError("Out of sound data slots (%u). Increase the project setting 'sound.max_sound_data'", sound->m_SoundDataPool.Capacity());
            return RESULT_OUT_OF_INSTANCES;
        }
        uint16_t index = sound->m_SoundDataPool.Pop();

        SoundData* sd = &sound->m_SoundData[index];
        sd->m_NameHash = name;
        sd->m_Type = type;
        sd->m_Index = index;
        sd->m_Data = 0;
        sd->m_Size = 0;

        Result result = SetSoundData(sd, sound_buffer, sound_buffer_size);
        if (result == RESULT_OK)
            *sound_data = sd;
        else
            DeleteSoundData(sd);

        return result;
    }

    Result SetSoundData(HSoundData sound_data, const void* sound_buffer, uint32_t sound_buffer_size)
    {
        free(sound_data->m_Data);
        sound_data->m_Data = malloc(sound_buffer_size);
        sound_data->m_Size = sound_buffer_size;
        memcpy(sound_data->m_Data, sound_buffer, sound_buffer_size);
        return RESULT_OK;
    }

    uint32_t GetSoundResourceSize(HSoundData sound_data)
    {
        return sound_data->m_Size + sizeof(SoundData);
    }

    Result DeleteSoundData(HSoundData sound_data)
    {
        if (sound_data->m_Data != 0x0)
            free((void*) sound_data->m_Data);

        SoundSystem* sound = g_SoundSystem;
        sound->m_SoundDataPool.Push(sound_data->m_Index);
        sound_data->m_Index = 0xffff;

        return RESULT_OK;
    }

    Result NewSoundInstance(HSoundData sound_data, HSoundInstance* sound_instance)
    {
        SoundSystem* ss = g_SoundSystem;

        if (ss->m_InstancesPool.Remaining() == 0)
        {
            *sound_instance = 0;
            dmLogError("Out of sound data instance slots (%u). Increase the project setting 'sound.max_sound_instances'", ss->m_InstancesPool.Capacity());
            return RESULT_OUT_OF_INSTANCES;
        }

        dmSoundCodec::HDecoder decoder;

        dmSoundCodec::Format codec_format = dmSoundCodec::FORMAT_WAV;
        if (sound_data->m_Type == SOUND_DATA_TYPE_WAV) {
            codec_format = dmSoundCodec::FORMAT_WAV;
        } else if (sound_data->m_Type == SOUND_DATA_TYPE_OGG_VORBIS) {
            codec_format = dmSoundCodec::FORMAT_VORBIS;
        } else {
            assert(0);
        }

        dmSoundCodec::Result r = dmSoundCodec::NewDecoder(ss->m_CodecContext, codec_format, sound_data->m_Data, sound_data->m_Size, &decoder);
        if (r != dmSoundCodec::RESULT_OK) {
            dmLogError("Failed to decode sound (%d)", r);
            return RESULT_INVALID_STREAM_DATA;
        }

        dmSoundCodec::Info info;
        dmSoundCodec::GetInfo(ss->m_CodecContext, decoder, &info);

        uint16_t index = ss->m_InstancesPool.Pop();
        SoundInstance* si = &ss->m_Instances[index];
        assert(si->m_Index == 0xffff);

        si->m_SoundDataIndex = sound_data->m_Index;
        si->m_Index = index;
        si->m_Gain.Reset(1.0f);
        si->m_Pan.Reset(0.5f);
        si->m_Looping = 0;
        si->m_EndOfStream = 0;
        si->m_Playing = 0;
        si->m_Decoder = decoder;
        si->m_Group = MASTER_GROUP_HASH;

        *sound_instance = si;

        return RESULT_OK;
    }

    Result DeleteSoundInstance(HSoundInstance sound_instance)
    {
        SoundSystem* sound = g_SoundSystem;
        if (IsPlaying(sound_instance))
        {
            dmLogError("Deleting playing sound instance (%s)", GetSoundName(sound, sound_instance));
            Stop(sound_instance);
        }
        uint16_t index = sound_instance->m_Index;
        sound->m_InstancesPool.Push(index);
        sound_instance->m_Index = 0xffff;
        sound_instance->m_SoundDataIndex = 0xffff;
        dmSoundCodec::DeleteDecoder(sound->m_CodecContext, sound_instance->m_Decoder);
        sound_instance->m_Decoder = 0;
        sound_instance->m_FrameCount = 0;
        sound_instance->m_Speed = 1.0f;

        return RESULT_OK;
    }

    Result SetInstanceGroup(HSoundInstance instance, const char* group_name)
    {
        dmhash_t group_hash = dmHashString64(group_name);
        return SetInstanceGroup(instance, group_hash);
    }

    Result SetInstanceGroup(HSoundInstance instance, dmhash_t group_hash)
    {
        SoundSystem* sound = g_SoundSystem;
        int* index = sound->m_GroupMap.Get(group_hash);
        if (!index) {
            return RESULT_NO_SUCH_GROUP;
        }
        instance->m_Group = group_hash;
        return RESULT_OK;
    }

    Result AddGroup(const char* group)
    {
        int index = GetOrCreateGroup(group);
        if (index == -1) {
            return RESULT_OUT_OF_GROUPS;
        }
        return RESULT_OK;
    }

    Result SetGroupGain(dmhash_t group_hash, float gain)
    {
        SoundSystem* sound = g_SoundSystem;
        int* index = sound->m_GroupMap.Get(group_hash);
        if (!index) {
            return RESULT_NO_SUCH_GROUP;
        }

        // If all playing sounds is currently at gain zero
        // we can safely do a hard reset of the group gain
        bool reset = true;
        uint32_t instances = sound->m_Instances.Size();
        for (uint32_t i = 0; i < instances; ++i)
        {
            SoundInstance* instance = &sound->m_Instances[i];
            if (instance->m_Group != group_hash)
            {
                continue;
            }
            if (instance->m_Playing || instance->m_FrameCount > 0)
            {
                if (instance->m_Gain.m_Prev == 0.0)
                {
                    continue;
                }
                reset = false;
                break;
            }
        }
        SoundGroup* group = &sound->m_Groups[*index];
        group->m_Gain.Set(gain, reset);
        return RESULT_OK;
    }

    Result GetGroupGain(dmhash_t group_hash, float* gain)
    {
        SoundSystem* sound = g_SoundSystem;
        int* index = sound->m_GroupMap.Get(group_hash);
        if (!index) {
            return RESULT_NO_SUCH_GROUP;
        }

        SoundGroup* group = &sound->m_Groups[*index];
        *gain = group->m_Gain.m_Next;
        return RESULT_OK;
    }

    uint32_t GetGroupCount()
    {
        SoundSystem* sound = g_SoundSystem;
        return sound->m_GroupMap.Size();
    }

    Result GetGroupHash(uint32_t index, dmhash_t* hash)
    {
        SoundSystem* sound = g_SoundSystem;
        if (index >= sound->m_GroupMap.Size()) {
            return RESULT_NO_SUCH_GROUP;
        }

        SoundGroup* group = &sound->m_Groups[index];
        *hash = group->m_NameHash;
        return RESULT_OK;
    }

    Result GetGroupRMS(dmhash_t group_hash, float window, float* rms_left, float* rms_right)
    {
        SoundSystem* sound = g_SoundSystem;
        int* index = sound->m_GroupMap.Get(group_hash);
        if (!index) {
            return RESULT_NO_SUCH_GROUP;
        }

        SoundGroup* g = &sound->m_Groups[*index];
        uint32_t rms_frames = (uint32_t) (sound->m_MixRate * window);
        int left = rms_frames;
        int ss_index = (g->m_NextMemorySlot - 1) % GROUP_MEMORY_BUFFER_COUNT;
        float sum_sq_left = 0;
        float sum_sq_right = 0;
        int count = 0;
        while (left > 0) {
            sum_sq_left += g->m_SumSquaredMemory[2 * ss_index + 0];
            sum_sq_right += g->m_SumSquaredMemory[2 * ss_index + 1];

            left -= sound->m_FrameCount;
            ss_index = (ss_index - 1) % GROUP_MEMORY_BUFFER_COUNT;
            count++;
        }

        *rms_left = sqrtf(sum_sq_left / (float) (count * sound->m_FrameCount)) / 32767.0f;
        *rms_right = sqrtf(sum_sq_right / (float) (count * sound->m_FrameCount)) / 32767.0f;

        return RESULT_OK;
    }

    Result GetGroupPeak(dmhash_t group_hash, float window, float* peak_left, float* peak_right)
    {
        SoundSystem* sound = g_SoundSystem;
        int* index = sound->m_GroupMap.Get(group_hash);
        if (!index) {
            return RESULT_NO_SUCH_GROUP;
        }

        SoundGroup* g = &sound->m_Groups[*index];
        uint32_t rms_frames = (uint32_t) (sound->m_MixRate * window);
        int left = rms_frames;
        int ss_index = (g->m_NextMemorySlot - 1) % GROUP_MEMORY_BUFFER_COUNT;
        float max_peak_left_sq = 0;
        float max_peak_right_sq = 0;
        int count = 0;
        while (left > 0) {
            max_peak_left_sq = dmMath::Max(max_peak_left_sq, g->m_PeakMemorySq[2 * ss_index + 0]);
            max_peak_right_sq = dmMath::Max(max_peak_right_sq, g->m_PeakMemorySq[2 * ss_index + 1]);

            left -= sound->m_FrameCount;
            ss_index = (ss_index - 1) % GROUP_MEMORY_BUFFER_COUNT;
            count++;
        }

        *peak_left = sqrtf(max_peak_left_sq) / 32767.0f;
        *peak_right = sqrtf(max_peak_right_sq) / 32767.0f;

        return RESULT_OK;
    }

    Result Play(HSoundInstance sound_instance)
    {
        sound_instance->m_Playing = 1;
        return RESULT_OK;
    }

    Result Stop(HSoundInstance sound_instance)
    {
        SoundSystem* sound = g_SoundSystem;
        sound_instance->m_Playing = 0;
        dmSoundCodec::Reset(sound->m_CodecContext, sound_instance->m_Decoder);
        return RESULT_OK;
    }

    Result Pause(HSoundInstance sound_instance, bool pause)
    {
        sound_instance->m_Playing = (uint8_t)!pause;
        return RESULT_OK;
    }

    uint32_t GetAndIncreasePlayCounter()
    {
    	if (g_SoundSystem->m_PlayCounter == dmSound::INVALID_PLAY_ID)
    	{
    		g_SoundSystem->m_PlayCounter = 0;
    	}
        return g_SoundSystem->m_PlayCounter++;
    }

    bool IsPlaying(HSoundInstance sound_instance)
    {
        return sound_instance->m_Playing; // && !sound_instance->m_EndOfStream;
    }

    Result SetLooping(HSoundInstance sound_instance, bool looping)
    {
        sound_instance->m_Looping = (uint32_t) looping;
        return RESULT_OK;
    }

    Result SetParameter(HSoundInstance sound_instance, Parameter parameter, const Vector4& value)
    {
        bool reset = !sound_instance->m_Playing;
        switch(parameter)
        {
            case PARAMETER_GAIN:
                sound_instance->m_Gain.Set(dmMath::Max(0.0f, value.getX()), reset);
                break;
            case PARAMETER_PAN:
                {
                    float pan = dmMath::Max(-1.0f, dmMath::Min(1.0f, value.getX()));
                    pan = (pan + 1.0f) * 0.5f; // map [-1,1] to [0,1] for easier calculations later
                    sound_instance->m_Pan.Set(pan, reset);
                }
                break;
            case PARAMETER_SPEED:
                sound_instance->m_Speed = dmMath::Max(0.0f, dmMath::Min((float)SOUND_MAX_SPEED, value.getX()));
                break;
            default:
                dmLogError("Invalid parameter: %d (%s)\n", parameter, GetSoundName(g_SoundSystem, sound_instance));
                return RESULT_INVALID_PROPERTY;
        }
        return RESULT_OK;
    }

    static inline void GetPanScale(float pan, float* left_scale, float* right_scale)
    {
        // Constant power panning: https://www.cs.cmu.edu/~music/icm-online/readings/panlaws/index.html
        const float theta = pan * M_PI_2;
        *left_scale = cosf(theta);
        *right_scale = sinf(theta);
    }

    template <typename T, int offset, int scale>
    static void MixResampleUpMono(const MixContext* mix_context, SoundInstance* instance, uint32_t rate, uint32_t mix_rate, float* mix_buffer, uint32_t mix_buffer_count)
    {
        const uint32_t mask = (1U << RESAMPLE_FRACTION_BITS) - 1U;
        const float range_recip = 1.0f / mask; // TODO: Divide by (1 << RESAMPLE_FRACTION_BITS) OR (1 << RESAMPLE_FRACTION_BITS) - 1?

        uint64_t frac = instance->m_FrameFraction;
        uint32_t prev_index = 0;
        uint32_t index = 0;
        uint64_t delta = (((uint64_t) rate) << RESAMPLE_FRACTION_BITS) / mix_rate;
        delta *= instance->m_Speed;

        T* frames = (T*) instance->m_Frames;

        // Typically when the buffer is less than a mix-buffer we might overfetch
        // We never overfetch for identity mixing as identity mixing is a special case
        frames[instance->m_FrameCount] = frames[instance->m_FrameCount-1];

        Ramp gain_ramp = GetRamp(mix_context, &instance->m_Gain, mix_buffer_count);
        Ramp pan_ramp = GetRamp(mix_context, &instance->m_Pan, mix_buffer_count);
        for (uint32_t i = 0; i < mix_buffer_count; i++)
        {
            float gain = gain_ramp.GetValue(i);
            float pan = pan_ramp.GetValue(i);
            float mix = frac * range_recip;
            T s1 = frames[index];
            T s2 = frames[index + 1];
            s1 = (s1 - offset) * scale;
            s2 = (s2 - offset) * scale;

            float left_scale, right_scale;
            GetPanScale(pan, &left_scale, &right_scale);

            float s = (1.0f - mix) * s1 + mix * s2;
            mix_buffer[2 * i] += s * gain * left_scale;
            mix_buffer[2 * i + 1] += s * gain * right_scale;

            prev_index = index;
            frac += delta;

            index += (uint32_t)(frac >> RESAMPLE_FRACTION_BITS);

            frac &= ((1U << RESAMPLE_FRACTION_BITS) - 1U);
        }
        instance->m_FrameFraction = frac;

        assert(prev_index <= instance->m_FrameCount);

        memmove(instance->m_Frames, (char*) instance->m_Frames + index * sizeof(T), (instance->m_FrameCount - index) * sizeof(T));
        instance->m_FrameCount -= index;
    }

    template <typename T, int offset, int scale>
    static void MixResampleUpStereo(const MixContext* mix_context, SoundInstance* instance, uint32_t rate, uint32_t mix_rate, float* mix_buffer, uint32_t mix_buffer_count)
    {
        const uint32_t mask = (1U << RESAMPLE_FRACTION_BITS) - 1U;
        const float range_recip = 1.0f / mask; // TODO: Divide by (1 << RESAMPLE_FRACTION_BITS) OR (1 << RESAMPLE_FRACTION_BITS) - 1?

        uint64_t frac = instance->m_FrameFraction;
        uint32_t prev_index = 0;
        uint32_t index = 0;
        uint64_t delta = (((uint64_t) rate) << RESAMPLE_FRACTION_BITS) / mix_rate;
        delta *= instance->m_Speed;

        T* frames = (T*) instance->m_Frames;

        // Typically when the buffer is less than a mix-buffer we might overfetch
        // We never overfetch for identity mixing as identity mixing is a special case
        frames[2 * instance->m_FrameCount] = frames[2 * instance->m_FrameCount - 2];
        frames[2 * instance->m_FrameCount + 1] = frames[2 * instance->m_FrameCount - 1];

        Ramp gain_ramp = GetRamp(mix_context, &instance->m_Gain, mix_buffer_count);
        Ramp pan_ramp = GetRamp(mix_context, &instance->m_Pan, mix_buffer_count);
        for (uint32_t i = 0; i < mix_buffer_count; i++)
        {
            float gain = gain_ramp.GetValue(i);
            float pan = pan_ramp.GetValue(i);
            float mix = frac * range_recip;
            T sl1 = frames[2 * index];
            T sl2 = frames[2 * index + 2];
            sl1 = (sl1 - offset) * scale;
            sl2 = (sl2 - offset) * scale;

            T sr1 = frames[2 * index + 1];
            T sr2 = frames[2 * index + 3];
            sr1 = (sr1 - offset) * scale;
            sr2 = (sr2 - offset) * scale;

            float left_scale, right_scale;
            GetPanScale(pan, &left_scale, &right_scale);

            float sl = (1.0f - mix) * sl1 + mix * sl2;
            float sr = (1.0f - mix) * sr1 + mix * sr2;
            mix_buffer[2 * i]       += sl * gain * left_scale;
            mix_buffer[2 * i + 1]   += sr * gain * right_scale;

            prev_index = index;
            frac += delta;
            index += (uint32_t)(frac >> RESAMPLE_FRACTION_BITS);

            frac &= ((1U << RESAMPLE_FRACTION_BITS) - 1U);
        }
        instance->m_FrameFraction = frac;

        assert(prev_index <= instance->m_FrameCount);

        memmove(instance->m_Frames, (char*) instance->m_Frames + index * sizeof(T) * 2, (instance->m_FrameCount - index) * sizeof(T) * 2);
        instance->m_FrameCount -= index;
    }

    template <typename T, int offset, int scale>
    static void MixResampleIdentityMono(const MixContext* mix_context, SoundInstance* instance, uint32_t rate, uint32_t mix_rate, float* mix_buffer, uint32_t mix_buffer_count)
    {
        (void)rate;
        (void)mix_rate;
        assert(instance->m_FrameCount == mix_buffer_count);
        T* frames = (T*) instance->m_Frames;
        Ramp gain_ramp = GetRamp(mix_context, &instance->m_Gain, mix_buffer_count);
        Ramp pan_ramp = GetRamp(mix_context, &instance->m_Pan, mix_buffer_count);

        for (uint32_t i = 0; i < mix_buffer_count; i++)
        {
            float gain = gain_ramp.GetValue(i);
            float pan = pan_ramp.GetValue(i);
            float s = frames[i];
            s = (s - offset) * scale * gain;

            float left_scale, right_scale;
            GetPanScale(pan, &left_scale, &right_scale);
            mix_buffer[2 * i]       += s * left_scale;
            mix_buffer[2 * i + 1]   += s * right_scale;
        }
        instance->m_FrameCount -= mix_buffer_count;
    }

    template <typename T, int offset, int scale>
    static void MixResampleIdentityStereo(const MixContext* mix_context, SoundInstance* instance, uint32_t rate, uint32_t mix_rate, float* mix_buffer, uint32_t mix_buffer_count)
    {
        (void)rate;
        (void)mix_rate;
        assert(instance->m_FrameCount == mix_buffer_count);
        T* frames = (T*) instance->m_Frames;
        Ramp gain_ramp = GetRamp(mix_context, &instance->m_Gain, mix_buffer_count);
        Ramp pan_ramp = GetRamp(mix_context, &instance->m_Pan, mix_buffer_count);

        for (uint32_t i = 0; i < mix_buffer_count; i++)
        {
            float gain = gain_ramp.GetValue(i);
            float pan = pan_ramp.GetValue(i);
            float s1 = frames[2 * i];
            float s2 = frames[2 * i + 1];
            s1 = (s1 - offset) * scale * gain;
            s2 = (s2 - offset) * scale * gain;

            float left_scale, right_scale;
            GetPanScale(pan, &left_scale, &right_scale);
            mix_buffer[2 * i]       += s1 * left_scale;
            mix_buffer[2 * i + 1]   += s2 * right_scale;
        }
        instance->m_FrameCount -= mix_buffer_count;
    }

    typedef void (*MixerFunction)(const MixContext* mix_context, SoundInstance* instance, uint32_t rate, uint32_t mix_rate, float* mix_buffer, uint32_t mix_buffer_count);

    struct Mixer
    {
        uint32_t        m_Channels;
        uint32_t        m_BitsPerSample;
        MixerFunction   m_Mixer;
        Mixer(uint32_t channels, uint32_t bits_per_sample, MixerFunction mixer)
        {
            m_Channels = channels;
            m_BitsPerSample = bits_per_sample;
            m_Mixer = mixer;
        }
    };

    Mixer g_Mixers[] = {
            Mixer(1, 8, MixResampleUpMono<uint8_t, 128, 255>),
            Mixer(1, 16, MixResampleUpMono<int16_t, 0, 1>),
            Mixer(2, 8, MixResampleUpStereo<uint8_t, 128, 255>),
            Mixer(2, 16, MixResampleUpStereo<int16_t, 0, 1>),
    };

    Mixer g_IdentityMixers[] = {
            Mixer(1, 8, MixResampleIdentityMono<uint8_t, 128, 255>),
            Mixer(1, 16, MixResampleIdentityMono<int16_t, 0, 1>),
            Mixer(2, 8, MixResampleIdentityStereo<uint8_t, 128, 255>),
            Mixer(2, 16, MixResampleIdentityStereo<int16_t, 0, 1>),
    };

    static void MixResample(const MixContext* mix_context, SoundInstance* instance, const dmSoundCodec::Info* info, uint32_t mix_rate, float* mix_buffer, uint32_t mix_buffer_count)
    {
        const uint32_t rate = info->m_Rate;
        assert(rate <= mix_rate);

        void (*mixer)(const MixContext* mix_context, SoundInstance* instance, uint32_t rate, uint32_t mix_rate, float* mix_buffer, uint32_t mix_buffer_count) = 0;

        bool identity_mixer = rate == mix_rate && instance->m_Speed == 1.0f;

        if (identity_mixer) {
            uint32_t n = sizeof(g_IdentityMixers) / sizeof(g_IdentityMixers[0]);
            for (uint32_t i = 0; i < n; i++) {
                const Mixer& m = g_IdentityMixers[i];
                if (m.m_BitsPerSample == info->m_BitsPerSample &&
                    m.m_Channels == info->m_Channels) {
                    mixer = m.m_Mixer;
                    break;
                }
            }
        } else {
            uint32_t n = sizeof(g_Mixers) / sizeof(g_Mixers[0]);
            for (uint32_t i = 0; i < n; i++) {
                const Mixer& m = g_Mixers[i];
                if (m.m_BitsPerSample == info->m_BitsPerSample &&
                    m.m_Channels == info->m_Channels) {
                    mixer = m.m_Mixer;
                    break;
                }
            }
        }
        mixer(mix_context, instance, rate, mix_rate, mix_buffer, mix_buffer_count);
    }

    static void Mix(const MixContext* mix_context, SoundInstance* instance, const dmSoundCodec::Info* info)
    {
        DM_PROFILE(Sound, "Mix")

        SoundSystem* sound = g_SoundSystem;
        uint64_t delta = (uint32_t) ((((uint64_t) info->m_Rate) << RESAMPLE_FRACTION_BITS) / sound->m_MixRate);
        uint32_t mix_count = ((uint64_t) (instance->m_FrameCount) << RESAMPLE_FRACTION_BITS) / (delta * instance->m_Speed);
        mix_count = dmMath::Min(mix_count, sound->m_FrameCount);
        assert(mix_count <= sound->m_FrameCount);

        int* index = sound->m_GroupMap.Get(instance->m_Group);
        if (index) {
            SoundGroup* group = &sound->m_Groups[*index];
            MixResample(mix_context, instance, info, sound->m_MixRate, group->m_MixBuffer, mix_count);
        } else {
            dmLogError("Sound group not found");
        }
    }

    static bool IsMuted(SoundInstance* instance) {
        SoundSystem* sound = g_SoundSystem;

        if (instance->m_Gain.IsZero()) {
            return true;
        }

        if (instance->m_Speed == 0.0f) {
            return true;
        }

        int* group_index = sound->m_GroupMap.Get(instance->m_Group);
        if (group_index != NULL) {
            SoundGroup* group = &sound->m_Groups[*group_index];
            if (group->m_Gain.IsZero()) {
                return true;
            }
        }

        int* master_index = sound->m_GroupMap.Get(MASTER_GROUP_HASH);
        if (master_index != NULL) {
            SoundGroup* master = &sound->m_Groups[*master_index];
            if (master->m_Gain.IsZero()) {
                return true;
            }
        }

        return false;
    }

    static void MixInstance(const MixContext* mix_context, SoundInstance* instance) {
        SoundSystem* sound = g_SoundSystem;
        uint32_t decoded = 0;

        dmSoundCodec::Info info;
        dmSoundCodec::GetInfo(sound->m_CodecContext, instance->m_Decoder, &info);
        if (info.m_BitsPerSample == 16 && info.m_Channels > SOUND_MAX_MIX_CHANNELS ) {
            dmLogError("Only mono/stereo with 16 bits per sample is supported (%s)", GetSoundName(sound, instance));
            return;
        }

        if (info.m_Rate > sound->m_MixRate) {
            dmLogError("Sounds with rate higher than sample-rate not supported (%d > %d) (%s)", info.m_Rate, sound->m_MixRate, GetSoundName(sound, instance));
            return;
        }

        bool is_muted = dmSound::IsMuted(instance);

        dmSoundCodec::Result r = dmSoundCodec::RESULT_OK;

        if (instance->m_FrameCount < sound->m_FrameCount && instance->m_Playing) {

            const uint32_t stride = info.m_Channels * (info.m_BitsPerSample / 8);
            uint32_t n = sound->m_FrameCount * dmMath::Max(1.0f, instance->m_Speed) - instance->m_FrameCount;

            if (!is_muted)
            {
                r = dmSoundCodec::Decode(sound->m_CodecContext,
                                         instance->m_Decoder,
                                         ((char*) instance->m_Frames) + instance->m_FrameCount * stride,
                                         n * stride,
                                         &decoded);
            }
            else
            {
                r = dmSoundCodec::Skip(sound->m_CodecContext, instance->m_Decoder, n * stride, &decoded);
                memset(((char*) instance->m_Frames) + instance->m_FrameCount * stride, 0x00, n * stride);
            }

            assert(decoded % stride == 0);
            instance->m_FrameCount += decoded / stride;

            if (instance->m_FrameCount < sound->m_FrameCount) {

                if (instance->m_Looping) {
                    dmSoundCodec::Reset(sound->m_CodecContext, instance->m_Decoder);

                    uint32_t n = sound->m_FrameCount - instance->m_FrameCount;
                    if (!is_muted)
                    {
                        r = dmSoundCodec::Decode(sound->m_CodecContext,
                                                 instance->m_Decoder,
                                                 ((char*) instance->m_Frames) + instance->m_FrameCount * stride,
                                                 n * stride,
                                                 &decoded);
                    }
                    else
                    {
                        r = dmSoundCodec::Skip(sound->m_CodecContext, instance->m_Decoder, n * stride, &decoded);
                        memset(((char*) instance->m_Frames) + instance->m_FrameCount * stride, 0x00, n * stride);
                    }

                    assert(decoded % stride == 0);
                    instance->m_FrameCount += decoded / stride;

                } else {
                    instance->m_EndOfStream = 1;
                }
            }
        }

        if (r != dmSoundCodec::RESULT_OK) {
            dmLogWarning("Unable to decode file '%s'. Result %d", GetSoundName(sound, instance), r);

            instance->m_Playing = 0;
            return;
        }

        if (instance->m_FrameCount > 0)
            Mix(mix_context, instance, &info);

        if (instance->m_FrameCount <= 1 && instance->m_EndOfStream) {
            // NOTE: Due to round-off errors, e.g 32000 -> 44100,
            // the last frame might be partially sampled and
            // used in the *next* buffer. We truncate such scenarios to 0
            instance->m_FrameCount = 0;
        }
    }

    static void MixInstances(const MixContext* mix_context) {
        DM_PROFILE(Sound, "MixInstances")
        SoundSystem* sound = g_SoundSystem;

        for (uint32_t i = 0; i < MAX_GROUPS; i++) {
            SoundGroup* g = &sound->m_Groups[i];

            if (g->m_MixBuffer) {
                uint32_t frame_count = sound->m_FrameCount;
                float sum_sq_left = 0;
                float sum_sq_right = 0;
                float max_sq_left = 0;
                float max_sq_right = 0;
                for (uint32_t j = 0; j < frame_count; j++) {

                    float gain = g->m_Gain.m_Current;

                    float left = g->m_MixBuffer[2 * j + 0] * gain;
                    float right = g->m_MixBuffer[2 * j + 1] * gain;
                    float left_sq = left * left;
                    float right_sq = right * right;
                    sum_sq_left += left_sq;
                    sum_sq_right += right_sq;
                    max_sq_left = dmMath::Max(max_sq_left, left_sq);
                    max_sq_right = dmMath::Max(max_sq_right, right_sq);
                }
                g->m_SumSquaredMemory[2 * g->m_NextMemorySlot + 0] = sum_sq_left;
                g->m_SumSquaredMemory[2 * g->m_NextMemorySlot + 1] = sum_sq_right;
                g->m_PeakMemorySq[2 * g->m_NextMemorySlot + 0] = max_sq_left;
                g->m_PeakMemorySq[2 * g->m_NextMemorySlot + 1] = max_sq_right;
                g->m_NextMemorySlot = (g->m_NextMemorySlot + 1) % GROUP_MEMORY_BUFFER_COUNT;

                memset(g->m_MixBuffer, 0, sound->m_FrameCount * sizeof(float) * 2);
            }
        }

        uint32_t instances = sound->m_Instances.Size();
        for (uint32_t i = 0; i < instances; ++i) {
            SoundInstance* instance = &sound->m_Instances[i];
            if (instance->m_Playing || instance->m_FrameCount > 0)
            {
                MixInstance(mix_context, instance);
            }

            if (instance->m_EndOfStream && instance->m_FrameCount == 0) {
                instance->m_Playing = 0;
            }
        }
    }

    static void Master(const MixContext* mix_context) {
        DM_PROFILE(Sound, "Master")

        SoundSystem* sound = g_SoundSystem;
        uint32_t n = sound->m_FrameCount;
        int16_t* out = sound->m_OutBuffers[sound->m_NextOutBuffer];
        int* master_index = sound->m_GroupMap.Get(MASTER_GROUP_HASH);
        SoundGroup* master = &sound->m_Groups[*master_index];
        float* mix_buffer = master->m_MixBuffer;

        if (master->m_Gain.IsZero())
        {
            memset(out, 0, n * sizeof(uint16_t) * 2);
            return;
        }

        for (uint32_t i = 0; i < MAX_GROUPS; i++) {
            SoundGroup* g = &sound->m_Groups[i];
            if (g->m_MixBuffer == 0x0)
            {
                continue;
            }
            if (g->m_NameHash == MASTER_GROUP_HASH)
            {
                continue;
            }
            if (g->m_Gain.IsZero())
            {
                continue;
            }
            Ramp ramp = GetRamp(mix_context, &g->m_Gain, n);
            for (uint32_t i = 0; i < n; i++) {
                float gain = ramp.GetValue(i);
                gain = dmMath::Clamp(gain, 0.0f, 1.0f);

                float s1 = g->m_MixBuffer[2 * i];
                float s2 = g->m_MixBuffer[2 * i + 1];
                mix_buffer[2 * i] += s1 * gain;
                mix_buffer[2 * i + 1] += s2 * gain;
            }
        }

        Ramp ramp = GetRamp(mix_context, &master->m_Gain, n);
        for (uint32_t i = 0; i < n; i++) {
            float gain = ramp.GetValue(i);
            float s1 = mix_buffer[2 * i] * gain;
            float s2 = mix_buffer[2 * i + 1] * gain;
            s1 = dmMath::Min(32767.0f, s1);
            s1 = dmMath::Max(-32768.0f, s1);
            s2 = dmMath::Min(32767.0f, s2);
            s2 = dmMath::Max(-32768.0f, s2);
            out[2 * i] = (int16_t) s1;
            out[2 * i + 1] = (int16_t) s2;
        }
    }

    static void StepGroupValues()
    {
        SoundSystem* sound = g_SoundSystem;

        for (uint32_t i = 0; i < MAX_GROUPS; i++) {
            SoundGroup* g = &sound->m_Groups[i];
            if (g->m_MixBuffer) {
                g->m_Gain.Step();
            }
        }
    }

    static void StepInstanceValues()
    {
        SoundSystem* sound = g_SoundSystem;

        uint32_t instances = sound->m_Instances.Size();
        for (uint32_t i = 0; i < instances; ++i) {
            SoundInstance* instance = &sound->m_Instances[i];
            if (instance->m_Playing || instance->m_FrameCount > 0) {
                instance->m_Gain.Step();
                instance->m_Pan.Step();
            }
        }
    }

    Result Update()
    {
        DM_PROFILE(Sound, "Update")
        SoundSystem* sound = g_SoundSystem;

        uint16_t active_instance_count = sound->m_InstancesPool.Size();

        bool currentIsPhoneCallActive = IsPhoneCallActive();
        if (!sound->m_IsPhoneCallActive && currentIsPhoneCallActive)
        {
            sound->m_IsPhoneCallActive = true;
            if (sound->m_IsDeviceStarted)
            {
                sound->m_DeviceType->m_DeviceStop(sound->m_Device);
                sound->m_IsDeviceStarted = false;
            }
        }
        else if (sound->m_IsPhoneCallActive && !currentIsPhoneCallActive)
        {
            sound->m_IsPhoneCallActive = false;
            if (active_instance_count == 0 && sound->m_IsDeviceStarted == false)
            {
                return RESULT_NOTHING_TO_PLAY;
            }
        }

        if (sound->m_IsPhoneCallActive)
        {
            // We can't play sounds when the phone is active.
            return RESULT_OK;
        }

        if (active_instance_count == 0 && sound->m_IsDeviceStarted == false)
        {
            return RESULT_NOTHING_TO_PLAY;
        }

        if (active_instance_count == 0)
        {
            #if defined(ANDROID)
            if (sound->m_IsDeviceStarted)
            {
                // DEF-3512 Wait with stopping the device until all our queued buffers have played, if any queued buffers are
                // still playing we will get the wrong result in isMusicPlaying on android if we release audio focus to soon
                // since it detects our buffered sounds as "other application".
                uint32_t free_slots = sound->m_DeviceType->m_FreeBufferSlots(sound->m_Device);
                if (free_slots == SOUND_OUTBUFFER_COUNT)
                {
                    sound->m_DeviceType->m_DeviceStop(sound->m_Device);
                    sound->m_IsDeviceStarted = false;
                }
            }
            #endif
            return RESULT_NOTHING_TO_PLAY;
        }
        // DEF-3130 Don't start the device unless something is being played
        // This allows the client to check for sound.is_music_playing() and mute sounds accordingly

        if (sound->m_IsDeviceStarted == false)
        {
            sound->m_DeviceType->m_DeviceStart(sound->m_Device);
            sound->m_IsDeviceStarted = true;
        }

        uint32_t free_slots = sound->m_DeviceType->m_FreeBufferSlots(sound->m_Device);
        if (free_slots > 0) {
            StepGroupValues();
            StepInstanceValues();
        }

        uint32_t current_buffer = 0;
        uint32_t total_buffers = free_slots;
        while (free_slots > 0) {
            MixContext mix_context(current_buffer, total_buffers);
            MixInstances(&mix_context);

            Master(&mix_context);

            // DEF-2540: Make sure to keep feeding the sound device if audio is being generated,
            // if you don't you'll get more slots free, thus updating sound (redundantly) every call,
            // resulting in a huge performance hit. Also, you'll fast forward the sounds.
            sound->m_DeviceType->m_Queue(sound->m_Device, (const int16_t*) sound->m_OutBuffers[sound->m_NextOutBuffer], sound->m_FrameCount);

            sound->m_NextOutBuffer = (sound->m_NextOutBuffer + 1) % SOUND_OUTBUFFER_COUNT;
            current_buffer++;
            free_slots--;
        }

        return RESULT_OK;
    }

    bool IsMusicPlaying()
    {
        return PlatformIsMusicPlaying(g_SoundSystem->m_IsDeviceStarted, g_SoundSystem->m_HasWindowFocus);
    }

    bool IsPhoneCallActive()
    {
        return PlatformIsPhoneCallActive();
    }

    void OnWindowFocus(bool focus)
    {
        SoundSystem* sound = g_SoundSystem;
        if (sound) {
            sound->m_HasWindowFocus = focus;
        }
    }

}


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <functional>
#include <unordered_map>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/dynamic_spatial_hashed_voxel_grid.hpp>
#include <sdf_tools/collision_map.hpp>

#ifndef DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP
#define DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP

namespace sdf_tools
{

    class DynamicSpatialHashedCollisionMapGrid
    {
    protected:

        VoxelGrid::DynamicSpatialHashedVoxelGrid<COLLISION_CELL> collision_field_;
        uint32_t number_of_components_;
        std::string frame_;
        bool initialized_;
        bool components_valid_;

    public:

        DynamicSpatialHashedCollisionMapGrid(std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value);

        DynamicSpatialHashedCollisionMapGrid(Eigen::Affine3d origin_transform, std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value);

        DynamicSpatialHashedCollisionMapGrid() : number_of_components_(0), initialized_(false), components_valid_(false) {}

        inline bool IsInitialized() const
        {
            return initialized_;
        }

        inline bool AreComponentsValid() const
        {
            return components_valid_;
        }

        inline std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> Get(const double x, const double y, const double z) const
        {
            return collision_field_.GetImmutable(x, y, z);
        }

        inline std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> Get(const Eigen::Vector3d& location) const
        {
            return collision_field_.GetImmutable(location);
        }

        inline VoxelGrid::SET_STATUS SetCell(const double x, const double y, const double z, COLLISION_CELL value)
        {
            return collision_field_.SetCellValue(x, y, z, value);
        }

        inline VoxelGrid::SET_STATUS SetCell(const Eigen::Vector3d& location, COLLISION_CELL value)
        {
            return collision_field_.SetCellValue(location, value);
        }

        inline VoxelGrid::SET_STATUS SetChunk(const double x, const double y, const double z, COLLISION_CELL value)
        {
            return collision_field_.SetChunkValue(x, y, z, value);
        }

        inline VoxelGrid::SET_STATUS SetChunk(const Eigen::Vector3d& location, COLLISION_CELL value)
        {
            return collision_field_.SetChunkValue(location, value);
        }

        inline Eigen::Affine3d GetOriginTransform() const
        {
            return collision_field_.GetOriginTransform();
        }

        std::vector<visualization_msgs::Marker> ExportForDisplay(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const;
    };

}

#endif // DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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 Autonomous Systems Lab, ETH Zurich 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 Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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 <iostream>

#include <Eigen/Core>

#include <gtest/gtest.h>

#include "kindr/poses/PoseDiff.hpp"
#include "kindr/poses/PoseDiffBase.hpp"
#include "kindr/phys_quant/PhysicalQuantities.hpp"

namespace pose = kindr;
namespace pos = kindr;
namespace rot = kindr;

typedef ::testing::Types<
    pose::TwistLinearVelocityRotationQuaternionDiffD,
    pose::TwistLinearVelocityRotationQuaternionDiffF
> Types;

template <typename PoseImplementation>
struct TwistTest: public ::testing::Test {
  typedef PoseImplementation PoseDiff;
};

TYPED_TEST_CASE(TwistTest, Types);

TYPED_TEST(TwistTest, testConstructor)
{

}

TYPED_TEST(TwistTest, testInitial)
{
  typedef typename TestFixture::PoseDiff PoseDiff;
  typedef typename TestFixture::PoseDiff::Scalar Scalar;
  typedef rot::RotationQuaternion<Scalar> Rotation;

  typename PoseDiff::PositionDiff linearVelocity(0.1, 0.2, 0.3);
  rot::LocalAngularVelocityD angularVelocity;
  typename PoseDiff::RotationDiff rquatDiff(1, 2, 3, 4);
  PoseDiff twist(linearVelocity, rquatDiff);
  std::cout << twist << std::endl;

  Rotation rot;
  std::cout << twist.getVector(rot) << std::endl;
}

TYPED_TEST(TwistTest, testSetZero)
{
  typedef typename TestFixture::PoseDiff PoseDiff;

  PoseDiff twist(typename PoseDiff::PositionDiff(0.1, 0.2, 0.3), typename PoseDiff::RotationDiff(1, 2, 3, 4));

  twist.setZero();

  ASSERT_EQ(twist.getTranslationalVelocity().vector().x(), 0);
  ASSERT_EQ(twist.getTranslationalVelocity().vector().y(), 0);
  ASSERT_EQ(twist.getTranslationalVelocity().vector().z(), 0);

  ASSERT_EQ(twist.getRotationalVelocity().w(), 0);
  ASSERT_EQ(twist.getRotationalVelocity().x(), 0);
  ASSERT_EQ(twist.getRotationalVelocity().y(), 0);
  ASSERT_EQ(twist.getRotationalVelocity().z(), 0);
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IFNOT_DEC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IFNOT_DEC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object conditionally returns  @c x (respectively @c x-1)
    if @c c is @ref True (respectively  @ref False)

    @par Header <boost/simd/function/ifnot_dec.hpp>

    @par Example:

      @snippet ifnot_dec.cpp ifnot_dec

    @par Possible output:

      @snippet ifnot_dec.txt ifnot_dec

  **/
  Value1 ifnot_dec(Value0 const& c, Value1 const& x);
} }
#endif

#include <boost/simd/function/scalar/ifnot_dec.hpp>
#include <boost/simd/function/simd/ifnot_dec.hpp>

#endif


#ifndef BART_SRC_DOMAIN_DOMAIN_I_HPP_
#define BART_SRC_DOMAIN_DOMAIN_I_HPP_

#include <vector>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/vector.h>

#include "domain/domain_types.hpp"
#include "problem/parameter_types.hpp"
#include "system/system_types.h"
#include "utility/has_description.h"

//! Components of the finite-element domain
namespace bart::domain {

/*! \brief Defines a domain that couples a cartesian mesh with a finite element basis.
 *
 * This class provides the framework on which the domain can be solved. The
 * purpose of the coupling is to distribute the finite element basis functions
 * and needed degrees of freedom throughout the provided mesh. Most of this is
 * done under the hood by a dealii object called the DofHandler. This class
 * is a mediator for all the required interactions between Dealii objects. It
 * maintains ownership of the mesh and finite element basis through unique pointers.
 *
 * The class has two major functions: (1) it acts as a wrapper for the necessary
 * dealii classes to couple a finite element basis to a mesh, and (2) provides
 * matrices based on this coupling. The functions SetUpDOF() and SetUpMesh() are
 * required for initializing the underlying objects.
 *
 * Many portions of this class are adapted from the BARTDriver class written by
 * Weixiong Zheng.
 *
 * \author Joshua Rehak
 * \date 2019/02
 */
template <int dim>
class DomainI : public utility::HasDescription {
 public:
  using CellRange = std::vector<domain::CellPtr<dim>>;

  virtual ~DomainI() = default;

  /*! Set up the DOF handler, to access sparsity patterns, etc */
  virtual auto SetUpDOF() -> DomainI<dim>& = 0;

  /*! \brief Fills triangulation with mesh defined in MeshI object
   * Creates mesh shape, sets up boundary ids and material ids. Requires that
   * the mesh has a material mapping setup.
   */
  virtual auto SetUpMesh() -> DomainI<dim>& = 0;

  /*! \brief Fills triangulation with mesh defined in MeshI object.
   *
   * Creates mesh shape, sets up boundary ids and material ids. Requires that
   * the mesh has a material mapping setup. Also performs global refinements.
   *
   * @param global_refinements number of global refinements to perform
   */
  virtual auto SetUpMesh(const int global_refinements) -> DomainI<dim>& = 0;

  /*! Get a matrix suitible for a cell matrix.
   *
   * \return a dealii FullMatrix<double> of appropriate size.
   */
  virtual auto GetCellMatrix() const -> dealii::FullMatrix<double> = 0;
  /*! Get a matrix suitible for a cell rhs.
   *
   * \return a dealii Vector<double> of appropriate size.
   */
  virtual auto GetCellVector() const -> dealii::Vector<double> = 0;

  /*! Get an MPI matrix suitable for the system */
  virtual auto MakeSystemMatrix() const -> std::shared_ptr<bart::system::MPISparseMatrix> = 0;

  /*! Get an MPI vector suitable for the system */
  virtual auto MakeSystemVector() const -> std::shared_ptr<bart::system::MPIVector> = 0;

  /*! Get a range of all cells to allow iterating over them */
  virtual auto Cells() const -> CellRange = 0;

  /*! Get discretization type */
  virtual auto discretization_type() const -> problem::DiscretizationType = 0;

  /*! Get locally owned degrees of freedom */
  virtual auto locally_owned_dofs() const -> dealii::IndexSet = 0;

  /*! Get internal DOF object */
  virtual auto dof_handler() const -> const dealii::DoFHandler<dim>& = 0;

  /*! Get total degrees of freedom */
  virtual auto total_degrees_of_freedom() const -> int = 0;
};

} // namespace bart::domain

#endif // BART_SRC_DOMAIN_DOMAIN_I_HPP_


// Copyright Yangqing Jia 2013
// pycaffe provides a wrapper of the caffe::Net class as well as some
// caffe::Caffe functions so that one could easily call it from Python.
// Note that for python, we will simply use float as the data type.

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION

#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <numpy/arrayobject.h>
#include "caffe/caffe.hpp"

// Temporary solution for numpy < 1.7 versions: old macro, no promises.
// You're strongly advised to upgrade to >= 1.7.
#ifndef NPY_ARRAY_C_CONTIGUOUS
#define NPY_ARRAY_C_CONTIGUOUS NPY_C_CONTIGUOUS
#define PyArray_SetBaseObject(arr, x) (PyArray_BASE(arr) = (x))
#endif

using namespace caffe;
using boost::python::extract;
using boost::python::len;
using boost::python::list;
using boost::python::object;
using boost::python::handle;
using boost::python::vector_indexing_suite;

// wrap shared_ptr<Blob<float> > in a class that we construct in C++ and pass
//  to Python
class CaffeBlob {
 public:

  CaffeBlob(const shared_ptr<Blob<float> > &blob)
      : blob_(blob) {}

  CaffeBlob()
  {}

  int num() const { return blob_->num(); }
  int channels() const { return blob_->channels(); }
  int height() const { return blob_->height(); }
  int width() const { return blob_->width(); }
  int count() const { return blob_->count(); }

  bool operator == (const CaffeBlob &other)
  {
      return this->blob_ == other.blob_;
  }

 protected:
  shared_ptr<Blob<float> > blob_;
};

// we need another wrapper (used as boost::python's HeldType) that receives a
//  self PyObject * which we can use as ndarray.base, so that data/diff memory
//  is not freed while still being used in Python
class CaffeBlobWrap : public CaffeBlob {
 public:
  CaffeBlobWrap(PyObject *p, shared_ptr<Blob<float> > &blob)
      : CaffeBlob(blob), self_(p) {}

  CaffeBlobWrap(PyObject *p, const CaffeBlob &blob)
      : CaffeBlob(blob), self_(p) {}

  object get_data()
  {
      npy_intp dims[] = {num(), channels(), height(), width()};

      PyObject *obj = PyArray_SimpleNewFromData(4, dims, NPY_FLOAT32,
                                                blob_->mutable_cpu_data());
      PyArray_SetBaseObject(reinterpret_cast<PyArrayObject *>(obj), self_);
      Py_INCREF(self_);
      handle<> h(obj);

      return object(h);
  }

  object get_diff()
  {
      npy_intp dims[] = {num(), channels(), height(), width()};

      PyObject *obj = PyArray_SimpleNewFromData(4, dims, NPY_FLOAT32,
                                                blob_->mutable_cpu_diff());
      PyArray_SetBaseObject(reinterpret_cast<PyArrayObject *>(obj), self_);
      Py_INCREF(self_);
      handle<> h(obj);

      return object(h);
  }

 private:
  PyObject *self_;
};

// A simple wrapper over CaffeNet that runs the forward process.
struct CaffeNet
{
  CaffeNet(string param_file, string pretrained_param_file) {
    net_.reset(new Net<float>(param_file));
    net_->CopyTrainedLayersFrom(pretrained_param_file);
  }

  virtual ~CaffeNet() {}

  inline void check_array_against_blob(
      PyArrayObject* arr, Blob<float>* blob) {
    CHECK(PyArray_FLAGS(arr) & NPY_ARRAY_C_CONTIGUOUS);
    CHECK_EQ(PyArray_NDIM(arr), 4);
    CHECK_EQ(PyArray_ITEMSIZE(arr), 4);
    npy_intp* dims = PyArray_DIMS(arr);
    CHECK_EQ(dims[0], blob->num());
    CHECK_EQ(dims[1], blob->channels());
    CHECK_EQ(dims[2], blob->height());
    CHECK_EQ(dims[3], blob->width());
  }

  // The actual forward function. It takes in a python list of numpy arrays as
  // input and a python list of numpy arrays as output. The input and output
  // should all have correct shapes, are single-precisionabcdnt- and c contiguous.
  void Forward(list bottom, list top) {
    vector<Blob<float>*>& input_blobs = net_->input_blobs();
    CHECK_EQ(len(bottom), input_blobs.size());
    CHECK_EQ(len(top), net_->num_outputs());
    // First, copy the input
    for (int i = 0; i < input_blobs.size(); ++i) {
      object elem = bottom[i];
      PyArrayObject* arr = reinterpret_cast<PyArrayObject*>(elem.ptr());
      check_array_against_blob(arr, input_blobs[i]);
      switch (Caffe::mode()) {
      case Caffe::CPU:
        memcpy(input_blobs[i]->mutable_cpu_data(), PyArray_DATA(arr),
            sizeof(float) * input_blobs[i]->count());
        break;
      case Caffe::GPU:
        cudaMemcpy(input_blobs[i]->mutable_gpu_data(), PyArray_DATA(arr),
            sizeof(float) * input_blobs[i]->count(), cudaMemcpyHostToDevice);
        break;
      default:
        LOG(FATAL) << "Unknown Caffe mode.";
      }  // switch (Caffe::mode())
    }
    //LOG(INFO) << "Start";
    const vector<Blob<float>*>& output_blobs = net_->ForwardPrefilled();
    //LOG(INFO) << "End";
    for (int i = 0; i < output_blobs.size(); ++i) {
      object elem = top[i];
      PyArrayObject* arr = reinterpret_cast<PyArrayObject*>(elem.ptr());
      check_array_against_blob(arr, output_blobs[i]);
      switch (Caffe::mode()) {
      case Caffe::CPU:
        memcpy(PyArray_DATA(arr), output_blobs[i]->cpu_data(),
            sizeof(float) * output_blobs[i]->count());
        break;
      case Caffe::GPU:
        cudaMemcpy(PyArray_DATA(arr), output_blobs[i]->gpu_data(),
            sizeof(float) * output_blobs[i]->count(), cudaMemcpyDeviceToHost);
        break;
      default:
        LOG(FATAL) << "Unknown Caffe mode.";
      }  // switch (Caffe::mode())
    }
  }

  void Backward(list top_diff, list bottom_diff) {
    vector<Blob<float>*>& output_blobs = net_->output_blobs();
    vector<Blob<float>*>& input_blobs = net_->input_blobs();
    CHECK_EQ(len(bottom_diff), input_blobs.size());
    CHECK_EQ(len(top_diff), output_blobs.size());
    // First, copy the output diff
    for (int i = 0; i < output_blobs.size(); ++i) {
      object elem = top_diff[i];
      PyArrayObject* arr = reinterpret_cast<PyArrayObject*>(elem.ptr());
      check_array_against_blob(arr, output_blobs[i]);
      switch (Caffe::mode()) {
      case Caffe::CPU:
        memcpy(output_blobs[i]->mutable_cpu_diff(), PyArray_DATA(arr),
            sizeof(float) * output_blobs[i]->count());
        break;
      case Caffe::GPU:
        cudaMemcpy(output_blobs[i]->mutable_gpu_diff(), PyArray_DATA(arr),
            sizeof(float) * output_blobs[i]->count(), cudaMemcpyHostToDevice);
        break;
      default:
        LOG(FATAL) << "Unknown Caffe mode.";
      }  // switch (Caffe::mode())
    }
    //LOG(INFO) << "Start";
    net_->Backward();
    //LOG(INFO) << "End";
    for (int i = 0; i < input_blobs.size(); ++i) {
      object elem = bottom_diff[i];
      PyArrayObject* arr = reinterpret_cast<PyArrayObject*>(elem.ptr());
      check_array_against_blob(arr, input_blobs[i]);
      switch (Caffe::mode()) {
      case Caffe::CPU:
        memcpy(PyArray_DATA(arr), input_blobs[i]->cpu_diff(),
            sizeof(float) * input_blobs[i]->count());
        break;
      case Caffe::GPU:
        cudaMemcpy(PyArray_DATA(arr), input_blobs[i]->gpu_diff(),
            sizeof(float) * input_blobs[i]->count(), cudaMemcpyDeviceToHost);
        break;
      default:
        LOG(FATAL) << "Unknown Caffe mode.";
      }  // switch (Caffe::mode())
    }
  }

  // The caffe::Caffe utility functions.
  void set_mode_cpu() { Caffe::set_mode(Caffe::CPU); }
  void set_mode_gpu() { Caffe::set_mode(Caffe::GPU); }
  void set_phase_train() { Caffe::set_phase(Caffe::TRAIN); }
  void set_phase_test() { Caffe::set_phase(Caffe::TEST); }
  void set_device(int device_id) { Caffe::SetDevice(device_id); }

  vector<CaffeBlob> blobs() {
      return vector<CaffeBlob>(net_->blobs().begin(), net_->blobs().end());
  }

  vector<CaffeBlob> params() {
      return vector<CaffeBlob>(net_->params().begin(), net_->params().end());
  }

  // The pointer to the internal caffe::Net instant.
	shared_ptr<Net<float> > net_;
};

// The boost python module definition.
BOOST_PYTHON_MODULE(pycaffe)
{

  boost::python::class_<CaffeNet>(
      "CaffeNet", boost::python::init<string, string>())
      .def("Forward",         &CaffeNet::Forward)
      .def("Backward",        &CaffeNet::Backward)
      .def("set_mode_cpu",    &CaffeNet::set_mode_cpu)
      .def("set_mode_gpu",    &CaffeNet::set_mode_gpu)
      .def("set_phase_train", &CaffeNet::set_phase_train)
      .def("set_phase_test",  &CaffeNet::set_phase_test)
      .def("set_device",      &CaffeNet::set_device)
      .def("blobs",           &CaffeNet::blobs)
      .def("params",          &CaffeNet::params)
  ;

  boost::python::class_<CaffeBlob, CaffeBlobWrap>(
      "CaffeBlob", boost::python::no_init)
      .add_property("num",      &CaffeBlob::num)
      .add_property("channels", &CaffeBlob::channels)
      .add_property("height",   &CaffeBlob::height)
      .add_property("width",    &CaffeBlob::width)
      .add_property("count",    &CaffeBlob::count)
      .add_property("data",     &CaffeBlobWrap::get_data)
      .add_property("diff",     &CaffeBlobWrap::get_diff)
  ;

  boost::python::class_<vector<CaffeBlob> >("BlobVec")
      .def(vector_indexing_suite<vector<CaffeBlob>, true>());

  import_array();

}


#include <boost/icl/predicates/sub_super_set.hpp>


//| This file is a part of the sferes2 framework.
//| Copyright 2009, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software.  You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.

#ifndef BEST_FIT_MAP_IMAGE_
#define BEST_FIT_MAP_IMAGE_

#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/nvp.hpp>
#include "image_stat.hpp"
#include <sferes/fit/fitness.hpp>
#include <boost/lexical_cast.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>

// Headers specifics to the computations we need
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/max.hpp>

#include <exp/images/util/median.hpp>
#include <boost/filesystem.hpp>

namespace sferes {
  namespace stat {
    // assume that the population is sorted !
    SFERES_STAT(BestFitMapImage, ImageStat){

    public:
    	typedef boost::shared_ptr<Phen> indiv_t;
    	typedef std::vector<indiv_t> pop_t;

      template<typename E>
      void refresh(const E& ea) {
        assert(!ea.pop().empty());

        // Create the log file
        this->_create_log_file(ea, "bestfit.dat");

        if (ea.dump_enabled() && ( ea.gen() % Params::pop::dump_period == 0 ))
        {
        	// The data for which we wish to calculate median as the boost accumulator does not.
        	std::vector< double > data;

        	// Calculate all stats: mean, max, min
        	boost::accumulators::accumulator_set<double, boost::accumulators::stats<
        	boost::accumulators::tag::mean,
        	boost::accumulators::tag::max,
        	boost::accumulators::tag::min
        	> > stats;

        	int best_x = 0;
        	int best_y = 0;
        	float best_fitness = 0.0f;

        	// Iterate through the map of phenotypes in MAP-Elites
        	for (int x = 0; x < Params::ea::res_x; ++x)
					{
						for (int y = 0; y < Params::ea::res_y; ++y)
						{
							indiv_t i = ea.archive()[x][y];

							float fitness = i->fit().value(y);		// Get the fitness of individual
							stats(fitness);												// Add it to the stats accumulator
							data.push_back(fitness);							// Add it to the list for calculating median later

							// Get the best individual by fitness
							if (fitness > best_fitness)
							{
								best_x = x;		// Record the best x and y
								best_y = y;
								best_fitness = fitness;
							}
						}
					}

        	// Best individual in the current pop
        	_best = ea.archive()[best_x][best_y];

        	assert(data.size() == Params::ea::res_y);	// Make sure the list of values is of correct size
        	double median = sferes::util::Median::calculate_median(data); // Get the mdian

        	// Dump best_fit.dat file
          (*this->_log_file) << ea.gen()
          		<< " " << median
          		<< " " << boost::accumulators::mean(stats)
          		<< " " << boost::accumulators::max(stats)
          		<< " " << ea.jumps()
          		<< " " << boost::accumulators::min(stats)
          		<< std::endl;

          // Dump best image
					if (Params::log::best_image)
					{
						std::string image_fitness = boost::lexical_cast<std::string>(best_fitness);
						std::string image_gen = boost::lexical_cast<std::string>(ea.gen());
						std::string image_file = ea.res_dir() + "/" + image_gen + "_" + image_fitness;

						_best->log_best_image_fitness(image_file);
					}

        }
      }
      void show(std::ostream& os, size_t k) {
        _best->develop();
        _best->show(os);
        _best->fit().set_mode(fit::mode::view);
        _best->fit().eval(*_best);
      }

      const boost::shared_ptr<Phen> best() const {
        return _best;
      }

      template<class Archive>
			void serialize(Archive& ar, const unsigned int version)
			{
				ar & BOOST_SERIALIZATION_NVP(_best);
			}

    protected:
      indiv_t _best;

    };
  }
}
#endif


/*
 * Copyright (c) 2013 Steffen Kieß
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#ifndef CT_DATAFILES_HPP
#define CT_DATAFILES_HPP

#include <Math/Vector3.hpp>
#include <Math/DiagMatrix3.hpp>
#include <Math/Float.hpp>

#include <HDF5/Matlab.hpp>
#include <HDF5/MatlabVector2.hpp>
#include <HDF5/MatlabVector3.hpp>
#include <HDF5/MatlabDiagMatrix3.hpp>
#include <HDF5/DelayedArray.hpp>
#include <HDF5/Array.hpp>

#include <boost/scoped_ptr.hpp>

template <typename T, bool delayLoading = false>
struct VolumeGen;

template <typename T, bool delayLoading>
struct VolumeGen {
  // TODO: make data type dynamic?
  typedef typename boost::mpl::if_c<delayLoading, HDF5::DelayedArray<T, 3>, Math::Array<T, 3> >::type ArrayType;

  boost::optional<std::string> Type; // = "Volume"

  // Distance between two points in X/Y/Z-direction, in m
  boost::optional<Math::DiagMatrix3<ldouble> > GridSpacing;

  // Position of the first grid cell in m
  boost::optional<Math::Vector3<ldouble> > GridOrigin;

  // On loading, all values are multiplied by this value (defaults to 1)
  boost::optional<ldouble> VolumeScalingFactor;

  // VolumeStorageOrder shows how the dimensions of the data are stored.
  // The first value shows where the X dimension is stored:
  //   1: Stored in the first HDF5 dimension (i.e. the one with the largest stride)
  //  -1: Stored in the first HDF5 dimension, but mirrored
  //   2: Stored in the second HDF5 dimension
  //  -2: Stored in the second HDF5 dimension, but mirrored
  //   3: Stored in the third HDF5 dimension (i.e. the one with the smallest stride)
  //  -3: Stored in the third HDF5 dimension, but mirrored
  // The second value contains the same information for the Y dimension, the third value for the Z dimension
  // The default is "3 2 1"
  boost::optional<std::vector<int32_t> > VolumeStorageOrder;

  ArrayType Volume;

  Math::ArrayView<const T, 3> transposeVolume (const Math::ArrayView<const T, 3>& view) const {
    if (VolumeStorageOrder) {
      std::vector<int32_t> swapDim13;
      swapDim13.push_back (3);
      swapDim13.push_back (2);
      swapDim13.push_back (1);
      return Math::reorderDimensions (Math::reorderDimensions (view, swapDim13), *VolumeStorageOrder);
    } else {
      return view;
    }
  }

  void scaleValues (const Math::ArrayView<T, 3>& values) const {
    if (!VolumeScalingFactor)
      return;
    T factor = (T) *VolumeScalingFactor;
    for (size_t z = 0; z < values.template size<2> (); z++)
      for (size_t y = 0; y < values.template size<1> (); y++)
        for (size_t x = 0; x < values.template size<0> (); x++)
          values (x, y, z) *= factor;
  }

#define MEMBERS(m)                              \
  m (Type)                                      \
  m (GridSpacing)                               \
  m (GridOrigin)                                \
  m (VolumeScalingFactor)                       \
  m (VolumeStorageOrder)                        \
  m (Volume)
  HDF5_MATLAB_DECLARE_TYPE (VolumeGen, MEMBERS)
#undef MEMBERS
};

template <typename T>
Math::ArrayView<const T, 3> transposedUnscaledVolume (const VolumeGen<T>& self) {
  return self.transposeVolume (self.Volume.view ());
}
namespace DataFilesIntern {
  struct NoopDeallocator {
    template <typename U>
    void operator() (UNUSED const U& val) {
    }
  };
}
// Rather slow (will transpose volume in memory)
template <typename T>
std::shared_ptr<const Math::Array<T, 3> > transformedTransposedVolume (const VolumeGen<T>& self, bool forceCopy = false) {
  if (forceCopy || self.VolumeScalingFactor || self.VolumeStorageOrder) {
    std::shared_ptr<Math::Array<T, 3> > copyPtr = std::make_shared<Math::Array<T, 3> > (transposedUnscaledVolume (self));
    scaleValues (copyPtr->view ());
    return copyPtr;
  } else {
    return std::shared_ptr<const Math::Array<T, 3> > (&self.Volume, DataFilesIntern::NoopDeallocator ());
  }
}
template <typename T>
std::shared_ptr<const Math::ArrayView<const T, 3> > transformedVolume (const VolumeGen<T>& self, bool forceCopy = false) {
  if (forceCopy || self.VolumeScalingFactor) {
    //std::shared_ptr<Math::Array<T, 3> > copyPtr = std::make_shared<Math::Array<T, 3> > (Volume);
    std::shared_ptr<std::pair<Math::Array<T, 3>, boost::scoped_ptr<Math::ArrayView<const T, 3> > > > copyPtr = std::make_shared<std::pair<Math::Array<T, 3>, boost::scoped_ptr<Math::ArrayView<const T, 3> > > > ();
    copyPtr->first.recreate (self.Volume);
    self.scaleValues (copyPtr->first);
    copyPtr->second.reset (new Math::ArrayView<const T, 3> (self.transposeVolume (copyPtr->first.view ())));
    //return std::shared_ptr<const Math::ArrayView<const T, 3> > (copyPtr, &copyPtr->view ()); // Only works if Array::view() returns a reference
    return std::shared_ptr<const Math::ArrayView<const T, 3> > (copyPtr, copyPtr->second.get ());
  } else {
    return std::make_shared<const Math::ArrayView<const T, 3> > (transposedUnscaledVolume (self));
  }
}

template <typename T>
std::shared_ptr<VolumeGen<T> > load (const VolumeGen<T, true>& self) {
  std::shared_ptr<VolumeGen<T> > data = std::make_shared<VolumeGen<T> > ();
  data->Type = self.Type;
  data->GridSpacing = self.GridSpacing;
  data->GridOrigin = self.GridOrigin;
  data->VolumeScalingFactor = self.VolumeScalingFactor;
  data->VolumeStorageOrder = self.VolumeStorageOrder;
  data->Volume.recreate (self.Volume.size);
  self.Volume.read (data->Volume.view ());
  return data;
}
template <typename T>
Math::Vector3<size_t> getSize (const VolumeGen<T, true>& self) {
  Math::Vector3<size_t> s (self.Volume.size[0], self.Volume.size[1], self.Volume.size[2]);
  Math::Vector3<size_t> s2;
  if (!self.VolumeStorageOrder) {
    s2 = s;
  } else {
    for (size_t i = 0; i < 3; i++) {
      int32_t val = (*self.VolumeStorageOrder)[i];
      if (val < 0)
        val = -val;
      ASSERT (val > 0 && val <= 3);
      val = val - 1;
      val = 2 - val;
      s2[i] = s[val];
    }
  }
  return s2;
}
template <typename BufferType, typename T, typename Callback>
void loadAndTransformTo (const VolumeGen<T, true>& self, const Math::ArrayView<T, 3>& view, Callback callback) {
  static const bool assertions = false;

  std::vector<int32_t> swapDim13;
  swapDim13.push_back (3);
  swapDim13.push_back (2);
  swapDim13.push_back (1);

  Math::ArrayView<T, 3, false, Math::ArrayConfig, Math::ArrayAssertions<assertions> > transformedView = Math::reorderDimensions (view, self.VolumeStorageOrder ? *self.VolumeStorageOrder : swapDim13, true);

  std::size_t sizeX = transformedView.template size<0> ();
  std::size_t sizeY = transformedView.template size<1> ();
  std::size_t sizeZ = transformedView.template size<2> ();

  ASSERT (sizeX == self.Volume.size[2]);
  ASSERT (sizeY == self.Volume.size[1]);
  ASSERT (sizeZ == self.Volume.size[0]);

  bool useDirectLoad =
    !self.VolumeScalingFactor
    && transformedView.template strideBytes<0> () == (ptrdiff_t) (sizeof (T) * self.Volume.size[0] * self.Volume.size[1])
    && transformedView.template strideBytes<1> () == (ptrdiff_t) (sizeof (T) * self.Volume.size[0])
    && transformedView.template strideBytes<2> () == (ptrdiff_t) (sizeof (T));
  useDirectLoad = false; // Disable direct loads because of HDFFV-9634
  //QDateTime start = QDateTime::currentDateTimeUtc ();
  if (useDirectLoad) {
    //std::cerr << "DIRECT LOAD" << std::endl;
    self.Volume.read (Math::reorderDimensions (transformedView, swapDim13));
  } else {
    //std::cerr << "BUFFERED LOAD" << std::endl;
    std::size_t sliceSizeBytes = sizeZ * sizeY * sizeof (T);
    std::size_t maxBufferSize = 1 * 1024 * 1024; // 1MB
    std::size_t slices = maxBufferSize / sliceSizeBytes;
    if (slices == 0)
      slices = 1;
    std::size_t bufferShape[3] = { slices, sizeY, sizeZ };
    Math::Array<BufferType, 3> buffer (bufferShape, false);
    Math::ArrayView<BufferType, 3, false, Math::ArrayConfig, Math::ArrayAssertions<assertions> > bufferView (buffer.view ());
    HDF5::DataSpace memDataSpace = HDF5::DataSpace::createSimple (slices, sizeY, sizeZ);
    HDF5::DataSpace fileDataSpace = HDF5::DataSpace::createSimple (sizeX, sizeY, sizeZ);

    BufferType scalingFactor = self.VolumeScalingFactor ? *self.VolumeScalingFactor : 1;

    for (std::size_t firstSlice = 0; firstSlice < sizeX; firstSlice += slices) {
      callback(firstSlice, sizeX);
      std::size_t slicesCur = slices;
      if (firstSlice + slices > sizeX) {
        slicesCur = sizeX - firstSlice;
        memDataSpace = HDF5::DataSpace::createSimple (slicesCur, sizeY, sizeZ);
      }

      hsize_t start[3] = { firstSlice, 0, 0 };
      hsize_t count[3] = { slicesCur, sizeY, sizeZ };
      fileDataSpace.selectHyperslab (H5S_SELECT_SET, count, start);

      self.Volume.dataSet.read (buffer.data (), HDF5::getH5Type<BufferType> (), memDataSpace, fileDataSpace);

      if (self.VolumeScalingFactor) {
        for (std::size_t x = 0; x < slicesCur; x++)
          for (std::size_t y = 0; y < sizeY; y++)
            for (std::size_t z = 0; z < sizeZ; z++)
              transformedView (firstSlice + x, y, z) = bufferView (x, y, z) * scalingFactor;
      } else {
        for (std::size_t x = 0; x < slicesCur; x++)
          for (std::size_t y = 0; y < sizeY; y++)
            for (std::size_t z = 0; z < sizeZ; z++)
              transformedView (firstSlice + x, y, z) = bufferView (x, y, z);
      }
    }
    callback(sizeX, sizeX);
  }
  //QDateTime end = QDateTime::currentDateTimeUtc ();
  //qDebug() << (end.toMSecsSinceEpoch() - start.toMSecsSinceEpoch());
}
static inline void callbackDoNothing(UNUSED size_t pos, UNUSED size_t count) {}
template <typename BufferType, typename T>
void loadAndTransformTo (const VolumeGen<T, true>& self, const Math::ArrayView<T, 3>& view) {
  loadAndTransformTo<BufferType, T>(self, view, callbackDoNothing);
}

#endif // !CT_DATAFILES_HPP


/**
 * @file   Keypoint.hpp
 * @author Paul Furgale <paul.furgale@gmail.com>
 * @date   Sat Nov 24 19:47:51 2012
 *
 * @brief  A class template for a single image point
 *
 *
 */

#ifndef ASLAM_KEYPOINT_HPP
#define ASLAM_KEYPOINT_HPP

#include <Eigen/Core>
#include <aslam/KeypointBase.hpp>
#include <boost/serialization/access.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/version.hpp>
#include <sm/boost/serialization.hpp>
#include <sm/eigen/static_assert.hpp>

namespace aslam {

template <int KEYPOINT_DIMENSION>
class Keypoint : public KeypointBase {
  public:
    SM_DEFINE_EXCEPTION(Exception, std::runtime_error);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    enum {
        KeypointDimension =
            KEYPOINT_DIMENSION /*!< The dimension of the keypoint associated with this geometry policy */
    };

    typedef Eigen::Matrix<double, KeypointDimension, 1> measurement_t;
    typedef Eigen::Matrix<double, KeypointDimension, KeypointDimension> inverse_covariance_t;

    /// \brief default constructor
    Keypoint();

    /// \brief the copy constructor makes a deep copy
    Keypoint(const Keypoint& rhs);

    /// \brief the assignment operator makes a deep copy
    Keypoint& operator=(const Keypoint& rhs);

    virtual ~Keypoint();

    /// \brief swap this keypoint with the rhs
    void swap(Keypoint& rhs);

    virtual size_t dimension() const { return KeypointDimension; }

    ///////////////////////////////////////////////////
    // The measurement and uncertainty
    ///////////////////////////////////////////////////

    /// \brief the image measurement
    const measurement_t& measurement() const;

    /// \brief the image measurement
    const measurement_t& y() const;

    /// \brief set the image measurement
    void setMeasurement(const measurement_t& m);

    /// \brief the octave this keypoint was detected in
    int octave() const;

    /// \brief set the octave this keypoint was detected in
    void setOctave(const int octave);

    /// \breif get/set the trace id
    void setTraceId(const size_t traceId);
    size_t getTraceId();

    /// \brief compute the mahalonibis distance between y and this keypoint
    template <typename DERIVED>
    double mahalonibisDistance(const Eigen::MatrixBase<DERIVED>& y) const;

    /// \brief get the image measurement inverse covariance
    const inverse_covariance_t& inverseMeasurementCovariance() const;

    /// \brief get the image measurement inverse covariance
    const inverse_covariance_t& invR() const;

    /// \brief set the image measurement inverse covariance
    void setInverseMeasurementCovariance(const inverse_covariance_t& invR);
    void setInverseCovariance(const inverse_covariance_t& invR) { setInverseMeasurementCovariance(invR); }

    /// \brief get the measurement
    virtual Eigen::VectorXd vsMeasurement() const;
    /// \brief set the measurement
    virtual void vsSetMeasurement(const Eigen::VectorXd& m);

    /// \brief get the inverse measurement covariance
    virtual Eigen::MatrixXd vsInverseMeasurementCovariance() const;
    /// \brief set the inverse measurement covariance.
    virtual void vsSetInverseMeasurementCovariance(const Eigen::MatrixXd& invR);

    virtual double vsMahalonibisDistance(const Eigen::VectorXd& y) const;

    ///////////////////////////////////////////////////
    // The descriptor
    ///////////////////////////////////////////////////

    /// \brief get the keypoint descriptor
    virtual const DescriptorBase* descriptor() const;

    /// \brief get the keypoint descriptor
    virtual boost::shared_ptr<DescriptorBase> descriptorPtr() const;

    /// \brief set the keypoint descriptor
    virtual void setDescriptor(const DescriptorBase& descriptor);

    /// \brief set the keypoint descriptor
    ///        the keypoint takes ownership of the pointer
    virtual void setDescriptorRawPtr(DescriptorBase* descriptor);

    /// \brief set the keypoint descriptor
    ///        the keypoint takes ownership of the pointer
    virtual void setDescriptorPtr(boost::shared_ptr<DescriptorBase> descriptor);

    ///////////////////////////////////////////////////
    // The landmark
    ///////////////////////////////////////////////////

    /// \brief get the landmark
    virtual const sm::kinematics::UncertainHomogeneousPoint& landmark() const;
    virtual const sm::kinematics::UncertainHomogeneousPoint* landmarkPtr() const;

    /// \brief set the landmark
    virtual void setLandmark(const sm::kinematics::UncertainHomogeneousPoint& landmark);

    /// \brief get the landmark id
    virtual const LandmarkId& landmarkId() const;

    /// \brief set the landmark id
    virtual void setLandmarkId(const LandmarkId& landmarkId);

    /// \brief is the landmark initialized
    virtual bool isLandmarkInitialized() const;

    /// \brief reset and clear the landmark.
    virtual void clearLandmark();

    ///////////////////////////////////////////////////
    // The back projection
    ///////////////////////////////////////////////////

    /// \brief set the back projection
    virtual void setBackProjection(const sm::kinematics::UncertainVector3& v);

    /// \brief set the back projection
    virtual void setBackProjectionVector(const Eigen::Vector3d& v);

    /// \brief get the back projection pointer
    virtual const sm::kinematics::UncertainVector3* backProjection() const;

    /// \brief is the back projection initialized?
    virtual bool isBackProjectionSet() const;

    /// \brief clear the back projection
    virtual void clearBackProjection();

    ///////////////////////////////////////////////////
    // Serialization
    ///////////////////////////////////////////////////

    enum { CLASS_SERIALIZATION_VERSION = 1 };
    BOOST_SERIALIZATION_SPLIT_MEMBER();

    template <class Archive>
    void load(Archive& ar, const unsigned int version) {
        SM_ASSERT_LE(std::runtime_error, version, (unsigned int)CLASS_SERIALIZATION_VERSION,
                     "Unsupported serialization version");

        ar >> BOOST_SERIALIZATION_NVP(_measurement);
        ar >> BOOST_SERIALIZATION_NVP(_inverseMeasurementCovariance);
        ar >> BOOST_SERIALIZATION_NVP(_landmarkId);
        ar >> BOOST_SERIALIZATION_NVP(_landmark);
        ar >> BOOST_SERIALIZATION_NVP(_descriptor);
        ar >> BOOST_SERIALIZATION_NVP(_backProjection);
        if (version > 0) {
            ar >> BOOST_SERIALIZATION_NVP(_octave);
        }
    }

    template <class Archive>
    void save(Archive& ar, const unsigned int /* version */) const {
        ar << BOOST_SERIALIZATION_NVP(_measurement);
        ar << BOOST_SERIALIZATION_NVP(_inverseMeasurementCovariance);
        ar << BOOST_SERIALIZATION_NVP(_landmarkId);
        ar << BOOST_SERIALIZATION_NVP(_landmark);
        ar << BOOST_SERIALIZATION_NVP(_descriptor);
        ar << BOOST_SERIALIZATION_NVP(_backProjection);
        ar << BOOST_SERIALIZATION_NVP(_octave);
    }

    /// \brief Support for unit testing. Is this keypoint binary equal to the other?
    bool isBinaryEqual(const Keypoint<KEYPOINT_DIMENSION>& rhs) const;

    /// \brief Support for unit testing. Set this to a random keypoint.
    void setRandom();

  private:
    /// \brief Frees all memory owned by the keypoint.
    void freeAll();

    /// \brief The keypoint measurement
    measurement_t _measurement;

    /// \brief The inverse covariance measurement
    inverse_covariance_t _inverseMeasurementCovariance;

    /// \brief the landmark id.
    LandmarkId _landmarkId;

    /// \brief the landmark.
    sm::kinematics::UncertainHomogeneousPoint* _landmark;

    /// \brief the keypoint descriptor
    boost::shared_ptr<DescriptorBase> _descriptor;

    /// \brief the back projection, a ray in space originating from the view origin.
    sm::kinematics::UncertainVector3* _backProjection;

    /// \brief the image octave this keypoint was observed in
    unsigned int _octave;

    /// \brief unique ground truth ID for a keypoint trace (i.e. to easily verifiy 2D2D matching with ground truth
    /// simulation)
    size_t _traceId;
};

}  // namespace aslam

#include <aslam/implementation/Keypoint.hpp>

// this is a fancy templated version of
// BOOST_CLASS_VERSION() for serialization versioning
SM_BOOST_CLASS_VERSION_I1(aslam::Keypoint);

#endif /* ASLAM_KEYPOINT_HPP */


#ifndef MARGINAL_DEF_VISITOR
#define MARGINAL_DEF_VISITOR

#include <boost/python.hpp>
#include <sstream>
#include <string>
#include "gil.hxx"
#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>
#include <opengm/python/pythonfunction.hxx>

#include <opengm/inference/inference.hxx>
#include <opengm/opengm.hxx>

template<class INF>
class MarginalSuite: public boost::python::def_visitor<MarginalSuite<INF> >{
public:
    friend class boost::python::def_visitor_access;
    typedef typename INF::GraphicalModelType                    GraphicalModelType;
    typedef typename GraphicalModelType::IndependentFactorType  IndependentFactorType;
    typedef typename IndependentFactorType::ShapeIteratorType   IFactorShapeType;
    typedef typename INF::Parameter                             ParameterType;
    typedef typename GraphicalModelType::IndexType              IndexType;
    typedef typename GraphicalModelType::LabelType              LabelType;
    typedef typename GraphicalModelType::ValueType              ValueType;

    template <class classT>
    void visit(classT& c) const{
        c
            // marginals for a single var and single
            .def("_marginals",&marginals)
            .def("_factorMarginals",&factorMarginals)
        ;
    }

    static boost::python::object marginals(
        const INF & inf,
        opengm::python::NumpyView<IndexType> vis
    ){
        const GraphicalModelType & gm     = inf.graphicalModel();
        const IndexType numberOfVariables = gm.numberOfVariables();
        const LabelType numLabels         = gm.numberOfLabels(vis(0));
        const size_t numPassedVis         = vis.size();

        // allocate 2d array
        boost::python::object obj    = opengm::python::get2dArray<ValueType>(numPassedVis,numLabels);
        opengm::python::NumpyView<ValueType,2> numpyArray(obj);

        {
            releaseGIL rgil;
            // allocate ifactor to store marginal and inference termination
            IndependentFactorType ifactor;
            opengm::InferenceTermination infTerm;
            for(size_t i=0;i<numPassedVis;++i){
                const IndexType vi      = vis[i];
                const LabelType nLabels = gm.numberOfLabels(vi);
                if(nLabels!=numLabels){
                    throw opengm::RuntimeError("all variables in ``vis`` must have the same number of Labels");
                }
                // get marginal
                infTerm = inf.marginal(vi,ifactor);
                if(infTerm == opengm::UNKNOWN){
                    throw opengm::RuntimeError("this inference class does not support marginalization");
                }
                // write marginals into numpy array
                for(LabelType l=0;l<numLabels;++l){
                    numpyArray(i,l)=ifactor(&l);
                }

            }
        }
        return obj;
    }

    static boost::python::object factorMarginals(
        const INF & inf,
        opengm::python::NumpyView<IndexType> fis
    ){
        const GraphicalModelType & gm     = inf.graphicalModel();
        const size_t order                = gm[fis(0)].numberOfVariables();
        const size_t numPassedFac         = fis.size();
        const size_t outNDim              = order+1;

        // fill out shape
        opengm::FastSequence<LabelType> outShape(outNDim);
        //opengm::FastSequence<LabelType> facShape(numVariables);
        outShape[0]=numPassedFac;

        for(size_t v=0;v<order;++v){
            outShape[v+1]=gm[fis(0)].numberOfLabels(v);
        }

        // allocate nd array
        boost::python::object obj    = opengm::python::getArray<ValueType>(outShape.begin(),outShape.end());
        opengm::python::NumpyView<ValueType,2> numpyArray(obj);
        std::vector<LabelType> outCoord(outNDim);
        OPENGM_ASSERT(numpyArray.dimension()==order+1)
        {
            releaseGIL rgil;
            // allocate ifactor to store factorMarginal and inference termination
            IndependentFactorType ifactor;
            opengm::InferenceTermination infTerm;
            // go over all factor(-indices) in fis
            for(size_t i=0;i<numPassedFac;++i){
                const IndexType fi      = fis[i];
                const LabelType nVar    = gm[fi].numberOfVariables();
                // check that order is the same for all factors
                if(nVar!=order){
                    throw opengm::RuntimeError("all factors in ``fis`` must have the same order");
                }
                // check that shape is the same for all factors
                for(size_t v=0;v<order;++v){
                    if(gm[fi].numberOfLabels(v)!=outShape[v+1]){
                        throw opengm::RuntimeError("all factors in ``fis`` must have the same shape ");
                    }
                }
                // get factor marginal
                infTerm = inf.factorMarginal(fi,ifactor);
                if(infTerm == opengm::UNKNOWN){
                    throw opengm::RuntimeError("this inference class does not support marginalization");
                }
                // fill marginal for factor in fi
                opengm::ShapeWalker<IFactorShapeType> walker(ifactor.shapeBegin(),ifactor.size());
                OPENGM_ASSERT(ifactor.numberOfVariables()==order)
                OPENGM_ASSERT(outCoord.size()==numpyArray.dimension())
                // first out coordinate is the index of the fis-range
                outCoord[0]=i;
                for (size_t e=0;e<ifactor.size();++e) {
                    ValueType marginalValue=ifactor.function()(e);
                    for(size_t c=0;c<order;++c){
                        outCoord[c+1]=walker.coordinateTuple()[c];
                    }
                    numpyArray(outCoord.begin())=marginalValue;
                    ++walker;
                }
            }
        }
        return obj;
    }
};

#endif // MARGINAL_DEF_VISITOR