#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#########EPICS COMMANDS

#read speed Omega Achse -> epics.caget("X06MX-ES-DF1:OMEGA-SETV")
#write speed Omega Achse -> epics.caput("X06MX-ES-DF1:OMEGA-SETV",20)
#
#write relative Omega Achse -> epics.caput("X06MX-ES-DF1:OMEGA-INCP",90)
#write absolute Omega Achse -> epics.caput("X06MX-ES-DF1:OMEGA-SETP",20)
#
#read ReadBAck Omega Achse -> epics.caget("X06MX-ES-DF1:OMEGA-RBV")
#read User ReadBAck Omega Achse -> epics.caget("X06MX-ES-DF1:OMEGA-GETP")
#
#GMX, GMY, GMZ

#Created on Tue May 18 18:03:02 2021

import rospy
from std_msgs.msg import Float32
from std_msgs.msg import Int32
from sensor_msgs.msg import JointState
import requests
import json 
import epics
import time
import csv
from datetime import date
import sys
import math
import smaract.ctl as ctl
import matplotlib.pyplot as plt
import numpy as np

#temporary subscriber to check function
DMS=[]
Position=[]
Secs=[]
Nsecs=[]
Seq=[]
Liste=[]
LJU6_JointStateOmega=0
CHI_Cal=0
PHI_Cal=0
OmegaRDB=0

#neccessary subscriber to put in csv
CHI=[]
PHI=[]
SHX=[]
SHY=[]
SHZ=[]
OX=[]
OY=[]
OZ=[]

p_x = []
p_y = []
p_z = []
#d_handle
    
Counter = 0
motion=True

#now = date.time()
today = date.today()
# dd/mm/YY
day = today.strftime("%Y_%m_%d_")
#print("d1 =", d1)
smargopolo_server = "http://smargopolo:3000"

def init_Smargon():
    smargopolo_server = "http://smargopolo:3000"
    response = requests.get(smargopolo_server+"/readbackSCS")

def create_CSV_File():
     #Writing CSV File from List 
    #generate new CSV fileLJU6_JointStateTimeSekLJU6_JointStateTimeSek
    with open(day+'ToolError.csv', 'w', newline='') as file:
        writer = csv.writer(file)
        writer.writerow(["OMEGA","Radius","AI0","AI1","Sequenz1","Sequenz2","Sekunden1","Sekunden1","NaoSek1","NanoSek2","DMS1","DMS2","DMS3","CHI","PHI"]) 

def move_to_CHI90_Start():
    print("==Move to ZERO at Start============")
    
    smargopolo_server = "http://smargopolo:3000"
    response = requests.get(smargopolo_server+"/readbackSCS")
    #Motion moving to yero Position
    epics.caput("X06MX-ES-DF1:OMEGA-SETV",50)
    OmegaRDB=epics.caget("X06MX-ES-DF1:OMEGA-RBV")
    
    if OmegaRDB > 180 or OmegaRDB < -180:
        print("Omega > 180")
        epics.caput("X06MX-ES-DF1:OMEGA-SETP",0)
        response = requests.put(smargopolo_server+'/targetSCS?CHI=90')
        response = requests.put(smargopolo_server+'/targetSCS?PHI=0')
        time.sleep(40)
    
    else:
        print("Omega < 180")
        epics.caput("X06MX-ES-DF1:OMEGA-SETP",0)
        response = requests.put(smargopolo_server+'/targetSCS?CHI=90')
        response = requests.put(smargopolo_server+'/targetSCS?PHI=0')
        time.sleep(5)
        
    #get Position Feedback
    readbackSCS = json.loads(response.text)
    get_CHI=readbackSCS['CHI']
    get_PHI=readbackSCS['PHI']
    OmegaRDB=epics.caget("X06MX-ES-DF1:OMEGA-RBV")
    OmegaRDB=round(OmegaRDB, 2)
    print("Omega:", OmegaRDB,"    CHI: ",get_CHI,"    PHI: ",get_PHI)
    print("")
    
def move_to_CHI90_Stopp():
    print("==Move to ZERO at Stopp============")
    
    smargopolo_server = "http://smargopolo:3000"
    response = requests.get(smargopolo_server+"/readbackSCS")
    #Motion moving to yero Position
    epics.caput("X06MX-ES-DF1:OMEGA-SETV",50)
    OmegaRDB=epics.caget("X06MX-ES-DF1:OMEGA-RBV")
    
    if OmegaRDB > 180 or OmegaRDB < -180:
        print("Omega > 180")
        epics.caput("X06MX-ES-DF1:OMEGA-SETP",0)
        response = requests.put(smargopolo_server+'/targetSCS?CHI=90')
        response = requests.put(smargopolo_server+'/targetSCS?PHI=0')
        time.sleep(20)
    
    else:
        print("Omega < 180")
        epics.caput("X06MX-ES-DF1:OMEGA-SETP",0)
        response = requests.put(smargopolo_server+'/targetSCS?CHI=90')
        response = requests.put(smargopolo_server+'/targetSCS?PHI=0')
        time.sleep(5)
        
    #get Position Feedback
    readbackSCS = json.loads(response.text)
    get_CHI=readbackSCS['CHI']
    get_PHI=readbackSCS['PHI']
    OmegaRDB=epics.caget("X06MX-ES-DF1:OMEGA-RBV")
    OmegaRDB=round(OmegaRDB, 2)
    print("Omega:", OmegaRDB,"    CHI: ",get_CHI,"    PHI: ",get_PHI)
    print("")
    
    
def callback_LJUE9_JointState(data):
    
    global Nsecs,Secs,Seq,Liste,CHI,PHI,Position,Counter,p_x,p_y,p_z   
                
    Position=(data.position)
    Secs=(data.header.stamp.secs)
    Nsecs=(data.header.stamp.nsecs)
    Seq=(data.header.seq)
    Liste=list(Position) 
    
    response = requests.get(smargopolo_server+"/readbackSCS")
    readbackSCS = json.loads(response.text)
    CHI=readbackSCS['CHI']
    PHI=readbackSCS['PHI']
    
    Liste.extend([Seq,Seq2,Secs,Secs2,Nsecs,Nsecs2,DMS_X,DMS_Y,DMS_Z,CHI,PHI])

    with open(day+'ToolError.csv', 'a', newline='') as file:
        writer = csv.writer(file)
        writer.writerow(Liste) 
        
def callback_readbackCAL_JointState(data):   
    
    global DMS_X,DMS_Y,DMS_Z,Seq2,Secs2,Nsecs2
    
    DMS_X = (data.position[0])
    DMS_Y = (data.position[1])
    DMS_Z = (data.position[2])
    Secs2=(data.header.stamp.secs)
    Nsecs2=(data.header.stamp.nsecs)
    Seq2=(data.header.seq)
        
def start_ToolError_routine():
    
    print()
    print("============First Motion===========")
    
    #Fget Smargon
    smargopolo_server = "http://smargopolo:3000"
    response = requests.get(smargopolo_server+"/readbackSCS")
    readbackSCS = json.loads(response.text)
    get_CHI=round(readbackSCS['CHI'])
    get_PHI=round(readbackSCS['PHI'])
    
    #get Aerotech
    OmegaRDB=epics.caget("X06MX-ES-DF1:OMEGA-RBV")
    OmegaRDB=round(OmegaRDB, 2)
    
    PHI_Rotation = 360
    
    if get_CHI==90 and OmegaRDB == 0:
            
        print("Move PHI to " +str(PHI_Rotation) +" degree")
        response = requests.put(smargopolo_server+'/targetSCS?PHI='+str(PHI_Rotation))
        time.sleep(15)
        print("Move PHI to 0 degree")
        response = requests.put(smargopolo_server+'/targetSCS?PHI='+str(0))
        time.sleep(15)
#        print("Move PHI to " +str(PHI_Rotation) +" degree")
#        response = requests.put(smargopolo_server+'/targetSCS?PHI='+str(PHI_Rotation))
#        time.sleep(15)
#        print("Move PHI to 0 degree")
#        response = requests.put(smargopolo_server+'/targetSCS?PHI='+str(0))
#        time.sleep(15)
            
    else:
        exit()
    #get Position Feedback
    response = requests.get(smargopolo_server+"/readbackSCS")
    readbackSCS = json.loads(response.text) 
    get_CHI=readbackSCS['CHI']
    get_PHI=readbackSCS['PHI']
    OmegaRDB=epics.caget("X06MX-ES-DF1:OMEGA-RBV")
    OmegaRDB=round(OmegaRDB, 2)
    print("Omega:", OmegaRDB,"    CHI: ",get_CHI,"    PHI: ",get_PHI)
    print("")
    #////////////////////////////////////////////////
    
    
if __name__ == '__main__':   
    
#    smargopolo_server = "http://smargopolo:3000"
#    response = requests.get(smargopolo_server+"/readbackSCS")
#    readbackMCS = json.loads(response.text)
    
    init_Smargon()
    create_CSV_File()
    move_to_CHI90_Start()
    rospy.init_node('listener', anonymous=True)
    subs1=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
    subs2=rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
    start_ToolError_routine()
        
    subs1.unregister() 
    subs2.unregister()           
    move_to_CHI90_Stopp()