import socket
import time
import binascii
import time
import struct

msgsbuffer=bytearray (0)

WP_Count=0
WP_Upload_Ready=True

MAVLINK_MSG_ID_HEARTBEAT = 0
MAVLINK_MSG_ID_GPS_RAW_INT = 24
MAVLINK_MSG_ID_MISSION_REQUEST = 40
MAVLINK_MSG_ID_MISSION_COUNT = 44
MAVLINK_MSG_ID_REQUEST_DATA_STREAM = 66
MAVLINK_MSG_ID_MISSION_ACK=47

MY_SYSID=12  #Airplane Number
MY_SUBSYSID=16  #Subsystem Number, does not have any meaning

MSG_MAGIC=0
MSG_LEN=1
MSG_MSGID=7
    
magic=253
length=0
packet_count=0

def crc_sum (data, accum): 
    tmp=data ^ (accum & 0xff)
    tmp=(tmp ^ (tmp<<4)) & 0xff
    accum = (accum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >>4)
    accum=accum & 0xffff
    return accum

def generate_checksum (msg,extra): 
    ac=0xffff
    for i in range (1,len(msg)-2,1):  # we exclude the first  and the last two entries
      ac=crc_sum(msg[i],ac)
    ac=crc_sum(extra,ac) 
    return ac

# ================ SENDING MESSAGES ROUTINES ============================
def special_routine (a,crc_extra): #Generate checksum and inserts it, and also increases packet_count
    global packet_count
    packet_count=packet_count+1
    if packet_count>255:
        packet_count=0
    chksum=generate_checksum (a,crc_extra)
    a[a[MSG_LEN]+10]=chksum-int (chksum/256)*256
    a[a[MSG_LEN]+10+1]=int (chksum/256)
    return a

def make_header (a,msgid):
    global packet_count,MY_SYSID,MY_SUBSYSID
    hdr=bytearray (struct.pack ('<BBBBBBBBBB',magic,len (a),0,0,packet_count,MY_SYSID,MY_SUBSYSID,msgid,0,0))
    return hdr

def heart_beat_send (custom_mode,mtype,autopilot,base_mode,system_status,mavlink_version):
    global packet_count,MY_SYSID,MY_SUBSYSID
    ordered_fieldnames = ['custom_mode', 'type', 'autopilot', 'base_mode', 'system_status', 'mavlink_version']
    packstring='<IBBBBB'
    crc_extra = 50

    a=struct.pack(packstring,custom_mode,mtype,autopilot,base_mode,system_status,mavlink_version)
    hdr=make_header (a,MAVLINK_MSG_ID_HEARTBEAT)
    a=hdr+a+bytearray(2)
    a=special_routine (a,crc_extra)
    print (" >>>>>>>>> HEART BEAT SENT:",binascii.hexlify(a))
 
    return a

 
def GPS_raw_send (time_usec,lat,lon,alt,eph,epv,vel,cog,fix_type,satellites_visible,alt_ellipsoid,h_acc,v_acc,vel_acc,hdg_acc):
    global packet_count,MY_SYSID,MY_SUBSYSID
    ordered_fieldnames = ['time_usec', 'lat', 'lon', 'alt', 'eph', 'epv', 'vel', 'cog', 'fix_type', 'satellites_visible']
    packstring='<QiiiHHHHBBiIIII'
    crc_extra = 24
    
    a=struct.pack(packstring, time_usec, lat, lon, alt,eph, epv, vel, cog, fix_type, satellites_visible,alt_ellipsoid,h_acc,v_acc,vel_acc,hdg_acc)
    hdr=make_header (a,MAVLINK_MSG_ID_GPS_RAW_INT)
    a=hdr+a+bytearray(2)
    a=special_routine (a,crc_extra)
    print (" >>>>>>>>> GPS RAW SENT:",binascii.hexlify(a))
 
    return a

def Mission_request_send (seq,target_system,target_component,mission_type):
    global packet_count,MY_SYSID,MY_SUBSYSID  
    ordered_fieldnames = ['seq', 'target_system', 'target_component','mission_type']
    packstring='<HBBB'
    crc_extra = 230
    a=struct.pack(packstring,seq,target_system,target_component,mission_type)
    hdr=make_header (a,MAVLINK_MSG_ID_MISSION_REQUEST)
    a=hdr+a+bytearray(2)
    a=special_routine (a,crc_extra)
    print (" >>>>>>>>> MISSION REQUEST SENT:", seq, MY_SYSID,MY_SUBSYSID,mission_type,binascii.hexlify(a))
    return a
 

def Mission_acknowledge_send (target_system,target_component,ack_type):
    global packet_count,MY_SYSID,MY_SUBSYSID  
    name = 'MISSION_ACK'
    ordered_fieldnames = ['target_system', 'target_component', 'type']
    packstring= '<BBB'
    crc_extra = 153
    a=struct.pack(packstring,target_system,target_component,ack_type)
    hdr=bytearray (struct.pack ('<BBBBBB',magic,len (a),packet_count,MY_SYSID,MY_SUBSYSID,MAVLINK_MSG_ID_MISSION_ACK))
    a=hdr+a+bytearray(2)
    a=special_routine (a,crc_extra)
    print (" >>>>>>>>> MISSION ACK SENT:", ack_type, MY_SYSID,MY_SUBSYSID,binascii.hexlify(a))
    return a


# ============= RECEIVING MESSAGES ROUTINES ====================================
def receive_HEART_BEAT (toparse):
    ordered_fieldnames = ['custom_mode', 'type', 'autopilot', 'base_mode', 'system_status', 'mavlink_version']
    packstring='<IBBBBB'
    name = 'HEARTBEAT'
    crc_extra = 50
    chksum_should=generate_checksum (toparse,crc_extra)  
    chksum_is=toparse[len(toparse)-2]+toparse[len(toparse)-1]*256
    if chksum_should !=chksum_is:        
        print ("CHECKSUM ERROR")   
    else:
        length=toparse[MSG_LEN]    
        custom_mode,mtype,autopilot,base_mode,system_status,mavlink_version=struct.unpack (packstring,toparse[10:10+length])
        print ("<<<<<<<<<<<<",name,custom_mode,mtype,autopilot,base_mode,system_status,mavlink_version,binascii.hexlify(toparse))
    return


def receive_MISSION_COUNT (toparse):
    global WP_Count,MY_SYSID,MY_SUBSYSID
  
    ordered_fieldnames = ['count', 'target_system', 'target_component','mission_type'] # From mavgen.py!!!
    packstring= '<HBB'  # Should be HBBB, but Mission Planner sends less data!!!!!
    name = 'MISSION_COUNT'
    crc_extra = 221
    chksum_should=generate_checksum (toparse,crc_extra)  #50 is the CRC_EXTRA we need to locate it on basis of  MSG_ID
    chksum_is=toparse[len(toparse)-2]+toparse[len(toparse)-1]*256
    
    if chksum_should !=chksum_is:
        print ("CHECKSUM ERROR")        
    else:
        length=toparse[MSG_LEN]
        print ("Mission Count Data Length:",length)
        # SHOULD BE: count, target_system,target_component,mission_type=struct.unpack (packstring,toparse[10:10+length])
        count, target_system,target_component=struct.unpack (packstring,toparse[10:10+length])

        if target_system==MY_SYSID and target_component==MY_SUBSYSID:
            WP_Count=count
            WP_Upload_Ready=True  # indicates that we are ready for the first WP
        print ("<<<<<<<<<<<<",name,count,target_system,target_component,binascii.hexlify(toparse))      
    return


def receive_REQUEST_DATA_STREAM (toparse):
     ordered_fieldnames = ['req_message_rate', 'target_system', 'target_component', 'req_stream_id', 'start_stop']
     packstring = '<HBBBB'
     name = 'REQUEST_DATA_STREAM'
     crc_extra = 148
     chksum_should=generate_checksum (toparse,crc_extra)  #50 is the CRC_EXTRA we need to locate it on basis of  MSG_ID
     chksum_is=toparse[len(toparse)-2]+toparse[len(toparse)-1]*256
     if chksum_should !=chksum_is:
        print ("CHECKSUM ERROR")        
     else:
        length=toparse[MSG_LEN]    
        req_message_rate,target_system,target_component,req_stream_id,start_stop=struct.unpack (packstring,toparse[10:10+length])
        print ("<<<<<<<<<<<<",name,req_message_rate,target_system,target_component,req_stream_id,start_stop,binascii.hexlify(toparse))      

     return

def receive_UNKNOWN (toparse):
     print ("An Unknown MAVLINK MESSAGE was received, MSG ID:",toparse[MSG_MSGID],"LENGTH:",toparse[MSG_LEN])
     print (" <<<<<<<<<<<<< Message:",binascii.hexlify(toparse))
    
     return   

def process_cmd (toparse):
    MAVLINK_MSGS = {
        MAVLINK_MSG_ID_HEARTBEAT: receive_HEART_BEAT,
        MAVLINK_MSG_ID_MISSION_COUNT: receive_MISSION_COUNT,
        MAVLINK_MSG_ID_REQUEST_DATA_STREAM: receive_REQUEST_DATA_STREAM,
                    }

    print ()
    func = MAVLINK_MSGS.get(toparse[MSG_MSGID], receive_UNKNOWN)
    func(toparse)   
    return

def parse (msg):
    global msgsbuffer
    msgsbuffer=msgsbuffer+msg
    while len (msgsbuffer)>12:
        i=0        
        while i<len (msgsbuffer)-1 and msgsbuffer[i] !=magic:
            i=i+1
        if i==len (msgsbuffer)-1:
            return  "No magic header"
        if len(msgsbuffer)<12:
            return "Not enough bytes"
        if msgsbuffer[i]==magic:
           length=msgsbuffer[i+1]
           if length<=len(msgsbuffer)-i-2:
               toparse=msgsbuffer[i:i+length+12]  #extract finished message
               process_cmd (toparse)
               msgsbuffer=msgsbuffer[i+length+12:] #extract remainder of the data stream
    return

def send(port):
    global WP_Count, WP_Upload_Ready,MY_SYSID,MY_SUBSYSID
    global c
    s = socket.socket()
    s.bind(('', port))
    s.listen(5)
    c, addr = s.accept()
    print ('Got connection from',addr)
    k=1
    while True:      
      data=heart_beat_send (0,1,0,4+8+16+64+128,0,3)
      c.send(data)
      data=GPS_raw_send (10000+k*1000,-355000000+k*60,-710000000+k*120,145+k,2,2,2345,135,1,14,7,8,9,10,11)
      k=k+10
      time.sleep (0.2)
      c.send(data)
      # data=Mission_acknowledge_send (MY_SYSID,MY_SUBSYSID,0)
      time.sleep (0.2)
      inp=c.recv (4096)
      parse (inp)
      if WP_Count>0 and WP_Upload_Ready:
        time.sleep (0.2)
        WP_Count=WP_Count-1
        data=Mission_request_send (1,MY_SYSID,MY_SUBSYSID,0)
        c.send(data)
        WP_Upload_Ready=False  # We set it to false because we need to receive one WP, where it will be reset to true

print ("STARTING THE TEST PROGRAM FOR MAVLINK COMMUNICATION")
port = 1032
send(port)