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

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

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

typedef struct {
    float xyz[3];
    float angle[3];
    float stack_xyz[3];
    float next_xyz[3];
    float v_xyz[3];
    float next_v_xyz[3];
    int property_index;
    int parent_index;
    int have_parent;
    int can_move;
    memaddr parent;
    memaddr children;
    memaddr node;
    int sgid;
} *PropertyPtr, Property;

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

    property[0].can_move = 0;

    for(int cnt = 0; cnt < 600; cnt++) {
	for(int i = 0; i < PROPERTY_LENGTH; i++) {
	    if(property[i].can_move) {
		double dx = property[i-1].xyz[0] - property[i].xyz[0];
		double dy = property[i-1].xyz[1] - property[i].xyz[1];
		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 < PROPERTY_LENGTH - 1) {
		    dx = property[i+1].xyz[0] - property[i].xyz[0];
		    dy = property[i+1].xyz[1] - property[i].xyz[1];
		    l = sqrt(dx * dx + dy * dy);
		    a = k * (l - chain_width) / m;
		    ax += a * dx / l;
		    ay += a * dy / l;
		}
		ay += g;
		property[i].v_xyz[0] *= safe;
		property[i].v_xyz[1] *= safe;
		property[i].next_v_xyz[0] = property[i].v_xyz[0] + ax * dt;
		property[i].next_v_xyz[1] = property[i].v_xyz[1] + ay * dt;
		property[i].next_xyz[0] = property[i].xyz[0] + property[i].v_xyz[0] * dt;
		property[i].next_xyz[1] = property[i].xyz[1] + property[i].v_xyz[1] * dt;
	    } else {
		property[i].next_xyz[0] = property[i].xyz[0];
		property[i].next_xyz[1] = property[i].xyz[1];
	    }
	}
	for(int i = 0; i < PROPERTY_LENGTH; i++) {
	    property[i].v_xyz[0] = property[i].next_v_xyz[0];
	    property[i].v_xyz[1] = property[i].next_v_xyz[1];
	    property[i].xyz[0] = property[i].next_xyz[0];
	    property[i].xyz[1] = property[i].next_xyz[1];
	}
    }

    for(int i = 0; i < PROPERTY_LENGTH; i++) {
	int id = property[i].property_index;
	int p, n;	
	p = n = id;
	if(p != 0) {
	    p--;
	}
	if(n != PROPERTY_LENGTH - 1) {
	    n++;
	}
	property[i].angle[2-(id%2)*2]
	    = 90 + atan((property[p].next_xyz[1] - property[n].next_xyz[1])
			/ (property[p].next_xyz[0] - property[n].next_xyz[0])) * 180 / M_PI;
    }

    memcpy((void*)update_property, (void*)property, sizeof(Property)*PROPERTY_LENGTH);

    return 0;
}