/* rb4_08a.nqc */

/*

   Copyright (c) 1999 Robert Munafo.

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

 */

/* Modification of rb_07.
   This is a bug-fixed version of rb4_07.
   - It keeps the previous trained values of the bumper reactions,
     so you can retrain just one and keep what you had for the other.
   - It no longer occasionally jerks forwards, back or rotate for no
     apparent reason. (This was a bug in NQC I think!)
   */

// SCALE_NUMER and SCALE_DENOM determine how much the measured speeds
// are scaled up (or down) to convert to output speeds. I found that
// the motors move slower in GO mode than in TRAIN mode unless I scale
// the average readings up by 2 or 3.
#define SCALE_NUMER 3
#define SCALE_DENOM 2

#define MASTER_DELAY 5
#define TRAIN_DELAY 1
#define MC_DELAY 1
#define SD_DELAY 10

// light input
int light;  // 0

// motor inputs and motor speed control (four variables are shared
// for these)
int speed_a; // 1
int error_a; // 2
int speed_c; // 3
int error_c; // 4

//#define raw_1 error_a
//#define raw_3 error_c
int raw_1; // 5
int raw_3; // 6

// sum and difference of motor speeds. Used in both modes (input
// and output)
// decay values are useful for the min/max light readings.
int rot_ac;  //  7
int decay_r; //  8
int fwd_ac;  //  9
int decay_f; // 10

// accumulated rot and fwd for standard (non-stimulus) motion
int m0_f; // 11
int m0_r; // 12
int ft;   // 13
#define n_0 ft

// accumulated rot and fwd for bumper stimuli
int m1_f; // 14
int m1_r; // 15
int rt;   // 16
#define n_1 rt

int m3_f; // 17
int m3_r; // 18
int td;   // 19
#define n_3 td

// feedback what type of data was taken
int dv;  // 20

// bumper inputs
int bump_1; // 21
int bump_3; // 22

// temp for intermediate result when computing a decay
int t;      // 23
#define decay(v,n1,n2) t=v; t=(n1*t)/n2; v=t;

// mode control temp variables -- used only while in idle mode
#define i_i speed_a
#define i_mode speed_c

#define minmax(t, v) if ((v) > 100) {t=100;} else if ((v) < -100) {t=-100;} else {t=v;}

/* sd_1 computes raw motor speeds from the speed and rotation
   variables */
task sd_1
{
  while(true) {
    minmax(speed_a, fwd_ac + rot_ac)
    minmax(speed_c, fwd_ac - rot_ac)

    if (rot_ac >= 0) {
      PlayNote(1000, SD_DELAY-1);
    } else {
      PlayNote(2500, SD_DELAY-1);
    }
    Sleep(SD_DELAY);
  }
}

/* mc_1 does the floyd-steinberg motor control */
task mc_1
{
  while(true) {
    error_a = error_a + speed_a;
    if (error_a > 50) {
      Fwd(OUT_A, 7);
      error_a = error_a - 100;
    } else if (error_a < -50) {
      Rev(OUT_A, 7);
      error_a = error_a + 100;
    } else {
      Off(OUT_A);
    }

    error_c = error_c + speed_c;
    if (error_c > 50) {
      Fwd(OUT_C, 7);
      error_c = error_c - 100;
    } else if (error_c < -50) {
      Rev(OUT_C, 7);
      error_c = error_c + 100;
    } else {
      Off(OUT_C);
    }
    
    Sleep(MC_DELAY);
  }
}

// is_1 contains the input code that is always enabled.
task is_1
{
  while (true) {
    light = IN_2;
    Sleep(SD_DELAY);
  }
}

// is_2 is the input code specific to GO mode.
task is_2
{
  while (true) {
    if (IN_1 > 700) {
      // do bumper decay
      decay(bump_1, 20, 21)
    } else {
      bump_1 = 100;
    }

    if (IN_3 > 700) {
      // do bumper decay
      decay(bump_3, 20, 21)
    } else {
      bump_3 = 100;
    }
    
    Sleep(SD_DELAY);
  }
}

// is_3 is used only in TRAIN mode. It reads the inputs and turns them
// into useful sensor values
task is_3
{
  while (true) {
    raw_1 = IN_1;
    if (raw_1 > 700) {
      // do motor speed
      speed_a = raw_1 - 924;
      minmax(speed_a, speed_a)
      
      // do bumper decay
      decay(bump_1, 20, 21)
    } else {
      speed_a = 0;
      bump_1 = 100;
    }

    raw_3 = IN_3;
    if (raw_3 > 700) {
      // do motor speed
      speed_c = raw_3 - 924;
      minmax(speed_c, speed_c)
      
      // do bumper decay
      decay(bump_3, 20, 21)
    } else {
      speed_c = 0;
      bump_3 = 100;
    }

    rot_ac = (speed_a - speed_c) / 2;
    fwd_ac = (speed_a + speed_c) / 2;

    decay_r = ((20*decay_r) + rot_ac) / 21;
    decay_f = ((20*decay_f) + fwd_ac) / 21;

    if ((bump_1 > 50) && (n_1 < 327)) {
      m1_f += fwd_ac;
      m1_r += rot_ac;
      n_1 += 1;
      dv = 2;
    } else if ((bump_3 > 50) && (n_3 < 327)) {
      m3_f += fwd_ac;
      m3_r += rot_ac;
      n_3 += 1;
      dv = 3;
    } else if (n_0 < 327) {
      m0_f += fwd_ac;
      m0_r += rot_ac;
      n_0 += 1;
      dv = 1;
    } else {
      dv = 0;
    }

    if (dv == 2) {
      PlayNote(1000, 1);
    } else if (dv == 3) {
      PlayNote(1500, 1);
    } else if (dv == 1) {
      PlayNote(55, 1);
    }
    Sleep(TRAIN_DELAY);
  }
}

task master
{
  while(true) {
    ft = bump_1 * m1_f;
    rt = bump_1 * m1_r;
    td = bump_1;

    ft += (bump_3 * m3_f);
    rt += (bump_3 * m3_r);
    td += bump_3;

    if (td < 100) {
      td = 100 - td;
      ft += (td * m0_f);
      rt += (td * m0_r);
      td = 100;
    }
    ft /= td; minmax(fwd_ac, ft)
    rt /= td; minmax(rot_ac, rt)
    Sleep(MASTER_DELAY);
  }
}

#define IDLE 0
#define ONE_PULSE 1
#define ENTER_GO 2
#define ENTER_TRAIN 3

task main
{
  // initialize the hardware
  SensorMode(IN_1, SMODE_RAW);
  SensorType(IN_2, STYPE_LIGHT);
  SensorMode(IN_3, SMODE_RAW);
  Off(OUT_A); Off(OUT_C);

  rot_ac = 0; decay_r = 0;
  fwd_ac = 0; decay_f = 0;

  m0_f = 50; m0_r = 0;
  m1_f = -50; m1_r = 25;
  m3_f = 50; m3_r = 25;

  start is_1;

  while(true) {
    // idle mode
    PlayNote(196, 20);
    stop sd_1; stop mc_1; stop is_3;

    i_mode = IDLE;

    // nothing to do until they turn the light sensor on.
    wait (light > 1) {
      Sleep(5);
    }

    PlayNote(262, 10);
    i_i = 20;
    while (i_i > 0) {
      Sleep(5);
      i_i -= 1;
      if (light < 2) {
        i_mode = ONE_PULSE;
        i_i = 0; // no need to wait any longer
      }
    }

    if (i_mode == IDLE) {
      // it's still on
      i_mode = ENTER_GO;
    } else {
      // we got a pulse, need to wait a while now to see
      // if they turn it on again
      PlayNote(330, 10);
      i_i = 20;
      while (i_i > 0) {
        Sleep(5);
        i_i -= 1;
        if (light > 1) {
          i_mode = ENTER_TRAIN;
          i_i = 0; // no need to wait any longer
        }
      }
    }
    
    if (i_mode == ENTER_GO) {
      // still on -- that means run
      PlayNote(392, 10);

      // take measured readings and send to output
      fwd_ac = 0;
      rot_ac = 0;
      speed_a = 0; speed_c = 0;
      error_a = 0; error_c = 0;
      bump_1 = 0; bump_3 = 0;

      start sd_1; start mc_1; start is_2;
      start master;

      wait (light < 2) {
        Sleep(10);
      }

      stop master;
      stop sd_1; stop mc_1; stop is_2;
      Off(OUT_A); Off(OUT_C);
    } else if (i_mode == ENTER_TRAIN) {
      // input mode
      
      // zero out our statistics
      n_0 = 20; m0_f *= n_0; m0_r *= n_0;
      
      // try to maintain the status quo
      bump_1 = 0; bump_3 = 0;
      n_1 = 5; m1_f *= n_1; m1_r *= n_1;
      n_3 = 5; m3_f *= n_3; m3_r *= n_3;
      
      // motors need to coast now
      Float(OUT_A); Float(OUT_C);

      // collect data
      start is_3;
      wait (light < 2) {
        Sleep(10);
      }
      stop is_3;

      // process results.
      PlayNote(2500, 1); Sleep(1);
      m0_f /= n_0; m0_r /= n_0;
      m0_f = (SCALE_NUMER * m0_f)/SCALE_DENOM; minmax(m0_f, m0_f)
      m0_r = (SCALE_NUMER * m0_r)/SCALE_DENOM; minmax(m0_r, m0_r)

      m1_f /= n_1; m1_r /= n_1;
      m1_f = (SCALE_NUMER * m1_f)/SCALE_DENOM; minmax(m1_f, m1_f)
      m1_r = (SCALE_NUMER * m1_r)/SCALE_DENOM; minmax(m1_r, m1_r)

      m3_f /= n_3; m3_r /= n_3;
      m3_f = (SCALE_NUMER * m3_f)/SCALE_DENOM; minmax(m3_f, m3_f)
      m3_r = (SCALE_NUMER * m3_r)/SCALE_DENOM; minmax(m3_r, m3_r)
      PlayNote(2500, 1);
    }
  }
}