/*
Motor.cpp
(C)2014 kou029w - MIT License
*/
#include "Motor.h"

Motor::Motor(){
  _pin1 = -1;
  _pin2 = -1;
}

void Motor::mode(uint8_t mode){
  if(_pin1 != -1) digitalWrite(_pin1, (mode>>0)&1);
  if(_pin2 != -1) digitalWrite(_pin2, (mode>>1)&1);
}

void Motor::mode(uint8_t mode, uint8_t speed){
  switch(mode){
  case GO:
    if(_pin1 != -1) analogWrite(_pin1, speed);
    if(_pin2 != -1) digitalWrite(_pin2, LOW);
    break;
  case BACK:
    if(_pin1 != -1) analogWrite(_pin1, speed);
    if(_pin2 != -1) digitalWrite(_pin2, HIGH);
    break;
  default:
    if(_pin1 != -1) digitalWrite(_pin1, (mode>>0)&1);
    if(_pin2 != -1) digitalWrite(_pin2, (mode>>1)&1);
    break;
  }
}

void Motor::speed(int speed){
  if(speed>=0) mode(GO, speed);
  else mode(BACK, -speed);
}

void Motor::attach(uint8_t pin1){
  _pin1 = pin1;
  _pin2 = -1;
  pinMode(_pin1, OUTPUT);
  digitalWrite(_pin1, LOW);
}

void Motor::attach(uint8_t pin1, uint8_t pin2){
  _pin1 = pin1;
  _pin2 = pin2;
  pinMode(_pin1, OUTPUT);
  pinMode(_pin2, OUTPUT);
  digitalWrite(_pin1, LOW);
  digitalWrite(_pin2, LOW);
}

void Motor::detach(){
  mode(STOP);
  pinMode(_pin1, INPUT);
  pinMode(_pin2, INPUT);
  _pin1 = -1;
  _pin2 = -1;
}