# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# Copyright (c) 2019, Eurecat / UPF
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the <organization> nor the
#       names of its contributors may be used to endorse or promote products
#       derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
#   @file   quantise.py
#   @author Andrés Pérez-López
#   @date   09/09/2019
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #

import numpy as np

from masp.shoebox_room_sim.echogram import QuantisedEchogram
from masp.utils import sph2cart, C
from masp.validate_data_types import _validate_echogram, _validate_ndarray_2D, _validate_int, _validate_ndarray_1D


def get_echo2gridMap(echogram, grid_dirs_rad):
    """
    Generate a mapping between echogram reflections and closest given grid positions.

    Parameters
    ----------
    echogram : Echogram
        Target Echogram
        Number of grid points where to render reflections.
    grid_dirs_rad: ndarray
        Grid positions in [azimuth, elevation] pairs (in radians). Dimension = (nRefl, 2).

    Returns
    -------
    echo2gridMap : ndarray, dtype: int
        Map array between echogram reflection indices and grid points. Dimension = (nRefl).

    Raises
    -----
    TypeError, ValueError: if method arguments mismatch in type, dimension or value.

    """

    _validate_echogram(echogram)
    _validate_ndarray_2D('grid_dirs_rad', grid_dirs_rad, shape1=C-1)

    # Number of reflections
    nRefl = np.shape(echogram.value)[0]
    # Number of rendering directions
    nGrid = np.shape(grid_dirs_rad)[0]

    # Unit vectors pointing to the rendering directions
    grid_xyz = sph2cart(np.column_stack((grid_dirs_rad, np.ones(np.shape(grid_dirs_rad)[0]))))
    # Unit vectors pointing to the image sources
    refl_coords = echogram.coords
    refl_coords = refl_coords/(np.sqrt(np.sum(np.power(refl_coords, 2), axis=1)))[:,np.newaxis]

    echo2gridMap = np.empty(nRefl, dtype='int')
    for i in range(nRefl):
        nearest = np.argmin(np.sum(np.power(refl_coords[i,:] - grid_xyz, 2), axis=1))
        echo2gridMap[i] = nearest

    return echo2gridMap


def quantise_echogram(echogram, nGrid, echo2gridMap):
    """
    Quantise the echogram reflections to specific rendering directions.

    Parameters
    ----------
    echogram : Echogram
        Target Echogram
    nGrid: int
        Number of grid points where to render reflections.
    echo2gridMap: ndarray, dtype: int
        Mapping between echgram and grid points, as generated by `get_echo2gridMap()`

    Returns
    -------
    q_echogram : ndarray, dtype: QuantisedEchogram
        Quantised echograms. Dimension = (nGrid)

    Raises
    -----
    TypeError, ValueError: if method arguments mismatch in type, dimension or value.

    """

    _validate_echogram(echogram)
    _validate_int('nGrid', nGrid)
    _validate_ndarray_1D('echo2gridMap', echo2gridMap, positive=True, dtype=int)

    # Initialise echogram structure with each quantised echogram
    # Adjust array length to the common minimum so that there is no index overflow
    if np.size(echo2gridMap) > np.size(echogram.time):
        echo2gridMap = echo2gridMap[:np.size(echogram.time)]
    elif np.size(echo2gridMap) < np.size(echogram.time):
        echogram.value = echogram.value[:np.size(echo2gridMap)]
        echogram.time = echogram.time[:np.size(echo2gridMap)]

    q_echogram = np.empty(nGrid, dtype=QuantisedEchogram)
    for n in range(nGrid):
        value = echogram.value[echo2gridMap == n]
        # print(len(echogram.value[echo2gridMap == n]))
        time = echogram.time[echo2gridMap == n]
        isActive = False if np.size(time) == 0 else True
        q_echogram[n] = QuantisedEchogram(value, time, isActive)
    return q_echogram
