import numpy as np

import json
import os
import serial
import subprocess
import threading
import time

SERIAL_PATH = "/dev/ttyACM0"
SERIAL_BAUD = 115200

SAMPLE_PERIOD = 0.5
DECIMATION_RATE = 1

try:
  is_mock = os.environ['MOCK']
except KeyError:
  is_mock = False

print("controller start")
process = None
def start_process():
  global process
  if is_mock:
    process = subprocess.Popen(["/usr/bin/env", "python", "/home/kelvin/src/shroom-server/shroom_pipe.py"], stdin=subprocess.PIPE, stdout=subprocess.PIPE)
  else:
    process = subprocess.Popen(["ssh", "shrooms@threefortiethofonehamster.com", "/usr/bin/env", "python3", "/home/shrooms/shrooms-server/shroom_pipe.py"], stdin=subprocess.PIPE, stdout=subprocess.PIPE)
start_process()

def send_update(msg):
  global process
  process.stdin.write(bytes(json.dumps(msg) + "\n", "utf8"))
  process.stdin.flush()

class MockSerial:
  def __init__(self):
    self.humidity = np.zeros(100)
    self.humidifier_on = False
    self.humidity[:] = 80
    self.humidity[-1] = 20
    self.humidity[0] = 20

  def flush(self):
    pass

  def write(self, msg):
    if msg == b'h':
      print("mock hum toggle")
      self.humidifier_on = not self.humidifier_on

  def read(self, _):
    t = time.time()
    temp = 25 + np.sin(0.01*2*np.pi*t) + 0.5*np.sin(0.0001*2*np.pi*t + 7)

    # very janky model of humidity diffusion
    # fix end conditions
    for _ in range(20):
      self.humidity[-1] = 0.2*20 + 0.8*self.humidity[-2]
      self.humidity[0] = 20
      if self.humidifier_on:
        self.humidity[20] = 2
      # use the gradient to determine the change in humidity
      avg = 0.5*(self.humidity[:-2] + self.humidity[2:])
      self.humidity[1:-1] += 0.10*(avg - self.humidity[1:-1])
    #print(self.humidity)

    humidity = self.humidity[60] + np.random.random()*0.003
    if self.humidifier_on:
      hv = 3.3
    else:
      hv = 0.0
    return bytes("{},{},{}\n".format(humidity, temp, hv), "utf8")

if is_mock:
  s = MockSerial()
else:
  s = serial.Serial(SERIAL_PATH, SERIAL_BAUD, timeout = 0.3)
  print("pausing for bootloader...")
  time.sleep(10)
  print("pause done")

def reset_serial():
  global s
  if not is_mock:
    s.close()
    s = serial.Serial(SERIAL_PATH, SERIAL_BAUD, timeout = 0.3)
  time.sleep(10)

class Humidifier:
  def __init__(self):
    self.off_threshold = 0.4
    self.on_threshold = 1.9
    self.toggle_cooldown = 7

    self._on = False
    self.history = np.zeros(10)
    self.switch_timeout = 0

  @property
  def on(self):
    return self._on

  @on.setter
  def on(self, nv):
    old_on = self._on
    self._on = nv
    if nv:
      print("send hum on")
    else:
      print("send hum off")
    send_update({"status": {"humidifier": nv}})

  @property
  def off(self):
    return not self.on

  def update(self, volts):
    self.history[1:] = self.history[:-1]
    self.history[0] = volts
    #print(self.history)
    avg = np.sum(self.history)/self.history.shape[0]
    if self.on:
      if avg < self.off_threshold:
        self.on = False
        self.switch_timeout = time.time() + 1
    else:
      if avg > self.on_threshold:
        self.on = True
        self.switch_timeout = time.time() + 1

  def toggle(self, s):
    if time.time() > self.switch_timeout:
      s.write(b"h")
      s.flush()
      self.switch_timeout = time.time() + self.toggle_cooldown

# the wiring's a little different so the thresholds for detecting on/off are inverted but hopefully a lot more reliable than the original
class HumidifierV2:
  def __init__(self, toggle_cmd=b"i", humidifier_id="humdifier2"):
    self.on_threshold = 1.5
    self.off_threshold = 2.5
    self.toggle_cooldown = 7
    self.toggle_command = toggle_cmd

    self._on = False
    self.history = np.zeros(10)
    self.switch_timeout = 0

  @property
  def on(self):
    return self._on

  @on.setter
  def on(self, nv):
    old_on = self._on
    self._on = nv
    if nv:
      print("send hum 2 on")
    else:
      print("send hum 2 off")
    send_update({"status": {humdifier_id: nv}})

  @property
  def off(self):
    return not self.on

  def update(self, volts):
    self.history[1:] = self.history[:-1]
    self.history[0] = volts
    #print(self.history)
    avg = np.sum(self.history)/self.history.shape[0]
    if self.on:
      if avg > self.off_threshold:
        self.on = False
        self.switch_timeout = time.time() + 1
    else:
      if avg < self.on_threshold:
        self.on = True
        self.switch_timeout = time.time() + 1

  def toggle(self, s):
    if time.time() > self.switch_timeout:
      s.write(self.toggle_command)
      s.flush()
      self.switch_timeout = time.time() + self.toggle_cooldown


class Controller:
  def __init__(self):
    self.target_lower = 85
    self.target_upper = 90
    self.feedforward_coeff = 50
    self.last_toggle = 0

    self._manual_mode = False
    self.manual_on = False
    self.manual_timeout = 0
    self.manual_duration = 40

    self.humidifier_history = np.zeros(50)
    self.first_sample = False

  @property
  def manual_mode(self):
    return self._manual_mode

  @manual_mode.setter
  def manual_mode(self, on):
    self._manual_mode = on
    send_update({"status": {"manual_mode": on}})

  def toggle_checked(self, humidifier, s):
    if time.time() - self.last_toggle > 0.8:
      humidifier.toggle(s)
      self.last_toggle = time.time()

  def update(self, humidifier, humidifier2, humidity):
    if self.first_sample:
      self.humidifier_history[:] = humidity
      self.first_sample = False
    else:
      self.humidifier_history[:-1] = self.humidifier_history[1:]
      self.humidifier_history[-1] = humidity

    # compensate for the slow response time by adding a little feed forward
    # using the slope of the humidifier data
    slope = (self.humidifier_history[-1] - self.humidifier_history[0])/self.humidifier_history.shape[0]
    comp_humidity = humidity + self.feedforward_coeff*slope

    if self.manual_mode and time.time() > self.manual_timeout:
      self.manual_mode = False

    # TODO add in support for manual control of second humidifier
    if self.manual_mode:
      if humidifier.off and self.manual_on:
        self.toggle_checked(humidifier, s)
      elif humidifier.on and not self.manual_on:
        self.toggle_checked(humidifier, s)
      if humidifier2.off and self.manual_on:
        self.toggle_checked(humidifier2, s)
      elif humidifier2.on and not self.manual_on:
        self.toggle_checked(humidifier2, s)
    else:
      if comp_humidity < self.target_lower:
        if humidifier.off:
          self.toggle_checked(humidifier, s)
        if humidifier2.off:
          self.toggle_checked(humidifier2, s)
      elif comp_humidity > self.target_upper:
        if humidifier.on:
          self.toggle_checked(humidifier, s)
        if humidifier2.on:
          self.toggle_checked(humidifier2, s)

humidifier = HumidifierV2(b"h", "humidifier")
humidifier2 = HumidifierV2()
controller = Controller()

exiting = False
# run thread to process data from process's stdout
def stdout_loop():
  global process, controller, humidifier, humidifier2
  while not exiting:
    msg = process.stdout.readline()
    if len(msg) <= 1:
      continue
    print("got message ", msg)
    try:
      msg_js = json.loads(msg)
      if "query_params" in msg_js:
        if msg_js["query_params"]:
          send_update({"params": {
            "target_lower": controller.target_lower,
            "target_upper": controller.target_upper,
            "feedforward_coeff": controller.feedforward_coeff,
            "manual_timeout": controller.manual_timeout,
            "manual_duration_s": controller.manual_duration,
            "manual_mode": 1.0 if controller.manual_mode else 0.0,
            "manual_hum_on": 1.0 if controller.manual_on else 0.0,

            "off_threshold_volts": humidifier.off_threshold,
            "on_threshold_volts": humidifier.on_threshold,
            "toggle_cooldown": humidifier.toggle_cooldown
            }
          })
      elif "set_params" in msg_js:
        if type(msg_js["set_params"]) is dict:
          set_params = msg_js["set_params"]
          if "name" in set_params and "value" in set_params:
            name, value = set_params["name"], set_params["value"]
            if type(value) is float:
              if name == "target_lower":
                controller.target_lower = value
              elif name == "target_upper":
                controller.target_upper = value
              elif name == "feedforward_coeff":
                controller.feedforward_coeff = value
              elif name == "manual_timeout":
                controller.manual_timeout = value
              elif name == "manual_duration_s":
                controller.manual_duration = value
              elif name == "off_threshold_volts":
                humidifier.off_threshold = value
              elif name == "on_threshold_volts":
                humidifier.on_threshold = value
              elif name == "toggle_cooldown":
                humidifier.toggle_cooldown = value
              elif name == "off_threshold_volts2":
                humidifier2.off_threshold = value
              elif name == "on_threshold_volts2":
                humidifier2.on_threshold = value
              elif name == "toggle_cooldown2":
                humidifier2.toggle_cooldown = value
      elif "manual_mode" in msg_js:
        controller.manual_mode = msg_js["manual_mode"]
        if controller.manual_mode:
          controller.manual_timeout = time.time() + controller.manual_duration
      elif "manual_mode_on" in msg_js:
        controller.manual_on = msg_js["manual_mode_on"]
    except json.JSONDecodeError as e:
      print("received bad json ", msg)
stdout_thread = threading.Thread(target=stdout_loop)
stdout_thread.start()


frame_num = 0
last_sample = 0
try:
  while True:
    now = time.time()
    if now - last_sample < SAMPLE_PERIOD:
      time.sleep(SAMPLE_PERIOD - (now - last_sample) + 0.001)
      continue
    last_sample = now

    #print("write s")
    s.write(b"s")
    s.flush()
    resp = s.read(120)
    #print("read", resp)
    if len(resp) == 0:
      reset_serial()
      time.sleep(5)
      continue
    parts = resp.split(b",")
    humidity = float(parts[0])
    temp = float(parts[1])
    volts = float(parts[2])
    volts2 = float(parts[3])

    try:
      humidifier.update(volts)
      humidifier2.update(volts2)
      controller.update(humidifier, humidifier2, humidity)

      if frame_num == 0:
        #print(humidity, temp, volts)
        update = {
            "data": {
              "time": int(now*1000),
              "temp": temp,
              "hum": humidity,
              "hv": volts,
              "hv2": volts2,
            }
        }
        send_update(update)
      frame_num = (frame_num + 1) % DECIMATION_RATE
    except Exception as e:
      print("pipe errored out, restarting: ", e)
      # restart the process I guess
      exiting = True
      process.kill()
      time.sleep(0.1)
      stdout_thread.join()
      exiting = False

      start_process()

      stdout_thread = threading.Thread(target=stdout_loop)
      stdout_thread.start()

finally:
  # kill ssh connection
  exiting = True
  process.kill()
  stdout_thread.join()