#!/usr/bin/env python
# -*- coding: utf-8 -*-
# generated by wxGlade 0.6.4 on Mon Jun 17 15:15:28 2013

import wx
from SerialM8 import SerialM8
from SensorMagnetometer import MagnetometerHMC
from GpsSensor import GpsPoller
import gps
import time
import sys
# begin wxGlade: extracode
# end wxGlade


class MyFrame(wx.Frame):
    def __init__(self, *args, **kwds):
        # begin wxGlade: MyFrame.__init__
        kwds["style"] = wx.DEFAULT_FRAME_STYLE
        wx.Frame.__init__(self, *args, **kwds)
        self.button_6 = wx.Button(self, -1, "SetKiri")
        self.button_1 = wx.Button(self, -1, "UP")
        self.button_7 = wx.Button(self, -1, "SetKanan\n")
        self.button_5 = wx.Button(self, -1, "Kiri")
        self.button_2 = wx.Button(self, -1, "Stop")
        self.button_4 = wx.Button(self, -1, "Kanan")
        self.button_9 = wx.Button(self, -1, "Kembali")
        self.button_3 = wx.Button(self, -1, "Mundur")
        self.button_8 = wx.Button(self, -1, "GetData")

        self.__set_properties()
        self.__do_layout()

        self.Bind(wx.EVT_BUTTON, self.HalfLeft, self.button_6)
        self.Bind(wx.EVT_BUTTON, self.Maju, self.button_1)
        self.Bind(wx.EVT_BUTTON, self.HalfRight, self.button_7)
        self.Bind(wx.EVT_BUTTON, self.Left, self.button_5)
        self.Bind(wx.EVT_BUTTON, self.Stop, self.button_2)
        self.Bind(wx.EVT_BUTTON, self.Right, self.button_4)
        self.Bind(wx.EVT_BUTTON, self.Back, self.button_9)
        self.Bind(wx.EVT_BUTTON, self.Reverse, self.button_3)
        self.Bind(wx.EVT_BUTTON, self.GetData, self.button_8)
        # end wxGlade
        con=Controler(mode="manual")
		
    def __set_properties(self):
        # begin wxGlade: MyFrame.__set_properties
        self.SetTitle("GetDataUGV-PI")
        # end wxGlade

    def __do_layout(self):
        # begin wxGlade: MyFrame.__do_layout
        sizer_1 = wx.BoxSizer(wx.VERTICAL)
        grid_sizer_1 = wx.GridSizer(3, 3, 0, 0)
        grid_sizer_1.Add(self.button_6, 0, wx.EXPAND | wx.ADJUST_MINSIZE, 0)
        grid_sizer_1.Add(self.button_1, 0, wx.EXPAND | wx.ADJUST_MINSIZE, 0)
        grid_sizer_1.Add(self.button_7, 0, wx.EXPAND | wx.ADJUST_MINSIZE, 0)
        grid_sizer_1.Add(self.button_5, 0, wx.EXPAND | wx.ADJUST_MINSIZE, 0)
        grid_sizer_1.Add(self.button_2, 0, wx.EXPAND | wx.ADJUST_MINSIZE, 0)
        grid_sizer_1.Add(self.button_4, 0, wx.EXPAND | wx.ADJUST_MINSIZE, 0)
        grid_sizer_1.Add(self.button_9, 0, wx.EXPAND | wx.ADJUST_MINSIZE, 0)
        grid_sizer_1.Add(self.button_3, 0, wx.EXPAND | wx.ADJUST_MINSIZE, 0)
        grid_sizer_1.Add(self.button_8, 0, wx.EXPAND | wx.ADJUST_MINSIZE, 0)
        sizer_1.Add(grid_sizer_1, 1, wx.EXPAND, 0)
        self.SetSizer(sizer_1)
        sizer_1.Fit(self)
        self.Layout()
        # end wxGlade

    def HalfLeft(self, event):  # wxGlade: MyFrame.<event_handler>
        print "HalfLeft"
        event.Skip()

    def Maju(self, event):  # wxGlade: MyFrame.<event_handler>
        print "Maju"
        event.Skip()

    def HalfRight(self, event):  # wxGlade: MyFrame.<event_handler>
        print "HalfRight"
        event.Skip()

    def Left(self, event):  # wxGlade: MyFrame.<event_handler>
        print "Left"
        event.Skip()

    def Stop(self, event):  # wxGlade: MyFrame.<event_handler>
        print "Stop"
        event.Skip()

    def Right(self, event):  # wxGlade: MyFrame.<event_handler>
        print "Right"
        event.Skip()

    def Back(self, event):  # wxGlade: MyFrame.<event_handler>
        print "Back"
        event.Skip()

    def GetData(self, event):  # wxGlade: MyFrame.<event_handler>
        print "GetData"
        event.Skip()

    def Reverse(self, event):  # wxGlade: MyFrame.<event_handler>
        print "Reverse"
        event.Skip()
        
	con.manual()
# end of class MyFrame
class MyApp(wx.App):
    def OnInit(self):
        wx.InitAllImageHandlers()
        frame_1 = MyFrame(None, -1, "")
        self.SetTopWindow(frame_1)
        frame_1.Show()
        return 1


# end of class MyApp

class Controler:
	def __init__(self,mode):
		self.mode = mode
		self.master = SerialM8()
		self.dataSerial=[]
		self.Heading= Heading#compas
		gpsSensor=GpsPoller()#gps thread
		gpsSensor.running=True
		gpsSensor.start()
		GpsStatus=gpsSensor.status()
	def manual(self,pwml,pwmr,left,right):
		if self.mode =="manual":
			command = "$master,%s,%s,%i,%i" %(pwml,pwmr,left,right) #data yang dikirim
			self.dataSerial=self.master.SerialSend(command) #data Serial
			GpsStatus=gpsSensor.status()
			print "encoder,%s,%s, %s" %(encL,encR,errorHeading)
			print "GpsData:", gpsSensor.data() #output data gps
			print time.strftime("%Y:%m:%d:%H:%M:%S", time.gmtime()), self.Range, command
			print >> sys.stderr,"encoder,%s,%s, %s" %(encL,encR,errorHeading)
			print >> sys.stderr,"GpsData:", gpsSensor.data() #output data gps
			print >> sys.stderr,time.strftime("%Y:%m:%d:%H:%M:%S", time.gmtime()), self.Range, command
		else:
			pass
	def killgps(self):
		gpsSensor.running=False #kill thread GPS
		gpsSensor.join() 
	def semiauto(self,Heading,Range):
		self.Heading= Heading
		self.Range = Range
		if self.mode =="semiauto":
			#using gps
			gpsSensor=GpsPoller()
			gpsSensor.running=True
			gpsSensor.start()
			GpsStatus=gpsSensor.status()
			#f.write ("%b", %GpsStatus)
			print GpsStatus
			#using compass
			errorHeading=self.error()
			while (errorHeading>10 or errorHeading<-10):
				if errorHeading > 10:
					command=self.putarkanan()
					self.dataSerial=self.master.SerialSend(command)
				else:
					command=self.putarkiri()
					self.dataSerial=self.master.SerialSend(command)
				errorHeading=self.error()
				#f.write(errorHeading, command)
				print errorHeading, command
				time.sleep(0.1)
			print "lurus"
			while self.Range!=0:
				command=self.maju()
				self.dataSerial=self.master.SerialSend(command)
				#get data encoder
				(statusenc,encL,encR)=self.master.GetDataEncoder()
				errorHeading=self.error()
				print "encoder,%s,%s, %s" %(encL,encR,errorHeading)
				print "GpsData:", gpsSensor.data() #output data gps
				print time.strftime("%Y:%m:%d:%H:%M:%S", time.gmtime()), self.Range, command
				print >> sys.stderr,"encoder,%s,%s, %s" %(encL,encR,errorHeading)
				print >> sys.stderr,"GpsData:", gpsSensor.data() #output data gps
				print >> sys.stderr,time.strftime("%Y:%m:%d:%H:%M:%S", time.gmtime()), self.Range, command
				self.Range-=1
				time.sleep(0.1)
			command=self.stop()
			self.dataSerial=self.master.SerialSend(command)

		else:
			pass
			
	def error(self):
		self.Magnetometer=MagnetometerHMC()
		(MagnetometerHeading,MagnetometerMinute)=self.Magnetometer.data()
		errorHeading = MagnetometerHeading-self.Heading 
		return errorHeading
		
	def putarkanan(self):
		pwml=250
		pwmr=250
		left =1
		right =0
		command ="$master,%s,%s,%i,%i\r\n" % (pwml,pwmr,left,right)
		return command
	def putarkiri(self):
		pwml=250
		pwmr=250
		left =0
		right =1
		command ="$master,%s,%s,%i,%i\r\n" % (pwml,pwmr,left,right)
		return command
	def maju(self):
		pwml=255
		pwmr=255
		left =1
		right =1
		command ="$master,%s,%s,%i,%i\r\n" % (pwml,pwmr,left,right)
		return command
	def stop(self):
		pwml=0
		pwmr=0
		left =1
		right =1
		command ="$master,%s,%s,%i,%i\r\n" % (pwml,pwmr,left,right)
if __name__ == "__main__":
    app = MyApp(0)
    app.MainLoop()
