/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers
//
#include <mrpt/hwdrivers/CWirelessPower.h>

#include <iostream>

#ifdef MRPT_OS_LINUX
#include <iostream>
#include <sstream>
#endif

#ifdef _WIN32
#if defined(__GNUC__)
// MinGW: Nothing to do here (yet)
#else

#define WIN32_LEAN_AND_MEAN
#include <windows.h>
//
#include <objbase.h>
#include <wlanapi.h>
#include <wtypes.h>
#pragma comment(lib, "Wlanapi.lib")

#endif

#endif

using namespace mrpt::hwdrivers;
using namespace std;

IMPLEMENTS_GENERIC_SENSOR(CWirelessPower, mrpt::hwdrivers)

CWirelessPower::CWirelessPower() { m_sensorLabel = "WIRELESS_POWER"; }
#ifdef _WIN32
#if defined(__GNUC__)
// MinGW: Nothing to do here (yet)
#else
/*---------------------------------------------------------------
          ConnectWlanServerW
   Get a connection to the WLAN server
 ---------------------------------------------------------------*/

/** Gets a connection to the server
 * \exception std::exception In case there is a failure (happens when WiFi is
 * not started)
 */

void* ConnectWlanServerW()
{
  DWORD dwMaxClient = 2;
  DWORD dwCurVersion = 0;
  DWORD dwResult = 0;  // Result of the API call
  HANDLE hClient;
  // open connection to server
  dwResult = WlanOpenHandle(dwMaxClient, nullptr, &dwCurVersion, &hClient);
  if (dwResult != ERROR_SUCCESS)
  {
    // if an error ocurred
    std::stringstream excmsg;
    excmsg << "WlanOpenHandle failed with error: " << dwResult << std::endl;

    // You can use FormatMessage here to find out why the function failed
    THROW_EXCEPTION(excmsg.str());
  }
  return (void*)hClient;
}

/*---------------------------------------------------------------
          ListInterfacesW
   Gets a list of the interfaces (Windows)
 ---------------------------------------------------------------*/

/** Gets a list of the interfaces available in the system (in Windows format)
 * \exception std::exception In case there is a failure
 * \return std::vector returns handles to the available interfaces
 */

std::vector<PWLAN_INTERFACE_INFO> ListInterfacesW(HANDLE hClient)
{
  // Get a list of the available interfaces

  std::vector<PWLAN_INTERFACE_INFO> outputVector;  // start the output vector
  PWLAN_INTERFACE_INFO_LIST pIfList = nullptr;     // list of WLAN interfaces
  PWLAN_INTERFACE_INFO pIfInfo = nullptr;          // information element for one interface
  DWORD dwResult = 0;

  int i;

  // Call the interface enumeration function of the API
  dwResult = WlanEnumInterfaces(hClient, nullptr, &pIfList);

  // check result
  if (dwResult != ERROR_SUCCESS)
  {
    // In case of error, raise an exception
    std::stringstream excmsg;
    excmsg << "WlanEnumInterfaces failed with error: " << dwResult << std::endl;

    THROW_EXCEPTION(excmsg.str());
    // You can use FormatMessage here to find out why the function failed
  }
  else
  {
    // iterate through interfaces to add them to the output vector
    for (i = 0; i < (int)pIfList->dwNumberOfItems; i++)
    {
      pIfInfo = (WLAN_INTERFACE_INFO*)&pIfList->InterfaceInfo[i];
      outputVector.push_back(pIfInfo);
    }
  }
  return outputVector;
}

/*---------------------------------------------------------------
          GUID2Str
   Gets the GUID of a network based on its handler in Windows
 ---------------------------------------------------------------*/
/** Transforms a GUID structure (in Windows format) to a string
 * \exception std::exception In case there is a failure
 * \return std::string returns a string containing the GUID
 */

std::string GUID2Str(const GUID& ifaceGuid)
{
  // Variables
  int iRet;
  errno_t wctostr;
  size_t sizeGUID;

  WCHAR GuidString[39] = {0};
  char GuidChar[100];

  std::string outputString;

  // Call the API function that gets the name of the GUID as a WCHAR[]
  iRet =
      StringFromGUID2(ifaceGuid, (LPOLESTR)&GuidString, sizeof(GuidString) / sizeof(*GuidString));
  // For c rather than C++ source code, the above line needs to be
  // iRet = StringFromGUID2(&pIfInfo->InterfaceGuid, (LPOLESTR) &GuidString,
  //     sizeof(GuidString)/sizeof(*GuidString));

  // translate from a WCHAR to string if no error happened
  if (iRet == 0)
  {
    THROW_EXCEPTION("StringFromGUID2 failed\n");
  }
  else
  {
    wctostr = wcstombs_s(&sizeGUID, GuidChar, 100, GuidString, 100);
    if ((wctostr == EINVAL) || (wctostr == ERANGE))
    {
      THROW_EXCEPTION("wcstombs_s failed\n");
    }
    else
    {
      outputString = std::string(GuidChar);
    }
  }

  return outputString;
}

/*---------------------------------------------------------------
          GetInterfaceW
   Gets a handler for the interface (Windows)
 ---------------------------------------------------------------*/

/** Gets a handle to the interface that has been set by setNet() (in Windows
 * format)
 * \exception std::exception In case there is a failure
 * \return PWLAN_INTERFACE_INFO returns a handle to the interface
 */

PWLAN_INTERFACE_INFO GetInterfaceW(std::string guid, HANDLE hClient)
{
  // Get interface given the GUID as a string (by the guid property of the
  // object)

  std::vector<PWLAN_INTERFACE_INFO> ifaceList;            // interface list
  std::vector<PWLAN_INTERFACE_INFO>::iterator ifaceIter;  // iterator
  PWLAN_INTERFACE_INFO output = nullptr;                  // interface info element

  // get a list of all the interfaces
  ifaceList = ListInterfacesW(hClient);

  // search for the interface that has the given GUID
  for (ifaceIter = ifaceList.begin(); ifaceIter != ifaceList.end(); ++ifaceIter)
  {
    if (GUID2Str((*ifaceIter)->InterfaceGuid) == guid)
    {
      output = *ifaceIter;
      break;
    }
  }

  return output;
}

/*---------------------------------------------------------------
          ListNetworksW
   Gets a list of the networks available for the interface (in Windows)
 ---------------------------------------------------------------*/

/** Gets a list of the networks available for an interface (in Windows format)
 * \exception std::exception In case there is a failure
 * \return std::vector returns handles to the available networks of a given
 * interface
 * \param iface handle to the WiFi interface
 */
std::vector<PWLAN_AVAILABLE_NETWORK> ListNetworksW(PWLAN_INTERFACE_INFO iface, HANDLE hClient)
{
  // Start variables

  DWORD dwResult = 0;
  PWLAN_AVAILABLE_NETWORK_LIST pBssList = nullptr;  // list of available networks
  PWLAN_AVAILABLE_NETWORK pBssEntry = nullptr;      // information element for one interface

  GUID ifaceGuid = iface->InterfaceGuid;  // Get GUID of the interface

  std::vector<PWLAN_AVAILABLE_NETWORK> outputVector;  // output vector

  //	WCHAR GuidString[39] = {0};

  // Force a scan (to obtain new data)
  WLAN_RAW_DATA IeData;
  WlanScan((HANDLE)hClient, &ifaceGuid, nullptr, &IeData, nullptr);

  // Call the Windows API and get a list of the networks available through the
  // interface
  dwResult = WlanGetAvailableNetworkList((HANDLE)hClient, &ifaceGuid, 0, nullptr, &pBssList);

  // Check the result of the call
  if (dwResult != ERROR_SUCCESS)
  {
    // In case an error ocurred
    std::stringstream excmsg;
    excmsg << "WlanGetAvailableNetworkList failed with error: " << dwResult << std::endl;
    //	THROW_EXCEPTION(excmsg.str();;
  }
  else
  {
    // for each network, get its info and save it
    for (unsigned int j = 0; j < pBssList->dwNumberOfItems; j++)
    {
      pBssEntry = (WLAN_AVAILABLE_NETWORK*)&pBssList->Network[j];  // get entry for network
      outputVector.push_back(pBssEntry);                           // save entry
    }
  }

  return outputVector;
}

/*---------------------------------------------------------------
          GetNetworkW
   Gets a handler to a wireless network in Windows
 ---------------------------------------------------------------*/
/** Gets a handle to the network that has been set by setNet() (in Windows
 * format)
 * \exception std::exception In case there is a failure
 * \return PWLAN_AVAILABLE_NETWORK returns a handle to the network
 */
PWLAN_AVAILABLE_NETWORK GetNetworkW(
    HANDLE hClient, const std::string& ssid, const std::string& guid)
{
  // Variables
  PWLAN_INTERFACE_INFO iface;      // interface handler
  PWLAN_AVAILABLE_NETWORK output;  // output network handler

  // Get a handler to the interface
  iface = GetInterfaceW(guid, hClient);

  // Get the list of networks
  std::vector<PWLAN_AVAILABLE_NETWORK> pBssList = ListNetworksW(iface, hClient);

  // Iterate through the list and find the network that has the matching SSID
  std::vector<PWLAN_AVAILABLE_NETWORK>::iterator netIter;
  for (netIter = pBssList.begin(); netIter != pBssList.end(); ++netIter)
  {
    if (std::string((char*)((*netIter)->dot11Ssid.ucSSID)) == ssid)
    {
      output = *netIter;
      break;
    }
  }

  return output;
}

#endif

#endif  // end of Windows auxiliary functions definition

/*---------------------------------------------------------------
          ListInterfaces
   Gets a list of the interfaces
 ---------------------------------------------------------------*/

std::vector<std::string> CWirelessPower::ListInterfaces()
{
  std::vector<std::string> output;  // output vector of strings

#ifdef MRPT_OS_LINUX
  // in linux, the command line is used to get all the relevant information
  FILE* cmdoutput;                // file handler for the executed command line
  char ifaceread[256], *netname;  // strings used to read the output of the
  // command line and get the name of each
  // network

  // Run the command line: get the info frim /proc/net/wireless and cut out
  // the names of the interfaces

  // commandl << "cat /proc/net/wireless|grep \"wlan\"|cut -d\" \" -f2|cut
  // -d\":\" -f1";
  cmdoutput = popen("cat /proc/net/wireless|grep \"wlan\"|cut -d\" \" -f2|cut -d\":\" -f1", "r");
  if (!fgets(ifaceread, 3, cmdoutput))  // read output
    THROW_EXCEPTION("Error reading /proc/net/wireless");

  // iterate thrugh list and get each interface as a string
  netname = ::strtok(ifaceread, "\n");
  while (netname)
  {
    output.emplace_back(netname);
    netname = ::strtok(nullptr, "\n");
  }
#endif

#ifdef _WIN32
#if defined(__GNUC__)
  THROW_EXCEPTION("Sorry, method not available for MinGW");
#else
  // In windows, this function is a wrapper to ListInterfacesW

  std::vector<PWLAN_INTERFACE_INFO>
      ifaces;  // vector containing the interface entries (Windows format)
  std::vector<PWLAN_INTERFACE_INFO>::iterator
      ifacesIter;  // iterator to run through the previous list

  // get the list
  ifaces = ListInterfacesW(hClient);

  // iterate thrugh list and get each GUID as a string
  for (ifacesIter = ifaces.begin(); ifacesIter != ifaces.end(); ++ifacesIter)
  {
    output.push_back(GUID2Str((*ifacesIter)->InterfaceGuid));
  }
#endif
#endif

  return output;
}

/*---------------------------------------------------------------
          ListNetworks
   Gets a list of the networks available for the interface
 ---------------------------------------------------------------*/

std::vector<std::string> CWirelessPower::ListNetworks()
{
  std::vector<std::string> output;  // output vector of strings

#ifdef MRPT_OS_LINUX

  std::stringstream commandl;  // command to be executed

  FILE* cmdoutput;
  char listread[1024];
  char* netname;

  // Run command: get a list of networks and cut out their names. Note: this
  // must be done as a superuser, so the command is executed with sudo.
  // Usually it should ask for the password only the first time.
  // To avoid the inconvenience of having to write the password, it would be
  // useful to configure sudo to allow the user to run this command. See "man
  // sudoers"
  commandl << "sudo iwlist "
           << "wlan0"
           << " scan|grep ESSID|cut -d\"\\\"\" -f2";
  cmdoutput = popen(commandl.str().c_str(), "r");
  if (!fgets(listread, 3, cmdoutput)) THROW_EXCEPTION("Error reading response from iwlist");

  netname = ::strtok(listread, "\n");
  while (netname)
  {
    output.emplace_back(netname);
    netname = ::strtok(nullptr, "\n");
  }

#endif

#ifdef _WIN32
#if defined(__GNUC__)
  THROW_EXCEPTION("Sorry, method not available for MinGW");
#else

  PWLAN_INTERFACE_INFO iface;  // Information element for an interface

  iface = GetInterfaceW(m_guid, (HANDLE)hClient);  // Get the interface handler

  // Get the list of networks
  std::vector<PWLAN_AVAILABLE_NETWORK> pBssList = ListNetworksW(iface, (HANDLE)hClient);

  // Iterate through the list and save the names as strings
  std::vector<PWLAN_AVAILABLE_NETWORK>::iterator netIter;
  for (netIter = pBssList.begin(); netIter != pBssList.end(); ++netIter)
  {
    output.push_back(std::string((char*)((*netIter)->dot11Ssid.ucSSID)));
  }
#endif

#endif

  return output;
}

/*---------------------------------------------------------------
          GetPower
   Gets the power of the network
 ---------------------------------------------------------------*/
int CWirelessPower::GetPower()
{
#ifdef MRPT_OS_LINUX
  FILE* cmdoutput;
  char* powerReadL;
  std::stringstream commandl;
  // Run command: get the power of the networks and additional info
  commandl << "sudo iwlist "
           << "wlan0"
           << " scan";
  cmdoutput = popen(commandl.str().c_str(), "r");

  std::vector<std::string> powerReadV;
  size_t readBytes;

  powerReadL = (char*)malloc(256);
  std::stringstream ssidLine;

  ssidLine << "ESSID:\"" << m_ssid << "\"";
  if (getline(&powerReadL, &readBytes, cmdoutput) < 0)
    THROW_EXCEPTION("Error reading response from iwlist");

  while (!strstr(powerReadL, ssidLine.str().c_str()))
  {
    powerReadV.emplace_back(powerReadL);
    if (getline(&powerReadL, &readBytes, cmdoutput))
      THROW_EXCEPTION("Error reading response from iwlist");
  }

  auto ssiter = powerReadV.end() - 2;

  char powerLine[256];

  // now we have a string per output line of iwlist. We must find the line
  // containing the desired ESSID and read the power from two lines before

  strcpy(powerLine, (*ssiter).c_str());

  char level[10];
  // meaning that the ESSID was found
  // Example pf the value of poerLine: Quality=57/100  Signal level=57/100
  char* fraction;

  ::strtok(powerLine, "=");
  ::strtok(nullptr, "=");
  fraction = ::strtok(nullptr, "=");
  strcpy(level, ::strtok(fraction, "/"));

  free(powerReadL);

  return atoi(level);

#elif defined(_WIN32)
#if defined(__GNUC__)
  THROW_EXCEPTION("Sorry, method not available for MinGW");
#else
  PWLAN_AVAILABLE_NETWORK wlan;  // handler to the network

  // Get a handler to the network
  wlan = GetNetworkW((HANDLE)hClient, m_ssid, m_guid);

  return wlan->wlanSignalQuality;
#endif
#else
  THROW_EXCEPTION("Method not implemented for this platform/OS!");
#endif
}

/*---------------------------------------------------------------
          getObservation
  Get the power of a given network as an observation
  NOTE: Deprecated, use getObservations. Use this class as
      GenericSensor. See the CGenericSensor documentation
 ---------------------------------------------------------------*/
bool CWirelessPower::getObservation(mrpt::obs::CObservationWirelessPower& outObservation)
{
  try
  {
    //	outObservation.m_readings.clear();
    outObservation.power = (float)GetPower();

    outObservation.timestamp = mrpt::Clock::now();

    outObservation.sensorLabel = m_sensorLabel;
    //	std::cout << "mrpt::hwdrivers::CWirelessPower::getObservation() " <<
    //"\n\tsensorLabel: " << outObservation.sensorLabel << "\n\ttimestamp: "
    //<< outObservation.timestamp << "\n\tpower: " << outObservation.power
    //<< std::endl;
    return true;
  }
  catch (exception& e)
  {
    cerr << "[CWirelessPower::getObservation] Returning false due to "
            "exception: "
         << endl;
    cerr << e.what() << endl;
    return false;
  }
}

void CWirelessPower::doProcess()
{
  // Wrapper to getObservation
  auto outObservation = mrpt::obs::CObservationWirelessPower::Create();
  getObservation(*outObservation);

  appendObservation(mrpt::obs::CObservationWirelessPower::Create(*outObservation));
}

void CWirelessPower::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
{
  MRPT_START
  pose_x = configSource.read_float(iniSection, "pose_x", 0, true);
  pose_y = configSource.read_float(iniSection, "pose_y", 0, true);
  pose_z = configSource.read_float(iniSection, "pose_z", 0, true);
  pose_roll = configSource.read_float(iniSection, "pose_roll", 0, true);
  pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, true);
  pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, true);

  m_ssid = configSource.read_string(iniSection, "ssid", "", true);
  m_guid = configSource.read_string(iniSection, "guid", "", true);
  // in the case of Linux, the "GUID" is  the interface name (wlanX)

#ifdef _WIN32
#if defined(__GNUC__)
  THROW_EXCEPTION("Sorry, method not available for MinGW");
#else
  hClient = ConnectWlanServerW();
#endif
#endif

  MRPT_END
}

void CWirelessPower::setNet(std::string ssid, std::string guid)
{
  m_ssid = ssid;
  m_guid = guid;
}
