#!/usr/bin/env python

NAME = 'machine_learning_server'
# import rospy package
import rospy
import pandas as pd
from sklearn.preprocessing import LabelEncoder
import pickle
from geometry_msgs.msg import Vector3, Quaternion, Transform, Point, Pose

# import the machine_learning service
from machine_learning.srv import *


# grasp_feasibility_evaluator will take the vector of grasping poses (obtained from the graspit) along with the action type and object info and returns the most feasible grasp. The feasibility
# is evauated using the machine learning techniques such as binary decision trees, ...
def grasp_feasibility_evaluator(request):
    print("grasp pos is [%s]" % (request.graspVector))
    print("Action is [%s]" % (request.manipulationAction))
    print("object is [%s]" % (request.objectClass))
    # print("position ", request.graspVector[0].position.x,request.graspVector[0].position.y,request.graspVector[0].position.z,
    # request.graspVector[0].orientation.x,request.graspVector[0].orientation.y,request.graspVector[0].orientation.z,request.graspVector[0].orientation.w)
    # print("Action and object name",request.manipulationAction,request.objectClass)

    predictionModals = {
        'Quaternion.w': [request.graspVector[0].orientation.w],
        'Quaternion.x': [request.graspVector[0].orientation.x],
        'Quaternion.y': [request.graspVector[0].orientation.y],
        'Quaternion.z': [request.graspVector[0].orientation.z],
        'Translation.x': [request.graspVector[0].position.x],
        'Translation.y': [request.graspVector[0].position.y],
        'Translation.z': [request.graspVector[0].position.z],
        'object_name': [request.objectClass],
        'actions': [request.manipulationAction]
    }
    data = pd.DataFrame(predictionModals,
                        columns=['Quaternion.w', 'Quaternion.x', 'Quaternion.y', 'Quaternion.z', 'Translation.x',
                                 'Translation.y', 'Translation.z', 'object_name', 'actions'])
    print("Labling ", data)
    print(Votingoutput(data))
    # this function will call the learning algos then it will apply the voting scheme and finally it will return the best grasp as response.
    return grasp_feasibility_evaluationResponse(request.graspVector)


# this function must received dataframe to predict the labels
def decision_trees(req):
    # load model and label encoders from models folder to perform prediction
    model = pickle.load(open('Models/DecisionTreeClassifier', 'rb'))
    object_encoder = pickle.load(open('Models/objectEncoder', 'rb'))
    actions_encoder = pickle.load(open('Models/actionEncoder', 'rb'))
    label_encoder = pickle.load(open('Models/labelEncoder', 'rb'))
    # perform label encoding for  object name and actions
    req['object_name'] = object_encoder.fit_transform(req['object_name'])
    req['actions'] = actions_encoder.fit_transform(req['actions'])
    x = req.values
    y = model.predict(x)  # this line predict the output
    return label_encoder.inverse_transform(y)  # convert into valid/invalid from 0/1


def GradientBoostingClassifier(req):
    # load model and label encoders from models folder to perform prediction
    model = pickle.load(open('Models/GradientBoostingClassifier', 'rb'))
    object_encoder = pickle.load(open('Models/objectEncoder', 'rb'))
    actions_encoder = pickle.load(open('Models/actionEncoder', 'rb'))
    label_encoder = pickle.load(open('Models/labelEncoder', 'rb'))
    # perform label encoding for  object name and actions
    req['object_name'] = object_encoder.fit_transform(req['object_name'])
    req['actions'] = actions_encoder.fit_transform(req['actions'])
    x = req.values
    y = model.predict(x)  # this line predict the output
    return label_encoder.inverse_transform(y)  # convert into valid/invalid from 0/1


def RandomForestClassifier(req):
    # load model and label encoders from models folder to perform prediction
    model = pickle.load(open('Models/RandomForestClassifier', 'rb'))
    object_encoder = pickle.load(open('Models/objectEncoder', 'rb'))
    actions_encoder = pickle.load(open('Models/actionEncoder', 'rb'))
    label_encoder = pickle.load(open('Models/labelEncoder', 'rb'))
    # perform label encoding for  object name and actions
    req['object_name'] = object_encoder.fit_transform(req['object_name'])
    req['actions'] = actions_encoder.fit_transform(req['actions'])
    x = req.values
    y = model.predict(x)  # this line predict the output
    return label_encoder.inverse_transform(y)  # convert into valid/invalid from 0/1


def VotingScheme(req):  # output of the above functions will be input to this function
    valid = 0
    if decision_trees(req) == 'valid':
        valid += 1
    if GradientBoostingClassifier(req):
        valid += 1
    if RandomForestClassifier(req):
        valid += 1
    if valid > 1:
        return 'valid'
    else:
        return 'invalid'


def Votingoutput(data):
    return
    #  no need to implement this function anymore


# main function that spawn the services.
def machine_learning_server():
    rospy.init_node(NAME)
    # print(Votingoutput('/home/muhayyuddin/graspit_ws/src/machine_learning/scripts/unseen.csv'))
    s1 = rospy.Service('grasp_feasibility_evaluator', grasp_feasibility_evaluation, grasp_feasibility_evaluator)

    # spin() keeps Python from exiting until node is shutdown
    rospy.spin()


if __name__ == "__main__":
    machine_learning_server()
