//-----------------------------------------------------------------------------
//  ThirdPartyDLL.cpp
//
//  DLL Example for AeroSIM RC
//
//  Compile this project with VC++ and copy ThirdPartyDLL.dll to the same folder as AeroSIM-RC.exe folder
//
//  This example is a simple autopilot that senses the roll angle and move the ailerons to keep roll at 0º
//  Place your own code in the function AeroSIMRC_ThirdParty_Run()
// 
//  Please contact us at info@aerosimrc.com if you need support
//-----------------------------------------------------------------------------
#include "ThirdPartyDLL.h"
#define WIN32_LEAN_AND_MEAN		// Exclude rarely-used stuff from Windows headers
#include <windows.h>
#include <stdio.h>

//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
BOOL APIENTRY DllMain( HANDLE hModule, 
                       DWORD  ul_reason_for_call, 
                       LPVOID lpReserved)
{
  switch (ul_reason_for_call)
  {
 	  case DLL_PROCESS_ATTACH:
		  break;
	  case DLL_THREAD_ATTACH:
		  break;
	  case DLL_THREAD_DETACH:
		  break;
	  case DLL_PROCESS_DETACH:
		  break;
  }
  return TRUE;
}


//-----------------------------------------------------------------------------
// This function is called at each program cycle from AeroSIM RC
//-----------------------------------------------------------------------------
AeroSIMRC_DLL_EXPORT void AeroSIMRC_ThirdParty_Run(const TDataFromAeroSimRC  *ptDataFromAeroSimRC,
                                                         TDataToAeroSimRC    *ptDataToAeroSimRC)
{
  //-----------------------------------
  // Check that the structs are the expected ones
  //-----------------------------------
  if(ptDataFromAeroSimRC->nStructSize != sizeof(TDataFromAeroSimRC))
  {
    ptDataToAeroSimRC->pucOnScreenDebugInfoText = "ERROR: struct TDataFromAeroSimRC is not the right size";  // received a different struct than expected
    return;
  }
    
  if(ptDataToAeroSimRC->nStructSize != sizeof(TDataToAeroSimRC))
  {
    ptDataToAeroSimRC->pucOnScreenDebugInfoText = "ERROR: struct TDataToAeroSimRC is not the right size";  // received a different struct than expected
    return;
  }
    


  //-----------------------------------
  // SIMPLE AUTO PILOT EXAMPLE THAT SENSES THE ROLL ANGLE 
  //   AND MOVE THE AILERONS TO KEEP THE ROLL AT 0º
  //   WHILE LETTING THE PILOT CONTROL THE AILERON AS WELL
  //-----------------------------------
  char strImplementation[1000];

  ptDataToAeroSimRC->bForceShowInfoText = true;  // force show the info text. See ThirdPartyDLL.h for more detail

  {
    // Our Autopilot will move the ailerons to cancel the roll angle
    float fAutopilotContribution = ptDataFromAeroSimRC->fRoll * 2.0f;

    // Limit the action of autopilot
    if(fAutopilotContribution >  0.5f) fAutopilotContribution =  0.5f;
    if(fAutopilotContribution < -0.5f) fAutopilotContribution = -0.5f;

    // New stick position is the actual pilot position plus our autopilot increment
    float fNewAileronPosition = ptDataFromAeroSimRC->afChannelValue_RX[CH_AILERON] + fAutopilotContribution;

    // Limit the channel travel
    if(fNewAileronPosition >  1.0f) fNewAileronPosition =  1.0f;
    if(fNewAileronPosition < -1.0f) fNewAileronPosition = -1.0f;

    // Set new channel position to output struct
    ptDataToAeroSimRC->afNewChannelValue_RX[CH_AILERON] = fNewAileronPosition;

    // Override this channel, so the model takes our input
    ptDataToAeroSimRC->abOverrideChannel_RX[CH_AILERON] = true;

    sprintf(strImplementation,
      "fAutopilotContribution = %f\n"
      "Stick Position = %f\n"
      "Input Aileron %f\n"
      ,
      fAutopilotContribution,
      ptDataFromAeroSimRC->afChannelValue_RX[CH_AILERON],
      fNewAileronPosition
    );
  }

  //-----------------------------------
  // The string returned in pucOnScreenDebugInfoText will be displayed on AeroSIMRC
  // In this example, the all data received from AeroSIMRC is displayed on the screen
  //-----------------------------------
  static char str[10000];
  sprintf(str,
    "nStructSize = %d\n"
    "\n" // Simulation Data
    "fIntegrationTimeStep = %f\n"
    "\n" // Model data
    "fPosX = %f\n"
    "fPosY = %f\n"
    "fPosZ = %f\n"
    "fVelX = %f\n"
    "fVelY = %f\n"
    "fVelZ = %f\n"
    "\n"
    "fAngVelX = %f\n"
    "fAngVelY = %f\n"
    "fAngVelZ = %f\n"
    "\n"
    "fHeightAboveTerrain = %f\n"
    "\n"
    "fHeading = %f\n"
    "fPitch   = %f\n"
    "fRoll    = %f\n"
    "\n"
    "TX Aileron  = %f\n"
    "TX Elevator = %f\n"
    "TX Throttle = %f\n"
    "TX Rudder   = %f\n"
    "\n"
    "RX Aileron  = %f\n"
    "RX Elevator = %f\n"
    "RX Throttle = %f\n"
    "RX Rudder   = %f\n"
    "\n"
    "DEVELOPER DATA\n"
    "%s"
    ,
    
    ptDataFromAeroSimRC->nStructSize,
    
    // Simulation Data
    ptDataFromAeroSimRC->fIntegrationTimeStep,

    // Model data
    ptDataFromAeroSimRC->fPosX,     ptDataFromAeroSimRC->fPosY,     ptDataFromAeroSimRC->fPosZ,
    ptDataFromAeroSimRC->fVelX,     ptDataFromAeroSimRC->fVelY,     ptDataFromAeroSimRC->fVelZ,
    ptDataFromAeroSimRC->fAngVelX,  ptDataFromAeroSimRC->fAngVelY,  ptDataFromAeroSimRC->fAngVelZ,

    ptDataFromAeroSimRC->fHeightAboveTerrain,

    ptDataFromAeroSimRC->fHeading,
    ptDataFromAeroSimRC->fPitch,
    ptDataFromAeroSimRC->fRoll,

    ptDataFromAeroSimRC->afChannelValue_TX[CH_AILERON],
    ptDataFromAeroSimRC->afChannelValue_TX[CH_ELEVATOR],
    ptDataFromAeroSimRC->afChannelValue_TX[CH_THROTTLE],
    ptDataFromAeroSimRC->afChannelValue_TX[CH_RUDDER],

    ptDataFromAeroSimRC->afChannelValue_RX[CH_AILERON],
    ptDataFromAeroSimRC->afChannelValue_RX[CH_ELEVATOR],
    ptDataFromAeroSimRC->afChannelValue_RX[CH_THROTTLE],
    ptDataFromAeroSimRC->afChannelValue_RX[CH_RUDDER],
    
    strImplementation
  );


  ptDataToAeroSimRC->pucOnScreenDebugInfoText = str;
}