/**
 *
 *  MOTOR.cpp
 *  Jan Dvorak z Vozerovic - dvorkaman@centrum.cz - dvorkaman.php5.cz
 */

#ifndef motor_cpp
#define motor_cpp

#include "WProgram.h"
#include "motor.h"

/***************************************************
/**
 * Constructor
 * @param {int,int} forwardPin backwardPin 
 */
Motor::Motor(int fw, int bw){ 
  Motor(fw,bw,-1); 
}

/***************************************************
/**
 * Constructor
 * @param {int,int,int} forwardPin backwardPin enablePin 
 */
Motor::Motor(int fw, int bw,int enable)
{
  pinFW=fw;
  pinBW=bw;
  pinEN=enable;

  dir=0; //FREE
  lastDir=1; //FW
  spd=255; //full
  lastSpd=spd;  

  minSpeed=0;
  maxSpeed=100; //in %

  init();
}

/***************************************************
/**
 * Inicializes pins
 * PRIVATE 
 */
inline void Motor::init()
{
  pinMode(pinFW,OUTPUT);
  pinMode(pinBW,OUTPUT);
  if(pinEN!=-1)pinMode(pinEN,OUTPUT);  
}

/***************************************************
/**
 * Set speed 0 - 100%; motor can't be in mode BREAK and Enable pin must be specified
 * @param int motorSpeed <0%,100%> 
 */
void Motor::changeMotorSpeed(int mSpeed)
{  
  if( pinEN==-1 )return;
  if( dir==2 )return; //motor in mode BREAK

  if( mSpeed<minSpeed )mSpeed=minSpeed;
  if( mSpeed>maxSpeed )mSpeed=maxSpeed;

  mSpeed=map(mSpeed,0,100,0,255); //remap to 0-255

  setMotorSpeed(mSpeed);
}

/***************************************************
/**
 * Returns speed
 * @returns int motorSpeed 
 */
int Motor::getMotorSpeed()
{
  return map(spd,0,255,0,100);
}

/***************************************************
/**
 * Returns direction
 * @returns int motorDirection (+-1; 0 = free; 2 = break) 
 */
int Motor::getMotorDirection()
{
  return dir;
}

/***************************************************
/**
 * Stop motor
 */
void Motor::stopMotor()
{
  setMotorSpeed(0);  
  setDirection(0); //FREE
}

/***************************************************
/**
 * Resume motor speed
 */
void Motor::startMotor()
{
  if(dir==2 || dir==0)dir=lastDir; //resume motor direction
  setDirection(dir); //Resume previous direction

  if(spd==0)spd=lastSpd; //resume previous speed
  setMotorSpeed(spd);    
}

/***************************************************
/**
 * Rotate motor Forwards, motor cant't be rotating backwards 
 */
void Motor::forward()
{
  if(dir==-1)return; //cannot switch without stopping

  setDirection(1); //FW

  if(spd==0)spd=lastSpd; //resume previous speed
  setMotorSpeed(spd);  
}

/***************************************************
/**
 * Rotate motor Forwards and set speed, motor cant't be rotating backwards
 * @param int motorSpeed <0%,100%> 
 */
void Motor::forward(int mSpeed)
{
  if(dir==-1)return; //cannot switch without stopping

  setDirection(1);
  setMotorSpeed(mSpeed);
}

/***************************************************
/**
 * Rotate motor Backwards, motor cant't be rotating forwards
 */
void Motor::backward()
{
  if(dir==1)return; //cannot switch without stopping

  setDirection(-1); //BW

  if(spd==0)spd=lastSpd; //resume previous speed
  setMotorSpeed(spd);  
}

/***************************************************
/**
 * Rotate motor Backwards and set speed, motor cant't be rotating forwards
 * @param int motorSpeed <0%,100%> 
 */
void Motor::backward(int mSpeed)
{
  if(dir==1)return; //cannot switch without stopping

  setDirection(-1);
  setMotorSpeed(mSpeed);
}

/***************************************************
/**
 * Break motor - short motor pins
 */
void Motor::breakMotor()
{
  setDirection(2);

  if(pinEN!=-1) digitalWrite(pinEN,HIGH); //FULL power
}

/***************************************************
/**
 * Set speed 0 - 255; motor can't be in mode BREAK and Enable pin must be specified
 * PRIVATE; 
 */
void Motor::setMotorSpeed(int mSpeed)
{
  if( pinEN==-1 )return;
  if( dir==2 )return; //motor in mode BREAK

  if( mSpeed<0 )mSpeed=0;
  if( mSpeed>255 )mSpeed=255;

  if(mSpeed==0) if(spd>0) lastSpd=spd; //saves actual value - resume from stop; double if becouse of user's multiple call
  spd=mSpeed;

  analogWrite(pinEN,spd); //apply new speed
}

/***************************************************
/**
 * Set motor direction - only pin voltages, no PWM
 * PRIVATE
 */
void Motor::setDirection(int d)
{
  if(d==2 || d==0) if(dir!=2 && dir!=0) lastDir=dir; //saves value of previous direction if entering mode BREAK or FREE; double if becouse of user's multiple call
  dir=d; //new value of direction

  if(d==1)
  {
    digitalWrite(pinFW,HIGH); //FW
    digitalWrite(pinBW,LOW);  

  }
  else if(d==-1){
    digitalWrite(pinFW,LOW);  //BW
    digitalWrite(pinBW,HIGH);  

  }
  else if(d==2){
    digitalWrite(pinFW,HIGH); //BREAK
    digitalWrite(pinBW,HIGH);  

  }
  else if(d==0){
    digitalWrite(pinFW,LOW);  //FREE
    digitalWrite(pinBW,LOW);    
  }
}

/***************************************************
/**
 * Reverse motor direction (only from mode FW or BW)
 */
void Motor::reverseMotor()
{
  if(dir==0 || dir==2)return; //can't revese break of free
  int d=dir;

  stopMotor();
  delay(100);
  breakMotor();
  delay(400); //wait for stop

  if(d==-1)forward();
  else backward();
}

/***************************************************
/**
 * Change pwm directly - usefull for synchronizing 2+ motors
 * @param int pwm 0 -> 255
 */
void Motor::changePWM(int pwm)
{
  if( pinEN==-1 )return;
  if( dir==2 )return; //motor in mode BREAK

  if( pwm<0 )pwm=0;
  if( pwm>255 )pwm=255;

  setMotorSpeed(pwm);
}

#endif