#include <stdio.h>
#include <string.h>
#include <math.h>
#include "ChainCal.h"
#include "Func.h"
#include "types.h"

/* これは必須 */
SchedDefineTask(ChainCal);

#define CHAIN_LEN	50

static const    double	m	    = 100.0;
static const    double	k	    = 7000.0;
static const    double	g	    = 9.8;
static const    double	dt	    = 0.003;
static const    double	chain_width = 10;
static const    double	safe	    = 0.995;

typedef struct {
    double	x, y, next_x, next_y;
    double	vx, vy, next_vx, next_vy;
    double	angle[3];
    int		can_move;
    uint32	parent;
    int		id;
    //int parent;
} ChainProperty, *ChainPropertyPtr;

static int
run(SchedTask *s,void *rbuf, void *wbuf)
{
    ChainPropertyPtr	property	= (ChainPropertyPtr)s->get_input(rbuf, 0);
    ChainPropertyPtr	update_property = (ChainPropertyPtr)s->get_output(wbuf, 0);

//    ChainPropertyPtr property = (ChainPropertyPtr)rbuf;
//    int id			= get_param(0);
    
    //ChainPropertyPtr o_property = (ChainPropertyPtr)wbuf;
    
    for(int cnt = 0; cnt < 600; cnt++) {
	for(int i = 0; i < CHAIN_LEN; i++) {
	    if(property[i].can_move) {
		double	dx  = property[i-1].x - property[i].x;
		double	dy  = property[i-1].y - property[i].y;
		double	l   = sqrt(dx * dx + dy * dy);
		double	a   = k * (l - chain_width) / m;
		double	ax  = a * dx / l;
		double	ay  = a * dy / l;
		if(i < CHAIN_LEN - 1) {
		    dx  = property[i+1].x - property[i].x;
		    dy  = property[i+1].y - property[i].y;
		    l   = sqrt(dx * dx + dy * dy);
		    a   = k * (l - chain_width) / m;
		    ax += a * dx / l;
		    ay += a * dy / l;
		}
		ay		    += g;
		property[i].vx	    *= safe;
		property[i].vy	    *= safe;
		property[i].next_vx  = property[i].vx + ax *		dt;
		property[i].next_vy  = property[i].vy + ay *		dt;
		property[i].next_x   = property[i].x + property[i].vx *	dt;
		property[i].next_y   = property[i].y + property[i].vy *	dt;
	    } else {
		property[i].next_x   = property[i].x;
		property[i].next_y   = property[i].y;
	    }
	}
	for(int i = 0; i < CHAIN_LEN; i++) {
	    property[i].vx = property[i].next_vx;
	    property[i].vy = property[i].next_vy;
	    property[i].x  = property[i].next_x;
	    property[i].y  = property[i].next_y;
	}
    }
	
    for (int j = 0; j < CHAIN_LEN; j++) {
	int	p, n;
	int	id = property[j].id;
	p	   = n = id;
	if(p != 0) {
	    p--;
	}
	if(n != CHAIN_LEN - 1) {
	    n++;
	}
	property[j].angle[2-(id%2)*2]
	    = 90 + atan((property[p].next_y - property[n].next_y) / (property[p].next_x - property[n].next_x)) * 180 / M_PI;
    }

    memcpy((void*)update_property, (void*)property, sizeof(ChainProperty) *	CHAIN_LEN);    
    return 0;
}