#!/usr/bin/env python
#
#   2019 Marco Bergman
#
# This Program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public
# License as published by the Free Software Foundation; either
# version 3 of the License, or (at your option) any later version.

import sys, os, time, math

from datetime import datetime
from pypilot.client import pypilotClient
from pypilot import pyjson
from values import *


class MyClient():
    def __init__(self):
        self.client = False
        self.last_servo_command = 0
        self.last_bell = datetime.now()


    def connect(self):
        watchlist = ['ap.enabled', 'ap.mode', 'ap.pilot', 'ap.bell_server', 'ap.heading_command',
                     'gps.source', 'wind.source', 'servo.controller', 'servo.flags',
                     'ap.pilot.basic.P', 'ap.pilot.basic.I', 'ap.pilot.basic.D']

        self.last_msg = {}

        host = ''
        if len(sys.argv) > 1:
            host = sys.argv[1]

        try:
            self.client = pypilotClient(host)

            for name in watchlist:
                self.client.watch(name)

            print('connected')
        except Exception as e:
            print(e)
            self.client = False
            time.sleep(1)


    def last_val(self, name):
        if name in self.last_msg:
            return self.last_msg[name]
        return 'N/A'


    def set(self, name, value):
        if self.client:
            # print ("setting {} to {}".format(name, value))
            self.client.set(name, value)


    def adjust_gain (self, mode, factor):
        if (mode == MODE_P):
                gain = "P"
        if (mode == MODE_I):
                gain = "I"
        if (mode == MODE_D):
                gain = "D"
        gain_name = "ap.pilot." + self.last_val("ap.pilot") + "." + gain
        gain_name = gain_name.replace("pilot..", "")
        current_gain = self.last_val (gain_name)
        new_gain = current_gain * factor
        print (gain_name + " = " + str(current_gain) + " * " + str(factor) + " = " + str(new_gain))
        self.set (gain_name, new_gain)


    def getMessages(self):
        # Listen out for pypilotServer messages; if they arrive, update them in self.last_msg dictionary
        while True:
            result = False
            if not self.client:
                print("reconnecting...")
                self.connect()
                print("reconnected...")
                break
            try:
                msgs = self.client.receive()
            except Exception as e:
                print('disconnected', e)
                self.client = False

            if not msgs:
                break

            for name, value in msgs.items():
                self.last_msg[name] = value
                print("New value:", str(name) + " = " + str(value))


    def processThings(self):
        self.getMessages()
        time.sleep (1)
        print ("P", self.last_val('ap.pilot.basic.P'), "I", self.last_val('ap.pilot.basic.I'), "D", self.last_val('ap.pilot.basic.D'))


def main():
    print('init...')

    myclient = MyClient()
    myclient.connect()

    while True:
        myclient.processThings()

if __name__ == '__main__':
    main()