#!/usr/bin/env python

import rospy
import random
import numpy as np
from std_msgs.msg import String
from std_msgs.msg import Float64
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

prev_dist = 0
state = 0

def callBack_dist(msg):
    global prev_dist, state
    new_dist = msg.data
    if new_dist != prev_dist:
        state = -1
    else:
        state = 1
    prev_dist = new_dist

def callBack_scan(msg):
    pos = Twist()
    pub = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)

    size = len(msg.ranges) # get the length of scan data
    norm = np.linalg.norm(np.multiply(msg.ranges, np.arange(-size/2,size/2)))
    rot = sum(np.multiply(msg.ranges, np.arange(-size/2,size/2))/norm)

    if min(msg.ranges[size/2-40:size/2+40]) < 0.6:
        pos.linear.x = 0
        pos.angular.z = state
        pub.publish(pos)

    elif max(msg.ranges[size/2-40:size/2+40]) >= 0.7*max(msg.ranges):
        pos.linear.x = min(msg.ranges[size/2-40:size/2+40])
        pos.angular.z = 0
        pub.publish(pos)

    else:
        pos.linear.x = min(msg.ranges[size/2-40:size/2+40])
        pos.angular.z = rot
        pub.publish(pos)



def main():
    rospy.init_node('ForceMapper')
    rospy.Subscriber("/scan", LaserScan, callBack_scan)
    rospy.Subscriber("/dist", Float64, callBack_dist)
    # Spin until ctrl + c
    rospy.spin()

if __name__ == '__main__':
    main()