import csv
import numpy as np
import math
import cv2
import scipy.optimize
import time


def vlen(v):
    return np.sqrt(np.dot(v, v.T))

def normalize(v):
    return v/np.linalg.norm(v)

def angle_v3v3(v1, v2):
    return np.dot(v1, v2)/vlen(v1)/vlen(v2)

def div_last_coord(pts):
    return np.vstack((pts[0, :]/pts[2, :], pts[1, :]/pts[2, :]))

class Muscle:
    def __init__(self, name, type):
        self.name = name
        self.type = type

    def coeff(self, p):
        pass


class LinearMuscle(Muscle, object):
    def __init__(self, name, origin, insertion, omega, fs, ff, k):
        super(LinearMuscle, self).__init__(name, 'LINEAR')
        self.name = name
        self.origin = origin
        self.insertion = insertion
        self.omega = omega
        self.fs = fs
        self.ff = ff
        self.k = k

    def coeff(self, p):
        d = p - self.origin
        v = self.insertion - self.origin
        dlen = np.sqrt(np.dot(d, d))
        vlen = np.sqrt(np.dot(v, v))
        a = np.dot(d, v)/(dlen*vlen)
        if (math.acos(a) < self.omega/2) and (dlen < vlen*self.ff):
            if dlen > vlen*self.fs:
                r = math.cos((dlen - vlen*self.fs)/(vlen*self.ff - vlen*self.fs)*math.pi/2)
            else:
                r = math.cos((1 - dlen/(vlen*self.fs)) * math.pi/2)
            c = self.k*a*r*d
        else:
            c = np.array([0., 0., 0.])
        return c

    def __str__(self):
        return self.name + " " + str(self.origin) + " -> " + str(self.insertion) + " omega=" + str(self.omega) + " fs=" + str(self.fs) + " ff=" + str(self.ff) + " k=" + str(self.k)

    def __repr__(self):
        return str(self)


class ParallelMuscle(Muscle, object):
    def __init__(self, name, origin, cp1, cp2, height, fs, ff, k):
        super(ParallelMuscle, self).__init__(name, 'PARALLEL')
        self.name = name
        self.origin = origin
        self.cp1 = cp1
        self.cp2 = cp2
        self.height = height
        self.fs = fs
        self.ff = ff
        self.k = k

    def coeff(self, p):
        pv1 = p - self.origin
        v1cp1 = self.cp1 - self.origin
        v1cp2 = self.cp2 - self.origin
        normcp1 = normalize(v1cp1)
        normcp2 = normalize(v1cp2)
        l1 = vlen(v1cp1)
        l2 = vlen(v1cp2)
        Fs = self.fs*l1
        Ff = self.ff*l1
        plane_norm = normalize(np.cross(normcp1, normcp2))
        D = np.dot(pv1, v1cp1)
        in_bounds = (D >= 0.0) and (D <= Ff) and (np.fabs(np.dot(pv1, normcp2)) <= l2) and (math.fabs(np.dot(pv1, plane_norm)) <= self.height/2.0)
        if in_bounds:
            if D > Fs:
                r = math.cos((D - Fs)/(Ff-Fs)*math.pi/2)
            else:
                r = math.cos((1 - D/Fs) * math.pi/2)
            c = self.k*r*normcp1
        else:
            c = np.array([0., 0., 0.])
        return c

class SphincterMuscle(Muscle, object):
    def __init__(self, name, origin, cp1, cp2, Fs, Ff, r, h):
        super(SphincterMuscle, self).__init__(name, 'SPHINCTER')
        self.name = name
        self.origin = origin
        self.cp1 = cp1
        self.cp2 = cp2
        self.r = r
        self.h = h
        self.fs = Fs
        self.ff = Ff

    def coeff(self, p):
        pv1 = self.origin - p
        v1cp1 = self.cp1 - self.origin
        v1cp2 = self.cp2 - self.origin
        normcp1 = normalize(v1cp1)
        normcp2 = normalize(v1cp2)
        l1 = vlen(v1cp1)
        l2 = vlen(v1cp2)
        plane_norm = normalize(np.cross(normcp1, normcp2))
        projected = pv1 - plane_norm*np.dot(pv1, plane_norm)
        D = vlen(projected)
        d1 = -np.dot(projected, normcp1)
        d2 = -np.dot(projected, normcp2)
        # mu = angle_v3v3(projected, v1cp1)
        # d1 = math.cos(mu)*D
        # mu = angle_v3v3(projected, v1cp2)
        # d2 = math.cos(mu)*D
        if (math.fabs(d1) < l1) and (math.fabs(d2) < l2):
            pv1 = normalize(pv1)
            norm = np.array([0., -1., 0.])
            c = np.array([0., 0., 0.])
            c += normcp1*d1*(1 - self.r / l1)
            c += normcp2*d2*(1 - self.r / l2)
            c += norm*self.h*min(self.r/max(l1,l2), (1 - D*D / (self.ff * self.ff)))
        else:
            c = np.array([0., 0., 0.])
        return c

class Tracker:
    def __init__(self, K, points, muscles, jaw_bone, jaw, static_points, frame, img_pos, scale = 1.0, window_size = 30, optimize_global = False):
        self.K = K
        self.points = points
        self.muscles = muscles
        self.jaw_bone = jaw_bone
        self.jaw = jaw
        self.static_points = static_points
        self.prev_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        if scale != 1.0:
            self.prev_frame = cv2.resize(self.prev_frame, None, fx=scale, fy=scale)
        self.prev_points = img_pos
        self.jaw_residual = np.zeros((1, 2))
        self.residuals = np.zeros((len(self.points), 2))
        self.prev_x = np.zeros((len(muscles), 1))
        self.prev_jaw = 0.0
        self.coeffs = []
        self.scale = scale
        self.bounds = []
        self.window_size = window_size
        self.optimize_global = optimize_global
        for i in range(0, len(self.muscles)):
            self.bounds.append((0.0, 1.0))
        for pid in xrange(0, len(self.points)):
            self.coeffs.append(np.zeros([3, self.prev_x.shape[0]]))
            for i in xrange(0, len(self.muscles)):
                self.coeffs[pid][:, i] = self.muscles[i].coeff(self.points[pid])

    def find_P(self, pts):
        _, rvecs, tvecs = cv2.solvePnP(np.array(self.points)[self.static_points], np.float32(pts)[self.static_points].reshape(-1, 1, 2), self.K,  np.zeros(4), flags=cv2.CV_EPNP)
        R, _ = cv2.Rodrigues(rvecs)
        R = np.matrix(R)
        P = self.K*np.hstack((R, np.matrix(tvecs)))
        return P

    @staticmethod
    def rotm_x(angle):
        R = np.matrix([[1.0, 0.0,             0.0             ],
                       [0.0, math.cos(angle), -math.sin(angle)],
                       [0.0, math.sin(angle), math.cos(angle) ]
                       ])
        return R

    @staticmethod
    def jaw_rot(xrot, point, bone, P):
        b = point - bone
        # TODO CHECK CHECK!!!!
        b = Tracker.rotm_x(xrot/2)*np.asmatrix(b).T
        npoint = bone + b.T
        return div_last_coord(P*np.hstack((npoint, [[1.0]])).T)

    @staticmethod
    def jaw_error(xrot, prev_img, point, image_coords, bone, P, return_vector = False):
        cur_img = Tracker.jaw_rot(xrot, point, bone, P)
        v = image_coords - (cur_img - prev_img).T
        if return_vector:
            return v
        return vlen(v)

    @staticmethod
    def jaw_derivative(xrot, prev_img, point, image_coords, bone, P):
        b = point - bone
        A = P*np.hstack((bone + (Tracker.rotm_x(xrot/2)*np.asmatrix(b).T).T, [[1.0]])).T
        Ad = P[:, 1:3]*0.5*np.array([[-math.sin(xrot/2.0)*b[1] - math.cos(xrot/2.0)*b[2]],
                                     [ math.cos(xrot/2.0)*b[1] - math.sin(xrot/2.0)*b[2]]])
        err_x = 2.0*(A[0]*Ad[2] - Ad[0]*A[2])/(A[2]*A[2])*(image_coords[0, 0] + A[0]/A[2] - prev_img[0])
        err_y = 2.0*(A[1]*Ad[2] - Ad[1]*A[2])/(A[2]*A[2])*(image_coords[0, 1] + A[1]/A[2] - prev_img[1])
        return err_x + err_y

    @staticmethod
    def project(x, point, P, coeffs):
        #coeffs = np.zeros([3, x.shape[0]])
        #for i in xrange(0, len(muscles)):
        #    coeffs[:, i] = muscles[i].coeff(point)
        px = point - np.dot(coeffs, x).T
        p = np.dot(P, np.vstack((px.T, 1)))
        xx = p[0, 0]
        yy = p[1, 0]
        zz = p[2, 0]
        xx = xx/zz
        yy = yy/zz
        return p/p[2, 0]

    @staticmethod
    def error(x, p0, point, image_coords, P, coeffs, return_vector=False):
        # p0 = project(prev_x, point, prev_P, coeffs)
        p1 = Tracker.project(np.asmatrix(x).T, point, P, coeffs)
        v = image_coords - p1[0:2] + p0[0:2]  # np.array([image_coords[0] - xx + xn, image_coords[1] - yy + yn])
        if return_vector:
            return v
        return vlen(v.T)

    @staticmethod
    def gradient(x, p0, point, image_coords, P, coeffs):
        # p0 = project(prev_x, point, prev_P, coeffs)
        point = np.asmatrix(point).T
        x = np.asmatrix(x).T
        px = point - coeffs*x
        p = P*np.vstack((px, 1))
        # xx = p[0, 0]
        # yy = p[1, 0]
        zz = p[2, 0]
        p12 = p[0:2, 0]
        p_coeffs = P[:, 0:3]*coeffs
        result = (np.asarray(p_coeffs[2, :])*np.asarray(p12) - p_coeffs[0:2, :]*zz)/(zz*zz)

        result = np.dot(result.T, -2.0*(image_coords - p12/zz + p0[0:2]))
        return result

    @staticmethod
    def global_error(x, p0, points, image_coords, P, coeffs):
        result = 0.0
        for i in range(0, len(points)):
            result += Tracker.error(x, p0[i], points[i], image_coords[i], P, coeffs[i])
        return result

    @staticmethod
    def global_gradient(x, p0, points, image_coords, P, coeffs):
        result = np.asmatrix(np.zeros(x.shape)).T
        for i in range(0, len(points)):
            result += Tracker.gradient(x, p0[i], points[i], image_coords[i], P, coeffs[i])
        return result


    def track(self, frame):
        lk_params = dict(winSize  = (self.window_size, self.window_size),
                  maxLevel = 3,
                  criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        if self.scale != 1.0:
            frame = cv2.resize(frame, None, fx=self.scale, fy=self.scale)
        p1, st, err = cv2.calcOpticalFlowPyrLK(self.prev_frame, frame, self.prev_points*self.scale, None, **lk_params)
        p1 /= self.scale
        #optimize from prev state
        x = np.asmatrix(self.prev_x.copy()).T
        prev_P = self.find_P(self.prev_points)
        P = self.find_P(p1)
        d = self.jaw_residual + [p1[self.jaw, 0] - self.prev_points[self.jaw, 0], p1[self.jaw, 1] - self.prev_points[self.jaw, 1]]
        prev_img = Tracker.jaw_rot(self.prev_jaw, self.points[self.jaw], self.jaw_bone, prev_P)
        xjaw, evals, ret = scipy.optimize.fmin_tnc(Tracker.jaw_error, self.prev_jaw, Tracker.jaw_derivative, args=(prev_img, self.points[self.jaw], d, self.jaw_bone, P), bounds = [(0.0, math.pi/4)], messages=0)
        self.jaw_residual = Tracker.jaw_error(xjaw, prev_img, self.points[self.jaw], d, self.jaw_bone, P, True)
        self.prev_jaw = xjaw

        if self.optimize_global:
            points = []
            coeffs = []
            ds = []
            p0s = []
            for i in range(0, len(p1)):
                if i == self.jaw or i in self.static_points or st[i] == 0:
                    continue
                d = self.residuals[i] + np.array([p1[i, 0] - self.prev_points[i, 0], p1[i, 1] - self.prev_points[i, 1]])
                d = np.asmatrix(d).T
                ds.append(d)
                p0s.append(Tracker.project(self.prev_x, self.points[i], prev_P, self.coeffs[i]))
                coeffs.append(self.coeffs[i])
                points.append(self.points[i])
            x, evals, ret = scipy.optimize.fmin_tnc(Tracker.global_error, x, Tracker.global_gradient, (p0s, points, ds, P, coeffs), bounds=self.bounds, messages=0)
        else:
            for i in range(0, len(p1)):
                if i == self.jaw or i in self.static_points or st[i] == 0:
                    continue
                d = self.residuals[i] + np.array([p1[i, 0] - self.prev_points[i, 0], p1[i, 1] - self.prev_points[i, 1]])
                d = np.asmatrix(d).T
                p0 = Tracker.project(self.prev_x, self.points[i], prev_P, self.coeffs[i])
                x, evals, ret = scipy.optimize.fmin_tnc(Tracker.error, x, Tracker.gradient, (p0, self.points[i], d, P, self.coeffs[i]), bounds=self.bounds, messages=0)

        for i in range(0, len(p1)):
            if i == self.jaw or i in self.static_points or st[i] == 0:
                continue
            p0 = Tracker.project(self.prev_x, self.points[i], prev_P, self.coeffs[i])
            d = self.residuals[i] + np.array([p1[i, 0] - self.prev_points[i, 0], p1[i, 1] - self.prev_points[i, 1]])
            d = np.asmatrix(d).T
            self.residuals[i] = Tracker.error(x, p0, self.points[i], d, P, self.coeffs[i], True).reshape(1, 2)
        self.prev_x = np.asmatrix(x).T
        self.prev_frame = frame
        self.prev_points = p1
        return self.prev_x, self.prev_jaw, P


class MTest:

    def __init__(self, name, scale = 1.0, window_size = 30, overwrite = {}, optimize_global = False):
        f = open('tests/' + name + '.txt', 'r')
        muscles_name = f.readline().strip()
        # not nice, but simple
        if 'muscles' in overwrite:
            muscles_name = overwrite['muscles']
        muscles = self.read_muscles('tests/' + muscles_name)
        keypoints_name = f.readline().strip()
        # and again
        if 'points' in overwrite:
            keypoints_name = overwrite['points']
        points, static_points, jaw = self.read_keypoints('tests/' + keypoints_name)
        self.video = cv2.VideoCapture('tests/' + f.readline().strip())
        _, frame = self.video.read()
        K = self.read_k('tests/' + f.readline().strip())
        x, y, z = f.readline().split(' ')
        jaw_bone = np.array([float(x), float(y), float(z)])
        n = int(f.readline())
        prev_points = []
        for i in range(0, n):
            x, y = f.readline().split(' ')
            prev_points.append(np.array([float(x), float(y)]))
        prev_points = np.float32(prev_points)

        n, nframes = (int(i) for i in f.readline().split(' '))
        kf = [int(i) for i in f.readline().split(' ')]
        self.muscle_values = {}
        for i in range(0, n):
            m_name = f.readline().strip()
            contr = [float(j) for j in f.readline().split(' ')]
            for j in range(0, nframes):
                if kf[j] not in self.muscle_values:
                    self.muscle_values[kf[j]] = {}
                self.muscle_values[kf[j]][m_name] = contr[j]

        self.tracker = Tracker(K, points, muscles, jaw_bone, jaw, static_points, frame, prev_points, scale, window_size, optimize_global)

        self.output = csv.writer(open('tests/' + name + '_' + str(scale) + '_' + str(window_size) + '_result.txt', 'wb'))
        header = [muscle.name for muscle in muscles]
        header.insert(0, 'jaw')
        header.insert(0, 'frame')
        self.output.writerow(header)
        self.err_output = csv.writer(open('tests/' + name + '_' + str(scale) + '_' + str(window_size) + '_error.csv', 'wb'))
        header = self.muscle_values.values()[0].keys()
        header.insert(0, 'frame')
        self.err_output.writerow(header)
        self.err_header = header

    def read_muscles(self, fname):
        f = open(fname, 'r')
        n = int(f.readline().strip())
        muscles = []
        for i in range(0, n):
            name = f.readline().strip()
            line = f.readline().strip()
            mtype, data = line.split(' ', 1)
            if mtype == 'LINEAR':
                ox, oy, oz, ix, iy, iz, omega, fs, ff, k = [float(i) for i in data.split(' ')]
                muscle = LinearMuscle(name, np.array([ox, oy, oz]), np.array([ix, iy, iz]), omega, fs, ff, k)
            elif mtype == 'PARALLEL':
                ox, oy, oz, cp1x, cp1y, cp1z, cp2x, cp2y, cp2z, height, fs, ff, k = [float(i) for i in data.split(' ')]
                muscle = ParallelMuscle(name, np.array([ox, oy, oz]), np.array([cp1x, cp1y, cp1z]), np.array([cp2x, cp2y, cp2z]), height, fs, ff, k)
            elif mtype == 'SPHINCTER':
                ox, oy, oz, cp1x, cp1y, cp1z, cp2x, cp2y, cp2z, fs, ff, r, h = [float(i) for i in data.split(' ')]
                muscle = SphincterMuscle(name, np.array([ox, oy, oz]), np.array([cp1x, cp1y, cp1z]), np.array([cp2x, cp2y, cp2z]), fs, ff, r, h)
            else:
                raise NameError("Unknown muscle type " + mtype)
            muscles.append(muscle)
        f.close()
        return muscles

    def read_keypoints(self, fname):
        f = open(fname, 'r')
        n = int(f.readline().strip())
        points = []
        static_points = []
        jaw = None
        for i in range(0, n):
            while True:
                line = f.readline()
                if line[0] != '#':
                    break
            type, id, x, y, z = line.split(' ')
            if type == "static":
                static_points.append(i)
            elif type == "jaw":
                jaw = i
            points.append(np.array([float(x), float(y), float(z)]))
        f.close()
        return points, static_points, jaw

    def read_k(self, fname):
        if '.npy' in fname:
            K = np.load(fname)
        else:
            f = open(fname, 'r')
            K = np.empty((3, 3))
            for i in range(0, 3):
                a, b, c = f.readline().split(' ')
                K[i, 0] = float(a)
                K[i, 1] = float(b)
                K[i, 2] = float(c)
            f.close()
        return K

    def do_frame(self):
        has_frame, frame = self.video.read()
        if not has_frame:
            return False, 0.0
        frame_cpy = frame.copy()
        self.frame_rgb = frame.copy()

        start = time.clock()
        x, xjaw, P = self.tracker.track(frame)
        took = time.clock() - start

        cv2.putText(frame_cpy, 'Jaw: ' + str(xjaw/math.pi*180.0), (10, 25), cv2.FONT_HERSHEY_PLAIN, 1, (255, 0, 0))
        ty = 40
        p1 = self.tracker.prev_points
        for i in range(0, len(p1)):
            cv2.circle(frame_cpy, (p1[i, 0], p1[i, 1]), 10, (0, 255, 0))

        print "Took " + str(took)
        cv2.putText(frame_cpy, "Took " + str(took), (10, ty), cv2.FONT_HERSHEY_PLAIN, 1, 255)
        ty += 20
        for i in range(0, len(self.tracker.muscles)):
            xv = x[i]
            cv2.putText(frame_cpy, self.tracker.muscles[i].name + ': ' + str(xv), (10, ty), cv2.FONT_HERSHEY_PLAIN, 1, (255, 0, 0))
            ty += 20
        cv2.imshow('img', frame_cpy)
        return True, took

    def solve(self):
        i = 1
        total_time = 0.0
        total_error = 0.0
        total_err_frames = 0
        while True:
            print "Frame #" + str(i)
            ret, took = self.do_frame()
            total_time += took
            if not ret:
                break
            if i in self.muscle_values:
                terr = 0.0
                err_out = [None]*(len(self.err_header))
                err_out[self.err_header.index('frame')] = i
                for mname, mval in self.muscle_values[i].iteritems():
                    if mname == 'jaw':
                        mval = mval/180.0*math.pi
                        err = ((self.tracker.prev_jaw[0] - mval)*(self.tracker.prev_jaw[0] - mval))/(math.pi/4.0)
                    else:
                        mid = next(k for k in range(0, len(self.tracker.muscles)) if self.tracker.muscles[k].name == mname)
                        err = ((self.tracker.prev_x[mid] - mval)*(self.tracker.prev_x[mid] - mval))[0, 0]
                    err_out[self.err_header.index(mname)] = err
                    terr += err
                total_error += terr
                total_err_frames += 1
                self.err_output.writerow(err_out)
                print "Total error for keyframe " + str(i) + " is " + str(terr)
            l = self.tracker.prev_x.T.tolist()[0]
            l.insert(0, self.tracker.prev_jaw[0])
            l.insert(0, i)
            self.output.writerow(l)
            i+=1
            while True:
                k = cv2.waitKey(1) & 0xFF
                if k == 27:
                    ret = False
                    break
                #if k == 32:
                break
                if k == ord('s'):
                    frame_cpy = self.frame_rgb.copy()
                    for j in range(0, self.tracker.prev_points.shape[0]):
                        cv2.circle(frame_cpy, (self.tracker.prev_points[j, 0], self.tracker.prev_points[j, 1]), 10, (0, 0, 255), -1)
                    cv2.imwrite('screenshots/' + str(i) + '.png', self.frame_rgb)
                    cv2.imwrite('screenshots/' + str(i) + '_marks.png', frame_cpy)
            if not ret:
                break
        self.err_output.writerow([])
        self.err_output.writerow(['Average time per frame', '%.2f'%(1000*total_time/i)])
        return total_error/total_err_frames/(len(self.err_header) - 1), total_time/i

if __name__ == '__main__':
    # tests = ['m1s4', 'm1s5', 'm2s1', 'm2s2', 'm2s3', 'm2s4', 'm2s5'] # ['1551', '1542', '1543', '1544', '1546', '1548', '1550']
    # scales = [1.0, 0.75, 0.5, 0.25]
    # window_sizes = [60, 50, 40, 30, 20, 15, 10, 5]
    # statsfile = open('stats.csv', 'ab')
    # stats = csv.writer(statsfile)
    # stats.writerow(['test', 'scale', 'window size', 'error', 'time per frame'])
    # for test_name in tests:
    #     print 'Test ' + test_name
    #     for scale in scales:
    #         for ws in window_sizes:
    #             test = MTest(test_name, scale, ws)
    #             avg_err, avg_time = test.solve()
    #             stats.writerow([test_name, scale, ws, avg_err, avg_time])
    #             statsfile.flush()
    #             print '---------------------'

    # statsfile = open('stats_mesh_m1.csv', 'wb')
    # stats = csv.writer(statsfile)
    # meshes = ['new', 'spartak', 'random']
    # tests1 = ['m1s1', 'm1s2', 'm1s3', 'm1s4', 'm1s5']
    # for test_name in tests1:
    #     print 'Test ' + test_name
    #     for mesh in meshes:
    #         test = MTest(test_name, best_scale, best_ws, {'muscles': 'muscles_' + mesh + '.txt', 'points': 'keypoints_' + mesh + '.txt'})
    #         avg_err, avg_time = test.solve()
    #         stats.writerow([test_name, mesh, avg_err, avg_time])
    #         statsfile.flush()
    #         print '---------------------'
    #
    # statsfile.close()
    # statsfile = open('stats_mesh_m2.csv', 'wb')
    # stats = csv.writer(statsfile)
    # stats.writerow(['test','mesh','error','time per frame'])
    # tests2 = ['m2s1', 'm2s2', 'm2s3', 'm2s4', 'm2s5']
    # for test_name in tests2:
    #     print 'Test ' + test_name
    #     for mesh in meshes:
    #         test = MTest(test_name, best_scale, best_ws, {'muscles': 'muscles_' + mesh + '.txt', 'points': 'keypoints_' + mesh + '.txt'})
    #         avg_err, avg_time = test.solve()
    #         stats.writerow([test_name, mesh, avg_err, avg_time])
    #         statsfile.flush()
    #         print '---------------------'

    best_scale = 0.5
    best_ws = 20
    tests1 = ['m1s1', 'm1s2', 'm1s3', 'm1s4', 'm1s5']
    for test_name in tests1:
        print 'Test ' + test_name
        test = MTest(test_name, best_scale, best_ws, {'muscles': 'muscles_spartak.txt', 'points': 'keypoints_spartak.txt'})
        avg_err, avg_time = test.solve()
        print '---------------------'
    tests2 = ['m2s1', 'm2s2', 'm2s3', 'm2s4', 'm2s5']
    for test_name in tests2:
        print 'Test ' + test_name
        test = MTest(test_name, best_scale, best_ws)
        avg_err, avg_time = test.solve()
        print '---------------------'