##############################
import rhinoscriptsyntax as rs
import random, time, sys , os  ###                                              
sys.path.append("P:/WWW/selvo")
sys.path.append( "W:/diag/DM2/dominikS_monaco/" )
sys.path.append( "G:/Uni/Dm2/Finale/5_Progress/" )
import AudiPointsVer2 as ac                                              
import DM_lib as dm   
##############################


###De-/activate###############

if_view_mode = 1       ###aktiviert Perspektive und Rendered
if_hide = 1            ###versteckt Kontrollpunkte
if_drive = 0           ###faehrt auto
if_capture = 0         ###aktiviert Capture
if_auto_camera = 1     ###aktiviert automatische Kamera


###Customs################

number_points_audi = 50
road_divisions = 500       ###500
drift_angle = 8            ###8
audi_rotationspunkt = 60   ###60 ( hoeher = rotationspunkt weiter vorne )



###Restart####################

dm.eA()
rs.EnableRedraw(0)



###Viewmode####################

if if_view_mode:
    viewport = rs.CurrentView
    rs.ViewDisplayMode( "Perspective" , mode="Rendered" )



###Import Monaco####################

file_path = "G:\\Uni\\Dm2\\Finale\\5_Progress\\monaco_blender_01.3dm"
rs.Command("-_Import \"" + file_path + "\" _Enter")

mon = [mon for mon in rs.AllObjects() if rs.IsMesh(mon)]

cp_mon_alt = rs.AddPoint( [-42915 , -36346 , 7995])
cp_mon_vec = rs.VectorCreate( [0,0,0] , cp_mon_alt )
rs.MoveObject( mon , cp_mon_vec )
rs.DeleteObject( cp_mon_alt )
rs.UnselectAllObjects()



###Audi import####################

audiCoords = ac.allCoords#[:number_points_audi]

rs.DeleteObjects( rs.ObjectsByType( 1 ) )
audi0 = rs.AddPoints( audiCoords )
rs.ZoomBoundingBox( rs.BoundingBox( audi0 ) )
rs.MoveObject( audi0 , [0,0,-7])



###Road####################

p0 = [ 23900 , 38700 , -2320 ]
p1 = [ 26891 , 42300 , -2420 ]
p2 = [ 31970 , 47900 , -2490 ]
p3 = [ 34600 , 50000 , -2470 ]
p4 = [ 35900 , 50656 , -2430 ]
p5 = [ 37300 , 49400 , -2400 ]
p6 = [ 35900 , 48000 , -2450 ]
p7 = [ 34400 , 49400 , -2460 ]
p8 = [ p4[0] , p4[1]+200 , p4[2] ]
p9 = [ 38500 , 48400 , -2400 ]
p10 = [ 47900 , 51000 , -2790 ]
p11 = [ 47900 , 48500 , -2850 ]
p12 = [ 41000 , 49000 , -2850 ]
p13 = [ 37750 , 43000 , -2920 ]
p14 = [ 37000 , 38000 , -2920 ]
p15 = [ 37500 , 33000 , -2920 ]
p16 = [ 38000 , 31500 , -2920 ]
p17 = [ 40000 , 31000 , -2920 ]
p18 = [ 40200 , 31200 , -2920 ]


roadpnts_p = [p0,p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11,p12,p13,p14,p15,p16,p17,p18]
roadpnts = rs.AddPoints( roadpnts_p )
rs.ObjectColor( roadpnts , [255,0,0] )

road = rs.AddCurve( roadpnts , 2 )
rs.ObjectColor( road, [0,0,255])

divs = road_divisions

roaddiv = rs.DivideCurve( road , divs , False )
roadpnt = [rs.AddPoint(pt) for pt in roaddiv]



###ControlPoints####################

def controlpoints():
    pnts = []
    rot_pnt = audi_rotationspunkt
    ctrl_pnt = rs.AddPoint(0,rot_pnt,0)
    ctrl_crv_end = rs.AddPoint(0,500,0)
    ctrl_pnt_top = rs.AddPoint(0,rot_pnt,100)
    ctrl_pnt_right = rs.AddPoint(100,rot_pnt,0)
    ctrl_pnt_cam = rs.AddPoint(0,rot_pnt+100,150)
    ctrl_pnt_cam_foc = rs.AddPoint(0,rot_pnt+320,90)
    ctrl_pnt_b_cam = rs.AddPoint(0,rot_pnt-550,250)
    
    
    pnts.append(ctrl_pnt)
    pnts.append(ctrl_crv_end)
    pnts.append(ctrl_pnt_top)
    pnts.append(ctrl_pnt_right)
    pnts.append(ctrl_pnt_cam)
    pnts.append(ctrl_pnt_cam_foc)
    pnts.append(ctrl_pnt_b_cam)
    
    return pnts

ctrl_pnts = controlpoints()
audi = audi0 + ctrl_pnts

cp = ctrl_pnts[0]
cc_end = ctrl_pnts[1]
cp_t = ctrl_pnts [2]
cp_r = ctrl_pnts [3]
cp_c = ctrl_pnts [4]
cp_cf = ctrl_pnts [5]
cp_cb = ctrl_pnts [6]



###reset state####################

reset_position = [rs.PointCoordinates(pt) for pt in audi]

def reset_audi():
    global audi
    for i, pt in enumerate(audi):
        cur_position = rs.PointCoordinates(pt)
        reset_vec = rs.VectorCreate(reset_position[i], cur_position)
        rs.MoveObject(pt, reset_vec)



###Visuals####################

if if_hide:
    rs.HideObject( road )
    rs.HideObject( roadpnts )
    rs.HideObject( roadpnt )
    rs.HideObject( ctrl_pnts )



###Camera####################

def cam1():
    dm.setCameraTarget(camera=[ 26550 , 40900 , -2200 ], target=[ 26200 , 41200 , -2100 ], lens=15, rota=0, upVec=0, verbose=0 )

def cam2():
    gopro_cam = rs.PointCoordinates( cp_c )
    gopro_tar = rs.PointCoordinates( cp_cf )
    
    cam2 = dm.setCameraTarget(camera=gopro_cam, target=gopro_tar, lens=20, rota=0, upVec=0, verbose=0 )

def cam2_5():
    b_cam_plc = rs.PointCoordinates( cp_cb )
    gopro_tar = rs.PointCoordinates( cp_cf )
    
    dm.setCameraTarget(camera=b_cam_plc, target=gopro_tar, lens=40, rota=0, upVec=0, verbose=0 )

def cam3():
    dm.setCameraTarget(camera= [ 50000 , 49580 , -2650 ], target= [ 45000 , 49550 , -2485 ], lens=62, rota=0, upVec=0, verbose=0 )

def cam4():
    heli_vec = [ 5000 , -5000 , 5000 ]
    heli_pos = rs.PointAdd( cp_t , heli_vec )
    
    dm.setCameraTarget(camera=heli_pos, target=cp, lens=60, rota=0, upVec=0, verbose=0 )
    
def cam5():
    dm.setCameraTarget(camera=[ 40180 , 29300 , -2850 ], target=[ 40000 , 29700 , -2800 ], lens=60, rota=0, upVec=0, verbose=0 )



###movement audi####################

drift_ang = drift_angle

def movement( plc ):
    if plc < len(roadpnt) -drift_ang:
        
        reset_audi()
        
        movec = rs.VectorCreate( cp , roadpnt[plc] )
        rs.MoveObject( audi , -movec )
        
        
        dir_vec = rs.VectorCreate( roadpnt[ plc + drift_ang ] , roadpnt[ plc ])
        cur_dir_vec = rs.VectorCreate( cc_end , cp )
        
        cross_product = rs.VectorCrossProduct(cur_dir_vec, dir_vec)
        rot_ang = rs.VectorAngle( cur_dir_vec , [dir_vec[0] , dir_vec[1] , 0 ] )
        if cross_product[2] < 0:
            rot_ang = -rot_ang
        rot_ax = [0,0,1]
        
        yaw = rs.RotateObject( audi , cp , rot_ang , rot_ax )
        
        drive_vec = rs.VectorCreate( cp , cc_end )
        drive_vec = rs.VectorUnitize( drive_vec )
        
        pitch_vec = rs.VectorCreate( roadpnt[plc] , roadpnt[ plc + drift_ang] )
        pitch_vec = rs.VectorUnitize( pitch_vec )
        
        pitch_ang = rs.VectorAngle( drive_vec , pitch_vec )
        
        if pitch_vec[2] < drive_vec[2]:
            pitch_ang = -pitch_ang
        
        pitch_ax = rs.VectorCreate( cp , cp_r )
        pitch_ax = rs.VectorUnitize( pitch_ax )
        
        rs.RotateObject( audi , cp , pitch_ang , pitch_ax )



###color####################

rs.ObjectColor( cp , [255,0,0])
rs.ObjectColor( cc_end , [0,255,0])
rs.ObjectColor( cp_t , [0,0,255])
rs.ObjectColor( cp_r , [255,0,255])
rs.ObjectColor( cp_c , [0,255,255])
rs.ObjectColor( cp_cf , [0,255,255])



###Capture####################

path = "G:\\Uni\\Dm2\\Finale\\5_Progress\\Frames\\"
if not os.path.exists(path):
    os.makedirs(path)

frameNr = 0



###Drive + capture + cam####################

if if_drive:
    for plc in range (550):
        move = movement( plc )
        rs.Redraw()
        
        
        if if_capture:
            frameNam = dm.makeName("audi_X", frameNr, anzahl=4, format='jpg')
            rs.Command("-viewCaptureToFile Width=960 Height=540 Scale=2 DrawCPlaneAxes=No TransparentBackground=No "+path+frameNam+" ", 0)
            frameNr += 1
        
        
        ###automatische kamera####################
        if if_auto_camera:
            if 0 <= plc <= 50:
                cam1()
            
            if 51 <= plc <= 180:
                cam2()
            
            if 181 <= plc <= 240:
                cam2_5()
            
            if 241 <= plc <= 310:
                cam3 ()
            
            if 311 <= plc <= 430:
                cam4 ()
            
            if 431 <= plc <= 500:
                cam5 ()