#include <stdio.h>
#include <unistd.h>
#include <typeinfo>

#include "libPOD.h"

void TestPOD(int DeviceNum)
{
   POD *MyPOD;

   MyPOD = new POD(DeviceNum);
   MyPOD->DisplayPODInfo();
}

void TestPowerSensorPOD(PowerSensorPOD *MyPSPOD)
{
   int IntVar;

   if (MyPSPOD != NULL)
   {

//   MyPSPOD = new PowerSensorPOD(DeviceNum);
      MyPSPOD->DisplayPODInfo();
      IntVar = MyPSPOD->GetPowerStatus();
      printf("My status is %d\n", IntVar);
   }
   else printf("No PowerSensorPOD connected\n");
}

void TestLightSensorPOD(LightSensorPOD *MyLSPOD)
{
   int IntVar;
   
   if (MyLSPOD != NULL)
   {
      MyLSPOD->DisplayPODInfo();
      printf("My Reading is %d\n", MyLSPOD->GetIntensity());
      IntVar = MyLSPOD->GetThreshold();
      printf("My Threshold is %d\n", IntVar);
      MyLSPOD->SetThreshold(500);
      IntVar = MyLSPOD->GetThreshold();
//      printf("My Threshold is %d\n", IntVar);
//      IntVar = MyLSPOD->IsLightOn();
//      printf("Light is %d\n", IntVar);
      printf("With a threshold of %d, the light is %d\n", IntVar, MyLSPOD->IsLightOn());
   }
   else printf("No LightSensorPOD connected\n");
}

void TestThermometerPOD(ThermometerPOD *MyTPOD)
{
   float Temperature;
   char Units;

   if (MyTPOD != NULL)
   {
//      MyTPOD->SetName("Office");
      MyTPOD->DisplayPODInfo();
      MyTPOD->GetTemperature(Temperature);
      Units = MyTPOD->GetUnits();
      printf("Temperature is %.2f %c\n", Temperature, Units);
   }
   else printf("No ThermometerPOD connected\n");
}

void TestCompassPOD(CompassPOD *MyCompass)
{
   float FloatVar;

   if (MyCompass != NULL)
   {
      MyCompass->DisplayPODInfo();
      FloatVar = MyCompass->GetHeading();
      printf("My heading is %.1f\n", FloatVar);
   }
   else printf("No CompassPOD connected\n");
}

void TestSonarPOD(SonarPOD *MySonar)
{
   int IntVar;

   if (MySonar != NULL)
   {
      MySonar->DisplayPODInfo();
      IntVar = MySonar->GetDistanceInches();
      printf("I've got a distance of %d inches\n", IntVar);
   }
   else printf("No SonarPOD connected\n");
}

void TestLightPOD(LightPOD *MyLightPOD)
{
   int IntVar;

   if (MyLightPOD != NULL)
   {
      MyLightPOD->DisplayPODInfo();
      IntVar = 0;
      while (IntVar < 16)
      {
         printf("Enter a number 0-15 (higher to quit): ");
         scanf("%d", &IntVar);
         MyLightPOD->TurnOn(IntVar & 0x08, IntVar & 0x04,
         IntVar & 0x02, IntVar & 0x01);
      }
   }
   else printf("No LightPOD connected\n");
}

void TestAlarmPOD(AlarmPOD *MyAlarmPOD)
{
   if (MyAlarmPOD != NULL)
   {
      MyAlarmPOD->DisplayPODInfo();
   }
   else printf("No AlarmPOD connected\n");
}

void TestMotorControlPOD(MotorControlPOD *MyMotor)
{
   if (MyMotor != NULL)
   {
      MyMotor->DisplayPODInfo();
      MyMotor->Clockwise_Slow(); /* ahead */
      sleep(2);
      MyMotor->SetSpeed(0); /* stop */
      sleep(2);
      MyMotor->CounterClockwise_Slow(); /* backwards */
      sleep(2);
      MyMotor->Stop();
   }
   else printf("No MotorControlPOD connected\n");
}

void TestPODEnv()
{
   POD *AllPODs[16];
   PODEnv *MyPODs;
   int Counter;

   MyPODs = new PODEnv(1);
   MyPODs->GetAllPODs(AllPODs);

   printf("All connected PODS:\n");
   for (Counter = 0 ; Counter < 16 ; Counter ++)
   {
      if (AllPODs[Counter]->IsValid())
      {
         AllPODs[Counter]->DisplayPODInfo();
//         AllPODs[Counter]->Test();
         printf("\n");
      }
   }

   printf("\nTesting each POD (with FindPODByType):\n\n");
   TestLightSensorPOD((LightSensorPOD *)(MyPODs->FindPODByType(POD_TYPE_LIGHTSENSOR)));
   TestThermometerPOD((ThermometerPOD *)(MyPODs->FindPODByType(POD_TYPE_THERMOMETER)));
   TestSonarPOD((SonarPOD *)(MyPODs->FindPODByType(POD_TYPE_DISTANCE)));
   TestMotorControlPOD((MotorControlPOD *)(MyPODs->FindPODByType(POD_TYPE_MOTORCONTROL)));
   TestPowerSensorPOD((PowerSensorPOD *)(MyPODs->FindPODByType(POD_TYPE_POWERSENSOR)));
   TestCompassPOD((CompassPOD *)(MyPODs->FindPODByType(POD_TYPE_COMPASS)));
   TestLightPOD((LightPOD *)(MyPODs->FindPODByType(POD_TYPE_LIGHT)));
   TestAlarmPOD((AlarmPOD *)(MyPODs->FindPODByType(POD_TYPE_ALARM)));
//   printf("Type is %s\n", typeid(*MyTPOD).name());

   printf("\nTesting FindPODByName on Thermometer \"Office\"\n\n");
   TestThermometerPOD(((ThermometerPOD *)(MyPODs->FindPODByName(POD_TYPE_THERMOMETER, "Office"))));
}

int main(int argc, char *argv[])
{
   TestPODEnv();
   return(0);
}