#include <Keypad.h>
#include <Servo.h>

// Set up keypad variables:
int numKeyPresses = 0;            // Track number of key presses
int maxKeyPresses = 3;            // Only allow 3 digits to be entered
int keyPresses[3] = { 0, 0, 0 };  // Initialize an empty array to hold input
const byte numRows= 4;            // # of rows on the keypad
const byte numCols= 4;            // # of columns on the keypad

// Set up servo variables:
int angle = 0;                    // Angle in degrees to position servo [0-180]
int angleMultiplier = 1;          // Multiply by each digit, divide by 10 on each input
Servo servo;                      // Create the servo object
int servoPin = 3;                 // Set the servo pin

char keymap[numRows][numCols]=    // Setup the keypad layout
{
  {'1', '2', '3', 'A'}, 
  {'4', '5', '6', 'B'}, 
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'},
};

// Connect keypad ROW0, ROW1, ROW2 and ROW3 to these Arduino pins.
byte rowPins[numRows] = { 11, 10, 9, 8 }; 

// Connect keypad COL0, COL1 and COL2 to these Arduino pins.
byte colPins[numCols] = { 7, 6, 5, 4 };

// Create the Keypad
Keypad kpd = Keypad( makeKeymap( keymap ), rowPins, colPins, numRows, numCols );

void setup()
{
  Serial.begin(9600);           // Start up serial comms
  resetAngleMultiplier();       // Start accepting numeric input
  servo.attach( servoPin );     // Attaches the servo to the servo object
  
} // setup

void loop()
{
  char key = kpd.getKey();
  if( key )                     // Check for a valid key
  {
    if( key >= 0x41 && key <= 0x44 || key == 0x23 || key == 0x2A )
    {
      resetInput();
      Serial.println( "ERROR: Numeric input only!" );
    }                           // ^ if invalid entry
    else                        // Else, entry is valid:
    {
      angle += angleMultiplier * ( key - 0x30 );
      angleMultiplier /= 10;
      
      if( numKeyPresses == maxKeyPresses - 1 )
      {
        setServo( angle );            // Use the input to turn servo
        resetInput();
      }
      else
      {
        numKeyPresses++;
      }
      Serial.println( (String) angle );
    }
        
  } // if( key )

} // loop

void setServo( int angle )
{
  if( angle > 180 )
    angle = 180;
  Serial.println( "Setting servo to " + (String) angle + " degrees." );
  servo.write( angle );          // Set the servo position
  
} // setServo


void resetAngleMultiplier()
{
  angleMultiplier = 1;
  /* We started out with a multiplier of 10^0 (or 1). For each 
  number we want to accept, we want to have a multiplier one 
  order of magnitude greater. So, for example, for 5 digits, the 
  multiplier starts out as 10 000. */
  for( int i = 0; i < maxKeyPresses - 1; i++ )
    angleMultiplier *= 10;
    
} // resetAngleMultiplier


void resetInput()
{
  resetAngleMultiplier();       // Reset the numeric input
  angle = 0;                    // Reset the angle
  numKeyPresses = 0;            // Reset number of key presses
}