#!/usr/bin/env python
from __future__ import division
"""MODULE_DESCRIPTION"""

__author__ = "Nick Sweet"
__copyright__ = "Copyright 2015, Cohrint"
__credits__ = ["Nick Sweet", "Nisar Ahmed"]
__license__ = "GPL"
__version__ = "1.0.0"
__maintainer__ = "Nick Sweet"
__email__ = "nick.sweet@colorado.edu"
__status__ = "Development"

import logging
import numpy as np
import scipy as sp
import math
import time

from cops_and_robots.fusion.gaussian_mixture import (GaussianMixture,
                                                     fleming_prior,
                                                     uniform_prior,
                                                     )
from cops_and_robots.fusion.grid import Grid
from cops_and_robots.fusion.filter import Filter
from cops_and_robots.fusion.variational_bayes import VariationalBayes
from cops_and_robots.fusion.softmax import (geometric_model,
                                            neighbourhood_model,
                                            product_model, 
                                            )

class GaussSumFilter(Filter):
    """docstring for GaussSumFilter

    Fusion methods describe how to perform data fusion, with sequential updating
    at each time step, full batch doing a complete batch update of all sensor
    information from the initial prior, and windowed batch fusing all sensor
    information provided within a specific window.

    Compression methods describe how a batch (full or windowed) fusion is
    performed. Product is exact fusion, neighbourhood uses a reduced number
    of neighbour classes near the joint measurement class, and geometric uses
    the minimum number of classes next to the joint measurement class.

    """
    fusion_methods = ['sequential', 'full batch', 'windowed batch']
    compression_methods = ['product', 'neighbourhood', 'geometric']

    def __init__(self, 
                 fusion_method='sequential',
                 compression_method='geometric',
                 window=1,
                 *args,
                 **kwargs
                 ):
        super(GaussSumFilter, self).__init__(probability_type='gaussian_mixture',
                                             *args, **kwargs)

        self.fusion_method = fusion_method
        self.compression_method = compression_method
        self.window = window

        # Set up the VB fusion parameters
        self.vb = VariationalBayes()

    def _human_update(self, human_sensor):

        # No update if human sensor doesn't have a statement
        if not self._verify_human_update(human_sensor):
            return


        # Pause Rosbag process (if using one)
        if self.rosbag_process is not None:
            logging.info('Stopped rosbag to do fusion...')
            self.rosbag_process.stdin.write(' ')  # stop 
            self.rosbag_process.stdin.flush()
            time.sleep(0.5)

        if self.fusion_method == 'sequential':
            self.fusion(human_sensor)
        elif self.fusion_method in ['full batch', 'windowed batch']:
            self.batch_fusion(human_sensor)

        # Resume Rosbag process (if using one)
        if self.rosbag_process is not None:
            self.rosbag_process.stdin.write(' ')  # start rosbag
            self.rosbag_process.stdin.flush()
            logging.info('Restarted rosbag!')


    def batch_fusion(self, human_sensor):

        #<>TODO: update from new human_sensor class!
        measurement = human_sensor.get_statement_likelihood(discretized=False)
        self.measurements.append(measurement)
        # self.measurements.append(measurement)

        if len(self.measurements) >= self.window:
            
            # Create combined measurement labels
            measurement_labels = []
            for measurement in self.measurements:
                measurement_labels.append(measurement['relation'])
            measurement_label = " + ".join(measurement_labels)

            # Concatenate softmax models
            models = []
            for measurement in self.measurements:
                grounding = measurement['grounding']
                relation_class = measurement['relation class']
                model = grounding.relations.binary_models[relation_class]
                models.append(model)

            # Compress the likelihood
            if self.compression_method == 'product':
                likelihood = product_model(models)
            elif self.compression_method == 'neighbourhood':
                likelihood = neighbourhood_model(models, measurement_labels)
            elif self.compression_method == 'geometric':
                likelihood = geometric_model(models, measurement_labels)

            # Perform fusion
            self.fusion(likelihood, measurement_label, human_sensor)

            # Discard measurements for windowed, increase window size for full
            if self.fusion_method == 'windowed batch':
                self.measurements = []
            elif self.fusion_method == 'full batch':
                self.window += self.window


    def fusion(self, human_sensor):
        likelihood, label = human_sensor.get_statement_likelihood(discretized=False)

        prior = self.probability.copy()

        # Perform fusion
        if type(likelihood) is list:
            self.multi_likelihood_fusion(likelihood, label, human_sensor)
        else:
            self.probability.measurement_update(likelihood, label)

        # Include human false alarm rate
        posterior_weight = 1 - human_sensor.false_alarm_prob
        self.probability.combine_gms(prior, posterior_weight)


    def multi_likelihood_fusion(self, likelihoods, measurement_label, human_sensor):