#include "control.h"

#include <string.h>

Control::Control(Rotor *rotor)
{
  rotor_ = rotor;
  serial_communicator_ = new SerialCommunicator();
  udp_communicator_ = new UDPCommunicator();
  debugTime_ = 0;
}

void Control::doCommunicate()
{
  String command_string;
  Communicator::messageError msg_r;

  if((command_string = udp_communicator_->getCommandString(&msg_r)) && (msg_r == Communicator::MSG_OK)) {
    executeCommandString(&command_string, udp_communicator_);
  }

  if((command_string = serial_communicator_->getCommandString(&msg_r)) && (msg_r == Communicator::MSG_OK)) {
    executeCommandString(&command_string, serial_communicator_);
  }
}

uint16 Control::setElevation(uint16 el)
{
  return rotor_->setElevation(el);
}

uint16 Control::setAzimuth(uint16 az)
{
  return rotor_->setAzimuth(az);
}

void Control::doControl()
{
  rotor_->doRotor();
  doCommunicate();

  if(debugTime_ > 100)
  {
      //hamlib.debugOut();
      //rotor_->debugOut();
      debugTime_ = 0;
  }
  debugTime_++;
}

Control::hamlibError Control::executeCommandString(String *command_string, Communicator *communicator)
{
    String substring;
    String response = "";
    int lastPos = -1;
    int newPos = 0;

    while ((newPos = command_string->indexOf(";", lastPos + 1)) != -1) {
        substring = command_string->substring(lastPos + 1, newPos);
        lastPos = newPos;

        if ( substring == "state" )
        {
            // GNAAAARRR
            communicator->sendMessage(String(String(AZIM_MIN) + "/" + String(AZIM_MAX) + " " + String(ELEV_MIN) + "/" + String(ELEV_MAX)));
        }
        else if ( substring == "getpos" ) {
            // GNAAAARRR
            communicator->sendMessage(String(rotor_->getAzimuth()) + ";" + String(rotor_->getElevation()) + ";");
        }
        else {
            if(executeActiveCommandSubstring(substring) == HAMLIB_OK)
                response += "1";
            else
                response += "0";
        }
    }

    if(response != "")
        communicator->sendMessage(response);

    return HAMLIB_OK;
}

Control::hamlibError Control::executeActiveCommandSubstring(String substring)
{
    Motor::motorError err;
    float dst = 0.0;
    if (substring.startsWith("setaz") || substring.startsWith("setel")) {
        if( substring.length() < 7 )
            return HAMLIB_NOK;
        dst = substring.substring(5).toFloat();
        if ( dst == 0 && substring.substring(5) != "0" )
            return HAMLIB_NOK;
        if(substring.startsWith("setaz"))
            err = rotor_->setAzimuth(dst);
        else
            err = rotor_->setElevation(dst);
        if(err == Motor::ROT_VAL_OUT_OF_RANGE)
            return HAMLIB_NOK;
    }
    else if (substring == "stop") {
        rotor_->stopAzimuth();
        rotor_->stopElevation();
    }
    else if (substring == "brake") {
        rotor_->stopAndBreakAzimuth();
        rotor_->stopAndBreakElevation();
    }
    else
        return HAMLIB_NOK;
    return HAMLIB_OK;
}