/*****************************************************************************
 * viewpoint.c: viewpoint helpers for conversions and transformations
 *****************************************************************************
 * Copyright (C) 2019 VLC authors and VideoLAN
 *
 * Authors: Alexandre Janniaux <ajanni@videolabs.io>
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * This program 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston MA 02110-1301, USA.
 *****************************************************************************/

#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

#include <vlc_viewpoint.h>
#include <vlc_es.h>

void vlc_viewpoint_to_4x4( const vlc_viewpoint_t *vp, float *m )
{
    float yaw   = -vp->yaw   * (float)M_PI / 180.f;
    float pitch = -vp->pitch * (float)M_PI / 180.f;
    float roll  = -vp->roll  * (float)M_PI / 180.f;

    float s, c;

    s = sinf(pitch);
    c = cosf(pitch);
    const float x_rot[4][4] = {
        { 1.f,    0.f,    0.f,    0.f },
        { 0.f,    c,      -s,      0.f },
        { 0.f,    s,      c,      0.f },
        { 0.f,    0.f,    0.f,    1.f } };

    s = sinf(yaw);
    c = cosf(yaw);
    const float y_rot[4][4] = {
        { c,      0.f,    s,     0.f },
        { 0.f,    1.f,    0.f,    0.f },
        { -s,      0.f,    c,      0.f },
        { 0.f,    0.f,    0.f,    1.f } };

    s = sinf(roll);
    c = cosf(roll);
    const float z_rot[4][4] = {
        { c,      s,      0.f,    0.f },
        { -s,     c,      0.f,    0.f },
        { 0.f,    0.f,    1.f,    0.f },
        { 0.f,    0.f,    0.f,    1.f } };

    /**
     * Column-major matrix multiplication mathematically equal to
     * z_rot * x_rot * y_rot
     */
    memset(m, 0, 16 * sizeof(float));
    for (int i=0; i<4; ++i)
        for (int j=0; j<4; ++j)
            for (int k=0; k<4; ++k)
                for (int l=0; l<4; ++l)
                    m[4*i+l] += y_rot[i][j] * x_rot[j][k] * z_rot[k][l];
}

void vlc_viewpoint_from_orientation(vlc_viewpoint_t *vp,
                                    video_orientation_t orient)
{
    switch(orient)
    {
        default:
        case ORIENT_NORMAL:
        case ORIENT_HFLIPPED:
            vlc_viewpoint_from_euler(vp, 0.f, 0.f, 0.f);
            break;

        case ORIENT_ROTATED_90:
        case ORIENT_ANTI_TRANSPOSED:
            vlc_viewpoint_from_euler(vp, 0.f, 0.f, -90.f);
            break;

        case ORIENT_ROTATED_180:
        case ORIENT_VFLIPPED:
            vlc_viewpoint_from_euler(vp, 0.f, 0.f, -180.f);
            break;
        case ORIENT_ROTATED_270:
        case ORIENT_TRANSPOSED:
            vlc_viewpoint_from_euler(vp, 0.f, 0.f, -270.f);
            break;
    }
}

void vlc_viewpoint_from_euler(vlc_viewpoint_t *vp,
                              float yaw, float pitch, float roll)
{
    vp->yaw   = yaw;
    vp->pitch = pitch;
    vp->roll  = roll;
}

void vlc_viewpoint_to_euler(const vlc_viewpoint_t *vp,
                            float *yaw, float *pitch, float *roll)
{
    *yaw   = vp->yaw;
    *pitch = vp->pitch;
    *roll  = vp->roll;
}