/////////////////////////////////////////////////////////////////////////////////
//
//  Copyright (C) 2018-     Equinor ASA
//
//  ResInsight 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 3 of the License, or
//  (at your option) any later version.
//
//  ResInsight is distributed in the hope that it will be useful, but WITHOUT ANY
//  WARRANTY; without even the implied warranty of MERCHANTABILITY or
//  FITNESS FOR A PARTICULAR PURPOSE.
//
//  See the GNU General Public License at <http://www.gnu.org/licenses/gpl.html>
//  for more details.
//
/////////////////////////////////////////////////////////////////////////////////

#pragma once

#include "cvfVector3.h"
#include <array>
#include <cmath>

// Y - North,  X - East, Z - up but depth is negative Z
// azi is measured from the Northing (Y) Axis in Clockwise direction looking down
// inc is measured from the negative Z (depth) axis

const double singularityEpsilon = 1.0e-5;

class RiaOffshoreSphericalCoords
{
public:
    explicit RiaOffshoreSphericalCoords( const cvf::Vec3f& vec )
    {
        // Azimuth:
        if ( fabs( vec[0] ) < singularityEpsilon && fabs( vec[1] ) < singularityEpsilon )
            incAziR[1] = 0.0f;
        else
            incAziR[1] = atan2( vec[0], vec[1] ); // atan2(Y, X)

        // R
        incAziR[2] = vec.length();

        // Inclination from vertical down
        if ( fabs( incAziR[2] ) < singularityEpsilon )
            incAziR[0] = 0.0f;
        else
            incAziR[0] = acos( -vec[2] / incAziR[2] );
    }

    explicit RiaOffshoreSphericalCoords( const cvf::Vec3d& vec )
    {
        // Azimuth:
        if ( fabs( vec[0] ) < singularityEpsilon && fabs( vec[1] ) < singularityEpsilon )
            incAziR[1] = 0.0;
        else
            incAziR[1] = atan2( vec[0], vec[1] ); // atan2(Y, X)

        // R
        incAziR[2] = vec.length();

        // Inclination from vertical down
        if ( fabs( incAziR[2] ) < singularityEpsilon )
            incAziR[0] = 0.0;
        else
            incAziR[0] = acos( -vec[2] / incAziR[2] );
    }
    double inc() const { return incAziR[0]; }
    double azi() const { return incAziR[1]; }
    double r() const { return incAziR[2]; }

    // Note that this is a double function, while the rest of the class is float.
    // Todo: Convert class to a template to enable float and double versions of everything
    static cvf::Vec3d unitVectorFromAziInc( double azimuth, double inclination )
    {
        return cvf::Vec3d( sin( azimuth ) * sin( inclination ), cos( azimuth ) * sin( inclination ), -cos( inclination ) );
    }

private:
    std::array<double, 3> incAziR;
};