#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <utility>
#include <limits>
#include <stdexcept>
#include <vector>
#include  <iostream>

struct Vec3 { double x, y, z; };
using Formation = std::vector<Vec3>;
constexpr double pi = 3.14159265358979323846;

Vec3 add(Vec3 a, Vec3 b) {
return {a.x + b.x, a.y + b.y, a.z + b.z};
}
Vec3 sub(Vec3 a, Vec3 b) {
return {a.x - b.x, a.y - b.y, a.z - b.z};
}
Vec3 scale(Vec3 v, double k) {
return {v.x * k, v.y * k, v.z * k};
}
double dot(Vec3 a, Vec3 b) {
return a.x*b.x + a.y*b.y + a.z*b.z;
}

double length(Vec3 v) { return std::sqrt(dot(v,v)); }

bool near(Vec3 a, Vec3 b) {
return length(sub(a,b)) < 1e-8;
}

Formation square(Vec3 a, int l){

    if(l<0) throw std::invalid_argument("Il lato non può essere negativo!");
    
    Formation vertici(4); 

    for(int i=0; i<4; ++i){

        double rad = (((90*i)+225)*pi)/180;

        vertici[i].x = a.x + ((l/sqrt(2))*cos(rad));
        vertici[i].y = a.y + ((l/sqrt(2))*sin(rad));
        vertici[i].z = a.z; 
    }

    return {vertici};}

Formation esagono(int l, int el){

    if(l<0) throw std::invalid_argument("Il lato non può essere negativo!");
    
    Formation vertici(el); 

    for(int i = 0; i <el; ++i){

        double angolo = ((360/el) * i) + 180; 
        double rad = ((angolo)*pi)/180;
        
        vertici[i].x = l*cos(rad);
        vertici[i].y = l*sin(rad);
        vertici[i].z = 2;}

    return {vertici};}

Vec3 rotazione(Vec3 a, Vec3 b, double angolo){

    double rad = (angolo*pi)/180;
    Vec3 dist = sub(a, b);

    Vec3 new_pos;
   
    new_pos.x = b.x + (dist.x*cos(rad)) - (dist.y*sin(rad));
    new_pos.y = b.y + (dist.x*sin(rad)) + (dist.y*cos(rad));
    new_pos.z = a.z; 

    return {new_pos};}

Vec3 lerp(Vec3 a, Vec3 b, double u){
    Vec3 pos = scale(sub(b,a),u);
    pos = add(a,pos); 
    return{pos};}

double progress(double t, double start, double end){
    if(end<=start) throw std::invalid_argument("L'arrivo non può essere precedente alla partenza!");
    return{std::clamp((t-start)/(end-start),0.0,1.0)};
}

Vec3 positionAt(Vec3 a, Vec3 b, double t, double start, double end){
    return{lerp(a, b, progress(t, start, end))};
}

Vec3 smooth_position(Vec3 a, Vec3 b, double t, double start, double end){
    double u = progress(t, start, end);
    double sm = (3*u*u) - (2*u*u*u); 
    return{lerp(a, b, sm)};
}

Formation traslazione(Formation a, Vec3 b, int n){
    Formation new_f(n);

    for(int i = 0; i < n; ++i){
        new_f[i] = add(a[i], b); 
    };
    return{new_f};}

int main() {

    Formation starting_pos{
        {-5, 0, 2}, {-3, 0, 2}, {-1, 0, 2},
        { 1, 0, 2}, { 3, 0, 2}, { 5, 0, 2}
    }; 
    
    int raggio = 3; 
    Vec3 centro{0,0,2}; 
    double deg = 90; 
    int n = 6;

    Formation ending_pos = esagono(raggio, n); 
    Formation traslated_pos = traslazione(ending_pos, {2,0,0}, n); 

    // per conoscere lo spostamento txt
    for(int i = 0; i < n; ++i){

        Formation new_exagon(n);

        new_exagon[i] = smooth_position(starting_pos[i], traslated_pos[i], n, 0, n);
    }   

}