#!/usr/bin/env python

'''======================================================
    Created by:  	D. Spencer Maughan
    Last updated: 	March 2015
    File name: 		IRIS_DF_Demo.py
    Organization:	RISC Lab, Utah State University
    Notes:
        This file is meant for demonstrating Differential Flatness
    as described in "Aggressive Maneuvers" by Jeff Ferrin. It is
    set up to allow toggling between trajectories using the joystick.
    ======================================================'''

    #================================#
    #    Libraries/modules Needed    #
    #================================#

import roslib; roslib.load_manifest('ardrone_tutorials')
roslib.load_manifest('risc_msgs')
import rospy
from  math import *
import rospkg
import numpy as np
import scipy.linalg as la
import time

    #=======================#
    #    Messages Needed    #
    #=======================#

from risc_msgs.msg import *
from sensor_msgs.msg import Joy
from std_msgs.msg import Bool

    #========================#
    #        Globals         #
    #========================#

PI 	        = 3.141592653589793
Threshold 	= 1000
ThrustCap 	= .3
states          = Cortex()
states.Obj      = [States()]*1
euler_max       = 0.349066 #in radians
max_yaw_rate    = .3490659 #in radians/sec
max_alt_rate    = 1000     # in mm/sec
rate            = 45      # Hz
start_time      = 0
back            = 0
forward         = 0
mode            = 1 # mode of 4 listed under cases
old_mode        = 0
ctrl_status     = False
cases           = ['Origin','Slanted Figure Eight','Origin',\
                   'Flat Figure Eight','Origin','Circle','Origin','Toroid Knot']
    #==================#
    #    Publishers    #
    #==================#

pub_ctrl        = rospy.Publisher('/controls', Controls, queue_size = 1)
pub_traj        = rospy.Publisher('/trajectory', Trajectories, queue_size = 1)

    #=======================================#
    #    Estimated Optimal Gain Matrices    #
    #=======================================#
kp = 1.2
kd = 2.6

K_way           = np.matrix([[  4.2,    0,   0,  5.8,    0,   0,   0],\
                             [    0,  4.2,   0,    0,  5.8,   0,   0],\
                             [    0,    0,   2,    0,    0,   3,   0],\
                             [    0,    0,   0,    0,    0,   0,  .3]])
K_slf8          = np.matrix([[ 3.2,    0,   0, 4.6,    0,   0,   0],\
                             [    0, 3.2,   0,    0, 4.6,   0,   0],\
                             [    0,    0,   2,    0,    0,   3,   0],\
                             [    0,    0,   0,    0,    0,   0,  .3]])
K_flf8          = np.matrix([[ 3.2,    0,   0, 4.2,    0,   0,   0],\
                             [    0, 3.2,   0,    0, 4.2,   0,   0],\
                             [    0,    0,   2,    0,    0,   3,   0],\
                             [    0,    0,   0,    0,    0,   0,  .3]])
K_crcl          = np.matrix([[ kp,    0,   0, kd,    0,   0,   0],\
                             [    0, kp,   0,    0, kd,   0,   0],\
                             [    0,    0,   2,    0,    0,   3,   0],\
                             [    0,    0,   0,    0,    0,   0,  .3]])
K_toroid        = np.matrix([[ kp,    0,   0, kd,    0,   0,   0],\
                             [    0, kp,   0,    0, kd,   0,   0],\
                             [    0,    0,   2,    0,    0,   3,   0],\
                             [    0,    0,   0,    0,    0,   0,  .3]])

    #========================#
    #    Integrator gains    #
    #========================#

ki_x = .5
ki_y = .5
ki_z = .5
ki_xdot = 0
ki_ydot = 0
ki_zdot = 0
ki_psi = 0

K_Int           = np.matrix([[ ki_x,    0,      0, ki_xdot,       0,      0,       0],\
                             [    0, ki_y,      0,       0, ki_ydot,      0,       0],\
                             [    0,    0,   ki_z,       0,       0,ki_zdot,       0],\
                             [    0,    0,      0,       0,       0,      0,  ki_psi]])

Integrator      = np.asmatrix(np.zeros((7,1)))

    #============================#
    #    Integrator Threshold    #
    #============================#

x_int_thresh    = 2
y_int_thresh    = 2
z_int_thresh    = 2.5
xdot_int_thresh = 1
ydot_int_thresh = 1
zdot_int_thresh = 2.5
psi_int_thresh  = 0
Int_thresh      = np.matrix([[x_int_thresh],[y_int_thresh],[z_int_thresh],\
                             [xdot_int_thresh],[ydot_int_thresh],[zdot_int_thresh],\
                             [psi_int_thresh]])

    #=========================================#
    #    Bound Radians between + or - pi/2    #
    #=========================================#

def pi2pi(angle):
     if abs(angle)>PI/2:
           if angle>0:
                angle = angle-PI
           else:
                angle = angle+PI
     return angle

    #=====================#
    #    Integrator Cap   #
    #=====================#
def IntegratorCap(I):
    global Int_thresh
    good_terms = np.multiply(I,abs(I)<Int_thresh) # leave as is and set others to zero
    bad_terms  = abs(I)>Int_thresh # set bad terms to 1 and others to 0
    Int = good_terms + np.multiply(np.sign(I),np.multiply(bad_terms,Int_thresh))
    return Int

    #==============#
    #    Get Joy   #
    #==============#

def GetJoy(joy):
    global start_time, mode, old_mode, forward, back
    if (joy.buttons[5] == 1 and forward == 1) or (joy.buttons[4] == 1 and back == -1):
        mode = mode
    else:
        back    = -joy.buttons[4]
        forward = joy.buttons[5]
        old_mode    = mode
        start_time  = rospy.get_time()
        mode        = mode + back + forward
        if mode > 8:
            mode = 1
        if mode < 1:
            mode = 8

    #============================#
    #    Get Controller Status   #
    #============================#

def GetStatus(S):

    global ctrl_status
    ctrl_status = S.data


    #========================#
    #    Get Cortex States   #
    #========================#

def GetStates(S):

    global states
    states = S

    #==========================#
    #   Various Trajectories   #
    #==========================#

def Origin():

    global K_way
    traj = GetTrajectory(10, 0,0,0,1,0,0,0,mode)
    Basic_Controller(traj, K_way)

def Slanted_Figure_8():

    global K_slf8,cycles
    # Trajectory Variables
    period     = 12 # seconds
    a          = 1
    b          = 0.5
    c          = 0.5
    n          = 1
    w1         = 2*PI/period
    w2         = w1/2
    w3         = w1
    traj = GetTrajectory(period, a, b, c, n, w1, w2, w3,mode)
    Basic_Controller(traj, K_slf8)

def Flat_Figure_8():

    global K_flf8,cycles
    # Trajectory Variables
    period     = 10 # seconds
    a          = 1
    b          = 0.5
    c          = 0.0
    n          = 1
    w1         = 2*PI/period
    w2         = w1/2
    w3         = w1
    traj = GetTrajectory(period, a, b, c, n, w1, w2, w3,mode)
    Basic_Controller(traj, K_flf8)

def Circle():

    global K_crcl
    # Trajectory Variables
    period     = 10 # seconds
    a          = 0.8
    b          = 0.8
    c          = 0
    n          = 1
    w1         = 2*PI/period
    w2         = w1
    w3         = w1
    traj = GetTrajectory(period, a, b, c, n, w1, w2, w3,mode)
    Basic_Controller(traj, K_crcl)

def Toroid():

    global K_toroid,mode
    # Trajectory Variables
    period     = 15 # seconds
    a          = 0.65
    b          = 0.25
    c          = 0.25
    n          = 1
    q          = 3 # number of loops
    p          = q-1
    w1         = 2*PI/period
    traj = GetToroid(period, a, b, c, n, q, p, w1,mode)
    Basic_Controller(traj, K_toroid)


def Datahandler():
    global mode, old_mode
    if mode == 1 or mode == 3 or mode == 5 or mode == 7:
        Origin()
        if old_mode != mode:
            global Integrator
            Integrator      = np.asmatrix(np.zeros((7,1)))
            rospy.loginfo("Origin")
            old_mode = mode
    if mode == 2:
        Slanted_Figure_8()
        if old_mode != mode:
            global Integrator
            Integrator      = np.asmatrix(np.zeros((7,1)))
            rospy.loginfo("Slanted Figure 8 Trajectory")
            old_mode = mode
    if mode == 4:
        Flat_Figure_8()
        if old_mode != mode:
            global Integrator