#!/usr/bin/python

import sys
sys.path.append(".")
sys.path.append("..")
sys.path.append("../..") # Adds higher directory to python modules path

import os
import time
import numpy as np
from libs.spacemap import SpaceBlock
from libs.spacemap import SpaceRay
from libs.spacemap import SpaceMap
from libs.plotting import plotSpace


def test():

    def setWalls(i, j, orientation_deg):
        penetrations[i, j] = wall_penetration
        reflections[i, j] = wall_reflection
        orientations[i, j] = orientation_deg / 180 * np.pi

    def setGlasses(i, j, orientation_deg):
        penetrations[i, j] = glass_penetration
        reflections[i, j] = glass_reflection
        orientations[i, j] = orientation_deg / 180 * np.pi

    penetrations = np.zeros((64, 64), dtype=float)
    reflections = np.ones((64, 64), dtype=float) * -100.0
    orientations = np.empty((64, 64), dtype=float) * float('nan')

    spacemap = SpaceMap(width=6.4, length=6.4, block_size=0.1)

    # add walls
    wall_penetration = -30.0
    wall_reflection = -10.0
    glass_penetration = -3.0
    glass_reflection = -3.0
    # walls
    for j in range(5, 61):  # left
        setWalls(7, j, 90)
    for i in range(7, 62):  # up
        setWalls(i, 60, 0)
    for j in range(29, 60):  # right
        setWalls(61, j, 90)
    for j in range(5, 29):   # right
        setGlasses(61, j, 90)
    for i in range(7, 62):  # down
        setWalls(i, 5, 0)
    spacemap.setLosses(penetrations, reflections)
    spacemap.setOrientations(orientations)

    plotSpace(spacemap, cminmax=(-50, 0.0))

    rx_locs = [SpaceBlock(3.2, 3.3), SpaceBlock(3.2, 3.4), SpaceBlock(3.2, 3.5)]
    spacemap.traceRays(-40, SpaceBlock(3.2, 3.2))
    for rx_loc in rx_locs:
        rx_loc_rss, rx_loc_paths = spacemap.traceRay(rx_loc)
        multipaths = []
        for path in rx_loc_paths:
            print("found path: {}".format(path))
            multipaths.extend(list(path))
        print("rss = {:.6f}dB".format(rx_loc_rss))
        plotSpace(spacemap, space_rays=multipaths, cminmax=(-50, 0.0))
        


if __name__ == "__main__":
    test()