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


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

#define CHAIN_LEN 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 {
    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;
} CHAIN_VARS;

int
ChainCal::run(void *rbuf, void *wbuf)
{
    CHAIN_VARS* property = (CHAIN_VARS*)rbuf;
    int id = get_param(0);
    
    //CHAIN_VARS* o_property = (CHAIN_VARS*)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;
		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;
    }
    
    return 0;
}