import PY_VTS.gui.colours as Colour
from PY_VTS.srn.base  import *
from PY_VTS.mgr.screens import *
from PY_VTS.mgr.CAN import *
from PY_VTS.mgr.Files import *
from PY_VTS.gui.VG import *
from PY_VTS.gui.Vector_Graphics.Steering_Wheel_Sensor import *
from PY_VTS.gui.Vector_Graphics.Steering_Wheel import *
from PY_VTS.mgr.GV import CONST,GV
from PY_VTS.pyVBO import pyVBO

from math import radians, cos, sin
import gui
import gnss
import vbox
import vts

class ScreenWHEEL(Screen_Base):
    def __init__(self):
        super().__init__()

        self.VBO = pyVBO()
        self.SD_Colour = [Colour.RED]

        self.log_data = []
        self.log_data.append({
            "text" : "{:07.3f}",
            "number" : 0
        })
        gnss.new_data_callback(self.gnss_cb)
        self.logging = False
        vts.set_sd_callback(self.sd_cb)

        self.sd_cb()

        super().set_title("Steering Wheel")
        self.wheel = VG_SteeringWheelSensor("Wheel",Point(200,240))
        super().add_vsync_cb(self.wheel.vsync, "Wheel")
        super().add_swipe_cb(self.swipe_left, "left")

        super().add_tag(self.SD_icon_cb, "Wheel SD")

        MgrCAN.signals["Angle Sensor 0"].callback = self.nm_update
        MgrCAN.signals["Angle Sensor 1"].callback = self.nms_update
        MgrCAN.signals["Angle Sensor 2"].callback = self.deg_update
        MgrCAN.signals["Angle Sensor 3"].callback = self.degs_update

        self.nm = [gui.CTRL_TEXT, 675, 109, 29, gui.OPT_CENTER, '']
        self.nms = [gui.CTRL_TEXT, 675, 197, 29, gui.OPT_CENTER, '']
        self.degrees = [gui.CTRL_TEXT, 675, 284, 29, gui.OPT_CENTER, '']
        self.degreess = [gui.CTRL_TEXT, 675, 372, 29, gui.OPT_CENTER, '']

        self.GNSS_Colour = [Colour.RED]

        self.SD_Colour = [Colour.RED]

        self.gui_l.extend([
            [Colour.BLACK],
            self.wheel.gui_l,
            [Colour.BLACK],
            [gui.DL_COLOR_A(150)],
            [gui.DL_LINE_WIDTH(2)],
            [gui.DL_BEGIN(gui.PRIM_RECTS)],
            [gui.DL_COLOR_RGB(10, 50, 100)],
            #[gui.DL_VERTEX2F(400, 65)], [gui.DL_VERTEX2F(800, 120)], # accel title
            [gui.DL_VERTEX2F(400, 65)], [gui.DL_VERTEX2F(550, 415)],
            [gui.DL_END()],
            [gui.DL_COLOR_A(255)],
            [Colour.BLACK],
            [gui.DL_BEGIN(gui.PRIM_LINES)],
            [gui.DL_VERTEX2F(400,  65)], [gui.DL_VERTEX2F(400, 415)],
            [gui.DL_VERTEX2F(400, 153)], [gui.DL_VERTEX2F(800, 153)],
            [gui.DL_VERTEX2F(400, 240)], [gui.DL_VERTEX2F(800, 240)],
            [gui.DL_VERTEX2F(400, 328)], [gui.DL_VERTEX2F(800, 328)],
            [gui.DL_END()],
            [Colour.WHITE],
            [gui.CTRL_TEXT, 475, 109, 29, gui.OPT_CENTER, 'Nm'],
            [gui.CTRL_TEXT, 475, 197, 29, gui.OPT_CENTER, 'Nm /s'],
            [gui.CTRL_TEXT, 475, 284, 29, gui.OPT_CENTER, 'Degrees'],
            [gui.CTRL_TEXT, 475, 372, 29, gui.OPT_CENTER, 'Degrees /s'],
            [Colour.BLACK],
            self.nm,
            self.nms,
            self.degrees,
            self.degreess,
            self.GNSS_Colour,
            GV.image_bank.get("GNSS").generate_gui_l(None,(335, -5)),
            self.SD_Colour,
            GV.image_bank.get("SD").generate_gui_l(self.tags["Wheel SD"].Tag,(405,-5)),
        ])

    def SD_icon_cb(self,btn):
        print("SD")
        if self.logging:
            self.logging = False
            print("stop")
        else:
            example_channel = []
            example_channel.append({
                "Name"      : "Steering Angle",
                "Units"     : "degrees",
                "Column"    : "Angle"
            })
            self.VBO.header(vts.unit_info(), Channels = example_channel)
            s = vbox.get_sample()
            self.p_time = s.tod_ms
            self.logging = True
            print("start")


    def gnss_cb(self):
        def inc_i(i):
            i += 1
            if i > 16:
                i -= 16
            return i
        def sub_i(i,s):
            i -= s
            if i < 0:
                i += 16 # VBOX_SAMPLE_BUF_DEPT
            return i

        s = vbox.get_sample()
        if s.sats_used >= 3:
            self.GNSS_Colour[0] = Colour.GREEN
        else:
            self.GNSS_Colour[0] = Colour.RED

        if self.logging:
            if s.tod_ms != self.p_time:
                missed = int(s.tod_ms/100) - int(self.p_time/100)
                if missed == 1:
                    self.p_time = s.tod_ms
                    self.VBO.line(s,self.log_data)
                elif missed < 8:
                    i = sub_i(s.index,missed-1)
                    while (s.tod_ms != self.p_time):
                        s = vbox.get_sample(int(i))
                        self.VBO.line(s,self.log_data)
                        inc_i(i)
                        self.p_time = s.tod_ms
                else:
                    self.p_time = s.tod_ms

    def sd_cb(self, *args):   # triggered when SD card is put in or removed
        # if the SD card is not intialised then stop logging
            if not (vts.get_sdcard_state() == 2):
                self.logging = False
                self.SD_Colour[0] = Colour.RED
            else:
                self.SD_Colour[0] = Colour.GREEN


    def swipe_left(self):
        Mgr_Screens.Set_Screen("Info")

    def deg_update(self, signal):
        self.log_data[0]["number"] = signal.data
        self.wheel.set_angle(Degrees = signal.data)
        self.degrees[5] = "{:.1f}".format(signal.data)

    def nm_update(self, signal):
        self.nm[5] = "{:.1f}".format(signal.data)

    def nms_update(self, signal):
        self.nms[5] = "{:.1f}".format(signal.data)

    def degs_update(self, signal):
        self.degreess[5] = "{:.1f}".format(signal.data)
