#include "JoystickDriver.c"

int handleAxis(int val, int dbMin, int dbMax)
{

  // 1. Remove the Deadband and shift everything back to zero
   if (val <= dbMin)
      val = (val - dbMin);
   else if (val >= dbMax)
      val = (val - dbMax);
   else
      val = 0;

   // 2. Now realign the axis so that the output is 0-100 between dbMax and +127
   //    and -100 to 0 between -128 and dbMin.

   if (val > 0)
   {
    val *= 100;
    val /=  (127 - dbMax);          // dbMax should be >= 0
   }
   else
   {
    val *= 100;
    val /= (128 + dbMin);             // dbMin should be <= 0
   }


   return val;
}

void handleJoystick()
{
   joystick.joy1_x1 = handleAxis(joystick.joy1_x1, -10, 10);
   joystick.joy1_y1 = handleAxis(joystick.joy1_y1, -10, 10);
   joystick.joy1_x2 = handleAxis(joystick.joy1_x2, -10, 10);
   joystick.joy1_y2 = handleAxis(joystick.joy1_y2, -10, 10);

   joystick.joy2_x1 = handleAxis(joystick.joy2_x1, -10, 10);
   joystick.joy2_y1 = handleAxis(joystick.joy2_y1, -10, 10);
   joystick.joy2_x2 = handleAxis(joystick.joy2_x2, -10, 10);
   joystick.joy2_y2 = handleAxis(joystick.joy2_y2, -10, 10);
}

task main()
{
  getJoystickSettings(joystick);
  handleJoystick();
}