#include <algorithm>

//#include <iostream>

#include "Animation/RgbwPsychedelicAnimation.h"

RgbwPsychedelicAnimation::RgbwPsychedelicAnimation(Fader *fader,
                                       const rgbw_val_array &move_speeds,
                                       const rgbw_val_array &tilt_speeds,
                                       int32_t  tilt_maxperiods,
                                       uint32_t move_speed_divider,
                                       uint32_t tilt_speed_divider,
                                       uint16_t brightness_scale)
    : Animation(fader),
      m_move_speeds(move_speeds),
      m_tilt_speeds(move_speeds),
      m_moveSpeedDivider(move_speed_divider),
      m_tiltSpeedDivider(tilt_speed_divider),
      m_brightnessScale(brightness_scale),
      m_tilt_maxperiods(tilt_maxperiods),
      m_move_phi{800,600,400,200},
      m_tilt_phi{200,400,600,800},
      m_moveOverflowInterval(fasttrigon::LUT_SIZE * move_speed_divider),
      m_tiltOverflowInterval(fasttrigon::LUT_SIZE * tilt_speed_divider)
{
    reset();
}

void RgbwPsychedelicAnimation::loop(uint64_t frame)
{
    std::size_t nled = m_fader->modules_per_strip();
    std::size_t nstrip = m_fader->strips();

    if(!m_stopping) {
        for(unsigned i = 0; i < m_move_speeds.size(); i++) {
            m_move_phi[i] += m_move_speeds[i];

            if(m_move_phi[i] > m_moveOverflowInterval) {
                m_move_phi[i] -= m_moveOverflowInterval;
            }
        }

        rgbw_val_array tilt_nperiods;

        for(unsigned i = 0; i < m_tilt_speeds.size(); i++) {
            m_tilt_phi[i] += m_tilt_speeds[i];

            if(m_tilt_phi[i] > m_tiltOverflowInterval) {
                m_tilt_phi[i] -= m_tiltOverflowInterval;
            }

            tilt_nperiods[i] = m_tilt_maxperiods * fasttrigon::fastsin(m_tilt_phi[i] / m_tiltSpeedDivider);
        }


        for(std::size_t led = 0; led < nled; led++) {
            for(std::size_t strip = 0; strip < nstrip; strip++) {
                int32_t x = fasttrigon::LUT_SIZE * led / nled;
                int32_t y = fasttrigon::LUT_SIZE * strip / nstrip;

                rgbw_val_array rgbw {
                    (fasttrigon::SCALE + fasttrigon::fastsin(m_move_phi[0] / m_moveSpeedDivider + ((x * tilt_nperiods[0]) >> fasttrigon::UNIT_SHIFT) + y)),
                    (fasttrigon::SCALE + fasttrigon::fastsin(m_move_phi[1] / m_moveSpeedDivider + ((x * tilt_nperiods[1]) >> fasttrigon::UNIT_SHIFT) + y)),
                    (fasttrigon::SCALE + fasttrigon::fastsin(m_move_phi[2] / m_moveSpeedDivider + ((x * tilt_nperiods[2]) >> fasttrigon::UNIT_SHIFT) + y)),
                    (fasttrigon::SCALE + fasttrigon::fastsin(m_move_phi[3] / m_moveSpeedDivider + ((x * tilt_nperiods[3]) >> fasttrigon::UNIT_SHIFT) + y))
                };

                Fader::Color color{
                    static_cast<int16_t>((FASTTRIGON_8BIT(rgbw[0] * rgbw[0] >> fasttrigon::UNIT_SHIFT) * m_brightnessScale) >> fasttrigon::PRECISION_BITS),
                    static_cast<int16_t>((FASTTRIGON_8BIT(rgbw[1] * rgbw[1] >> fasttrigon::UNIT_SHIFT) * m_brightnessScale) >> fasttrigon::PRECISION_BITS),
                    static_cast<int16_t>((FASTTRIGON_8BIT(rgbw[2] * rgbw[2] >> fasttrigon::UNIT_SHIFT) * m_brightnessScale) >> fasttrigon::PRECISION_BITS),
                    static_cast<int16_t>((FASTTRIGON_8BIT(rgbw[3] * rgbw[3] >> fasttrigon::UNIT_SHIFT) * m_brightnessScale) >> (fasttrigon::PRECISION_BITS + 2))}; // white is too bright otherwise

                //std::cerr << rgbw[0] << " " << rgbw[1] << " " << rgbw[2] << " " << rgbw[3] << std::endl;
                //std::cerr << color.r << " " << color.g << " " << color.b << " " << color.w << std::endl;

                m_fader->set_color(strip, led, color);
            }
        }
    } else {
        m_fader->fade_color(Fader::Color{0, 0, 0, 0});
    }

    m_fader->update();
    m_running = !m_stopping || m_fader->something_changed();
}

void RgbwPsychedelicAnimation::reset(void)
{
    m_stopping = false;
    m_running = true;
}