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

#define PI 3.1416f
#define MAX_ANGULAR_SPEED 0.05f
#define MAX_FORWARD_SPEED 1.f
#define MAX_REVERSE_SPEED 0.5f
#define MAX_ACCELERATION 0.02f
#define MAX_DECELERATION (MAX_ACCELERATION*2)

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()%30;
    info.y = std::rand()%30;
    info.angle = ((float)(std::rand()%360))*PI/180.f;
    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]);
    float dvforward;
    // handling speed
    if(delta.acceleration > 0.01f) // if accelerating
    {
        if(fabs(delta.acceleration) < 1)
            dvforward = delta.acceleration*MAX_ACCELERATION;
        else
            dvforward = info->vforward >= 0 ? MAX_ACCELERATION : -MAX_ACCELERATION;
    }
    else if(delta.acceleration < -0.01f)
    {
        if(fabs(delta.acceleration) < 1)
            dvforward = delta.acceleration*MAX_DECELERATION;
        else
            dvforward = info->vforward >= 0 ? -MAX_DECELERATION : MAX_DECELERATION;
    }
    else // not accelerating
        dvforward = -info->vforward/8;
    info->vforward += dvforward;
    if(info->vforward>MAX_FORWARD_SPEED)
        info->vforward = MAX_FORWARD_SPEED;
    if(info->vforward<-MAX_REVERSE_SPEED)
        info->vforward = -MAX_REVERSE_SPEED;
    // handling angle
    info->vangle = delta.turning;
    if(abs(info->vangle) > 0.01f)
    {
        float x = 2*PI*fabs(info->vforward)/MAX_FORWARD_SPEED;
        float maxAngularSpeed = (x/5 + 1 + cos(PI + x))*MAX_ANGULAR_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;
    }
}