#include <Servo.h>
#include <Wire.h>
#include "WiiNun.h"
#include "Motor.h"

const byte PIN_LED = 13;

const byte PIN_MOTOR_HORIZON_1  = 5;  // 水平アームモーター
const byte PIN_MOTOR_HORIZON_2  = 4;  // 水平アームモーター
const byte PIN_MOTOR_VERTICAL_1 = 6;  // 垂直アームモーター
const byte PIN_MOTOR_VERTICAL_2 = 7;  // 垂直アームモーター

const byte PIN_SERVO_BASE    =  9;  // 首ふりサーボ
const byte PIN_SERVO_CATCHER = 10;  // キャッチャーサーボ

// ヌンチャクのためのピン
const byte PIN_PWR = A3;
const byte PIN_GND = A2;

Servo base;      // 首ふりサーボ
Servo catcher;   // キャッチャーサーボ
WiiNun WiiNun;   // ヌンチャク
Motor horMotor;  // 水平アームモーター
Motor verMotor;  // 垂直アームモーター

void setup(){
  pinMode(PIN_LED, OUTPUT);
  pinMode(PIN_PWR, OUTPUT);
  pinMode(PIN_GND, OUTPUT);
  digitalWrite(PIN_PWR, HIGH);
  digitalWrite(PIN_GND, LOW);
  delay(100);
//  Serial.begin(19200);
  WiiNun.begin();
  base.attach(PIN_SERVO_BASE);
  catcher.attach(PIN_SERVO_CATCHER);
  horMotor.attach(PIN_MOTOR_HORIZON_1, PIN_MOTOR_HORIZON_2);
  verMotor.attach(PIN_MOTOR_VERTICAL_1, PIN_MOTOR_VERTICAL_2);
}

void loop(){
  // ヌンチャクを読む
  WiiNun.get();
  byte joyx = WiiNun.data[0];
  byte joyy = WiiNun.data[1];
  byte accx = WiiNun.data[2];
  byte accy = WiiNun.data[3];
  byte accz = WiiNun.data[4];
  byte zbut = ((~WiiNun.data[5] >> 0) & 1);
  byte cbut = ((~WiiNun.data[5] >> 1) & 1);
  if((WiiNun.data[5] >> 2) & 1) accx += 2;
  if((WiiNun.data[5] >> 3) & 1) accx += 1;
  if((WiiNun.data[5] >> 4) & 1) accy += 2;
  if((WiiNun.data[5] >> 5) & 1) accy += 1;
  if((WiiNun.data[5] >> 6) & 1) accz += 2;
  if((WiiNun.data[5] >> 7) & 1) accz += 1;
  // ヌンチャクおわり
  
  int jx = (signed)(joyx-0x7F);  // スティックの中心を0にする
  int jy = (signed)(joyy-0x7F);  // スティックの中心を0にする
//  int ax = (signed)(accx-0x7F); // 使うときはコメントアウトすること
  int ay = (signed)(accy-0x7F); // 水平状態を0にする
  
  /* 速くて危険
  static byte baseDeg = 90;
  if(zbut){  // zボタン押しながら
    digitalWrite(PIN_LED, HIGH);
    if(jy>0x1F) baseDeg = 180 - atan2(jy, jx)*180/PI;  // 首をスティックの角度にする
    if(baseDeg < 10) baseDeg = 10;
    else if(baseDeg > 170) baseDeg = 170;
  }else{  // zボタン離しながら
    digitalWrite(PIN_LED, LOW);
    horMotor.speed(jy*2);
    baseDeg += jx/32; // 首をjxに応じて相対移動
  }
  base.write(baseDeg);
  */
  static unsigned int baseMicros = 1660;  // 首のデフォの角度(だいたい90度にしたい。。。)
  if(zbut){  // zボタン押しながら
    digitalWrite(PIN_LED, HIGH);
    baseMicros += jx/4; // 首をjxに応じて相対移動(x2速く)
  }else{  // zボタン離しながら
    digitalWrite(PIN_LED, LOW);
    baseMicros += jx/8; // 首をjxに応じて相対移動
  }
  baseMicros = constrain(baseMicros, 544, 2400);
  base.writeMicroseconds(baseMicros);
//  Serial.println(baseMicros);
  
  /* 粗い
  if(0);
  else if(ay > +0x20) verMotor.mode(GO);
  else if(ay > +0x10) verMotor.speed(+0xCC);
  else if(ay < -0x20) verMotor.mode(BACK);
  else if(ay < -0x10) verMotor.speed(-0xCC);
  else verMotor.mode(STOP);
  */
  int verMotorSpeed = 0;  // -255-255
  verMotorSpeed = constrain(ay*8, -255, 255);
  verMotor.speed(verMotorSpeed);
  
  int horMotorSpeed = 0;  // -255-255
  horMotorSpeed = constrain(jy*2, -255, 255);
  horMotor.speed(horMotorSpeed);
  
  byte catcherDeg = 135;
  if(cbut){  // cボタン押すとキャッチャーを離す
    catcherDeg = 45;
  }
  catcher.write(catcherDeg);
  
  delay(50);
}