/**
 * A Legoino example to connect two train hubs and one powered up remote. 
 * 
 * 1) Power up the ESP32
 * 2) Power up the Remote and the Train Hubs in any order
 * 
 * You can change the motor speed of train 1 with the left (A) remote buttons
 * You can change the motor speed of train 2 with the right (B) remote buttons
 * 
 * (c) Copyright 2020
 * Released under MIT License
 * 
 */

#include "Lpf2Hub.h"

// create a hub instance
Lpf2Hub myRemote;
Lpf2Hub myTrainHub1;


byte portLeft = (byte)PoweredUpRemoteHubPort::LEFT;
byte portA = (byte)PoweredUpHubPort::A;

int currentSpeedTrain1 = 0;
int updatedSpeedTrain1 = 0;

// additional rail rules

int speedLimit = 50;
int newSpeed;
bool stationBusy;


bool isInitialized = false;

// callback function to handle updates of remote buttons
void remoteCallback(void *hub, byte portNumber, DeviceType deviceType, uint8_t *pData)
{
  Lpf2Hub *myRemoteHub = (Lpf2Hub *)hub;

//  Serial.print("sensorMessage callback for port: ");
//  Serial.println(portNumber, DEC);
  if (deviceType == DeviceType::REMOTE_CONTROL_BUTTON)
  {
    ButtonState buttonState = myRemoteHub->parseRemoteButton(pData);
//    Serial.print("Buttonstate: ");
//    Serial.println((byte)buttonState, HEX);

    // Do the logic for left buttons of remote control and Train Hub 1
    if (portNumber == (byte)portLeft && buttonState == ButtonState::UP)
    {
      updatedSpeedTrain1 = min(100, currentSpeedTrain1 + 10);
    }
    else if (portNumber == (byte)portLeft && buttonState == ButtonState::DOWN)
    {
      updatedSpeedTrain1 = min(100, currentSpeedTrain1 - 10);
    }
    else if (portNumber == (byte)portLeft && buttonState == ButtonState::STOP)
    {
      updatedSpeedTrain1 = 0;
    }
    //Only allow train to update speed if less then speed limit.
    if (currentSpeedTrain1 != updatedSpeedTrain1 && updatedSpeedTrain1 <= speedLimit)
    {
        myTrainHub1.setBasicMotorSpeed(portA, updatedSpeedTrain1);
        currentSpeedTrain1 = updatedSpeedTrain1;
        Serial.print("Current speed :");
        Serial.println(currentSpeedTrain1, DEC);
    }
    // Prints warning to user that command didnt run becuase it was over the speedLimit.
    else if (updatedSpeedTrain1 > speedLimit)
    {
      Serial.print("Cant go faster then speed limit which is " + speedLimit);  
    }

    
  }
}

// Rules of the railway function // In a function cuase this logic will get alot bigger
// Function not being called at the moment for debugging purpose. // Function is called at top of void loop();
// At the moment the function constantly checks no trains are going faster then the speed limit and if they are... slows them down.
void rules(int current, int limit)
{
  //Checks current speed isnt higher then limit
  if (current > limit)
  {
    newSpeed = limit;
    myTrainHub1.setBasicMotorSpeed(portA, newSpeed);
    Serial.print("To Fast - Speed Limit: " + limit); // This line is getting partly called for some reason when hitting the speeLimit logic in the remote callback function.
    currentSpeedTrain1 = newSpeed;
  }
}

void setup()
{
  Serial.begin(115200);
  myRemote.init();                       // initialize the remote control hub
  myTrainHub1.init("90:84:2b:24:47:82"); // initialize the listening train hub 1 // here you have to use your own device ids
}

// main loop
void loop()
{
  //Checks rules of track 
  //rules(currentSpeedTrain1, speedLimit);
  
  //wait for three elements

  if (myRemote.isConnecting())
  {
    if (myRemote.getHubType() == HubType::POWERED_UP_REMOTE)
    {
      //This is the right device
      if (!myRemote.connectHub())
      {
        Serial.println("Unable to connect to hub");
      }
      else
      {
        myRemote.setLedColor(GREEN);
        Serial.println("Remote connected.");
      }
    }
  }

  if (myTrainHub1.isConnecting())
  {
    if (myTrainHub1.getHubType() == HubType::POWERED_UP_HUB)
    {
      myTrainHub1.connectHub();
      myTrainHub1.setLedColor(BLUE);
      Serial.println("train hub 1 connected.");
    }
  }

  if (!myRemote.isConnected())
  {
    myRemote.init();
  }

  if (!myTrainHub1.isConnected())
  {
    myTrainHub1.init();
  }

  if (myRemote.isConnected() && myTrainHub1.isConnected() && !isInitialized)
  {
    Serial.println("System is initialized");
    isInitialized = true;
    delay(200); //needed because otherwise the message is to fast after the connection procedure and the message will get lost
    // both activations are needed to get status updates
    myRemote.activatePortDevice(portLeft, remoteCallback);
    myRemote.setLedColor(WHITE);
  }
} // End of loop