#include "SerialPort.hh"
#include "KbdInput.hh"

#include <iostream.h>

typedef unsigned char byte;

byte SERVO_0  = 128;
byte SERVO_1  = 64;
byte TURN_ABS = 2;
byte STOP     = 1;
byte CENTER   = 0;

byte ACK      = 255;

byte PAN_SERVO  = SERVO_0;
byte TILT_SERVO = SERVO_1;

void sendPan(SerialPort& p, byte degrees);
void sendTilt(SerialPort& p, byte degrees);
void sendCenter(SerialPort& p);

int main(int, char**) {
  SerialPort p;
  p.openPort();
  KbdInput k;
  
  sendCenter(p);
  byte x = 90;
  byte y = 90;
  
  while (true) {
    char c = k.nextChar();
    if (c == 'i') {
      //cout << "moving up" << endl;
      y += 2;
      sendTilt(p, y);
    }
    else if (c == 'k') {
      //cout << "moving down" << endl;
      y -= 2;
      sendTilt(p, y);
    }
    else if (c == 'j') {
      //cout << "moving left" << endl;
      x -= 2;
      sendPan(p, x);
    }
    else if (c == 'l') {
      //cout << "moving right" << endl;
      x += 2;
      sendPan(p, x);
    }
  }
  
  

}

void sendPan(SerialPort& p, byte degrees) {
  bool need_trans = true;
  byte command = 0;
  command |= PAN_SERVO;
  command |= TURN_ABS;
  while (need_trans) {
    p.send(command);
    need_trans = !(p.receive() == ACK);
   
  }
  
  need_trans = true;
  byte param = degrees;
  if (param == 0)
    param = 1;
  while (need_trans) {
    p.send(param);
    need_trans = !(p.receive() == ACK);
  }
}

void sendTilt(SerialPort& p, byte degrees) {
  bool need_trans = true;
  byte command = 0;

  command |= TILT_SERVO;
  command |= TURN_ABS;
  while (need_trans) {
    p.send(command);
    need_trans = !(p.receive() == ACK);
  }
  
  need_trans = true;
  byte param = degrees;
  if (param == 0)
    param = 1;
  while (need_trans) {
    p.send(param);
    need_trans = !(p.receive() == ACK);
  }
}

void sendCenter(SerialPort& p) {
  bool need_trans = true;
  byte command = 0;
  command |= PAN_SERVO;
  command |= TILT_SERVO;
  command |= CENTER;
  while (need_trans) {
    p.send(command);
    need_trans = !(p.receive() == ACK);
  }
}