/* MCP.c - Autonomous functionality for KCB/RCB
 * By Taylor Veltrop
 * License at end of file
 * Version 090927
 *
 * replacement for rcb3_motion_play (from rcb3.h), and method to keep com ports unused
 *
 * advantages versus rcb3_motion_play:
 *  com port on kcb-1 is free
 *  com port on rcb-3 is free
 *  this will actually work (becuase rcb3_motion_play is broken on non-updated rcb)
 *  
 * disadvantages versus rcb3_motion_play:
 *  with no bidirectional com port, cant do sophisticated commands or ask for data from rcb-3
 *  sio2 becomes occupied on kcb-1
 *  adX becomes occupoed on kcb-1, X is the port connected to 
 *  low speed com becomes used on rcb-3, if a transmitter/receiver is used this becomes complex
 */
 
#ifndef _RCB3_SIO2
#define _RCB3_SIO2

#include <krc.h>
#include <ad.h>
#include <sio.h>
#include <uart.h>

#ifndef MOTION_END_AD_PORT
    #define MOTION_END_AD_PORT 7
#endif

BYTE krc3buf[8];
//BYTE krt3buf[8];

void rcb_scenario_wait_ad(BYTE port=MOTION_END_AD_PORT);
void rcb_open_sio2();
void rcb_set_command_sio2(unsigned int code, BYTE PA1=0, BYTE PA2=0, BYTE PA3=0, BYTE PA4=0);
void rcb_send_command_sio2(_Bool prev_wait);
// do_command combines set+send
void rcb_do_command_sio2(unsigned int code, _Bool prev_wait=FALSE, BYTE PA1=0, BYTE PA2=0, BYTE PA3=0, BYTE PA4=0);
void rcb_tx_sio2(BYTE *data, size_t size);

void rcb_open_sio2(/*int startScenario*/)
{
	uart2_asyncmode(BR2400, 8, 1, PARITY_NONE, TRUE);
	iopol_u2mr = 1;			// reverse the polarity of the uart2 signal
	uart2_send_start(); 
}

// waits for a motion scenario on the rcb-3 to finish before returning.
// AD7 white wire of kcb-1 should be connected to white wire of servo 22 output of rcb-3.
// the motion scenario running on the rcb-3 must correctly set the HI/LO of motion scenario at start/end.
void rcb_scenario_wait_ad(BYTE port)
{
	while (ad_read(port) > 500) {}
}

void rcb_tx_sio2(BYTE *data, size_t size)
{
	unsigned int i;

	for (i = 0; i < size; i++) {
		wait (50); 
		uart2_putchar (*(data + i)); 
	}
}

/*
void rcb_sio2_krt_rx()
{
	uart2_rx(krt3buf, 8);
}

void fwd_krc3ad()
{
	rcb_sio2_krt_rx();
	rcb_tx_sio2(krt3buf, 8);
}
*/

// sends a command over the SIO1 of the kcb-1 to rcb-3.
// command is sent KRC3-AD style.
// white wire of SIO2 of kcb-1 should be connected to RX port of low speed serial of rcb-3.
// if wait is true, we wait for any previous operation to complete first
void rcb_send_command_sio2(_Bool prev_wait)
{
	if (prev_wait)
		rcb_scenario_wait_ad();

	//uart2_tx (krc3buf, 8);
	rcb_tx_sio2(krc3buf, 8);
	wait (20000);
	//uart2_tx (krc3buf, 8);
	rcb_tx_sio2(krc3buf, 8);
}

// コントロール入力値(2Byte)
// アナログ入力１(0 ~ 127：省略可)
void rcb_set_command_sio2(unsigned int code, BYTE PA1, BYTE PA2, BYTE PA3, BYTE PA4) 
{
	BYTE i;
	krc3buf[0] = 0x80;
	krc3buf[1] = code >> 8; // 上8ビット
	krc3buf[2] = code;      // 下8ビット
	krc3buf[3] = PA1;       // アナログ値
	krc3buf[4] = PA2;       // アナログ値
	krc3buf[5] = PA3;       // アナログ値
	krc3buf[6] = PA4;       // アナログ値
	krc3buf[7] = 0; // Check Sum
	
	for (i = 0; i < 7; i++)
		krc3buf[7] += krc3buf[i];
	
	krc3buf[7] &= 0x7F;
}

// コントロール入力値(2Byte)
// アナログ入力１(0 ~ 127：省略可)
// TODO: convert this function into a macro
void rcb_do_command_sio2(unsigned int code, _Bool prev_wait, BYTE PA1, BYTE PA2, BYTE PA3, BYTE PA4) 
{
	rcb_set_command_sio2(code, PA1, PA2, PA3, PA4);
	rcb_send_command_sio2(prev_wait);
}

/* these are probably impossible in this api...
void rcb_scenario_abort_sio2();
void rcb_scenario_pause_sio2();
void rcb_scenario_resume_sio2();
*/

#endif

/*
Copyright (c) 2009, Taylor Veltrop
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * 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.
    * Neither the name of the copyright holder 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 ''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 HOLDERS 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.
*/