#include "simulation.h"
#include <cmath>
#include <ctime>
#include <cstdlib>

#define PI 3.1416
#define MAX_ANGULAR_SPEED 1
#define MAX_SPEED 3

Simulation::Simulation()
{
    std::srand(std::time(NULL));
}

std::vector<InitClientPacket>* Simulation::getWorldData()
{
    return &worldData;
}

/**
 * @brief Simulation::addClient is only called server-side
 */
void Simulation::addClient()
{
    InitClientPacket info;
    info.x = std::rand()%50;
    info.y = std::rand()%50;
    info.angle = ((float)(std::rand()%360)) / PI;
    info.vforward = 0;
    info.vangle = 0;
    worldData.push_back(info);
}

/**
 * @brief Simulation::removeClient is only called server-side
 */
void Simulation::removeClient(int i)
{
    worldData[i] = worldData.back();
    worldData.pop_back();
}

void Simulation::clientInput(int i, const DeltaClientPacket &delta)
{
    InitClientPacket *info = &(worldData[i]);
    info->vangle += delta.dvangle;
    info->vforward += delta.dvforward;
    if(info->vforward>3)
        info->vforward = MAX_SPEED;
    if(info->vforward<0)
        info->vforward = 0;
    float speed = info->vforward;
    float maxAngularSpeed = speed <= 1 ? speed*MAX_ANGULAR_SPEED : MAX_ANGULAR_SPEED/speed;
    if(info->vangle > maxAngularSpeed)
        info->vangle = maxAngularSpeed;
    if(info->vangle < -maxAngularSpeed)
        info->vangle = -maxAngularSpeed;
}

void Simulation::update()
{
    for(InitClientPacket &info : worldData)
    {
        info.angle += info.vangle;
        info.x += std::cos(info.angle)*info.vforward;
        info.y += std::sin(info.angle)*info.vforward;
    }
}