float S0_MIN = 1000.0;
float S0_MAX = 4200.0;

float S1_MIN = 900.0;
float S1_MAX = 4000.0;

void main() {
  set_baud(9600);
  disable_pcode_serial();
  poke(SCCR2, 44);
  init_expbd_servos(1);
  commandLoop();

}

int angleToServo(int angle, int servo_num) {
  float fangle = (float)angle;
  float m;
  float servo_val;
  if (angle < 0)
    angle = 0;
  if (angle > 180)
    angle = 180;
  if (servo_num == 0) {
    m = (S0_MAX - S0_MIN) / 180.0;
    servo_val = m * fangle + S0_MIN;
  }
  else {
    m = (S1_MAX - S1_MIN) / 180.0;
    servo_val = m * fangle + S1_MIN;
  }
  return (int)servo_val;
}

int relativeToServo(int angle, int servo_num) {
  float fangle = (float)angle;
  float scale;
  if (servo_num == 0) {
    scale = (S0_MAX - S0_MIN) / 180.0;
  }
  else {
    scale = (S1_MAX - S1_MIN) / 180.0;
  }
  return (int)(scale * fangle);
}

void commandLoop() {
  int data;
  int param;
  while (1) {
    data = 0;
    param = 0;
    if (getNextCommand(&data, &param)) {
      printf("\nCMD: %d (%d)", data, param); 
      
      handleCommand(&data, &param);

    }
  }

}

void handleCommand(int* cmd, int* param) {
  if (*cmd & 0b00000010) {
    if (*cmd & 0b10000000) {
      moveServo(0, *param);
    }
    if (*cmd & 0b01000000) {
      moveServo(1, *param);
    }
  }
  else if (*cmd & 0b00000100) {         /*query; eventually send to server */
    printf("\n%d:%d", servo0, servo1);
  }
  else if (*cmd & 0b00000001) {         /* all stop */
    servo0 = servo0;
    servo1 = servo1;
  }
  else {
    moveServo(0, 90);
    moveServo(1, 90);
  }
}

void moveServo(int servo_num, int angle) {
  int servo_val;
  if (servo_num == 0) {
    servo_val = angleToServo(angle, 0);
    servo0 = servo_val;
  }
  else {
    servo_val = angleToServo(angle, 1);
    servo1 = servo_val;
  }
}

int getNextCommand(int* data, int* param) {
  *data = receiveByte();
  if (*data & 0b10) {
    *param = receiveByte();
    return 1;
  }
  else
    return 1;
}

int receiveByte() {
  int ack;
  ack = 255;
  while (1) {
    data = get_sci();
    if (data > 0) {
      put_byte(ack);
      return data;
    }
  }
}

void sendByte(int input) {
  put_byte(input);
}