/*

* Copyright (c) 2007 Nokia Corporation and/or its subsidiary(-ies). 

* All rights reserved.

* This component and the accompanying materials are made available

* under the terms of "Eclipse Public License v1.0"

* which accompanies this distribution, and is available

* at the URL "http://www.eclipse.org/legal/epl-v10.html".

*

* Initial Contributors:

* Nokia Corporation - initial contribution.

*

* Contributors:

*

* Description:  Compensate function definition.

*

*/

#include <e32math.h>

#include <tiltcompensation.h>

#include <e32debug.h>

//-----------------------------------------------------------------------------

// Direction6D

//-----------------------------------------------------------------------------

//

EXPORT_C TInt Compensate(

    const TTiltCompensationInput& aInput,

    TTiltCompensationOutput& aOutput,

    const TInt16 aPreviousTheta

     )

    {

    TReal target;

    TReal source( 0 );

    TReal inputY( aInput.iMagneticVector.iY );

    TReal inputZ( aInput.iMagneticVector.iZ );

    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ > 0 )

    // 0 - 90 degrees

        {

        source = inputY / inputZ;

        }

    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ < 0 )

    // 90 - 180 degrees

        {

        source = inputZ / inputY * -1;

        }

    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ < 0 )

    // 180 - 270 degrees

        {

        source = inputY / inputZ;

        }

    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ > 0 )

    // 270 - 360 degrees

        {

        source = inputZ / inputY * -1;

        }

    Math::ATan( target, source );

    TInt16 declination( ( target * 180 ) / 3.14 );

    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ > 0 )

    // 0 - 90 degrees

        {

        }

    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ < 0 )

    // 90 - 180 degrees

        {

        declination = declination + 90;

        }

    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ < 0 )

    // 180 - 270 degrees

        {

        declination = declination + 180;

        }

    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ > 0 )

    // 270 - 360 degrees

        {

        declination = declination + 270;

        }

    if( inputY == 0 && inputZ > 0 )

        {

        declination = 0;

        }

    if( inputY > 0 && inputZ == 0 )

        {

        declination = 90;

        }

    if( inputY == 0 && inputZ < 0 )

        {

        declination = 180;

        }

    if( inputY < 0 && inputZ == 0 )

        {

        declination = 270;

        }

    if( ( aPreviousTheta < ( aOutput.iTheta + 5 ) ) && ( aPreviousTheta > ( aOutput.iTheta - 5 ) ) )

        {

        aOutput.iTheta = aPreviousTheta;

        }

    aOutput.iTheta = declination;

    return KErrNone;

    }

//-----------------------------------------------------------------------------

// Direction6D

//-----------------------------------------------------------------------------

//

EXPORT_C TInt Compensate(

    const TTiltCompensationInput& aInput,

    TTiltCompensationOutput& aOutput,

    const TInt16 aPreviousTheta,

    const RParamsArray& aParamsArray )

    {

    TReal target;

    TReal source( 0 );

    TReal inputY( aInput.iMagneticVector.iY );

    TReal inputX( aInput.iMagneticVector.iX );

    // Get parameters

    for( TInt i = 0; i != aParamsArray.Count(); i++ )

        {

        TInt parameter = aParamsArray[ i ];

        }

    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX > 0 )

    // 0 - 90 degrees

        {

        source = inputX / inputY;

        }

    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX < 0 )

    // 90 - 180 degrees

        {

        source = inputX / inputY * -1;

        }

    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX < 0 )

    // 180 - 270 degrees

        {

        source = inputY / inputX;

        }

    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX > 0 )

    // 270 - 360 degrees

        {

        source = inputX / inputY * -1;

        }

    Math::ATan( target, source );

    TInt16 declination( ( target * 180 ) / 3.14 );

    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX > 0 )

    // 0 - 90 degrees

        {

        }

    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX < 0 )

    // 90 - 180 degrees

        {

        declination = declination + 90;

        }

    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX < 0 )

    // 180 - 270 degrees

        {

        declination = declination + 180;

        }

    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX > 0 )

    // 270 - 360 degrees

        {

        declination = declination + 270;

        }

    if( inputY == 0 && inputX > 0 )

        {

        declination = 0;

        }

    if( inputY > 0 && inputX == 0 )

        {

        declination = 90;

        }

    if( inputY == 0 && inputX < 0 )

        {

        declination = 180;

        }

    if( inputY < 0 && inputX == 0 )

        {

        declination = 270;

        }

    if( ( aPreviousTheta < ( aOutput.iTheta + 5 ) ) && ( aPreviousTheta > ( aOutput.iTheta - 5 ) ) )

        {

        aOutput.iTheta = aPreviousTheta;

        }

    // Filter data...

    // For testing purposes change angle with fixed values from input parameters

    aOutput.iTheta = declination;

    const TInt KAngleChange = 30;

    aOutput.iTheta += ( aParamsArray[ 0 ] * KAngleChange );

    aOutput.iTheta += ( aParamsArray[ 1 ] * KAngleChange );

    aOutput.iTheta += ( aParamsArray[ 2 ] * KAngleChange );

    aOutput.iTheta += ( aParamsArray[ 3 ] * KAngleChange );

    aOutput.iTheta %= 360; // take modulo to avoid angle larger than 360

    // ... end for testing purposes

    return KErrNone;

    }    

// End of File