#!/usr/bin/env python

import math, numpy as np
#from enthought.mayavi import mlab
import matplotlib.pyplot as pp
import matplotlib.cm as cm

import scipy.ndimage as ni

import roslib; roslib.load_manifest('sandbox_tapo_darpa_m3')
import rospy
import tf
#import hrl_lib.mayavi2_util as mu
import hrl_lib.viz as hv
import hrl_lib.util as ut
import hrl_lib.transforms as tr
import hrl_lib.matplotlib_util as mpu 
import pickle

from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
from hrl_haptic_manipulation_in_clutter_msgs.msg import SkinContact

from hrl_haptic_manipulation_in_clutter_msgs.msg import TaxelArray
from m3skin_ros.msg import TaxelArray as TaxelArray_Meka
from hrl_msgs.msg import FloatArrayBare
from geometry_msgs.msg import Point
from geometry_msgs.msg import Vector3

def callback(data, callback_args):

    rospy.loginfo('Getting data!')
    tf_lstnr = callback_args
    sc = SkinContact()
    sc.header.frame_id = '/torso_lift_link' # has to be this and no other coord frame.
    sc.header.stamp = data.header.stamp

    t1, q1 = tf_lstnr.lookupTransform(sc.header.frame_id,
                                      data.header.frame_id,
                                      rospy.Time(0))
                                     
    t1 = np.matrix(t1).reshape(3,1)
    r1 = tr.quaternion_to_matrix(q1)

    force_vectors = np.row_stack([data.forces_x, data.forces_y, data.forces_z])
    contact_vectors = np.row_stack([data.centers_x, data.centers_y, data.centers_z])
    fmags = ut.norm(force_vectors)
    threshold = 0.3
    fmags_tuned = fmags - threshold
    fmags_tuned[np.where(fmags_tuned<0)]=0
    fmags_tuned_reshaped = fmags_tuned.reshape((16,24))

    fmags_96_tuned_reshaped = np.zeros((8,12))
    fmags_24_tuned_reshaped = np.zeros((4,6))
    fmags_6_tuned_reshaped = np.zeros((2,3))


    # Change Resolution

    fmags_96_tuned = np.zeros(96)
    fmags_24_tuned = np.zeros(24)
    fmags_6_tuned = np.zeros(6)
    fmags_1_tuned = np.zeros(1)

    i = 0
    j = 0
    k = 0
    l = 0
        
    while i < 16:
        while j < 24:
            fmags_96_tuned_reshaped[k][l] = fmags_tuned_reshaped[i][j] + fmags_tuned_reshaped[i][j+1] + fmags_tuned_reshaped[i+1][j] + fmags_tuned_reshaped[i+1][j+1]
            j=j+2
            l=l+1
        i=i+2
        k=k+1
        j=0
        l=0
    fmags_96_tuned = fmags_96_tuned_reshaped.reshape(96)

    i = 0
    j = 0
    k = 0
    l = 0

    while i < 8:
        while j < 12:
            fmags_24_tuned_reshaped[k][l] = fmags_96_tuned_reshaped[i][j] + fmags_96_tuned_reshaped[i][j+1] + fmags_96_tuned_reshaped[i+1][j] + fmags_96_tuned_reshaped[i+1][j+1]
            j=j+2
            l=l+1
        i=i+2
        k=k+1
        j=0
        l=0
    fmags_24_tuned = fmags_24_tuned_reshaped.reshape(24)

    i = 0
    j = 0
    k = 0
    l = 0

    while i < 4:
        while j < 6:
            fmags_6_tuned_reshaped[k][l] = fmags_24_tuned_reshaped[i][j] + fmags_24_tuned_reshaped[i][j+1] + fmags_24_tuned_reshaped[i+1][j] + fmags_24_tuned_reshaped[i+1][j+1]
            j=j+2
            l=l+1
        i=i+2
        k=k+1
        j=0
        l=0
    fmags_6_tuned = fmags_6_tuned_reshaped.reshape(6)

    fmags_1_tuned = np.sum(fmags_6_tuned)
            

    cx_arr = contact_vectors[0]
    cx_arr_reshaped = cx_arr.reshape((16,24))
    cy_arr = contact_vectors[1]
    cy_arr_reshaped = cy_arr.reshape((16,24))
    cz_arr = contact_vectors[2]
    cz_arr_reshaped = cy_arr.reshape((16,24))

    cx_arr_96_reshaped = np.zeros((8,12))
    cx_arr_24_reshaped = np.zeros((4,6))
    cx_arr_6_reshaped = np.zeros((2,3))
    cx_arr_1_reshaped = np.zeros(1)

    cy_arr_96_reshaped = np.zeros((8,12))
    cy_arr_24_reshaped = np.zeros((4,6))
    cy_arr_6_reshaped = np.zeros((2,3))
    cy_arr_1_reshaped = np.zeros(1)

    cz_arr_96_reshaped = np.zeros((8,12))
    cz_arr_24_reshaped = np.zeros((4,6))
    cz_arr_6_reshaped = np.zeros((2,3))
    cz_arr_1_reshaped = np.zeros(1)

    cx_arr_96 = np.zeros(96)
    cx_arr_24 = np.zeros(24)
    cx_arr_6 = np.zeros(6)
    cx_arr_1 = np.zeros(1)

    cy_arr_96 = np.zeros(96)
    cy_arr_24 = np.zeros(24)
    cy_arr_6 = np.zeros(6)
    cy_arr_1 = np.zeros(1)

    cz_arr_96 = np.zeros(96)
    cz_arr_24 = np.zeros(24)
    cz_arr_6 = np.zeros(6)
    cz_arr_1 = np.zeros(1)

    contact_vectors_96 = np.zeros((3,96))
    contact_vectors_24 = np.zeros((3,24))
    contact_vectors_6 = np.zeros((3,6))
    contact_vectors_1 = np.zeros((3,1))
    
    i = 0
    j = 0
    k = 0
    l = 0

    while i < 16:
        while j < 24:
            cx_arr_96_reshaped[k][l] = np.mean((cx_arr_reshaped[i][j],cx_arr_reshaped[i][j+1],cx_arr_reshaped[i+1][j],cx_arr_reshaped[i+1][j+1]))
            cy_arr_96_reshaped[k][l] = np.mean((cy_arr_reshaped[i][j],cy_arr_reshaped[i][j+1],cy_arr_reshaped[i+1][j],cy_arr_reshaped[i+1][j+1]))
            cz_arr_96_reshaped[k][l] = np.mean((cz_arr_reshaped[i][j],cz_arr_reshaped[i][j+1],cz_arr_reshaped[i+1][j],cz_arr_reshaped[i+1][j+1]))
            j=j+2
            l=l+1
        i=i+2
        k=k+1
        j=0
        l=0
    cx_arr_96 = cx_arr_96_reshaped.reshape(96)
    cy_arr_96 = cy_arr_96_reshaped.reshape(96)
    cz_arr_96 = cz_arr_96_reshaped.reshape(96)

    i = 0
    j = 0
    k = 0
    l = 0

    while i < 8:
        while j < 12:
            cx_arr_24_reshaped[k][l] = np.mean((cx_arr_96_reshaped[i][j],cx_arr_96_reshaped[i][j+1],cx_arr_96_reshaped[i+1][j],cx_arr_96_reshaped[i+1][j+1]))
            cy_arr_24_reshaped[k][l] = np.mean((cy_arr_96_reshaped[i][j],cy_arr_96_reshaped[i][j+1],cy_arr_96_reshaped[i+1][j],cy_arr_96_reshaped[i+1][j+1]))
            cz_arr_24_reshaped[k][l] = np.mean((cz_arr_96_reshaped[i][j],cz_arr_96_reshaped[i][j+1],cz_arr_96_reshaped[i+1][j],cz_arr_96_reshaped[i+1][j+1]))
            j=j+2
            l=l+1
        i=i+2
        k=k+1
        j=0
        l=0
    cx_arr_24 = cx_arr_24_reshaped.reshape(24)
    cy_arr_24 = cy_arr_24_reshaped.reshape(24)
    cz_arr_24 = cz_arr_24_reshaped.reshape(24)

    i = 0
    j = 0
    k = 0
    l = 0

    while i < 4:
        while j < 6:
            cx_arr_6_reshaped[k][l] = np.mean((cx_arr_24_reshaped[i][j],cx_arr_24_reshaped[i][j+1],cx_arr_24_reshaped[i+1][j],cx_arr_24_reshaped[i+1][j+1]))
            cy_arr_6_reshaped[k][l] = np.mean((cy_arr_24_reshaped[i][j],cy_arr_24_reshaped[i][j+1],cy_arr_24_reshaped[i+1][j],cy_arr_24_reshaped[i+1][j+1]))
            cz_arr_6_reshaped[k][l] = np.mean((cz_arr_24_reshaped[i][j],cz_arr_24_reshaped[i][j+1],cz_arr_24_reshaped[i+1][j],cz_arr_24_reshaped[i+1][j+1]))
            j=j+2
            l=l+1
        i=i+2
        k=k+1
        j=0
        l=0
    cx_arr_6 = cx_arr_6_reshaped.reshape(6)
    cy_arr_6 = cy_arr_6_reshaped.reshape(6)
    cz_arr_6 = cz_arr_6_reshaped.reshape(6)

    cx_arr_1 = np.mean(cx_arr_6)
    cy_arr_1 = np.mean(cy_arr_6)
    cz_arr_1 = np.mean(cz_arr_6)    
    
    contact_vectors_96 = np.row_stack([cx_arr_96, cy_arr_96, cz_arr_96])
    contact_vectors_24 = np.row_stack([cx_arr_24, cy_arr_24, cz_arr_24])
    contact_vectors_6 = np.row_stack([cx_arr_6, cy_arr_6, cz_arr_6])
    contact_vectors_1 = np.row_stack([cx_arr_1, cy_arr_1, cz_arr_1])

    # Calculating no. of contact regions with hand-tuned force threshold
    
    contact_regions = fmags > threshold
    lb,ls = ni.label(contact_regions)
    total_contact = ni.sum(lb)# After thresholding, assuming one connected component (experiment designed that way)
    
    contact_regions_96 = fmags_96_tuned > 0
    lb_96,ls_96 = ni.label(contact_regions_96)
    total_contact_96 = ni.sum(lb_96)

    contact_regions_24 = fmags_24_tuned > 0
    lb_24,ls_24 = ni.label(contact_regions_24)
    total_contact_24 = ni.sum(lb_24)

    contact_regions_6 = fmags_6_tuned > 0
    lb_6,ls_6 = ni.label(contact_regions_6)
    total_contact_6 = ni.sum(lb_6)

    if fmags_1_tuned > 0:
        total_contact_1 = 1
    else:
        total_contact_1 = 0
    
    # Calculating force data for contact with hand-tuned force threshold
    temp = fmags*lb
    temp_96 = fmags_96_tuned*lb_96
    temp_24 = fmags_24_tuned*lb_24
    temp_6 = fmags_6_tuned*lb_6
 
    total_forces = ni.sum(temp)
    total_forces_96 = ni.sum(temp_96)
    total_forces_24 = ni.sum(temp_24)
    total_forces_6 = ni.sum(temp_6)
    total_forces_1 = fmags_1_tuned
        
    mean_forces = total_forces/total_contact # Don't Bother
    mean_forces_96 = mean_forces # Don't Bother
    mean_forces_24 = mean_forces # Don't Bother
    mean_forces_6 = mean_forces  # Don't Bother
    mean_forces_1 = mean_forces  # Don't Bother
    
    max_forces = np.max(temp)
    max_forces_96 = np.max(temp_96)
    max_forces_24 = np.max(temp_24)
    max_forces_6 = np.max(temp_6)
    max_forces_1 = fmags_1_tuned
    

    contact_point_local = [0,0,0]
    contact_point_world = [0,0,0]
    contact_point_local_96 = [0,0,0]
    contact_point_world_96 = [0,0,0]
    contact_point_local_24 = [0,0,0]
    contact_point_world_24 = [0,0,0]
    contact_point_local_6= [0,0,0]
    contact_point_world_6 = [0,0,0]
    contact_point_local_1= [0,0,0]
    contact_point_world_1 = [0,0,0]

    if total_contact >= 1:
        local_x = ni.mean(contact_vectors[0,:],lb) # After thresholding, assuming one connected component (experiment designed that way)
        local_y = ni.mean(contact_vectors[1,:],lb) # After thresholding, assuming one connected component (experiment designed that way)
        local_z = ni.mean(contact_vectors[2,:],lb) # After thresholding, assuming one connected component (experiment designed that way)
        contact_point_local = np.column_stack([local_x,local_y,local_z])
        contact_point_world = r1*(contact_point_local.T) + t1

    if total_contact_96 >= 1:
        local_x_96 = ni.mean(contact_vectors_96[0,:],lb_96) # After thresholding, assuming one connected component (experiment designed that way)
        local_y_96 = ni.mean(contact_vectors_96[1,:],lb_96) # After thresholding, assuming one connected component (experiment designed that way)
        local_z_96 = ni.mean(contact_vectors_96[2,:],lb_96) # After thresholding, assuming one connected component (experiment designed that way)
        contact_point_local_96 = np.column_stack([local_x_96,local_y_96,local_z_96])
        contact_point_world_96 = r1*(contact_point_local_96.T) + t1

    if total_contact_24 >= 1:
        local_x_24 = ni.mean(contact_vectors_24[0,:],lb_24) # After thresholding, assuming one connected component (experiment designed that way)
        local_y_24 = ni.mean(contact_vectors_24[1,:],lb_24) # After thresholding, assuming one connected component (experiment designed that way)
        local_z_24 = ni.mean(contact_vectors_24[2,:],lb_24) # After thresholding, assuming one connected component (experiment designed that way)
        contact_point_local_24 = np.column_stack([local_x_24,local_y_24,local_z_24])
        contact_point_world_24 = r1*(contact_point_local_24.T) + t1

    if total_contact_6 >= 1:
        local_x_6 = ni.mean(contact_vectors_6[0,:],lb_6) # After thresholding, assuming one connected component (experiment designed that way)
        local_y_6 = ni.mean(contact_vectors_6[1,:],lb_6) # After thresholding, assuming one connected component (experiment designed that way)
        local_z_6 = ni.mean(contact_vectors_6[2,:],lb_6) # After thresholding, assuming one connected component (experiment designed that way)
        contact_point_local_6 = np.column_stack([local_x_6,local_y_6,local_z_6])
        contact_point_world_6 = r1*(contact_point_local_6.T) + t1
    
    if total_contact_1 >= 1:
        local_x_1 = contact_vectors_1[0]
        local_y_1 = contact_vectors_1[1]
        local_z_1 = contact_vectors_1[2]
        contact_point_local_1 = np.column_stack([local_x_1,local_y_1,local_z_1])
        contact_point_world_1 = r1*(contact_point_local_1.T) + t1
 
    global time
    time = time + 0.01
    time_instant_data = [time,total_forces,mean_forces,max_forces,total_contact,contact_point_world[0],contact_point_world[1],contact_point_world[2]]
    time_instant_data_96 = [time,total_forces_96,mean_forces_96,max_forces_96,total_contact_96,contact_point_world_96[0],contact_point_world_96[1],contact_point_world_96[2]]
    time_instant_data_24 = [time,total_forces_24,mean_forces_24,max_forces_24,total_contact_24,contact_point_world_24[0],contact_point_world_24[1],contact_point_world_24[2]]
    time_instant_data_6 = [time,total_forces_6,mean_forces_6,max_forces_6,total_contact_6,contact_point_world_6[0],contact_point_world_6[1],contact_point_world_6[2]]
    time_instant_data_1 = [time,total_forces_1,mean_forces_1,max_forces_1,total_contact_1,contact_point_world_1[0],contact_point_world_1[1],contact_point_world_1[2]]
    global time_varying_data
    global time_varying_data_96
    global time_varying_data_24
    global time_varying_data_6
    global time_varying_data_1
    time_varying_data = np.row_stack([time_varying_data, time_instant_data])
    time_varying_data_96 = np.row_stack([time_varying_data_96, time_instant_data_96])
    time_varying_data_24 = np.row_stack([time_varying_data_24, time_instant_data_24])
    time_varying_data_6 = np.row_stack([time_varying_data_6, time_instant_data_6])
    time_varying_data_1 = np.row_stack([time_varying_data_1, time_instant_data_1])

def tracking_point():
    rospy.loginfo('Tracking Distance!')
    ta = time_varying_data
    ta_96 = time_varying_data_96
    ta_24 = time_varying_data_24
    ta_6 = time_varying_data_6
    ta_1 = time_varying_data_1
    
    k = 0
    for i in ta[:,0]:
        if i != ta[-1,0]:
            instant_dist = math.sqrt((ta[k+1,5]-ta[1,5])**2 + (ta[k+1,6]-ta[1,6])**2 + (ta[k+1,7]-ta[1,7])**2)
            time_instant_tracker = [ta[k+1,0], instant_dist]
            global time_varying_tracker
            time_varying_tracker = np.row_stack([time_varying_tracker,time_instant_tracker])
            k=k+1

    k = 0
    for i in ta_96[:,0]:
        if i != ta_96[-1,0]:
            instant_dist_96 = math.sqrt((ta_96[k+1,5]-ta_96[1,5])**2 + (ta_96[k+1,6]-ta_96[1,6])**2 + (ta_96[k+1,7]-ta_96[1,7])**2)
            time_instant_tracker_96 = [ta_96[k+1,0], instant_dist_96]
            global time_varying_tracker_96
            time_varying_tracker_96 = np.row_stack([time_varying_tracker_96,time_instant_tracker_96])
            k=k+1

    k = 0
    for i in ta_24[:,0]:
        if i != ta_24[-1,0]:
            instant_dist_24 = math.sqrt((ta_24[k+1,5]-ta_24[1,5])**2 + (ta_24[k+1,6]-ta_24[1,6])**2 + (ta_24[k+1,7]-ta_24[1,7])**2)
            time_instant_tracker_24 = [ta_24[k+1,0], instant_dist_24]
            global time_varying_tracker_24
            time_varying_tracker_24 = np.row_stack([time_varying_tracker_24,time_instant_tracker_24])
            k=k+1

    k = 0
    for i in ta_6[:,0]:
        if i != ta_6[-1,0]:
            instant_dist_6 = math.sqrt((ta_6[k+1,5]-ta_6[1,5])**2 + (ta_6[k+1,6]-ta_6[1,6])**2 + (ta_6[k+1,7]-ta_6[1,7])**2)
            time_instant_tracker_6 = [ta_6[k+1,0], instant_dist_6]
            global time_varying_tracker_6
            time_varying_tracker_6 = np.row_stack([time_varying_tracker_6,time_instant_tracker_6])
            k=k+1

    k = 0
    for i in ta_1[:,0]:
        if i != ta_1[-1,0]:
            instant_dist_1 = math.sqrt((ta_1[k+1,5]-ta_1[1,5])**2 + (ta_1[k+1,6]-ta_1[1,6])**2 + (ta_1[k+1,7]-ta_1[1,7])**2)
            time_instant_tracker_1 = [ta_1[k+1,0], instant_dist_1]
            global time_varying_tracker_1
            time_varying_tracker_1 = np.row_stack([time_varying_tracker_1,time_instant_tracker_1])
            k=k+1
    
    
def savedata():
    rospy.loginfo('Saving data!')

    ut.save_pickle(time_varying_data_96, '/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/96/Soft_Movable/Red_Pillow_Movable/time_varying_data_movable_red_pillow_trial_5.pkl')
    ut.save_pickle(time_varying_tracker_96, '/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/96/Soft_Movable/Red_Pillow_Movable/time_varying_tracking_data_movable_red_pillow_trial_5.pkl')

    ut.save_pickle(time_varying_data_24, '/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/24/Soft_Movable/Red_Pillow_Movable/time_varying_data_movable_red_pillow_trial_5.pkl')
    ut.save_pickle(time_varying_tracker_24, '/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/24/Soft_Movable/Red_Pillow_Movable/time_varying_tracking_data_movable_red_pillow_trial_5.pkl')

    ut.save_pickle(time_varying_data_6, '/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/6/Soft_Movable/Red_Pillow_Movable/time_varying_data_movable_red_pillow_trial_5.pkl')
    ut.save_pickle(time_varying_tracker_6, '/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/6/Soft_Movable/Red_Pillow_Movable/time_varying_tracking_data_movable_red_pillow_trial_5.pkl')

    ut.save_pickle(time_varying_data_1, '/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/1/Soft_Movable/Red_Pillow_Movable/time_varying_data_movable_red_pillow_trial_5.pkl')
    ut.save_pickle(time_varying_tracker_1, '/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/1/Soft_Movable/Red_Pillow_Movable/time_varying_tracking_data_movable_red_pillow_trial_5.pkl')

def plotdata():
    rospy.loginfo('Plotting data!')
    ta1 = ut.load_pickle('/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/96/Soft_Movable/Red_Pillow_Movable/time_varying_data_movable_red_pillow_trial_2.pkl')
    ta2 = ut.load_pickle('/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/24/Soft_Movable/Red_Pillow_Movable/time_varying_data_movable_red_pillow_trial_2.pkl')
    ta3 = ut.load_pickle('/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/6/Soft_Movable/Red_Pillow_Movable/time_varying_data_movable_red_pillow_trial_2.pkl')
    ta4 = ut.load_pickle('/home/tapo/svn/robot1_data/usr/tapo/data/Resolution/1/Soft_Movable/Red_Pillow_Movable/time_varying_data_movable_red_pillow_trial_2.pkl')

    mpu.figure(1)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Total Force')
    pp.plot(ta1[:,0], ta1[:,1])

    mpu.figure(2)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Mean Force')
    pp.plot(ta1[:,0], ta1[:,2])

    mpu.figure(3)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Max Force')
    pp.plot(ta1[:,0], ta1[:,3])

    mpu.figure(4)
    pp.title('Time-Varying Contact')
    pp.xlabel('Time (s)')
    pp.ylabel('No. of Contact Regions')
    pp.plot(ta1[:,0], ta1[:,4])

    mpu.figure(5)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Total Force')
    pp.plot(ta2[:,0], ta2[:,1])

    mpu.figure(6)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Mean Force')
    pp.plot(ta2[:,0], ta2[:,2])

    mpu.figure(7)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Max Force')
    pp.plot(ta2[:,0], ta2[:,3])

    mpu.figure(8)
    pp.title('Time-Varying Contact')
    pp.xlabel('Time (s)')
    pp.ylabel('No. of Contact Regions')
    pp.plot(ta2[:,0], ta2[:,4])

    mpu.figure(9)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Total Force')
    pp.plot(ta3[:,0], ta3[:,1])

    mpu.figure(10)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Mean Force')
    pp.plot(ta3[:,0], ta3[:,2])

    mpu.figure(11)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Max Force')
    pp.plot(ta3[:,0], ta3[:,3])

    mpu.figure(12)
    pp.title('Time-Varying Contact')
    pp.xlabel('Time (s)')
    pp.ylabel('No. of Contact Regions')
    pp.plot(ta3[:,0], ta3[:,4])

    mpu.figure(13)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Total Force')
    pp.plot(ta4[:,0], ta4[:,1])

    mpu.figure(14)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Mean Force')
    pp.plot(ta4[:,0], ta4[:,2])

    mpu.figure(15)
    pp.title('Time-Varying Force')
    pp.xlabel('Time (s)')
    pp.ylabel('Max Force')
    pp.plot(ta4[:,0], ta4[:,3])

    mpu.figure(16)
    pp.title('Time-Varying Contact')
    pp.xlabel('Time (s)')
    pp.ylabel('No. of Contact Regions')
    pp.plot(ta4[:,0], ta4[:,4])

def getdata():
    rospy.init_node('time_varying_data_resolution', anonymous=True)
    tf_lstnr = tf.TransformListener()
    rospy.Subscriber("/skin_patch_forearm_right/taxels/forces", TaxelArray_Meka, callback,callback_args = (tf_lstnr))
    rospy.spin()

if __name__ == '__main__':
    time = 0
    time_varying_data = [0,0,0,0,0,0,0,0]
    time_varying_data_96 = [0,0,0,0,0,0,0,0]
    time_varying_data_24 = [0,0,0,0,0,0,0,0]
    time_varying_data_6 = [0,0,0,0,0,0,0,0]
    time_varying_data_1 = [0,0,0,0,0,0,0,0]
    time_varying_tracker = [0,0]
    time_varying_tracker_96 = [0,0]
    time_varying_tracker_24 = [0,0]
    time_varying_tracker_6 = [0,0]
    time_varying_tracker_1 = [0,0]
    getdata()
    tracking_point()
    savedata()
    #plotdata()
    #pp.show()


