/*
 * 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
 *
 */

#define DEBUG

#include <sys/lcd.h>
#include <unistd.h>
#include <string.h>
#include <time.h>
#include <dsound.h>
#include <dmotor.h>
#include <dsensor.h>
#include <conio.h>
#include <remote.h>

// Light thresholds
// With AC power, 40 & 49 seem OK
// Can reduce to 40 & 46 with low power
#define THRESHOLD_LOW (unsigned short)40  // Low Light threshold
#define THRESHOLD_HI (unsigned short)46  // Low Light threshold
#define TOO_HEAVY -1
#define TOO_LIGHT 1
#define DELAY 250               // msec

#define UP rev
#define DOWN fwd

#define OUT fwd
#define IN rev

#define HOOK_BUTTON (SENSOR_3)

MotorDirection last_limit;     // Direction when limit switch pressed
time_t start_time;             // Time for motor to start

static note_t alarm[] = {{PITCH_H3,3},{PITCH_G3,3},{PITCH_END,1}};

MotorDirection motor_a, motor_b, motor_c;

// Wait for limit switch.
// If data is 1, wait for press
// If data is 0, play alarm & wait for release
static wakeup_t limit(wakeup_t data) {
  if (data)
    return (SENSOR_1<0xf000);
  else {
    if (SENSOR_1<0xf000) {
      if (!dsound_playing()) {
        dsound_play(alarm);
      }
      return 0;
    } else {
      dsound_stop();
      return 1;
    }
  }
}

int RemoteEvent(unsigned int etype, unsigned int key) {
int retcode = 0;
  if (etype == LREVT_KEYON) {
    switch (key) {
    case LRKEY_C1:
      motor_b = DOWN;
      retcode = 1;
      break;
    case LRKEY_C2:
      motor_b = UP;
      retcode = 1;
      break;
    case LRKEY_M3:
      motor_b = off;
      retcode = 1;
      break;
    }
  }
  if (etype == LREVT_KEYOFF && limit(1)) {
    motor_b = off;
    retcode = 1;
  }
  return retcode;
}

static wakeup_t chk_motor(wakeup_t motor_num) {
  switch((unsigned int)motor_num) {
  case 1:
    if (start_time == 0 || sys_time > start_time) {
      motor_a_dir(motor_a);
    }
    break;
  case 2:
    if ((motor_a == IN && motor_b == UP)
     || (motor_a == OUT && motor_b == DOWN)
     || (motor_b == UP && (HOOK_BUTTON<0xf000)))
      motor_b_dir(off);
    else
      motor_b_dir(motor_b);
    break;
  }
  return 0;
}

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

/*
** Counterweight stuff
*/

// Start motor in given direction, and record direction for later
void wt_move(MotorDirection dir) {
  motor_a = dir;
  if (dir == OUT || dir == IN) {
    start_time = sys_time + DELAY;    // will wait a bit before starting
  } else {
    start_time = 0;                   // force immediate stop
  }
}

// Stop the motor. Brake temporarily, then float.
void wt_stop() {
  wt_move(brake);
  msleep(60);
  wt_move(off);
}

// Wait for light to pass beyond threshold in either direction
static wakeup_t off_balance(wakeup_t sens_num) {
#ifdef DEBUG
lcd_unsigned(LIGHT(SENSOR_2));
#endif
  if (LIGHT(SENSOR_2) < THRESHOLD_LOW)
    return TOO_LIGHT;
  if (LIGHT(SENSOR_2) > THRESHOLD_HI)
    return TOO_HEAVY;
  return 0;
}

// Wait for light to return to median
static wakeup_t on_balance(wakeup_t sens_num) {
#ifdef DEBUG
lcd_unsigned(LIGHT(SENSOR_2));
#endif
  return (LIGHT(SENSOR_2) > THRESHOLD_LOW
       && LIGHT(SENSOR_2) < THRESHOLD_HI);
}

// Routine to start the counter-weight moving
// When sensor activated, check that limit switch isn't pressed.
// If it is, and we were going this direction when it was, then stop.
// Blocks until balance is restored
// Dont allow motor to continue more than MAX_LIMIT_MOVE msec after limit
// reached. This avoids CW mechanism damage.
void move_weight(MotorDirection newdir) {
  if (limit(1)) {
    if (last_limit == newdir)
      wt_stop();
    else
      wt_move(newdir);
  } else {
    wt_move(newdir);
  }
  wait_event(on_balance,0);
  if (motor_a == newdir)
    wt_stop();
}

// Thread to check for too little weight
// Move weight in appropriate direction depending on which way it tips.
int balance(int argc, char *argv[]) {
wakeup_t trans;
  while(1) {
    trans = wait_event(off_balance, 0);

    // move_weight will block until balance restored
    if (trans == TOO_HEAVY)
      move_weight(OUT);
    else
      move_weight(IN);
  }
  return 0;
}

// Main prog. Init sensors, stop motor, start watcher threads.
// When limit switch is pressed, record direction, stop, and
// wait till it's released.
int main()
{
  ds_active(&SENSOR_2);

  last_limit = off;
  start_time = 0;
  wt_stop();
  motor_a_speed(MAX_SPEED);

  motor_b = off;

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

  execi(&balance,0,0,PRIO_HIGHEST-1,DEFAULT_STACK_SIZE);
  execi(&watch_motor,1,0,PRIO_HIGHEST-1,DEFAULT_STACK_SIZE);
  execi(&watch_motor,2,0,PRIO_HIGHEST-1,DEFAULT_STACK_SIZE);

  while(1) {
    wait_event(limit,1);
    motor_b = off;
    last_limit = motor_a;
    wt_stop();
    wait_event(limit,0);
  }
  return 0;
}