#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 float m = 100.0;
static float k = 7000.0;
static float g = 9.8;
static float dt = 0.003;
static float chain_width = 10;
static float 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;
    //property[1].can_move = 0;

    for(int cnt = 0; cnt < 600; cnt++) {
	for(int i = 0; i < PROPERTY_LENGTH; i++) {
	    if(property[i].can_move) {
#if 0
		vector float v_x1 __attribute__((aligned(16))) = { property[i-1].xyz[0],
								   property[i].xyz[0],
								   property[i+1].xyz[0], 
								   property[i+2].xyz[0]};
		vector float v_x2 __attribute__((aligned(16))) = { property[i].xyz[0],
								   property[i+1].xyz[0],
								   property[i+2].xyz[0],
								   property[i+3].xyz[0]};
		vector float v_dx __attribute__((aligned(16))) = spu_sub(v_x1, v_x2);

		vector float v_y1 __attribute__((aligned(16))) = { property[i-1].xyz[1],
								   property[i].xyz[1],
								   property[i+1].xyz[1], 
								   property[i+2].xyz[1]};
		vector float v_y2 __attribute__((aligned(16))) = { property[i].xyz[1],
								   property[i+1].xyz[1],
								   property[i+2].xyz[1],
								   property[i+3].xyz[1]};
		vector float v_dy __attribute__((aligned(16))) = spu_sub(v_x1, v_x2);

		dx = spu_mule(dx, dx);
		dy = spu_mule(dy, dy);
		vector float v_l __attribute__((aligned(16)));
		
#else
		float dx = property[i-1].xyz[0] - property[i].xyz[0];
		float dy = property[i-1].xyz[1] - property[i].xyz[1];
		float l = sqrt(dx * dx + dy * dy);
		float a = k * (l - chain_width) / m;
		float ax = a * dx / l;
		float 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];
	    }
#endif
	}

	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;
}