/*	$OpenBSD$	*/
/*
 * Copyright (c) 2007 Hypertriton, Inc. <http://www.hypertriton.com/>
 * 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.
 * 
 * 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 AUTHOR 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.
 */

/*
 * Quintic velocity profile generation.
 */

#include <sys/param.h>
#include <sys/queue.h>
#include <sys/ioctl.h>
#include <sys/fcntl.h>
#include <sys/vnode.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/device.h>

#include <sys/gpio.h>
#include <sys/cnc.h>

#include <dev/gpio/gpiovar.h>

#include "cnc_devicevar.h"
#include "cncvar.h"

/*
 * Initialize constants to values that will yield an optimal time-energy
 * velocity profile under acceleration and jerk constraints.
 */
int
cnc_quintic_init(struct cnc_quintic_profile *q, const struct cnc_insn *insn,
    cnc_real_t L)
{
	cnc_real_t F = ((cnc_real_t)insn->i_F);
	cnc_real_t Amax = ((cnc_real_t)insn->i_Amax);
	cnc_real_t Jmax = ((cnc_real_t)insn->i_Jmax);
	cnc_real_t k;

	q->F = F;

//	if (F >= CNC_PI_2*((Amax*Amax)/Jmax) ) {
		/*
		 * Feedrate F is achievable by using maximum acceleration
		 * and maximum jerk.
		 */
		if (L >= ( (L*L)/Amax + CNC_PI_2*((Amax*F)/Jmax) )){
			/* Feedrate F is achievable at Amax acceleration. */
			printf("case 1.1\n");
			q->Ts = CNC_PI_2*(Amax/Jmax);
			q->Ta = (F/Amax) + q->Ts;
			q->To = L/F;
		} else if (L <  ( (F*F)/Amax + CNC_PI_2*((Amax*F)/Jmax) ) &&
	      		   L >= ( (CNC_PI*CNC_PI)/2.0) * (Amax*Amax*Amax) /
			                                     (Jmax*Jmax) ) {
			/* Feedrate F is unachievable, Amax is achievable. */
			printf("case 1.2\n");
			q->Ts = CNC_PI_2*(Amax/Jmax);
			q->Ta = ( (-CNC_PI*(Amax*Amax) +
			        cnc_sqrt((CNC_PI*CNC_PI)*(Amax*Amax*Amax*Amax) +
			                 16.0*Amax*(Jmax*Jmax)*L) ) /
			      (4.0*Jmax*Amax) ) + q->Ts;
			q->To = q->Ta;
		} else if (L <= ( ((CNC_PI*CNC_PI)/2.0)*(Amax*Amax*Amax) /
		                                            (Jmax*Jmax) )) {
			/* Neither F nor Amax are achievable. */
			printf("case 1.3\n");
			q->Ts = cnc_cbrt( (CNC_PI*L)/(4.0*Jmax) );
			q->Ta = 2.0*q->Ts;
			q->To = q->Ta;
//		} else {
//			printf("MOVE: Illegal parameters (1)\n");
//			return (-1);
		}
//	} else if (F < CNC_PI_2*((Amax*Amax)/Jmax) ) {
		/*
		 * Feedrate F is achievable without using maximum
		 * acceleration.
		 */
		if (L >= cnc_sqrt( (2.0*CNC_PI*(F*F*F))/Jmax ) ) {
			/* F is achievable, Amax is unachievable. */
			printf("case 2.1\n");
			q->Ts = cnc_sqrt( (CNC_PI*F)/(2.0*Jmax) );
			q->Ta = 2.0*q->Ts;
			q->To = L/F;
		} else if (L < cnc_sqrt( (2.0*CNC_PI*F*F*F)/Jmax )) {
			/* Neither F nor Amax are achievable. */
			printf("case 2.2\n");
			q->Ts = cnc_cbrt( (CNC_PI*L)/(4.0*Jmax) );
			q->Ta = 2.0*q->Ts;
			q->To = q->Ta;
		} else {
			printf("MOVE: Illegal parameters (2)\n");
			return (-1);
		}
//	}

	k = CNC_PI/(2.0*q->Ts);
	q->Aref = L/(q->Ta - q->Ts) * q->To;
	q->v0 = ((cnc_real_t)insn->i_v0);
	q->v1 = (q->Aref/2.0) * (q->Ts - cnc_sin(2.0*k*q->Ts)/(2.0*k) );
	q->v2 = q->Aref*(q->Ta - 2.0*q->Ts) + q->v1;

	printf("MOVE: ");
	printf("F=%s ", cnc_fmt_real(F));
	printf("v1=%s ", cnc_fmt_real(q->v1));
	printf("v2=%s ", cnc_fmt_real(q->v2));
	printf("v3=%s ", cnc_fmt_real(q->v1+q->v2));
	printf("Amax=%s ", cnc_fmt_real(Amax));
	printf("Jmax=%s ", cnc_fmt_real(Jmax));
	printf("Ta=%s ", cnc_fmt_real(q->Ta));
	printf("Ts=%s ", cnc_fmt_real(q->Ts));
	printf("To=%s\n", cnc_fmt_real(q->To));
	return (0);
}

cnc_real_t
cnc_quintic_step(const struct cnc_quintic_profile *q, cnc_real_t t)
{
	cnc_real_t t2, t5, t6;
	cnc_real_t k = CNC_PI/(2.0*q->Ts);
	
	t2 = q->Ta - q->Ts;
	t5 = q->To + q->Ts;
	t6 = q->To + q->Ta - q->Ts;

	if (t <= q->Ts) {
		return (q->Aref/2.0)*( t - cnc_sin(2.0*k*t)/(2.0*k) );
	} else if (t <= (q->Ta - q->Ts)) {
		return q->Aref*(t - q->Ts) + q->v1;
	} else if (t <= q->Ta) {
		return (q->Aref/2.0)*( (t-t2) + cnc_sin(2.0*k*(t-t2))/(2.0*k) )
		        + q->v2;
	} else if (t <= q->To) {
		return (q->v1 + q->v2);
	} else if (t <= t5) {
		return -(q->Aref/2.0) *
		         ( (t - q->To) - cnc_sin(2.0*k*(t - q->To))/(2.0*k) )
		       + (q->v1 + q->v2);
	} else if (t <= t6) {
		return -q->Aref*(t-t5) + q->v2;
	} else {
		return -(q->Aref/2.0)*( (t-t6) + cnc_sin(2.0*k*(t-t6))/(2.0*k) )
		        + q->v1;
	}
}
