

import math
import numpy as np
import json
import hashlib
import base64
import time
import os
import cv2

try:
    # python-freenect
    import freenect
    import_freenect = True
except ImportError:
    import_freenect = False
    pass

__kinect_depth_encoded = ''
__kinect_video_encoded = ''



def encode_ndarray(data):
    result = {
        'dtype': str(data.dtype),
        'shape': data.shape,
        'data': base64.b64encode(data),
    }
    return result


def validate(jdata, md5):
    return md5 == hashlib.md5(jdata).hexdigest()

def md5(jdata):
    return hashlib.md5(jdata).hexdigest()

def md5_image(image):
    return md5(base64.b64encode(image))


def decode(data_dict):
    result = ()
    for _n, _data in data_dict.iteritems():
        dtype = _data['dtype']
        shape = _data['shape']
        b64data = _data['data']
        _result = np.fromstring(
                base64.b64decode(b64data),
                dtype=np.dtype(dtype), ).reshape(shape)
        result += (_result, )
    return result



if import_freenect:
    str_to_video_type = {
        'bayer': freenect.VIDEO_BAYER,
        'ir_10bit': freenect.VIDEO_IR_10BIT,
        'ir_10bit_packed': freenect.VIDEO_IR_10BIT_PACKED,
        'ir_8bit': freenect.VIDEO_IR_8BIT,
        'rgb': freenect.VIDEO_RGB,
        'yuv_raw': freenect.VIDEO_YUV_RAW,
        'yum_rgb': freenect.VIDEO_YUV_RGB
    }

    video_type_to_str = {
        freenect.VIDEO_BAYER: 'bayer',
        freenect.VIDEO_IR_10BIT: 'ir_10bit',
        freenect.VIDEO_IR_10BIT_PACKED: 'ir_10bit_packed',
        freenect.VIDEO_IR_8BIT: 'ir_8bit',
        freenect.VIDEO_RGB: 'rgb',
        freenect.VIDEO_YUV_RAW: 'yuv_raw',
        freenect.VIDEO_YUV_RGB: 'yum_rgb',
    }

    str_to_depth_type = {
        '10bit': freenect.DEPTH_10BIT,
        '10bit_packed': freenect.DEPTH_10BIT_PACKED,
        '11bit': freenect.DEPTH_11BIT,
        '11bit_packed': freenect.DEPTH_11BIT_PACKED
    }

    depth_type_to_str = {
        freenect.DEPTH_10BIT: '10bit',
        freenect.DEPTH_10BIT_PACKED: '10bit_packed',
        freenect.DEPTH_11BIT: '11bit',
        freenect.DEPTH_11BIT_PACKED: '11bit_packed',
    }

# http://nicolas.burrus.name/index.php/Research/KinectCalibration
depth_scale1 = [0.1236 * math.tan(i / 2842.5 + 1.1863) for i in xrange(2**11+100)]
# http://groups.google.com/group/openkinect/browse_thread/thread/31351846fd33c78/e98a94ac605b9f21
depth_scale2 = [1. / (-i * 0.0030711016 + 3.3309495161) for i in xrange(2**11+100)]
# https://github.com/vokimon/freenect_python_processing/blob/master/src/kinectHeadPose.py
# depth_scale3 = [325.62 / (1084 - i) for i in xrange(2**11+100)]


def float_image(size):
    return np.zeros(size[::-1], dtype=np.float)

def __raw_depth_to_m(raw_depth, scale, float_depth=None):
    size = raw_depth.shape
    float_d_img = float_depth or float_image(size)
    np.take(scale, raw_depth, axis=0, out=float_d_img, mode='clip')
    return float_d_img

def __pretty_depth_cv(self, depth):
    np.clip(depth, 0, 2**10 - 1, depth)
    depth >>= 2
    depth = depth.astype(np.uint8)
    return depth

def raw_depth_to_m1(raw_depth, float_depth=None):
    return __pretty_depth_cv(raw_depth)


def raw_depth_to_m2(raw_depth, float_depth=None):
    return __raw_depth_to_m(raw_depth, depth_scale1, float_depth)


def raw_depth_to_m3(raw_depth, float_depth=None):
    return __raw_depth_to_m(raw_depth, depth_scale2, float_depth)


# def raw_depth_to_m3(raw_depth, float_depth=None):
#     return __raw_depth_to_m(raw_depth, depth_scale3, float_depth)

def freenect_sync_get_video():
    if import_freenect:
        return cv2.cvtColor(freenect.sync_get_video()[0], cv2.COLOR_RGB2BGR)
    else:
        fp = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'lena.png')
        return cv2.imread(fp,0)

def freenect_sync_get_depth(pretty_depth=raw_depth_to_m1):
    if import_freenect:
        return pretty_depth(freenect.sync_get_depth()[0])
    else:
        fp = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'lena.png')
        return cv2.imread(fp,0)

def freenect_sync_stop():
    if import_freenect:
        freenect.sync_stop()