#pragma once

#include "Lpf2Hub.h"

class MyHub
  : public Lpf2Hub
{
public:
  void activateBaseSpeaker()
  {
    byte setSoundMode[8] = { 0x41, (byte)DuploTrainHubPort::SPEAKER, 0x01, 0x01, 0x00, 0x00, 0x00, 0x01 };
    Lpf2Hub::WriteValue(setSoundMode, 8);
    m_last_cmd = millis();
  }

  void playSound(byte sound)
  {
    byte playSound[6] = { 0x81, (byte)DuploTrainHubPort::SPEAKER, 0x11, 0x51, 0x01, sound };
    Lpf2Hub::WriteValue(playSound, 6);
    m_last_cmd = millis();
  }

  void activateRgbLight()
  {
    byte port = getPortForDeviceType((byte)DeviceType::HUB_LED);
    byte setColorMode[8] = { 0x41, port, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00 };
    Lpf2Hub::WriteValue(setColorMode, 8);
    m_last_cmd = millis();
  }

  void setLedColor(Color color)
  {
    byte port = getPortForDeviceType((byte)DeviceType::HUB_LED);
    byte setColor[6] = { 0x81, port, 0x11, 0x51, 0x00, color };
    Lpf2Hub::WriteValue(setColor, 6);
    m_last_cmd = millis();
  }

  void setMotorSpeedLevel(int speed_level)
  {
    if (speed_level > 3)
      speed_level = 3;
    else if (speed_level < -3)
      speed_level = -3;
    Serial.print("new speed="); Serial.println(speed_level);

    int speed_pct = 0;
    switch(speed_level)
    {
      case 3:
        speed_pct = 70;
        break;
      case 2:
        speed_pct = 50;
        break;
      case 1:
        speed_pct = 30;
        break;
      case -1:
        speed_pct = -30;
        break;
      case -2:
        speed_pct = -50;
        break;
      case -3:
        speed_pct = -70;
        break;
      default:
        speed_pct = 0;
        break;
    }

    m_speed_level = speed_level;
    setBasicMotorSpeed(speed_pct);
    if (abs(m_speed_level) == 1)
    {
      delay(200);
      setBasicMotorSpeed(speed_pct);
    }
  }

  void incrMotorSpeed()
  {
    setMotorSpeedLevel(m_speed_level + 1);
  }

  void decrMotorSpeed()
  {
    setMotorSpeedLevel(m_speed_level - 1);
  }

  void stopMotor()
  {
    stopBasicMotor();
    m_speed_level = 0;
  }

  void stopMotorWithBreak()
  {
    if (abs(m_speed_level) == 3)
    {
      playSound((byte)DuploTrainBaseSound::BRAKE);
      delay(200);
    }
    return stopMotor();
  }

  int isMotorStopped()
  {
    // make sure last motor command is at least 1 seconds ago
    // otherwise issueing a stop motor command might also falsely trigger this
    return (m_speed_level == 0 && millis() - m_last_motor_cmd > 1000);
  }

  int currentSpeedLevel()
  {
    return m_speed_level;
  }

  void toggleLight()
  {
    if (m_color == Color::BLACK)
    {
      setLedColor(m_prev_color);
      m_color = m_prev_color;
      m_prev_color = Color::BLACK;
    }
    else
    {
      setLedColor(Color::BLACK);
      m_prev_color = m_color;
      m_color = Color::BLACK;
    }
  }

  void cycleLightColor()
  {
    m_color = (Color)(int(m_color) + 1);
    if (m_color >= Color::NUM_COLORS)
      m_color = Color::PINK;
    setLedColor(m_color);
  }

  void onConnect()
  {
    m_speed_level = 0;
    m_last_cmd = m_last_motor_cmd = millis();
    m_color = m_prev_color = Color::BLUE;
  }

  unsigned long millisLastCommand()
  {
    return m_last_cmd;
  }

private:
  void stopBasicMotor()
  {
    Lpf2Hub::stopBasicMotor((byte)DuploTrainHubPort::MOTOR);
    m_last_cmd = millis();
    m_last_motor_cmd = m_last_cmd;
  }

  void setBasicMotorSpeed(int speed = 0)
  {
    Lpf2Hub::setBasicMotorSpeed((byte)DuploTrainHubPort::MOTOR, speed);
    m_last_cmd = millis();
    m_last_motor_cmd = m_last_cmd;
  }

private:
  int m_speed_level = 0;
  unsigned long m_last_motor_cmd = 0;
  unsigned long m_last_cmd;
  Color m_color = Color::BLACK;
  Color m_prev_color = Color::BLACK;
};