"""
.. module: ransac
   :synopsis: Implementace RANSAC (RANdom SAmple Consensus) iterativní metody pro hledání matematických modelů v množině bodů.

"""

from clifford.cga import CGA, Round, Translation
from clifford.tools.g3c import *
import numpy as np
from datetime import datetime
from random import randint

from elements import * ## clp

_wallWidth = 0.1    # width of the wall to be checked for inliners
_radiusTolerance = 0.03  # translates to % difference in radius, which will be accepted

_candidateTreshold = 5 # Experimentálně určený dostatečný počet vhodných kandidátů v pointcloudu
_iterationTreshold = 1000 # Experimentálně určený dostatečný počet testovaných kandidátů
_inlinerTreshold = 1000  # Experimentálně určený dostatečný počet přislušných bodů
_inlinerTresholdCircle = 200
_dominantTreshold = 6200

_spaceDimensions = 3 # Pracujeme ve 3D prostoru

# Inicializace CGA parametrů
g3, blades = Cl(3)
cga = CGA(g3)  # make cga from existing ga
locals().update(cga.blades)  # put ga's blades in local namespace


class RanSac:

    @staticmethod
    def findSphere(pointcloud, targetRadius, radiusTolerance, wallWidth):
        start_time = datetime.now()
        print("Start of search for sphere of ", targetRadius, " radius")
        candidates = []
        candidInliners = []
        indexlist = []

        pointsCnt = int(np.asarray(pointcloud.points).size / _spaceDimensions)
        print(pointsCnt)

        iterCnt = 0
        sphFound = False

        # Najdi alespoň _candidateTreshold kandidátních sfér s alespoň inlinerTreshold body
        while len(candidates) < _candidateTreshold and iterCnt < _iterationTreshold and sphFound == False:

            # Vybere náhodné 4 indexy z Pointcloudu
            ran = [None,None,None,None]
            for i in range(len(ran)):
                tmp = randint(0, pointsCnt - 1)
                # Check whether the random index was not selected before
                while tmp in ran:
                    tmp = randint(0, pointsCnt - 1)
                ran[i] = tmp
            # Vytvoř z náhodných bodů sféru v CGA
            sphere = Sphere.fromPointcloud(pointcloud, ran[0], ran[1], ran[2], ran[3])
            # Pokud poloměr vytvořené sféry neodpovídá hledanému, přeskoč na další iteraci
            try:
                tstRadius = Sphere.radius(sphere)
                if ((tstRadius > targetRadius * (1 + radiusTolerance))
                    or (tstRadius < targetRadius * (1 - radiusTolerance))):
                    iterCnt += 1
                    continue

                # Spočítej počet příslušných bodů s vytvořenou sférou
                inlinerCnt = 0
                inliner_list = []
                print("COUNTING INLINERS")
                for i in range(pointsCnt - 1):
                    if Sphere.isInliner3D(sphere, wallWidth, pointcloud.points[i]):
                        inlinerCnt += 1
                        inliner_list.append(i)

                # Jestli je počet příslušných bodů větší než limit, přidej sféru ke kandidátům
                if inlinerCnt > _inlinerTreshold:
                    candidates.append(sphere)
                    candidInliners.append(inlinerCnt)
                    indexlist.append(inliner_list)
                    if inlinerCnt > _dominantTreshold:
                        sphFound=True
                    print("Candidate number", len(candidInliners),"found", inlinerCnt)
                else:
                    print("not enough Inliners found. Only ", inlinerCnt, "found ")

                iterCnt += 1
                if iterCnt == _iterationTreshold and len(candidates)==0:
                    print("No sphere of radius", targetRadius, "found")
                    break
            except:
                print("Fail")


        # seřazený seznam indexů kandidátů dle počtu příslušných bodů
        rankedCandidates = sorted(
            candidInliners,
            reverse = True
        )
        # vrať nejlepšího kandidáta u kterého se podaří vytvořit vhodnou sféru
        for i in range(len(rankedCandidates)):
            sphere = Sphere.fit(pointcloud, indexlist[Sphere.find_max_list(indexlist)])
            radiusOrig = Sphere.radius(candidates[i])
            centerOrig = Point.to3D(Sphere.center(candidates[i]))
            print("Original radius",radiusOrig, ", original center",centerOrig)
            radiusFit  = Sphere.radius3(sphere)
            centerFit = Point.to3D(Sphere.center2(sphere))
            print("LMS radius",radiusFit, ", LMS center",centerFit)

            # zkontroluj, jestli je sféra správně vytvořená
            if radiusOrig/radiusFit < 1.02 and radiusOrig/radiusFit > 0.98:
                print("Sphere found succesfully!")
                print("====================================")
                time_elapsed = datetime.now() - start_time
                print('Time elapsed {}'.format(time_elapsed))
                return sphere
