// --- 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_1
#define DROT_R SENSOR_2
#define CLAWLS SENSOR_3

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

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

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

int holding_obj, mood, fchange, sleepy;

#define TIMER_LASTBUMP 0
#define TIMER_SWAIT 1
#define TIMER_CARRY 2
#define TIMER_MWAIT 3

#define SEE_THRESH 42

#define MSG_TURN 1
#define MSG_RELEASE 2
#define MSG_SLEEP 3

#define NORMAL_POWER 4

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

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

  DPower(NORMAL_POWER);

  start drive;
  start grab;
  start noise;
  start sleep;
}

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;
 if (sleepy) return;
 while (1) {
  int isright, lastb;

  DFwd();
  Wait(30);
  until(!ok_l || !ok_r || Message() == MSG_TURN);
  if (ok_l) isright = 1; else isright = 0;  
  PlaySound(SOUND_CLICK);
  ClearMessage();
  mood -= 2;
  lastb = Timer(TIMER_LASTBUMP);
  ClearTimer(TIMER_LASTBUMP);

  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 > SEE_THRESH && !holding_obj);
    stop drive; DStop();
    Wait(20);
    if (CLAWLS <= SEE_THRESH) {start drive; start grab_wiggle; continue; }
    PlaySound(SOUND_UP);
    stop grab_wiggle; DPower(NORMAL_POWER);

    mood += 5;
    dograb();
    if (!sleepy) start drive;
    holding_obj = 1;
    start release;
  }
}

task grab_wiggle () {
  repeat (5) {
    Wait(50);
    SetPower(DRIVE_L, 0);
    SetPower(DRIVE_R, 4);
    Wait(50);
    SetPower(DRIVE_L, 4);
    SetPower(DRIVE_R, 0);
  }
  DPower(NORMAL_POWER);
}

task release () {
  ClearTimer(TIMER_CARRY);
  int delay = 200 + Random(1000);
  int got_it = 1;
  while (Timer(TIMER_CARRY) < delay && got_it && Message() != MSG_RELEASE) {
    if (CLAWLS > SEE_THRESH - 10) got_it = 1;
                             else got_it = 0;
  }
  ClearMessage();
  if (got_it) stop drive;
  PlaySound(SOUND_DOWN);
  dorelease();
  mood -= 1;
  holding_obj = 0;
  if (got_it && !sleepy) {
    DRev();
    Wait(80);
    DLeft();
    Wait(30);
    start drive;
  }
}

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

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

sub claw () {
  clawcode();
}

void sleep_mwait (int time) {
  ClearTimer(TIMER_MWAIT);
  while (Timer(TIMER_MWAIT) < time && Message() != MSG_SLEEP);
  ClearMessage();
}
task sleep () {
  while (1) {
    sleep_mwait(500);
    stop drive;
    DStop();
    sleep_noise();
    sleepy = 1;
    sleep_mwait(500 + Random(500));
    sleepy = 0;
    wake_noise();
    start drive;
  }
}

void sleep_noise () {
  PlayTone(1000 - 450, 12);
  PlayTone(1000 - 500, 12);
  PlayTone(1000 - 550, 12);
  PlayTone(1000 - 600, 22);
  PlayTone(0, 24);
  PlayTone(1000 - 550, 12);
  PlayTone(1000 - 620, 12);
  Wait(106);
}

void wake_noise () {
  PlayTone(450, 12);
  PlayTone(500, 12);
  PlayTone(550, 12);
  PlayTone(600, 22);
  PlayTone(0, 24);
  PlayTone(550, 12);
  PlayTone(620, 12);
  Wait(106);
}

task noise () { while (1) {
  npause();
  while (sleepy) 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(800);
  if (mood >  0) mood--;
  if (mood <  0) mood++;
  if (mood >  5) mood = 5;
  if (mood < -5) mood = -5;
  fchange = mood * 80;
}