/*
 * $Id: protocol.c 198 2005-10-01 14:20:12Z anders $
 * 
 * Copyright (C) 2005 Anders Dubgaard <anders@dubgaard.net>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "protocol.h"
#include <uart.h>
#include <psx.h>

/* Updates the package from the supplid data struct with new joystick
 * data and calculates the CRC checksum - returns a pointer to the package.
 */
volatile uint8_t *update_package(volatile psx_data *psx_pkg) {
	int n;
	uint8_t *psx_arr = (uint8_t *) psx_pkg;

  /* Create the package (preamble and SOF is done in package_init()
	 * 
	 * Add data to the package
	 */
	for(n = 0; n < PSX_DATA_LEN; n++) {
		package[n + HEADER_LEN] = psx_arr[n];
	}

	/* Calculate CRC16 sum
	 * - thanks to Michael Barr in a three part series at
	 *   http://www.embedded.com/2000/0001/0001connect.htm
	 */
	crc crc_val = crcSlow(psx_arr, PSX_DATA_LEN);
	uint8_t *crcp = (uint8_t *) &crc_val;

#ifdef _DEBUG
  printf("CRC: 0x%4X. ", crc_val);
#endif

	/* Add CRC16 sum to the package - low byte lowest.
   */
	for(n = 0; n < CRC_LEN; n++) {
		package[n + HEADER_LEN + PSX_DATA_LEN] = crcp[n];
	}
	return package;
}

/* Store joystick from the radio package in the psx_data array.
 */
void pkg2psxdata(volatile psx_data *psxp){
  uint8_t n;
	uint8_t *psx_arr = (uint8_t *) psxp;
  
	for(n = 0; n < PSX_DATA_LEN; n++) {
		psx_arr[n] = package[n + HEADER_LEN];
	}
}

/* Updates the supplid data struct with new joystick data and
 * checks the CRC checksum, and returns the result of the CRC calculation.
 */
int check_package_crc(volatile psx_data *psxp){
  crc crc_pkg = 0;  /* received CRC value */
	uint8_t *crcp = (uint8_t *) &crc_pkg;
	uint8_t *psx_arr = (uint8_t *) psxp;

  /* Read CRC from package */
  crcp[0] = package[11];
  crcp[1] = package[12];

  /* Read joystick data from package */
  pkg2psxdata(psxp);

  /* Calculate CRC or the package */
	crc crc_val = crcSlow(psx_arr, PSX_DATA_LEN);

#ifdef _DEBUG
  if(crc_val == crc_pkg) {
    printf("CRC: 0x%4X. ", crc_val);
  }
  else {
    printf("\n\r CRC_ERROR!! ");
    printf("CRC: Package said 0x%X, calculation was 0x%X\n\r",
        crc_pkg, crc_val);
  }
#endif

  return (crc_val == crc_pkg)? 1: 0;
}

/* Insert preamble and Start-of-Frame into the package.
 * It's a quick hack because of the endian-ness.
 */
void package_init(void) {
  package[0] = (uint8_t) (PREAMBLE>>8);
  package[1] = (uint8_t) PREAMBLE;
  package[2] = (uint8_t) (SOF>>8);
  package[3] = (uint8_t) SOF;
  package[4] = 0;  /* psx->type     */
  package[5] = 0;  /* psx->buttons  */
  package[6] = 0;  /* psx->buttons2 */
  package[7] = 0;  /* psx->r_horz   */
  package[8] = 0;  /* psx->r_vert   */
  package[9] = 0;  /* psx->l_horz   */
  package[10] = 0; /* psx->l_vert   */
  package[11] = 0; /* CRC high byte */
  package[12] = 0; /* CRC low byte  */
}