import time
import logging
import unittest
from collections import defaultdict
import tempfile

import scipy

from lws.localize import Environment, HMMLocalizer, LMSELocalizer, Measurements, errorsFromLists, PFLocalizer
from lws.evaluate import Evaluator

from utils.path import path

HERE = path(__file__).parent.abspath()
OBJFILE = HERE.joinpath('data/umic.obj')
LOCFILE = HERE.joinpath('data/locations.txt')
VI_PATH_MAT5 = HERE.joinpath('data/apdata/iconia.basic5mat/umic_%s.dat')
SYNTHETIC_VI_PATH = HERE.joinpath('data/apdata/iconia_1/umic_%s.dat')

TRACKED_PATH = path(r'D:\loco-dev\dirk\thesis\results\tracked_path')

# Hmmf, this is a bad way
TMP_DIR = path(tempfile.gettempdir()).joinpath('lws_tests')
if not TMP_DIR.exists():
    TMP_DIR.mkdir()

APS = [
        ('104', 44.8, 15.0, -1.0), 
        ('106', 36.9, 7.5, 5.1), 
        ('107', 35.3, 14.2, 1.0), 
        ('108', 36.1, 4.4, 1.0), 
        ('109p2', 51.7, 3.0, 1.0), 
        ('110p2', 45.8, 15.0, 3.2), 
        ('114', 36.6, 6.5, -1.5), 
        ('171', 36.5, 9.55, 2.0), 
        ('172', 23.4, 5.8, 2.6), 
        ('edueg031', 41.6, 7.4, -1.0), 
        ('eduegcorr1', 12.6, 4.9, -0.75), 
        ('eduegcorr2', 31.6, 11.3, -1.0), 
        ('eduegdemo', 28.7, 14.6, -2.6), 
        ('eduog1108', 11.6, 6.4, 2.0), 
        ('eduog1server', 48.5, 6.4, 2.0), 
        ('eduog2206', 5.7, 6.35, 5.2), 
        ('eduog2corr', 32.0, 11.2, 5.5), 
        ('eduog2kitchen', 19.2, 9.2, 5.5), 
        ('freya', 32.6, 10.5, 1.0),
        ('gaia', 45.8, 10.6, 1.0), 
        ('hyperion', 39.3, 1.0, 0.1), 
        ('iris', 9.8, 10.4, 0.1),
    ]
    
class LwsTest(unittest.TestCase):
    synth_track_0 = HERE.joinpath('data/synthetic/measurements_0.txt')
    
    def setUp(self):
        self.tmp_dir = TMP_DIR
    
    def tearDown(self):
        pass
        #self.tmp_dir.rmtree()
    
    def testSyntheticNoNoiseSequential(self):
        self.env = Environment(objfile=OBJFILE, locationfile=LOCFILE, 
            tmpdir=self.tmp_dir, aps=APS, vi_path=SYNTHETIC_VI_PATH, davalues=[])
        
        ms = Measurements(fname=self.synth_track_0)
        
        localizer = LMSELocalizer(self.env, cubewidth=2, verbose=False)
        full_run_paths, _ = localizer.evaluateMeasurements(ms)
        
        t = time.time()
        seq_path = []
        for m in ms:
            paths, _ = localizer.evaluateNext(Measurements([m]))
            self.assertEqual(len(paths['seq']), 1)
            seq_path.append(paths['seq'][0])
        
        self.assertEqual(seq_path[0], (89, 33, 11))
        
        for i, (p1, p2) in enumerate(zip(seq_path, full_run_paths['seq'])):
            self.assertEqual(p1, p2, 'pos %s: %s != %s' % (i, p1, p2))
            
        print '%.3f sec' % (time.time() - t)
        
        localizer = HMMLocalizer(self.env, cubewidth=2, verbose=False)
        t = time.time()
        full_run_paths, _ = localizer.evaluateMeasurements(ms)
        print 'full %.3f sec' % (time.time() - t)
        
        localizer.reset()
        
        t = time.time()
        seq_path = []
        for m in ms:
            paths, _ = localizer.evaluateNext(Measurements([m]))
            #~ self.assertEqual(len(paths['seq']), 1)
            for p in paths['seq']:
                seq_path.append(p)
        
        self.assertEqual(seq_path[0], (89, 33, 11))
        
        for i, (p1, p2) in enumerate(zip(seq_path, full_run_paths['seq'])):
            self.assertEqual(p1, p2, 'pos %s: %s != %s' % (i, p1, p2))
        
        print 'sequential %.3f sec' % (time.time() - t)
        
        
    def testSyntheticNoNoise(self):
        
        self.env = Environment(objfile=OBJFILE, locationfile=LOCFILE, 
            tmpdir=self.tmp_dir, aps=APS, vi_path=SYNTHETIC_VI_PATH, davalues=[])
        
        ms = Measurements(fname=self.synth_track_0)
        
        
        ## Test LMSE 
        localizer = LMSELocalizer(self.env, cubewidth=2, verbose=True)
        
        paths, measurements = localizer.evaluateMeasurements(ms)
        self.assertEqual(len(paths), 1)
        
        # ['end', 'seq', 'seq_avg']
        real_path, seq_path = localizer.toRealWordPaths(measurements, paths['seq'])
        err3ds = errorsFromLists(real_path, seq_path)
        
        self.assertLess(scipy.mean(err3ds), 0.43) # seen 0.416467731725
        
        ## Test HMM 
        localizer = HMMLocalizer(self.env, cubewidth=2, prune_to=10000, num_threads=4, verbose=True)
        
        paths, measurements = localizer.evaluateMeasurements(ms)
        self.assertEqual(len(paths), 3)
        
        real_path, est_path, seq_avg_path = localizer.toRealWordPaths(measurements, paths['end'], paths['seq_avg'])
        err3ds = errorsFromLists(real_path, est_path)
        
        self.assertLess(scipy.mean(err3ds), 0.42) # seen 0.409063706859
        
        err3ds = errorsFromLists(real_path, seq_avg_path)
        self.assertLess(scipy.mean(err3ds), 0.48) # seen 0.466431152044
        
        ## Test PF 
        localizer = PFLocalizer(self.env, cubewidth=2, num_particles=10000, do_blocking=True, verbose=True)
        
        paths, measurements = localizer.evaluateMeasurements(ms)
        self.assertEqual(len(paths), 3)
        
        real_path, est_path, seq_avg_path = localizer.toRealWordPaths(measurements, paths['end'], paths['seq_avg'])
        err3ds = errorsFromLists(real_path, est_path)
        
        self.assertLess(scipy.mean(err3ds), 1.0) # seen 0.993223448396
        
        err3ds = errorsFromLists(real_path, seq_avg_path)
        
        self.assertLess(scipy.mean(err3ds), 0.53) # seen 0.5188913299808956 
        
        
        #~ print err3d, err2d
        
       
    def testEvalAll(self):
        #~ pfconfig = {'num_particles': 10000, 'do_blocking': True, 'smooth': 1.0}
        #~ optrun = 'basic5mat'
        #~ device= 'iconia'
        #~ evaluator = Evaluator(optrun, device, self.env, 1, TRACKED_PATH, 
                    #~ algo='pf', verbose=False, output_html=False, pfconfig=pfconfig,
                    #~ errortype='mean', interpolate=False)
    
        
        print 'stuff'
        
        
if __name__ == "__main__":
    logging.basicConfig(level=logging.DEBUG)
    suite = unittest.TestSuite()

    #~ suite.addTest(LwsTest("testSyntheticNoNoise"))
    suite.addTest(LwsTest("testSyntheticNoNoiseSequential"))
    
    
    unittest.TextTestRunner(verbosity=2).run(suite)