#!/usr/bin/env python3
from abc import ABC, abstractmethod

from openpilot.common.realtime import DT_TRML
from openpilot.common.numpy_fast import interp
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.controls.lib.pid import PIDController


class BaseFanController(ABC):
  @abstractmethod
  def update(self, cur_temp: float, ignition: bool, standstill: bool = False,
             is_parked: bool = True, cruise_engaged: bool = False) -> int:
    pass


class TiciFanController(BaseFanController):
  def __init__(self) -> None:
    super().__init__()
    cloudlog.info("Setting up TICI fan handler")

    self.last_ignition = False
    self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_TRML))

  def update(self, cur_temp: float, ignition: bool, standstill: bool = False,
             is_parked: bool = True, cruise_engaged: bool = False) -> int:
    # CLEARPILOT fan range rules:
    #   parked                                              → 0-100% (full, no floor)
    #   in drive + cruise engaged (any speed, inc standstill) → 30-100%
    #   in drive + cruise off + standstill                  → 10-100%
    #   in drive + cruise off + moving                      → 30-100%
    # In the PID output, neg_limit is how negative it can go (= max fan as %),
    # pos_limit is how positive (= negative of min fan %).
    if is_parked:
      self.controller.neg_limit = -100
      self.controller.pos_limit = 0
    elif cruise_engaged:
      self.controller.neg_limit = -100
      self.controller.pos_limit = -30
    elif standstill:
      self.controller.neg_limit = -100
      self.controller.pos_limit = -10
    else:
      self.controller.neg_limit = -100
      self.controller.pos_limit = -30

    if ignition != self.last_ignition:
      self.controller.reset()

    error = 70 - cur_temp
    fan_pwr_out = -int(self.controller.update(
                      error=error,
                      feedforward=interp(cur_temp, [60.0, 100.0], [0, -100])
                    ))

    self.last_ignition = ignition
    return fan_pwr_out