/****************************************************************************
 *
 *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file RoboClaw.cpp
 *
 * RoboClaw Motor Driver
 *
 * references:
 * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
 *
 */

#include "RoboClaw.hpp"
#include <poll.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <fcntl.h>
#include <termios.h>

#include <systemlib/err.h>
#include <arch/board/board.h>
#include <mavlink/mavlink_log.h>

#include <uORB/Publication.hpp>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_hrt.h>

uint8_t RoboClaw::checksum_mask = 0x7f;

RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
		uint16_t pulsesPerRev):
	_address(address),
	_pulsesPerRev(pulsesPerRev),
	_uart(0),
	_controlPoll(),
	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
	_motor1Position(0),
	_motor1Speed(0),
	_motor1Overflow(0),
	_motor2Position(0),
	_motor2Speed(0),
	_motor2Overflow(0)
{
	// setup control polling
	_controlPoll.fd = _actuators.getHandle();
	_controlPoll.events = POLLIN;

	// start serial port
	_uart = open(deviceName, O_RDWR | O_NOCTTY);
	if (_uart < 0) err(1, "could not open %s", deviceName);
	int ret = 0;
	struct termios uart_config;
	ret = tcgetattr(_uart, &uart_config);
	if (ret < 0) err (1, "failed to get attr");
	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
	ret = cfsetispeed(&uart_config, B38400);
	if (ret < 0) err (1, "failed to set input speed");
	ret = cfsetospeed(&uart_config, B38400);
	if (ret < 0) err (1, "failed to set output speed");
	ret = tcsetattr(_uart, TCSANOW, &uart_config);
	if (ret < 0) err (1, "failed to set attr");

	// setup default settings, reset encoders
	resetEncoders();
}

RoboClaw::~RoboClaw()
{
	setMotorDutyCycle(MOTOR_1, 0.0);
	setMotorDutyCycle(MOTOR_2, 0.0);
	close(_uart);
}

int RoboClaw::readEncoder(e_motor motor)
{
	uint16_t sum = 0;
	if (motor == MOTOR_1) {
		_sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum);
	} else if (motor == MOTOR_2) {
		_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
	}
	uint8_t rbuf[50];
	usleep(5000);
	int nread = read(_uart, rbuf, 50);
	if (nread < 6) { 
		printf("failed to read\n");
		return -1;
	}
	//printf("received: \n");
	//for (int i=0;i<nread;i++) {
		//printf("%d\t", rbuf[i]);
	//}
	//printf("\n");
	uint32_t count = 0;
	uint8_t * countBytes = (uint8_t *)(&count);
	countBytes[3] = rbuf[0];
	countBytes[2] = rbuf[1];
	countBytes[1] = rbuf[2];
	countBytes[0] = rbuf[3];
	uint8_t status = rbuf[4];
	uint8_t checksum = rbuf[5];
	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & 
		checksum_mask;
	// check if checksum is valid
	if (checksum != checksum_computed) {
		printf("checksum failed: expected %d got %d\n",
				checksum, checksum_computed);
		return -1;
	}
	int overFlow = 0;

	if (status & STATUS_REVERSE) {
		//printf("roboclaw: reverse\n");
	}

	if (status & STATUS_UNDERFLOW) {
		//printf("roboclaw: underflow\n");
		overFlow = -1;
	} else if (status & STATUS_OVERFLOW) {
		//printf("roboclaw: overflow\n");
		overFlow = +1;
	}

	static int64_t overflowAmount = 0x100000000LL;
	if (motor == MOTOR_1) {
		_motor1Overflow += overFlow;
		_motor1Position = float(int64_t(count) + 
			_motor1Overflow*overflowAmount)/_pulsesPerRev;
	} else if (motor == MOTOR_2) {
		_motor2Overflow += overFlow;
		_motor2Position = float(int64_t(count) + 
			_motor2Overflow*overflowAmount)/_pulsesPerRev;
	}
	return 0;
}

void RoboClaw::printStatus(char *string, size_t n)
{
	snprintf(string, n,
		 "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
		 double(getMotorPosition(MOTOR_1)),
		 double(getMotorSpeed(MOTOR_1)),
		 double(getMotorPosition(MOTOR_2)),
		 double(getMotorSpeed(MOTOR_2)));
}

float RoboClaw::getMotorPosition(e_motor motor)
{
	if (motor == MOTOR_1) {
		return _motor1Position;
	} else if (motor == MOTOR_2) {
		return _motor2Position;
	} else {
		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
		return NAN;
    }
}

float RoboClaw::getMotorSpeed(e_motor motor)
{
	if (motor == MOTOR_1) {
		return _motor1Speed;
	} else if (motor == MOTOR_2) {
		return _motor2Speed;
	} else {
		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
		return NAN;
    }
}

int RoboClaw::setMotorSpeed(e_motor motor, float value)
{
	uint16_t sum = 0;
	// bound
	if (value > 1) value = 1;
	if (value < -1) value = -1;
	uint8_t speed = fabs(value)*127;
	// send command
	if (motor == MOTOR_1) {
		if (value > 0) {
			return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
		} else {
			return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
		}
	} else if (motor == MOTOR_2) {
		if (value > 0) {
			return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
		} else {
			return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
		}
	}
	return -1;
}

int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
{
	uint16_t sum = 0;
	// bound
	if (value > 1) value = 1;
	if (value < -1) value = -1;
	int16_t duty = 1500*value;
	// send command
	if (motor == MOTOR_1) {
		return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
				(uint8_t *)(&duty), 2, sum);
	} else if (motor == MOTOR_2) {
		return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
				(uint8_t *)(&duty), 2, sum);
	}
	return -1;
}

int RoboClaw::resetEncoders() 
{
	uint16_t sum = 0;
	return _sendCommand(CMD_RESET_ENCODERS,
			nullptr, 0, sum);
}

int RoboClaw::update()
{
	// wait for an actuator publication,
	// check for exit condition every second
	// note "::poll" is required to distinguish global
	// poll from member function for driver
	if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error

	// if new data, send to motors
	if (_actuators.updated()) {
		_actuators.update();
		setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
		setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
	}
	return 0;
}

uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
{
	uint16_t sum = 0;
	//printf("sum\n");
	for (size_t i=0;i<n;i++) {
		sum += buf[i];
		//printf("%d\t", buf[i]);
	}
	//printf("total sum %d\n", sum);
	return sum;
}

int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
		size_t n_data, uint16_t & prev_sum)
{
	tcflush(_uart, TCIOFLUSH); // flush  buffers
	uint8_t buf[n_data + 3];
	buf[0] = _address;
	buf[1] = cmd;
	for (size_t i=0;i<n_data;i++) {
		buf[i+2] = data[n_data - i - 1]; // MSB
	}
	uint16_t sum = _sumBytes(buf, n_data + 2);
	prev_sum += sum;
	buf[n_data + 2] = sum & checksum_mask;
	//printf("\nmessage:\n");
	//for (size_t i=0;i<n_data+3;i++) {
		//printf("%d\t", buf[i]);
	//}
	//printf("\n");
	return write(_uart, buf, n_data + 3);
}

int roboclawTest(const char *deviceName, uint8_t address, 
		uint16_t pulsesPerRev)
{
	printf("roboclaw test: starting\n");

	// setup
	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
	char buf[200];
	for (int i=0; i<10; i++) {
		usleep(100000);
		roboclaw.readEncoder(RoboClaw::MOTOR_1);
		roboclaw.readEncoder(RoboClaw::MOTOR_2);
		roboclaw.printStatus(buf,200);
		printf("%s", buf);
	}

	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3);
	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3);
	for (int i=0; i<10; i++) {
		usleep(100000);
		roboclaw.readEncoder(RoboClaw::MOTOR_1);
		roboclaw.readEncoder(RoboClaw::MOTOR_2);
		roboclaw.printStatus(buf,200);
		printf("%s", buf);
	}

	printf("Test complete\n");
	return 0;
}

// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78