/*
 * Name        : Accelerometer.cpp
 * Description : Accelerometer helper class
 * Project     : This file is part of OpenMAR, an Open Mobile Augmented Reality browser
 * Website     : http://OpenMAR.org
 *
 * Copyright (c) 2010 David Caabeiro
 *
 * All rights reserved. This program and the accompanying materials are made available 
 * under the terms of the Eclipse Public License v1.0 which accompanies this 
 * distribution, and is available at http://www.eclipse.org/legal/epl-v10.html
 *
 */

#include "Accelerometer.h"

#include <SensrvAccelerometerSensor.h>

#include <SensrvChannel.h>
#include <SensrvChannelFinder.h>
#include <SensrvChannelInfo.h>

CAccelerometer* CAccelerometer::NewL()
{
    CAccelerometer* self = new(ELeave) CAccelerometer;
    CleanupStack::PushL(self);
    self->ConstructL();
    CleanupStack::Pop(self);

    return self;
}

CAccelerometer::~CAccelerometer()
{
    iSensorChannel->CloseChannel();
}

CAccelerometer::CAccelerometer()
{}

void CAccelerometer::ConstructL()
{
    CSensrvChannelFinder* sensorChannelFinder = CSensrvChannelFinder::NewLC();

    RSensrvChannelInfoList channelInfoList;
    CleanupClosePushL(channelInfoList);

    TSensrvChannelInfo mySearchConditions;
    mySearchConditions.iChannelType = KSensrvChannelTypeIdAccelerometerXYZAxisData;

    sensorChannelFinder->FindChannelsL(channelInfoList, mySearchConditions);

    TSensrvChannelInfo channelInfo;

    if (channelInfoList.Count() > 0)
        channelInfo = channelInfoList[0];
    else
        User::Leave(KErrNotFound);

    CleanupStack::PopAndDestroy(&channelInfoList);
    CleanupStack::PopAndDestroy(sensorChannelFinder);

    iSensorChannel = CSensrvChannel::NewL(channelInfo);
    iSensorChannel->OpenChannelL();
}

void CAccelerometer::StartL()
{
    iSensorChannel->StartDataListeningL(this, 1, 8, 0);
}

void CAccelerometer::Stop()
{
    iSensorChannel->StopDataListening();
}

void CAccelerometer::DataReceived(CSensrvChannel& aChannel, TInt aCount, TInt aDataLost)
{
    if (aChannel.GetChannelInfo().iChannelType ==  KSensrvChannelTypeIdAccelerometerXYZAxisData)
    {
        TPckgBuf<TSensrvAccelerometerAxisData> dataBuffer;
        TSensrvAccelerometerAxisData data;

        for (TInt i = 0; i < aCount; ++i)
        {
            aChannel.GetData(dataBuffer);

            data = dataBuffer();
            // Do something with the date in data variable
            // data.iTimeStamp

            // Compensate axis for landscape mode
            TInt x = -data.iAxisY;
            TInt y =  data.iAxisX;
            TInt z =  data.iAxisZ;
#if 0
            TInt x = data.iAxisX;
            TInt y = data.iAxisY;
            TInt z = data.iAxisZ;
#endif
            const TReal K128 = 0.0078125;   // 1/128
            const TReal KPole1 = 0.97;      // Pole of lowpass filter applied to X and Y
            const TReal KPole2 = 0.90;      // Pole of lowpass filter applied to Z
            const TReal KPole3 = 0.75;      // Pole of highpass filter applied to Z

            // Single-pole lowpass-filtering of X and Y
            iX1 = KPole1 * iX1 + ( 1.0 - KPole1 ) * ( K128 * x );
            iY1 = KPole1 * iY1 + ( 1.0 - KPole1 ) * ( K128 * y );

            // Single-pole lowpass-filtering of Z
            iZ0 = KPole2 * iZ1 + ( 1.0 - KPole2 ) * ( K128 * z );
            // 1st order highpass-filtering of Z (zero at 1)
            iZ2 = KPole3 * iZ2 + ( 1.0 - KPole3 ) * ( iZ0 - iZ1 );

            // Update variables for use at next sample
            iZ1 = iZ0;
        }
    }
}

void CAccelerometer::DataError(CSensrvChannel& aChannel, TSensrvErrorSeverity aError)
{
}

void CAccelerometer::GetDataListenerInterfaceL(TUid aInterfaceUid, TAny*& aInterface)
{}