#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>

#define DEG2RAD ((3.14159265358979f * 2.0f) / 360.0f)

#include "planet.h"

void update_rotation(planet* p, double time) {
	double rotationState;
	if(p->isTidallyLocked) rotationState = time / p->orbitalPeriod;
	else rotationState = time / p->rotationPeriod;
	rotationState -= floor(rotationState);
	rotationState *= 360.0;
	p->axisRotation = rotationState;
}

void update_planet(planet* p, double time) {
	if(!p->parent) {
		p->posX = p->posY = p->posZ = p->absX = p->absY = p->absZ = 0;
		goto update_rotation_and_children;
	}
	if(!p->initialized) {
		p->orbitalPeriod = 2 * 3.14159265358979 * sqrt((p->SMA * p->SMA * p->SMA) / (BIG_G * ((planet*)p->parent)->mass));
		if(p->isTidallyLocked) p->rotationPeriod = p->orbitalPeriod;
		
		double inRads = p->AOP * DEG2RAD;
		double Cw = cos(inRads);
		double Sw = sin(inRads);
		inRads = p->LAN * DEG2RAD;
		double co = cos(inRads);
		double so = sin(inRads);
		inRads = p->inclination * DEG2RAD;
		double ci = cos(inRads);
		double si = sin(inRads);
		double swci = Sw * ci;
		double cwci = Cw * ci;
		p->pX = Cw * co - so * swci;
		p->pY = Cw * so + co * swci;
		p->pZ = Sw * si;
		p->qX = -Sw * co - so * cwci;
		p->qY = -Sw * so + co * cwci;
		p->qZ = Cw * si;
		
		p->e2 = p->SMA * sqrt(1 - p->eccentricity * p->eccentricity);
		p->totalMass = ((planet*)p->parent)->mass + p->mass;
		p->sqrtGravParamOverSMA = sqrt((BIG_G * p->totalMass) / p->SMA);
		
		p->initialized = 1;
	}
	
	double orbitState = time / p->orbitalPeriod + p->anomalyAtEpoch / 360.0;
	orbitState -= floor(orbitState);
	double meanAnomaly = orbitState * TAU;
	
	double eca = meanAnomaly + p->eccentricity / 2.0;
	double diff = 10000;
	double e1 = 0;
	
	while(1) {
		e1 = eca - (eca - meanAnomaly - p->eccentricity * sin(eca)) / (1 - p->eccentricity * cos(eca));
		diff = e1 - eca;
		if(diff < 0) diff = -diff;
		eca = e1;
		if(diff <= EPS) break;
	}
	
	double ceca = cos(eca);
	double seca = sin(eca);
	double xw = p->SMA * (ceca - p->eccentricity);
	double yw = p->e2 * seca;
	
	p->posX = xw * p->pX + yw * p->qX;
	p->posY = xw * p->pY + yw * p->qY;
	p->posZ = xw * p->pZ + yw * p->qZ;
	p->absX = p->posX + ((planet*)p->parent)->absX;
	p->absY = p->posY + ((planet*)p->parent)->absY;
	p->absZ = p->posZ + ((planet*)p->parent)->absZ;
	
update_rotation_and_children:
	update_rotation(p, time);
	for(int i = 0; i < p->child_count; i++) update_planet(((planet*)p->children) + i, time);
}
