#!/usr/bin/env python3


import time
import numpy as np
import math
from math import atan2
from geometry_msgs.msg import Point, Twist
from tf.transformations import euler_from_quaternion
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose2D
from open_base.msg import Movement, MovementGeneric
from std_msgs.msg import Float64, UInt8
import rospy
from numpy.core.fromnumeric import var

# ____ Tambah Coba Konsensus  BWLOM BERES


speed_vx = 0
speed_vy = 0

targetX1 = 0.0
beginX1 = 0.0
targetY1 = 0.0
beginY1 = 0.0

targetX2 = 0.0
beginX2 = 0.0
targetY2 = 0.0
beginY2 = 0.0

targetX3 = 0.0
beginX3 = 0.0
targetY3 = 0.0
beginY3 = 0.0

consenX = 0.0
consenY = 0.0

varA = 0
varB = 0
varC = 0
x1 = 0.0
y1 = 0.0
z1 = 0.0

x2 = 0.0
y2 = 0.0
z2 = 0.0

x3 = 0.0
y3 = 0.0
z3 = 0.0

value_small_12x = 0.0
value_small_12y = 0.0
value_safe_12x = 0.0
value_safe_12y = 0.0
value_big_12x = 0.0
value_big_12y = 0.0


def getOdomR1(msg):
    global x1, x2, x3, y1, y2, y3, z1, z2, z3, varA

    x1 = msg.x
    y1 = msg.y
    z1 = msg.theta

    if(varA == 0):
        beginX1 = x1
        beginY1 = y1
        varA = 1


def getOdomR2(msg):
    global x1, x2, x3, y1, y2, y3, z1, z2, z3, varB

    x2 = msg.x
    y2 = msg.y
    z2 = msg.theta

    if(varB == 0):
        beginX2 = x2
        beginY2 = y2
        varB = 1


def getOdomR3(msg):
    global x1, x2, x3, y1, y2, y3, z1, z2, z3, varC

    x3 = msg.x
    y3 = msg.y
    z3 = msg.theta

    if(varC == 0):
        beginX3 = x3
        beginY3 = y3
        varC = 1


def leader():
    global targetX1, targetY1

    movePub1 = rospy.Publisher('/open_base1/command', Movement, queue_size=1)
    rate = rospy.Rate(1)
    mov = Movement()
    mov.movement = 1
    mov.generic.type = 0
    mov.generic.frame = 3
    mov.generic.target.x = 2.0  # targetX1
    mov.generic.target.y = 7.0  # targetY1
    movePub1.publish(mov)


def fuzzy12():
    global x1, x2, x3, y1, y2, y3, z1, z2, z3, value_small_12y, value_small_12x, value_big_12x, value_big_12y, value_safe_12x, value_safe_12y, distance12x, distance12y

    distance12x = abs(x1 - x2)
    distance12y = abs(y1 - y2)

    if distance12x <= 0.2:
        value_small_12x = 1
        value_safe_12x = 0
        value_big_12x = 0
    elif distance12x > 0.2 and distance12x < 0.4:
        value_small_12x = (0.4-distance12x)/(0.4-0.2)
        value_safe_12x = 0
        value_big_12x = 0
    elif distance12x > 0.2 and distance12x < 0.5:
        value_safe_12x = (distance12x - 0.2)/(0.5-0.2)
        value_big_12x = 0
        value_small_12x = 0
    elif distance12x == 0.5:
        value_small_12x = 0
        value_safe_12x = 1
        value_big_12x = 0
    elif distance12x > 0.5 and distance12x < 0.8:
        value_small_12x = 0
        value_safe_12x = (0.8 - distance12x)/(0.8-0.5)
        value_big_12x = 0
    elif distance12x > 0.6 and distance12x < 0.8:
        value_small_12x = 0
        value_big_12x = (distance12x-0.6)/(0.8-0.6)
        value_safe_12x = 0
    elif distance12x >= 0.8:
        value_small_12x = 0
        value_safe_12x = 0
        value_big_12x = 1

    # print('derajat keanggoataan vx= ')
    # print('small ', value_small_12x)
    # print('safe ', value_safe_12x)
    # print('big ', value_big_12x)

    if distance12y <= 0.2:
        value_small_12y = 1
        value_safe_12y = 0
        value_big_12y = 0
    elif distance12y > 0.2 and distance12y < 0.4:
        value_small_12y = (0.4-distance12y)/(0.4-0.2)
        value_safe_12y = 0
        value_big_12y = 0
    elif distance12y > 0.2 and distance12y < 0.5:
        value_safe_12y = (distance12y - 0.2)/(0.5-0.2)
        value_big_12y = 0
        value_small_12y = 0
    elif distance12y == 0.5:
        value_small_12y = 0
        value_safe_12y = 1
        value_big_12y = 0
    elif distance12y > 0.5 and distance12y < 0.8:
        value_small_12y = 0
        value_safe_12y = (0.8 - distance12y)/(0.8-0.5)
        value_big_12y = 0
    elif distance12y > 0.6 and distance12y < 0.8:
        value_small_12y = 0
        value_big_12y = (distance12y-0.6)/(0.8-0.6)
        value_safe_12y = 0
    elif distance12y >= 0.8:
        value_small_12y = 0
        value_safe_12y = 0
        value_big_12y = 1

    # print('derajat keanggoataan vy= ')
    # print('small ', value_small_12y)
    # print('safe ', value_safe_12y)
    # print('big ', value_big_12y)

# VX


def fungsiinfslow_vx(variabel_distance_vx):
    global speed_vx
    if variabel_distance_vx != 0:
        speed_vx = 0.0  # 5.0

        movePub1 = rospy.Publisher(
            '/open_base2/command', Movement, queue_size=1)
        rate = rospy.Rate(1)
        mov = Movement()
        mov.movement = 1
        mov.generic.type = 2
        mov.generic.frame = 3
        mov.generic.target.x = speed_vx
        movePub1.publish(mov)


def fungsiinfaverage_vx(variabel_distance_vx):
    global speed_vx
    if variabel_distance_vx != 0:
        speed_vx = 3.0

        movePub1 = rospy.Publisher(
            '/open_base2/command', Movement, queue_size=1)
        rate = rospy.Rate(1)
        mov = Movement()
        mov.movement = 1
        mov.generic.type = 2
        mov.generic.frame = 3
        mov.generic.target.x = speed_vx
        movePub1.publish(mov)


def fungsiinffast_vx(variabel_distance_vx):
    global speed_vx
    if variabel_distance_vx != 0:
        speed_vx = 5.0  # 1.0

        movePub1 = rospy.Publisher(
            '/open_base2/command', Movement, queue_size=1)
        rate = rospy.Rate(1)
        mov = Movement()
        mov.movement = 1
        mov.generic.type = 2
        mov.generic.frame = 3
        mov.generic.target.x = speed_vx
        movePub1.publish(mov)

# VY


def fungsiinfslow_vy(variabel_distance_vy):
    global speed_vy
    if variabel_distance_vy != 0:
        speed_vy = 0.0  # 5.0

        movePub1 = rospy.Publisher(
            '/open_base2/command', Movement, queue_size=1)
        rate = rospy.Rate(1)
        mov = Movement()
        mov.movement = 1
        mov.generic.type = 2
        mov.generic.frame = 3
        mov.generic.target.y = speed_vy
        movePub1.publish(mov)


def fungsiinfaverage_vy(variabel_distance_vy):
    global speed_vy
    if variabel_distance_vy != 0:
        speed_vy = 3.0

        movePub1 = rospy.Publisher(
            '/open_base2/command', Movement, queue_size=1)
        rate = rospy.Rate(1)
        mov = Movement()
        mov.movement = 1
        mov.generic.type = 2
        mov.generic.frame = 3
        mov.generic.target.y = speed_vy
        movePub1.publish(mov)


def fungsiinffast_vy(variabel_distance_vy):
    global speed_vy
    if variabel_distance_vy != 0:
        speed_vy = 5.0  # 0

        movePub1 = rospy.Publisher(
            '/open_base2/command', Movement, queue_size=1)
        rate = rospy.Rate(1)
        mov = Movement()
        mov.movement = 1
        mov.generic.type = 2
        mov.generic.frame = 3
        mov.generic.target.y = speed_vy
        movePub1.publish(mov)


if __name__ == '__main__':
    try:
        while not rospy.is_shutdown():
            rospy.init_node('Fuzzy')

            rospy.Subscriber('/open_base1/pose/world', Pose2D, getOdomR1)
            rospy.Subscriber('/open_base2/pose/world', Pose2D, getOdomR2)

            leader()
            fuzzy12()
            fungsiinfslow_vx(value_small_12x)
            fungsiinfaverage_vx(value_safe_12x)
            fungsiinffast_vx(value_big_12x)
            fungsiinfslow_vy(value_small_12y)
            fungsiinfaverage_vy(value_safe_12y)
            fungsiinffast_vy(value_big_12y)

    # rospy.spin()

    except rospy.ROSInterruptException:
        rospy.loginfo("terminated")
        print('masuk except')
