#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Tue Jul 23 15:52:29 2019
@author: Hendry F Chame <hendryfchame@gmail.com>
CNRU - OIST

This class provides the functionality of parametrization to the controller:
external_force_following_online_trajectory_control verion 3.0

The control law is the following:

# Parameter       | Description 
# ----------------------------------------------------------------------
# tau_th          | compliance treshold for effort
# kp              | compliance gain 
# sum_e_max       | compliance reset windup
# d               | compliance damping factor
# kr              | deliberation control gain [0,1]
# theta_th        | deliberation control saturation in degree


# variable        | Description:
# ------------------------------------------------------------------------
# t               | Time
# tau[t]          | Observation of the meassured torque
# tau_dyn[t]      | Estimation of the torque if the robot would be moving 
#                 | alone (calculated by the inverse dynamic model)
# tau_ext[t]      | Estimation of the external torque induced by the human
# e[t]            | Torque error signal after aplying the treshold
# sum_e[t]        | Integration of external torque error 
# theta[t]        | Measured joint position
# theta_net[t]    | Target position generated by the RNN
# theta_delta[t]  | Desired deliberation correction
# theta_star[t+1] | Desired position sent to the robot motion planner

# Algorithm:
# ------------------------------------------------------------------------
# 
# tau_ext[t] = tau[t] - tau_dyn[t]
# 
# e[t] = 0.0
# 
# if |tau_ext|>tau_th:
# 
#    if tau_ext[t] > 0:
#       e[t] = tau_ext[t] - tau_th
# 
#    else:	
#       e[t] = tau_ext[t] + tau_th
# 
# v = d*sum_e[t-1] + e
# 
# sum_e[t] = max(min(v,sum_e_max),-sum_e_max)
# 
# theta_delta[t] = theta_net[t] - theta[t])
# 
# theta_sat[t] = max(min(theta_delta[t],theta_th),-theta_th)
# 
# theta_star[t+1] = kr*theta_sat[t] + theta[t] + kp*sum_e[t]
# 
# ------------------------------------------------------------------------

"""


import rospy
import numpy as np

from enum import Enum


from torobo_driver import torobo_easy_command

class ExtForce(object):    

    class Mode(Enum):
        Teaching = 0
        Experiment = 1

    def runCommands(self, _commandList, _controller):
          for command in _commandList:
                torobo_easy_command.SendEasyCommandText(_controller, command)
                rospy.sleep(0.01)

    def registerParameters(self, tau_th, kp, sum_e_max, d, kr, theta_th):
            # left and right arm 
            for i in range(0,6):
                
                # constructing the commands for the left arm
                commandList = []				
                commandList.append("param " + str(i+1) + " fftauth " +       str(tau_th[i]))
                commandList.append("param " + str(i+1) + " ffkp " +          str(kp[i]))
                commandList.append("param " + str(i+1) + " ffsigmaemax " +   str(sum_e_max[i]))
                commandList.append("param " + str(i+1) + " ffdamping " +     str(d[i]))
                commandList.append("param " + str(i+1) + " ffkr " +          str(kr[i]))
                commandList.append("param " + str(i+1) + " softki " +        str(theta_th[i]))	


                # send to execution
                self.runCommands(commandList, "left_arm_controller")

            # right arm 
            for i in range(0,6):
                ii = i + 6 
                # constructing the commands for the right arm
                commandList = []
                commandList.append("param " + str(i+1) + " fftauth " +       str(tau_th[ii]))
                commandList.append("param " + str(i+1) + " ffkp " +          str(kp[ii]))
                commandList.append("param " + str(i+1) + " ffsigmaemax " +   str(sum_e_max[ii]))
                commandList.append("param " + str(i+1) + " ffdamping " +     str(d[ii]))
                commandList.append("param " + str(i+1) + " ffkr " +          str(kr[ii]))
                commandList.append("param " + str(i+1) + " softki " +        str(theta_th[ii]))	

                # send to execution
                self.runCommands(commandList, "right_arm_controller")
                            
            for i in range(0,4):
                ii = i + 12 
                # constructing the commands for the head-torso chain
                commandList = []				
                commandList.append("param " + str(i+1) + " fftauth " +       str(tau_th[ii]))
                commandList.append("param " + str(i+1) + " ffkp " +          str(kp[ii]))
                commandList.append("param " + str(i+1) + " ffsigmaemax " +   str(sum_e_max[ii]))
                commandList.append("param " + str(i+1) + " ffdamping " +     str(d[ii]))
                commandList.append("param " + str(i+1) + " ffkr " +          str(kr[ii]))
                commandList.append("param " + str(i+1) + " softki " +        str(theta_th[ii]))	

                # send to execution
                self.runCommands(commandList, "torso_head_controller")


    def __init__(self, _mode):	
        
        if _mode == ExtForce.Mode.Teaching:

            tau_th = [4.0, 4.0, 2.0, 2.0, 1.5, 1.5,            4.0, 4.0, 2.0, 2.0, 1.5, 1.5,      20.5, 20.5, 20.5, 20.5]
            kp = [0.1, 0.1, 0.05, 0.05, 0.1, 0.1,            0.1, 0.1,0.05,0.05,0.1,0.1,        0.0,0.0,0.0,0.0]
            sum_e_max = [200.0,200.0,100.0,100.0,50.0,50.0,200.0,200.0,100.0,100.0,50.0,50.0,200.0,200.0,50.0,50.0]
            d = [0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9]
            # kr = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
            kr = [0.3]*16
            theta_th = [1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57]
            theta_th = [i*1.63 for i in theta_th]

            self.registerParameters( tau_th, kp, sum_e_max, d, kr, theta_th)


        elif _mode == ExtForce.Mode.Experiment:

            tau_th = [12.0,12.0,5.0,5.0,1.5,1.5,12.0,12.0,5.0,5.0,1.5,1.5,200.0,200.0,200.0,200.0]
            kp = [0.025,0.025,0.05,0.05,0.1,0.1,0.025,0.025,0.05,0.05,0.1,0.1,0.025,0.025,0.05,0.05]
            sum_e_max = [200.0,200.0,100.0,100.0,50.0,50.0,200.0,200.0,100.0,100.0,50.0,50.0,200.0,200.0,50.0,50.0]
            d = [0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9]
            kr = [0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8]
            # kr = [0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9]
            theta_th = [1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57]
            theta_th = [i*1.63 for i in theta_th]

            self.registerParameters( tau_th, kp, sum_e_max, d, kr, theta_th)



# Initialization values of the control variables in torobo driver
# -----------------------------------------------------------------
# Torso:
#
# .dampingEffortThreshold = 5.0f,
# .softnessGainP = 0.5f,
# .softnessGainI = 0.01f,
# .softnessWindup = 3.0f,
# .softnessOverride = 100.0f,
#
# .dampingEffortThreshold = 10.0f,
# .softnessGainP = 0.5f,
# .softnessGainI = 0.01f,
# .softnessWindup = 3.0f,
# .softnessOverride = 100.0f,
#
#
# Head:
#
# .dampingEffortThreshold = 0.5f,
# .softnessGainP = 1.0f,
# .softnessGainI = 0.01f,
# .softnessWindup = 3.0f,
# .softnessOverride = 100.0f,
#
# .dampingEffortThreshold = 1.0f,
# .softnessGainP = 1.0f,
# .softnessGainI = 0.01f,
# .softnessWindup = 3.0f,
# .softnessOverride = 100.0f,
#
# Left:
#
# .dampingEffortThreshold = 3.5f,
# .softnessGainP = 0.1f,
# .softnessGainI = 0.01f,
# .softnessWindup = 5.0f,
# .softnessOverride = 100.0f,
#
# .dampingEffortThreshold = 3.5f,
# .softnessGainP = 0.1f,
# .softnessGainI = 0.05f,
# .softnessWindup = 5.0f,
# .softnessOverride = 100.0f,
#
# .dampingEffortThreshold = 2.3f,
# .softnessGainP = 5.0f,
# .softnessGainI = 0.0f,
# .softnessWindup = 0.0f,
# .softnessOverride = 100.0f,

# .dampingEffortThreshold = 2.3f,
# .softnessGainP = 1.0f,
# .softnessGainI = 0.0f,
# .softnessWindup = 0.0f,
# .softnessOverride = 100.0f,

# .dampingEffortThreshold = 0.3f,
# .softnessGainP = 5.0f,
# .softnessGainI = 0.0f,
# .softnessWindup = 0.0f,
# .softnessOverride = 100.0f,

# .dampingEffortThreshold = 0.3f,
# .softnessGainP = 5.0f,
# .softnessGainI = 0.0f,
# .softnessWindup = 0.0f,
# .softnessOverride = 100.0f,

# Right:

# .dampingEffortThreshold = 3.2f,
# .softnessGainP = 0.1f,
# .softnessGainI = 0.01f,
# .softnessWindup = 5.0f,
# .softnessOverride = 100.0f,

# .dampingEffortThreshold = 3.5f,
# .softnessGainP = 0.1f,
# .softnessGainI = 0.05f,
# .softnessWindup = 5.0f,
# .softnessOverride = 100.0f,

# .dampingEffortThreshold = 2.3f,
# .softnessGainP = 5.0f,
# .softnessGainI = 0.0f,
# .softnessWindup = 0.0f,
# .softnessOverride = 100.0f,

# .dampingEffortThreshold = 2.3f,
# .softnessGainP = 1.0f,
# .softnessGainI = 0.0f,
# .softnessWindup = 0.0f,
# .softnessOverride = 100.0f,

# .dampingEffortThreshold = 0.3f,
# .softnessGainP = 5.0f,
# .softnessGainI = 0.0f,
# .softnessWindup = 0.0f,
# .softnessOverride = 100.0f,

# .dampingEffortThreshold = 0.3f,
# .softnessGainP = 5.0f,
# .softnessGainI = 0.0f,
# .softnessWindup = 0.0f,
# .softnessOverride = 100.0f,

