/*
 * program to do some simple calibrating of the compass.
 * this works with a motor driven turntable to rotate the
 * prototype compass.
 */

#define COMPASS_SENSOR  SENSOR_1

#define MOTOR           OUT_A

// variables for data measurement

int curve1, curve2;
int old1, old2;
int diff;

// the values we want to determine

int stable_delay, on_off_delay;
int upper_cross, lower_cross;

// this routine reads the compass values

sub read_raw()
{
  old1 = curve1;
  old2 = curve2;

  SetSensorMode(COMPASS_SENSOR, SENSOR_MODE_RAW);
  Wait(stable_delay);
  curve1 = COMPASS_SENSOR;

  SetSensorType(COMPASS_SENSOR, SENSOR_TYPE_TOUCH);
  Wait(on_off_delay);
  SetSensorType(COMPASS_SENSOR, SENSOR_TYPE_LIGHT);

  SetSensorMode(COMPASS_SENSOR, SENSOR_MODE_RAW);
  Wait(stable_delay);
  curve2 = COMPASS_SENSOR;

  SetSensorType(COMPASS_SENSOR, SENSOR_TYPE_TOUCH);
  Wait(on_off_delay);
  SetSensorType(COMPASS_SENSOR, SENSOR_TYPE_LIGHT);

  diff = curve1-curve2;
}

task main()
{
  int i;

  // default setup for compass sensor

  SetSensorType(COMPASS_SENSOR,  SENSOR_TYPE_LIGHT);

  // Some Conservative defaults

  stable_delay = 20;
  on_off_delay = 15;
  
  CreateDatalog(20);

  read_raw();

  // Determine how little delay we need to make sure the flip flop
  // toggles.

  SetUserDisplay(on_off_delay,0);

  for ( ; on_off_delay > 0; on_off_delay--) {
    read_raw();

    // keep reading until the readings go bad.

    if (abs(old1 - curve1) > 5 ||
        abs(old2 - curve2) > 5) {
      break;
    }
    PlaySound(SOUND_CLICK);
  }

  PlaySound(SOUND_UP);

  // Determine how little delay we need to make sure the power
  // is stable after tloggling flip flop

  SetUserDisplay(stable_delay,0);

  read_raw();

  for ( ; stable_delay > 0; stable_delay--) {
    read_raw();
    if (abs(old1 - curve1) > 5 ||
        abs(old2 - curve2) > 5) {
      break;
    }
    PlaySound(SOUND_CLICK);
  }

  AddToDatalog(on_off_delay);
  AddToDatalog(stable_delay);

  /* now determine the upper and lower crossover */

  SetUserDisplay(diff,0);

  OnFwd(MOTOR);

  // keep rotating the compass until the curves have same value

  while ( diff ) {
    read_raw();
  }

  // record the reading at the crossing

  Off(MOTOR);
  upper_cross = curve1;

  // keep searching for the other crossing

  On(MOTOR);

  Wait(1000);

  while (curve1 > curve2) {
    read_raw();
  }

  Off(MOTOR);

  // record the second crossing

  lower_cross = curve1;

  // make sure which crossing is upper and lower

  if (lower_cross > upper_cross) {
    diff = upper_cross;
    upper_cross = lower_cross;
    lower_cross = diff;
  }

  AddToDatalog(upper_cross);
  AddToDatalog(lower_cross);

  PlaySound(SOUND_LOW_BEEP);

  // finished

  // Things to add:
  //  Use rotation sensor to check compass sensor readings.
  //  Record periodic readings into data log and plot graph.
}