/*
 * Copyright (c) 2001 Ross Crawford
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <dmotor.h>
#include <dsensor.h>
#include <dsound.h>
#include <conio.h>
#include <remote.h>
#include <time.h>

#define DEBUG

unsigned int last_pos;
time_t last_time;
#define DELAY 20
#define UP rev
#define DOWN fwd

unsigned int wt_dir;
#define IN 1
#define NONE 0
#define OUT -1

#define LIMIT_BUTTON 1
#define BOOM_LIMIT 2

#define ALARM_SPEED (MAX_SPEED/2)

MotorDirection motor_b, motor_c;

static wakeup_t button_pressed(wakeup_t button_num) {
  switch ((unsigned int)button_num) {
  case 1:
    return (SENSOR_1<0xf000);
  case 2:
    return (SENSOR_2<0xf000);
  case 3:
    return (SENSOR_3<0xf000);
  }
  return 0;
}

static wakeup_t button_released(wakeup_t button_num) {
  switch ((unsigned int)button_num) {
  case 1:
    return (SENSOR_1>0xf000);
  case 2:
    return (SENSOR_2>0xf000);
  case 3:
    return (SENSOR_3>0xf000);
  }
  return 0;
}

int RemoteEvent(unsigned int etype, unsigned int key) {
int retcode = 0;
  if (etype == LREVT_KEYON) {
    switch (key) {
    case LRKEY_A1:
      motor_c = DOWN;
      retcode = 1;
      break;
    case LRKEY_A2:
      motor_c = UP;
      retcode = 1;
      break;
    case LRKEY_M1:
      motor_c = off;
      retcode = 1;
      break;
    case LRKEY_B1:
      motor_b = DOWN;
      retcode = 1;
      break;
    case LRKEY_B2:
      motor_b = UP;
      retcode = 1;
      break;
    case LRKEY_M2:
      motor_b = off;
      retcode = 1;
      break;
    }
  }
  if (etype == LREVT_KEYOFF && button_pressed(LIMIT_BUTTON)) {
    motor_b = off;
    motor_c = off;
    retcode = 1;
  }
  return retcode;
}

// Check for rotation. Timeout set above. This function never returns 1.
// Tried this as infinite loop in watch_rotation() with yield(), but had
// flaky results. This method seems to work better.
// When CW starts moving, reduce power on the boom motor, but dont stop it.
// When CW stops, increase power, as long as limit sensor not activated.
// This ensures boom moves slowly when alarm condition exists.
static wakeup_t pos_change(wakeup_t data) {
  if (last_pos != ROTATION_3) {
    if (wt_dir == NONE) {
      if (ROTATION_3 < last_pos)
        wt_dir = IN;
      else
        wt_dir = OUT;
      motor_b_speed(ALARM_SPEED);
    }
    last_pos = ROTATION_3;
    last_time = sys_time;
  }
  if (sys_time - last_time > DELAY) {
    if (wt_dir != NONE) {
      wt_dir = NONE;
      if (button_released(LIMIT_BUTTON))
        motor_b_speed(MAX_SPEED);
    }
  }
  return 0;
}

int watch_rotation(int argc, char *argv[]) {
  wait_event(&pos_change,0);
  return 0;
}

static wakeup_t chk_motor(wakeup_t motor_num) {
  switch((unsigned int)motor_num) {
  case 2:
    if (button_pressed(BOOM_LIMIT) && motor_b == UP)
      motor_b = off;
    motor_b_dir(motor_b);
    break;
  case 3:
    motor_c_dir(motor_c);
    break;
  }
  return 0;
}

int watch_motor(int argc, char *argv[]) {
  wait_event(&chk_motor,argc);
  return 0;
}

int watch_wt_limit(int argc, char *argv[]) {
  while(1) {
    wait_event(&button_pressed,LIMIT_BUTTON);
#ifdef DEBUG
dsound_system(DSOUND_BEEP);
#endif
    motor_b = off;
    motor_c = off;
    wait_event(&button_released,LIMIT_BUTTON);
  }
  return 0;
}

int main()
{
  // Init sensors
  ds_active(&SENSOR_3);
  ds_rotation_on(&SENSOR_3);

  last_pos = ROTATION_1;
  last_time = sys_time;

  motor_b = off;
  motor_c = off;
  wt_dir = NONE;

  motor_b_speed(MAX_SPEED);

  // Init remote
  lr_init();
  lr_set_handler(RemoteEvent);

  // Nice display
  cputs("run");

  execi(&watch_motor,2,0,PRIO_HIGHEST-1,DEFAULT_STACK_SIZE);
  execi(&watch_motor,3,0,PRIO_HIGHEST-1,DEFAULT_STACK_SIZE);
  execi(&watch_wt_limit,0,0,PRIO_HIGHEST-1,DEFAULT_STACK_SIZE);
  execi(&watch_rotation,0,0,PRIO_HIGHEST-1,DEFAULT_STACK_SIZE);

  return 0;
}