#include "CRumbleGenerator.hpp"
#include "GameGlobalObjects.hpp"

namespace urde
{

CRumbleGenerator::CRumbleGenerator()
{
    xf0_24_disabled = false;
    HardStopAll();
}

CRumbleGenerator::~CRumbleGenerator()
{
    HardStopAll();
}

#define PWM_MONITOR 0
#if PWM_MONITOR
static bool b_tp = false;
static std::chrono::steady_clock::time_point s_tp;
#endif

void CRumbleGenerator::Update(float dt)
{
#if PWM_MONITOR
    std::chrono::milliseconds::rep ms = 0;
    std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
    if (!b_tp)
    {
        b_tp = true;
        s_tp = now;
    }
    else
        ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - s_tp).count();
#endif

    if (!xf0_24_disabled)
    {
        bool updated = false;
        for (int i=0 ; i<4 ; ++i)
        {
            float intensity = x0_voices[i].GetIntensity();
            if (!x0_voices[i].Update(dt) || intensity <= 0.f)
            {
                xc0_periodTime[i] = 0.f;
                xd0_onTime[i] = 0.f;
                if (xe0_commandArray[i] != EMotorState::Stop)
                {
#if PWM_MONITOR
                    s_tp = now;
                    printf("%lldms ON\n", ms);
#endif
                    xe0_commandArray[i] = EMotorState::Stop;
                    updated = true;
                }
            }
            else
            {
                xc0_periodTime[i] += dt;
                if (xc0_periodTime[i] >= 1.f / (30.f * intensity))
                {
                    xc0_periodTime[i] = 0.f;
                    if (xe0_commandArray[i] != EMotorState::Rumble)
                    {
#if PWM_MONITOR
                        s_tp = now;
                        printf("%lldms Off\n", ms);
#endif
                        xe0_commandArray[i] = EMotorState::Rumble;
                        updated = true;
                    }
                }
                else
                {
                    xd0_onTime[i] += dt;
                    if (xd0_onTime[i] >= (1.f / 30.f))
                    {
                        xd0_onTime[i] = 0.f;
                        if (xe0_commandArray[i] != EMotorState::Stop)
                        {
#if PWM_MONITOR
                            s_tp = now;
                            printf("%lldms ON\n", ms);
#endif
                            xe0_commandArray[i] = EMotorState::Stop;
                            updated = true;
                        }
                    }
                }
            }
        }
        if (updated)
            g_InputGenerator->ControlAllMotors(xe0_commandArray);
    }
}

static const EMotorState HardStopCommands[] = { EMotorState::StopHard,
                                                EMotorState::StopHard,
                                                EMotorState::StopHard,
                                                EMotorState::StopHard };

void CRumbleGenerator::HardStopAll()
{
    for (int i=0 ; i<4 ; ++i)
    {
        xc0_periodTime[i] = 0.f;
        xd0_onTime[i] = 0.f;
        xe0_commandArray[i] = EMotorState::Stop;
        x0_voices[i].HardReset();
    }
    g_InputGenerator->ControlAllMotors(HardStopCommands);
}

s16 CRumbleGenerator::Rumble(const SAdsrData& adsr, float gain, ERumblePriority prio, EIOPort port)
{
    CRumbleVoice& vox = x0_voices[int(port)];
    s16 freeChan = vox.GetFreeChannel();
    if (prio >= vox.GetPriority(freeChan))
    {
        xc0_periodTime[int(port)] = 0.f;
        return vox.Activate(adsr, freeChan, gain, prio);
    }
    return -1;
}

void CRumbleGenerator::Stop(s16 id, EIOPort port)
{
    CRumbleVoice& vox = x0_voices[int(port)];
    vox.Deactivate(id, false);
}

}