#ifndef ADAFRUIT_ROCKET_TEST_ENV_HPP
#define ADAFRUIT_ROCKET_TEST_ENV_HPP

//ADAFRUIT_ROCKET_TEST_ENV_IMPL needs to be defined where the implementation needs to be stored.

/*Example usage:

int main() {

  world.init();
  world.setTargetTime(30000.0f); //Run the simulation for at maximum N ms

  setup();

  while (!world.stopped) {
    //world.rocket.debugPrint();
    world.tick();
    loop();
  }

  logFile.dumpToFile("log.txt");
  return 0;
}
//Where loop() and setup() are esp functionality implementations.

*/

#include <vector>
#include <tuple>
#include <string>
#include <fstream>
#include <iostream>
#include <cmath>
#include <cstdlib>

constexpr static const int OUTPUT = 1;
constexpr static const int LOW = 0;
constexpr static const int HIGH = 1;

constexpr int FILE_WRITE = 0;

constexpr int MPU6050_RANGE_2_G = 0;
constexpr int MPU6050_RANGE_4_G = 1;
constexpr int MPU6050_RANGE_8_G = 2;
constexpr int MPU6050_RANGE_16_G = 3;

constexpr int MPU6050_RANGE_250_DEG = 0;
constexpr int MPU6050_RANGE_500_DEG = 1;
constexpr int MPU6050_RANGE_1000_DEG = 2;
constexpr int MPU6050_RANGE_2000_DEG = 3;

constexpr int MPU6050_BAND_260_HZ = 0;
constexpr int MPU6050_BAND_184_HZ = 1;
constexpr int MPU6050_BAND_94_HZ = 2;

constexpr int MPU6050_HIGHPASS_DISABLE = 0;

//Transpiled from Javascript.
class Rocket {
    public:
    // Physical constants
    float rho;              // Air density
    float g;                // Gravity
    
    // Simulation state variables
    float t;                // Current time
    float v;                // Current velocity
    float h;                // Current height
    float mass;             // Current mass
    float mass_flow_rate;   // Mass flow rate during burn
    float remaining_chute_delay;
    bool chute_open;
    float vt;               // Terminal velocity with parachute
    bool simulation_active;

    public:
    float apogee;
    float flight_time;
    float ground_hit_velocity;
    float apogee_time;
    bool has_apogee;
    float currentAltitude;

    // Input parameters (these can be set by user)
    float input_mass_dry;
    float input_thrust;
    float input_burn_time;
    float input_mass_prop;
    float input_cd;
    float input_area;
    bool input_has_parachute;
    float input_chute_area;
    float input_chute_delay;
    float input_gravity;
    float input_air_density;
	
	float a;

    //Constructor
    Rocket(
        float mass_dry        = 1.0f,
        float thrust          = 36.0f,
        float burn_time       = 2.1f,
        float mass_prop       = 0.06f,
        float cd              = 0.75f,
        float area            = 0.00212372f,
        bool  has_parachute   = true,
        float chute_area      = 0.2f,
        float chute_delay     = 2.0f,
        float gravity         = 9.81f,
        float air_density     = 1.225f
    );

    void set_mass_dry(float m);
    void set_thrust(float th);
    void set_burn_time(float bt);
    void set_mass_propellant(float mp);
    void set_cd(float c);
    void set_area(float a);
    void set_has_parachute(bool hp);
    void set_chute_area(float ca);
    void set_chute_delay(float cd);
    void set_gravity(float gr);
    void set_air_density(float ad);

    void start_simulation();
	
	void debugPrint() const;

    void simulate_tick(float dt);

    bool is_simulating() const;

};

class World{
	public:
		float surfacePressure;
		float baseTemperature;
		float defaultHumidity;

		bool stopped = false;
		
		float dt = 0.0;
		float g = 9.81f; //Gravity in m/s²
		float rho = 1.225f; //Air density at sea level in kg/m³

		float elapsedTime = 0.0f; //MS
		float lastTime = 0.f; 
		float targetTime = -1.0f;
		
		Rocket rocket;

		void setTargetTime(float t);

		void init();
		void tick();
		
		World(float seaLevelPressure,float baseTemperature,float defaultHumidity);
		World();
		
		float readPressure();
		float readPressureWithNoise();

		float readPressureAtAltitude(float altitude);
		float readTemperature();
		float readTemperatureWithNoise();
		float readTemperatureAtAltitude(float altitude);

		float readHumidity();
		float readHumidityWithNoise();
		float readHumidityAtAltitude(float altitude);

};

class Adafruit_BME280{
	public:
		bool begin();
		float readHumidity();
		float readTemperature();
		float readPressure();

};

struct acceleration {
    float x;
    float y;
    float z;
};
struct gyro {
    float x;
    float y;
    float z;
};

typedef struct {
    struct acceleration acceleration;
    struct gyro gyro;

    float temperature;
} sensors_event_t;

class Adafruit_MPU6050{
	public:
		bool begin();
		
		void getEvent(sensors_event_t* a, sensors_event_t* g, sensors_event_t* temp);
		
		void setAccelerometerRange(int range);
		void setGyroRange(int range);
		void setFilterBandwidth(int bandwidth);
		void setHighPassFilter(int filter);

};

void delay(int ms);

class File{ //ESP file
	public:
		std::vector<std::string> lines;
		
		std::string cur_line = "";

		//Dump it to a normal txt file
		void dumpToFile(const std::string& filename);

		void println(const std::string& msg);

		void println(const char* msg);
		void print(const std::string& msg);
		void print(float num);

		void flush();
		operator bool() const;
};

class SerialClass{
	public:
		void println(const char* msg);
		void println(const std::string& msg);
};

class SDClass{
	public:
		bool begin(int csPin);
		File open(const std::string& filename, int mode);
};

void pinMode(int pin, int mode);
void digitalWrite(int pin, int value);

static World world(101325.0f, 24.0f, 50.0f);
static SerialClass Serial;
static SDClass SD;

#ifdef ADAFRUIT_ROCKET_TEST_ENV_IMPL

Rocket::Rocket(
        float mass_dry,
        float thrust,
        float burn_time,
        float mass_prop,
        float cd,
        float area,
        bool  has_parachute,
        float chute_area,
        float chute_delay,
        float gravity,
        float air_density
    )
        : input_mass_dry(mass_dry),
          input_thrust(thrust),
          input_burn_time(burn_time),
          input_mass_prop(mass_prop),
          input_cd(cd),
          input_area(area),
          input_has_parachute(has_parachute),
          input_chute_area(chute_area),
          input_chute_delay(chute_delay),
          input_gravity(gravity),
          input_air_density(air_density),

          apogee(0.0f),
          flight_time(0.0f),
          ground_hit_velocity(0.0f),
          apogee_time(0.0f),
          has_apogee(false),
          currentAltitude(0.0f),

          mass(mass_dry + mass_prop),
          mass_flow_rate(mass_prop / burn_time),
          remaining_chute_delay(0.0f),
          chute_open(false)
    {
		a = 0.0f;
		simulation_active = false;
	}

    // Setter methods for input parameters
    void Rocket::set_mass_dry(float m) { input_mass_dry = m; }
    void Rocket::set_thrust(float th) { input_thrust = th; }
    void Rocket::set_burn_time(float bt) { input_burn_time = bt; }
    void Rocket::set_mass_propellant(float mp) { input_mass_prop = mp; }
    void Rocket::set_cd(float c) { input_cd = c; }
    void Rocket::set_area(float a) { input_area = a; }
    void Rocket::set_has_parachute(bool hp) { input_has_parachute = hp; }
    void Rocket::set_chute_area(float ca) { input_chute_area = ca; }
    void Rocket::set_chute_delay(float cd) { input_chute_delay = cd; }
    void Rocket::set_gravity(float gr) { input_gravity = gr; }
    void Rocket::set_air_density(float ad) { input_air_density = ad; }

void Rocket::start_simulation() {

    g = input_gravity;
    rho = input_air_density;

	if (g <= 0.0f || rho <= 0.0f) {
        apogee = 0.0f;
        ground_hit_velocity = 0.0f;
        flight_time = t;
        simulation_active = false;
        return;
    }
    
    // Initial conditions
    t = 0.0f;                                      // Time
    v = 0.0f;                                      // Velocity
    h = 0.0f;                                      // Height
	a = 0.0f;
    mass = input_mass_dry + input_mass_prop;      // Total mass
    mass_flow_rate = input_mass_prop / input_burn_time;  // kg/s
    has_apogee = false;
    remaining_chute_delay = input_chute_delay;
    chute_open = false;
    
    // Terminal velocity with parachute
    vt = std::sqrt((mass * g * 2.0f) / (rho * input_cd * input_chute_area));
    ground_hit_velocity = vt;
    
    simulation_active = true;
}
	
void Rocket::debugPrint() const {
	std::cout << "========== ROCKET DEBUG ==========" << std::endl;
	std::cout << "=== Simulation State ===" << std::endl;
	std::cout << "  Time (t):              " << t << " s" << std::endl;
	std::cout << "  Height (h):            " << h << " m" << std::endl;
	std::cout << "  Velocity (v):          " << v << " m/s" << std::endl;
	std::cout << "  Acceleration (a):      " << a << " m/s²" << std::endl;
	std::cout << "  Mass:                  " << mass << " kg" << std::endl;
	std::cout << "  Simulation Active:     " << (simulation_active ? "YES" : "NO") << std::endl;
	std::cout << std::endl;
	
	std::cout << "=== Flight Status ===" << std::endl;
	std::cout << "  Apogee:                " << apogee << " m" << std::endl;
	std::cout << "  Apogee Time:           " << apogee_time << " s" << std::endl;
	std::cout << "  Has Reached Apogee:    " << (has_apogee ? "YES" : "NO") << std::endl;
	std::cout << "  Chute Open:            " << (chute_open ? "YES" : "NO") << std::endl;
	std::cout << "  Remaining Chute Delay: " << remaining_chute_delay << " s" << std::endl;
	std::cout << "  Terminal Velocity:     " << vt << " m/s" << std::endl;
	std::cout << "  Flight Time:           " << flight_time << " s" << std::endl;
	std::cout << "  Ground Hit Velocity:   " << ground_hit_velocity << " m/s" << std::endl;
	std::cout << std::endl;
	
	std::cout << "=== Physical Constants ===" << std::endl;
	std::cout << "  Gravity (g):           " << g << " m/s²" << std::endl;
	std::cout << "  Air Density (rho):     " << rho << " kg/m³" << std::endl;
	std::cout << "  Mass Flow Rate:        " << mass_flow_rate << " kg/s" << std::endl;
	std::cout << std::endl;
	
	std::cout << "=== Input Parameters ===" << std::endl;
	std::cout << "  Dry Mass:              " << input_mass_dry << " kg" << std::endl;
	std::cout << "  Propellant Mass:       " << input_mass_prop << " kg" << std::endl;
	std::cout << "  Thrust:                " << input_thrust << " N" << std::endl;
	std::cout << "  Burn Time:             " << input_burn_time << " s" << std::endl;
	std::cout << "  Drag Coefficient (cd): " << input_cd << std::endl;
	std::cout << "  Area:                  " << input_area << " m²" << std::endl;
	std::cout << "  Has Parachute:         " << (input_has_parachute ? "YES" : "NO") << std::endl;
	std::cout << "  Chute Area:            " << input_chute_area << " m²" << std::endl;
	std::cout << "  Chute Delay:           " << input_chute_delay << " s" << std::endl;
	std::cout << "  Input Gravity:         " << input_gravity << " m/s²" << std::endl;
	std::cout << "  Input Air Density:     " << input_air_density << " kg/m³" << std::endl;
	std::cout << "==================================" << std::endl;
}

void Rocket::simulate_tick(float dt) {

    if (!simulation_active) {
        return;
    }
    
    float current_thrust;
    if (t < input_burn_time) {
        current_thrust = input_thrust;
        mass -= mass_flow_rate * dt;
    } else {
        current_thrust = 0.0f;
    }
    
    float drag = 0.5f * rho * input_cd * input_area * (v * v);
    if (v > 0.0f) {
        drag *= 1.0f;
    } else {
        drag *= -1.0f;
    }
    
    // Acceleration (upwards positive)
    a = (current_thrust - drag - mass * g) / mass;
    
    // Update velocity
    if (chute_open) {
        v = -vt;  // Terminal velocity downward
    } else {
        v += a * dt;
    }
    
    // Update height
    h += v * dt;
    t += dt;
    
    // Check for apogee (velocity becomes negative after burn)
    if (v <= 0.0f && t > input_burn_time && !has_apogee) {
        apogee_time = t;
        apogee = h;
        has_apogee = true;
    }
    
    // Handle parachute deployment
    if (has_apogee && remaining_chute_delay > 0.0f && input_has_parachute) {
        remaining_chute_delay -= dt;
        if (!chute_open && remaining_chute_delay <= 0.0f) {
            chute_open = true;
        }
    }
    
    // Check for ground impact
	if (h <= 0.0f && v < 0.0f && chute_open == true) {
        if (!input_has_parachute) {
            ground_hit_velocity = std::abs(v);
        }
        flight_time = t;
        simulation_active = false;
        return;
    }
    
}

bool Rocket::is_simulating() const {
    return simulation_active;
}

void World::setTargetTime(float t){
	targetTime = t;
}

void World::init(){
	rocket.start_simulation();
}
void World::tick(){
	std::cout << "Tick: Elapsed Time: " << elapsedTime << " ms" << std::endl;
	float timeStep = elapsedTime - lastTime;
	lastTime = elapsedTime;

	dt = timeStep / 1000.0f;
	
	rocket.simulate_tick(dt);

	if(rocket.is_simulating()){
	} else {
		stopped = true;
		std::cout << "Stopped by rocket simulation landing." << std::endl;

	}

	//std::cout << "Time: " << elapsedTime << " ms, TT " << targetTime << " ms, Velocity: " << rocket.v << " m/s" << std::endl;
	if(elapsedTime >= targetTime && targetTime >= 0.0f){
		std::cout << "stop" << std::endl;
		stopped = true;
		std::cout << "Stopped by target time." << std::endl;

	}
}

World::World(float seaLevelPressure,float baseTemperature,float defaultHumidity) : surfacePressure(seaLevelPressure), baseTemperature(baseTemperature), defaultHumidity(defaultHumidity) {

}
World::World(){
	std::cerr << "Don't use this constructor for now!" << std::endl;
}

float World::readPressure() {
	return surfacePressure; // Return the surface pressure for testing
}
float World::readPressureWithNoise() {
	float noise = ((rand() % 2000) - 1000) / 1000.0f; // Random noise between -10 and +10
	return noise;
}

float World::readPressureAtAltitude(float altitude) {
	float base = 1.0f - (altitude / 44330.0f);
	base = std::max(base, 0.01f);

	return surfacePressure * pow(base, 5.255f) + readPressureWithNoise();
}
float World::readTemperature() {
	return baseTemperature;
}
float World::readTemperatureWithNoise() {
	float noise = ((rand() % 200) - 100) / 1000.0f; // Random noise between -1 and +1
	return baseTemperature + noise;
}
float World::readTemperatureAtAltitude(float altitude) {
	return readTemperatureWithNoise() - (altitude * 0.0065f); // Decrease temperature by 6.5°C per 1000m
}

float World::readHumidity() {
	return defaultHumidity;
}
float World::readHumidityWithNoise() {
	float noise = ((rand() % 200) - 100) / 100.0f; // Random noise between -1 and +1
	return defaultHumidity + noise;
}
float World::readHumidityAtAltitude(float altitude) {
	return readHumidityWithNoise() * pow(1 - (altitude / 44330.0f), 5.255f); // Decrease humidity with altitude
}

bool Adafruit_BME280::begin() {
	return true;
}
float Adafruit_BME280::readHumidity() {
	return world.readHumidityAtAltitude(world.rocket.h);
}
float Adafruit_BME280::readTemperature() {
	return world.readTemperatureAtAltitude(world.rocket.h);
}
float Adafruit_BME280::readPressure() {
	return world.readPressureAtAltitude(world.rocket.h); // Standard atmospheric pressure at sea level in Pascals
}

bool Adafruit_MPU6050::begin() {
	return true;
}

void Adafruit_MPU6050::getEvent(sensors_event_t* a, sensors_event_t* g, sensors_event_t* temp) {
	//Return world.rocket's acceleration and velocity as gyro data for testing
	a->acceleration.x = (rand() % 200 - 100) / 10000.0f; // Add some random noise to the acceleration
	a->acceleration.y = (rand() % 200 - 100) / 10000.0f; // Add some random noise to the acceleration
	a->acceleration.z = world.rocket.a+ 9.81f; // Vertical acceleration
	g->gyro.x = 0.0f; // No rotation in this simplified model
	g->gyro.y = 0.0f; // No rotation in this simplified model
	g->gyro.z = 0.0f; // No rotation in this simplified model
	temp->temperature = world.readTemperatureAtAltitude(world.rocket.h); // Temperature at current
}

void Adafruit_MPU6050::setAccelerometerRange(int range) {
	
}
void Adafruit_MPU6050::setGyroRange(int range) {
	
}
void Adafruit_MPU6050::setFilterBandwidth(int bandwidth) {
	
}
void Adafruit_MPU6050::setHighPassFilter(int filter) {
	
}

void delay(int ms) {
    world.elapsedTime += ms;
}

void File::dumpToFile(const std::string& filename) {
	std::ofstream outFile(filename);
	for (const auto& line : lines) {
		outFile << line << std::endl;
	}
	outFile.close();
}

void File::println(const std::string& msg) {
	cur_line += msg;
	lines.push_back(cur_line);
	cur_line="";
}

void File::println(const char* msg) {
	cur_line += msg;
	lines.push_back(cur_line);
	cur_line="";
}

void File::print(const std::string& msg) {
	cur_line += msg;
}

void File::print(float num) {
	cur_line += std::to_string(num);
}

void File::flush() {
	
}

File::operator bool() const {
	return true;
}

void SerialClass::println(const char* msg) {
	std::cout << msg << std::endl;
}

void SerialClass::println(const std::string& msg) {
	std::cout << msg << std::endl;
}

bool SDClass::begin(int csPin) {
	return true;
}

File SDClass::open(const std::string& filename, int mode) {
	return File();
}

void pinMode(int pin, int mode) {
	
}

void digitalWrite(int pin, int value) {
	
}

#endif //END IMPL

#endif //END HPP