// --- Hardware definitions --------------------------------------------------

#include "pswitch.nqh"
PSWITCH(OUT_B, ps_claw, clawcode)

#define DRIVE_L OUT_A
#define DRIVE_R OUT_C
#include "2wd.nqh"

#define DROT_L SENSOR_2
#define DROT_R SENSOR_3
#define CLAWLS SENSOR_1

// --- Misc ----------------------------------------------------

#define ROTPOLL 12
int ok_l, ok_r, opos_l, opos_r;

#define DPower(pow) SetPower(DRIVE_L|DRIVE_R, pow)

int holding_obj, mood, fchange;

#define TIMER_LASTBUMP 0
#define TIMER_SWAIT 1

// --- Basic tasks ----------------------------------------------------

task main () {
  ps_claw = 0;
  mood = 0;
  SelectDisplay(DISPLAY_USER);
  SetUserDisplay(mood, 0);
  start stallwatch;

  start drive;
  start grab;
  start noise;
}

task stallwatch () {
  SetSensor(DROT_L, SENSOR_ROTATION);
  SetSensor(DROT_R, SENSOR_ROTATION);
  ok_l = 1;
  ok_r = 1;
  while (1) {
    opos_l = DROT_L;
    opos_r = DROT_R;
    Wait(ROTPOLL);
    ok_l = DROT_L - opos_l;
    ok_r = DROT_R - opos_r;
  }
}

void MForward(const int cm) {
  int rots = cm * -46 / 15;
  ClearSensor(DROT_L);
  OnFwd(DRIVE_L + DRIVE_R);
  until(DROT_L <= rots);
  Off(DRIVE_L + DRIVE_R);
}

void MBackward(const int cm) {
  int rots = cm * 46 / 15;
  ClearSensor(DROT_L);
  OnRev(DRIVE_L + DRIVE_R);
  until(DROT_L >= rots);
  Off(DRIVE_L + DRIVE_R);
}

void MLeft(const int deg) {
  int rots = deg * 184 / 360;
  ClearSensor(DROT_L);
  SetDirection(DRIVE_R, OUT_FWD);
  SetDirection(DRIVE_L, OUT_REV);
  On(DRIVE_L + DRIVE_R);
  until(DROT_L >= rots);
  Off(DRIVE_L + DRIVE_R);
}

void MRight(const int deg) {
  int rots = deg * 184 / 360;
  ClearSensor(DROT_R);
  SetDirection(DRIVE_L, OUT_FWD);
  SetDirection(DRIVE_R, OUT_REV);
  On(DRIVE_L + DRIVE_R);
  until(DROT_R >= rots);
  Off(DRIVE_L + DRIVE_R);
}

// --- Behaviors ----------------------------------------------------

task drive () {
 int trapcount = 0;
 while (1) {
  int isright, lastb;

  DPower(OUT_HALF);
  DFwd();
  Wait(30);
  until(!ok_l || !ok_r);
  PlaySound(SOUND_CLICK);
  mood -= 2;
  lastb = Timer(TIMER_LASTBUMP);
  ClearTimer(TIMER_LASTBUMP);
  DPower(OUT_FULL);
  if (ok_l) isright = 1; else isright = 0;  
  DRev();
  if (trapcount > 3) SWait(50);
                else SWait(10);
  if (isright) DLeft(); else DRight();
  if (lastb < 50) SWait(25 - lastb / 2);
             else SWait(1 + Random(10));
  if (lastb < 30) trapcount++;
             else trapcount = 0;
}}

void SWait(int time) { // Wait for time, or until motor stalls
  ClearTimer(TIMER_SWAIT);
  Wait(ROTPOLL);
  while (Timer(TIMER_SWAIT) < time && ok_l && ok_r);
  PlayTone(200, 2);
}

task grab () {
  int threshold = CLAWLS + 10;
  holding_obj = 0;
  SetSensor(CLAWLS, SENSOR_LIGHT);
  while (1) {
    until (CLAWLS > 50 && !holding_obj);
    PlaySound(SOUND_UP);
    mood += 5;
    stop drive; DStop();
    DPower(OUT_FULL);
    dograb();
    start drive;
    holding_obj = 1;
    start release;
  }
}

task release () {
  Wait(500 + Random(500));
  stop drive;
  PlaySound(SOUND_DOWN);
  dorelease();
  mood -= 1;
  holding_obj = 0;
  DLeft();
  Wait(30);
  start drive;
}

void dograb () {
  claw();
  Wait(100);
}

void dorelease () {
  claw();
  DRev();
  Wait(80);
  DStop();
}

sub claw () {
  clawcode();
}

task noise () { while (1) {
  npause();
  switch (Random(2)) {
   case 0:
    PlayTone(300 + fchange, 12);
    PlayTone(350 + fchange, 12);
    PlayTone(400 + fchange, 12);
    PlayTone(340 + fchange, 12);
    PlayTone(420 + fchange, 12);
    PlayTone(280 + fchange, 12);
    break;
   case 1:
    PlayTone(500 + fchange, 12);
    PlayTone(400 + fchange, 12);
    PlayTone(450 + fchange, 12);
    PlayTone(350 + fchange, 12);
    PlayTone(300 + fchange, 12);
    PlayTone(200 + fchange, 12);
    break;
   case 2:
    PlayTone(300 + fchange, 12);
    PlayTone(350 + fchange, 12);
    PlayTone(500 + fchange, 12);
    PlayTone(400 + fchange, 12);
    PlayTone(360 + fchange, 12);
    PlayTone(320 + fchange, 12);
    break;
  }
}}

void npause () {
  Wait(500);
  if (mood >  0) mood--;
  if (mood <  0) mood++;
  if (mood >  5) mood = 5;
  if (mood < -5) mood = -5;
  fchange = mood * 80;
}