/***************************************************************************
                          rotary.cpp  -  description
                             -------------------
    begin                : Tue Sep 21 2004
    copyright            : (C) 2004 by Tue Haste Andersen
    email                : haste@diku.dk
***************************************************************************/

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

#include "rotary.h"
#include "mathstuff.h"
#include "controlobject.h"
#include <QDebug>

Rotary::Rotary()
{
    m_dCalibration = 1.;
    m_dLastValue = 0.;

    m_iFilterLength = kiRotaryFilterMaxLen;
    m_iFilterPos = 0;
    m_pFilter = new double[m_iFilterLength];
    for (int i=0; i<m_iFilterLength; ++i)
        m_pFilter[i] = 0.;
}

Rotary::~Rotary()
{
    delete [] m_pFilter;
}

/* Note: There's probably a bug in this function (or this class) somewhere.
   The filter function seems to be the cause of the "drifting" bug in the Hercules stuff.
   What happens is that filter() gets called to do some magic to a value that's returned
   from the Hercules device, and that magic adds "momentum" to it's motion (ie. it doesn't
   stop dead when you stop spinning the jog wheels.) The problem with this "magic" is that
   when herculeslinux.cpp passes the filtered value off to the wheel ControlObject (or what
   have you), the ControlObject's internal value never goes back to zero properly.
   - Albert (March 13, 2007)
 */
double Rotary::filter(double dValue)
{
    // Update filter buffer
    m_pFilter[m_iFilterPos] = dValue/m_dCalibration;
    m_iFilterPos = (m_iFilterPos+1)%m_iFilterLength;

    double dMagnitude = 0.;
    for (int i=0; i<m_iFilterLength; i++)
    {
        dMagnitude += m_pFilter[i];
    }
    dMagnitude /= (double)m_iFilterLength;
    //qDebug() << "filter in " << dValue << ", out " << dMagnitude;

    m_dLastValue = dMagnitude;

    return dMagnitude;
}

double Rotary::fillBuffer(double dValue)
{
	for (int i=0; i<m_iFilterLength; ++i)
	{
		m_pFilter[i] = dValue/m_dCalibration;
	}
	return dValue/m_dCalibration;
}

void Rotary::calibrate(double dValue)
{
    m_dCalibration += dValue;
    m_iCalibrationCount += 1;
}

void Rotary::calibrateStart()
{
    // Reset calibration data
    m_dCalibration = 0.;
    m_iCalibrationCount = 0;
}

double Rotary::calibrateEnd()
{
    m_dCalibration /= (double)m_iCalibrationCount;

    qDebug() << "Calibration " << m_dCalibration << ", count " << m_iCalibrationCount;

    return m_dCalibration;
}

void Rotary::setCalibration(double c)
{
    m_dCalibration = c;
}

double Rotary::getCalibration()
{
    return m_dCalibration;
}

void Rotary::setFilterLength(int i)
{
    if (i>kiRotaryFilterMaxLen)
        m_iFilterLength = kiRotaryFilterMaxLen;
    else if (i<1)
        m_iFilterLength = 1;
    else
        m_iFilterLength = i;
}

int Rotary::getFilterLength()
{
    return  m_iFilterLength;
}
