#define NOFIELD 505L       // Calibration constant
#define TOMILLIGAUSS 1953L // Unit conversion factor

#define P_led 13
#define R_led 12
#define N_led 11
#define D_led 10
#define S_led 9
#define U_led 8

#define numLED 5

struct interval
{
    int lower;
    int upper;
};

struct state
{
    char name;
    int led;
    interval gauss1;
    interval gauss2;
} ;

state shift_condition[] = {
  { 'P', P_led, {  74,  95 }, { -86, -40 } },
  { 'R', R_led, {  61,  92 }, { -20,  10 } },
  { 'N', N_led, { -30,   6 }, {  36,  90 } },
  { 'D', D_led, { -80, -53 }, {   2,  50 } },
  { 'S', S_led, { -25,  15 }, { -80, -25 } },
};

state prev_state;
long prev_gauss1;
long prev_gauss2;
bool gear_defined;

bool inRange( long value, struct interval range )
{
  return range.lower <= value && value <= range.upper;
}

void selectGear(int raw1, int raw2)
{

    long gauss1 = ( raw1 - NOFIELD ) * TOMILLIGAUSS / 1000;
    long gauss2 = ( raw2 - NOFIELD ) * TOMILLIGAUSS / 1000;

    // if no signal, no need to switch gear, bail out
    if ( prev_gauss1 == gauss1 && prev_gauss2 == gauss2 ) {
      return;
    }

    // update values for next evaluation

    prev_gauss1 = gauss1;
    prev_gauss2 = gauss2;

    // if new readings in existing state range,
    // no need to switch gear, bail out
    if ( gear_defined && inRange(gauss1, prev_state.gauss1) && inRange(gauss2,prev_state.gauss2) ) {
      return;
    }

    for( int i=0; i<numLED; i++) { // determine new state

        state curr_state = shift_condition[i];

        if ( inRange(gauss1, curr_state.gauss1) && inRange(gauss2, curr_state.gauss2) ) {

            Serial.print("Switching from ");
            Serial.print(prev_state.name);
            Serial.print(" to ");
            Serial.println(curr_state.name);

            digitalWrite(prev_state.led, LOW);
            digitalWrite(curr_state.led, HIGH);

            prev_state = curr_state;
            gear_defined = true;
            return;
        }
    }

    // no states matched, default to "undefined" state

    gear_defined = false;
    digitalWrite(prev_state.led, LOW);
    digitalWrite(U_led, HIGH);
}

void setup()
{
    Serial.begin(9600);
    pinMode(P_led, OUTPUT);
    pinMode(R_led, OUTPUT);
    pinMode(N_led, OUTPUT);
    pinMode(D_led, OUTPUT);
    pinMode(S_led, OUTPUT);
    pinMode(U_led, OUTPUT);
}

void loop()
{
    selectGear(analogRead(1),analogRead(2));
}

struct state
{
    char name;
    int led;
    struct interval gauss1;
    struct interval gauss2;
};

typedef struct interval_s
{
    int lower;
    int upper;
} interval;

state shift_condition[] = {

state shift_condition[numLED] = {