#!/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
i = 0

def callBack_dist(msg):
    global prev_dist, state, i
    new_dist = msg.data
    arr = np.arange(-15,15)
    if new_dist == prev_dist:
        state = arr[i % len(arr)]
        i += 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)
    print(sum(msg.ranges))
    if min(msg.ranges[size/2-50:size/2+50]) < 0.6:
        pos.linear.x = 0
        pos.angular.z = state*0.5
        pub.publish(pos)

    elif sum(msg.ranges) > 1450:
        pos.linear.x = 0.2*min(msg.ranges[size/2-40:size/2+40])
        pos.angular.z = state*0.5
        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()