#include "../include/circuit.h"
#include "../include/segment.h"
#include "../include/camera.h"
#include "../parameters.h"
#include <gint/display.h>

#include "../include/drawstuff.h"


extern std::vector<Segment*> circuit;
extern camera *cam;
extern uint16_t currentcurve;
extern uint8_t shiftcolor;
extern int MAX_SEGMENT;
extern bool ShowDebug1;

void initData( void )
{
    cam = new camera();
    cam->cX = fixdouble(0.0f);
    cam->cY = fixdouble(300.0f);
    cam->cZ = fixdouble(260.0f);
}


void createCircuit( void )
{
/*    for( int i=0; i<MAX_SEGMENT; i++)
    {
        Segment *seg=new Segment( i );
        if (seg!=nullptr) circuit.push_back( seg );
    }
*/

    addStraightLine( L_VERYSHORT );
    addStraightLine( L_VERYSHORT );
    addCurve( L_SHORT, C_EASY, LEFT_CURVE );
    addHill( L_MEDIUM, H_BIG, UP_HILL );
    addCurve( L_SHORT, C_HARD, RIGHT_CURVE );
    addStraightLine( L_LONG );
};


void addStraightLine( Length l )
{
    double lastZ=0;
    int16_t lastY=0;

    if (circuit.size()!=0)
    {
        lastY=circuit[circuit.size()-1]->wY;
        lastZ=circuit[circuit.size()-1]->wZ;
    }

    for( int i=0; i<l; i++)
    {
        //Segment *s=new Segment( i );
        Segment *seg=new Segment( 0, lastY, lastZ + (i+1)*SEGMENT_LENGTH, 0 );
        if (seg!=nullptr) circuit.push_back( seg );
    }
}

void addCurve( Length l, CurveStrength s, CurveType t )
{
    double lastZ=0;
    int16_t lastY=0;

    if (circuit.size()!=0)
    {
        lastY=circuit[circuit.size()-1]->wY;
        lastZ=circuit[circuit.size()-1]->wZ;
    }

    for( int i=0; i<l; i++)
    {
        //Segment *s=new Segment( i );
        Segment *seg=new Segment( 0, lastY, lastZ+(i+1)*SEGMENT_LENGTH, s*t );
        if (seg!=nullptr) circuit.push_back( seg );
    }
}


void addHill( Length l, HillSize s, HillType t )
{
    double lastZ=0;
    int16_t lastY=0;

    if (circuit.size()!=0)
    {
        lastY=circuit[circuit.size()-1]->wY;
        lastZ=circuit[circuit.size()-1]->wZ;
    }

    for( int i=0; i<l; i++)
    {
        //Segment *s=new Segment( i );
        Segment *seg=new Segment( 0, lastY+(i+1)*t*s, lastZ+(i+1)*SEGMENT_LENGTH, 0 );
        if (seg!=nullptr) circuit.push_back( seg );
    }
}


void projectCircuitFP( void )
{
    for( int i=0; i<circuit.size(); i++)
    {
        circuit[i]->Project3DFP( cam );
    }
};


void projectCircuitFP( uint16_t index )
{
    circuit[index]->Project3DFP( cam );
};


void printCircuit( void )
{
    for( int i=0; i<20; i++)
    {
        dprint(198, 1+10*(i), C_RED, "%d-(sX,Y,W)=(%d,%d,%d)", i, circuit[i]->X, circuit[i]->Y, circuit[i]->W );
        //dprint(1, 1+10*(i), C_GREEN, "%d-(_sY,Y,W)=(%.0f,%.0f,%.0f)", i, circuit[i]->_wX, circuit[i]->_wY, circuit[i]->_sW );
    }
};

void printCircuit( int i )
{
    dprint(1, 1+10*(i), C_RED, "%d-(wX,Y,Z)=(%d,%d,%d)", i, fround(circuit[i]->wX), fround(circuit[i]->wY), fround(circuit[i]->wZ) );
    dprint(198, 1+10*(i), C_RED, "%d-(sX,Y,W)=(%d,%d,%d)", i, circuit[i]->X, circuit[i]->Y, circuit[i]->W );
};


void drawCircuitSegment( uint16_t index )
{
    if (index>=circuit.size()-1) return;

    int X1 = (int) circuit[index]->X;
    int Y1 = (int) circuit[index]->Y;
    int W1 = (int) circuit[index]->W;
    int X2 = (int) circuit[(index+1)]->X + circuit[index]->Curve;
    int Y2 = (int) circuit[(index+1)]->Y;
    int W2 = (int) circuit[(index+1)]->W;

    if (Y1==Y2) return;

    if (index%2==0)
    {
        drawGrass( Y2, Y1, 0, 255-shiftcolor, 45-shiftcolor );

        // route
        drawPolygon( X2-W2+currentcurve, X2+W2+currentcurve, Y2, X1-W1+currentcurve, X1+W1+currentcurve, Y1, 196-shiftcolor, 196-shiftcolor, 196-shiftcolor );

        // ligne blanche centrale
        drawPolygon( X2-W2/50+currentcurve, X2+W2/50+currentcurve, Y2, X1-W1/50+currentcurve, X1+W1/50+currentcurve, Y1, 255-shiftcolor, 255-shiftcolor, 255-shiftcolor );
        // ligne blanche gauche
        drawPolygon( X2-W2/16-W2+currentcurve, X2-W2+currentcurve, Y2, X1-W1/16-W1+currentcurve, X1-W1+currentcurve, Y1, 255-shiftcolor, 255-shiftcolor, 255-shiftcolor );

        // ligne blanche centrale gauche
        drawPolygon( X2-W2/50-W2/2+currentcurve, X2+W2/50-W2/2+currentcurve, Y2, X1-W1/50-W1/2+currentcurve, X1+W1/50-W1/2+currentcurve, Y1, 255-shiftcolor, 255-shiftcolor, 255-shiftcolor );

        // ligne blanche centrale
        drawPolygon( X2-W2/50+currentcurve, X2+W2/50+currentcurve, Y2, X1-W1/50+currentcurve, X1+W1/50+currentcurve, Y1, 255-shiftcolor, 255-shiftcolor, 255-shiftcolor );

        // ligne blanche centrale droite
        drawPolygon( X2-W2/50+W2/2+currentcurve, X2+W2/50+W2/2+currentcurve, Y2, X1-W1/50+W1/2+currentcurve, X1+W1/50+W1/2+currentcurve, Y1, 255-shiftcolor, 255-shiftcolor, 255-shiftcolor );

        // ligne blanche droite
        drawPolygon( X2+W2+currentcurve, X2+W2+W2/16+currentcurve, Y2, X1+W1+currentcurve, X1+W1+W1/16+currentcurve, Y1, 255-shiftcolor, 255-shiftcolor, 255-shiftcolor );

    }
    else
    {

        drawGrass( Y2, Y1, 0, 255-shiftcolor, 0 );

        drawPolygon( X2-W2+currentcurve, X2+W2+currentcurve, Y2, X1-W1+currentcurve, X1+W1+currentcurve, Y1, 180-shiftcolor, 180-shiftcolor, 180-shiftcolor );
        // ligne rouge gauche
        drawPolygon( X2-W2/16-W2+currentcurve, X2-W2+currentcurve, Y2, X1-W1/16-W1+currentcurve, X1-W1+currentcurve, Y1, 255-shiftcolor, 0, 0 );
        // ligne rouge droite
        drawPolygon( X2+W2+currentcurve, X2+W2+W2/16+currentcurve, Y2, X1+W1+currentcurve, X1+W1+W1/16+currentcurve, Y1, 255-shiftcolor, 0, 0 );

    }
}


fixed_t interpolatePositionX( double currentZ )
{
    int index = (int) (currentZ / SEGMENT_LENGTH);
    fixed_t x1 = fix(circuit[index]->wX);
    fixed_t x2 = fix(circuit[index+1]->wX);
    fixed_t dZ = fixdouble(currentZ-index*SEGMENT_LENGTH);

    fixed_t result = x1 + fmul( x2-x1, fdiv( dZ, fix(SEGMENT_LENGTH)));

    if(ShowDebug1)
    {
        dprint( 120,  1, C_WHITE, "CZ=%.0lf", f2double(currentZ) );
        dprint( 120, 15, C_WHITE, "y1=%d", fround(x1) );
        dprint( 120, 29, C_WHITE, "y2=%d", fround(x2) );

        dprint( 240, 15, C_WHITE, "Dz=%d", fround(dZ) );
        dprint( 240, 29, C_GREEN, "CX=%d", fround(result) );
    }

    return result;
}


fixed_t interpolatePositionY( double currentZ )
{
    int index = (int) (currentZ / SEGMENT_LENGTH);
    fixed_t y1 = fix(circuit[index]->wY);
    fixed_t y2 = fix(circuit[index+1]->wY);
    fixed_t dZ = fixdouble(currentZ-index*SEGMENT_LENGTH);

    fixed_t result = y1 + fmul( y2-y1, fdiv( dZ, fix(SEGMENT_LENGTH)));

    if(ShowDebug1)
    {
        dprint( 120,  1, C_WHITE, "CZ=%.0lf", f2double(currentZ) );
        dprint( 120, 15, C_WHITE, "y1=%d", fround(y1) );
        dprint( 120, 29, C_WHITE, "y2=%d", fround(y2) );

        dprint( 240, 15, C_WHITE, "Dz=%d", fround(dZ) );
        dprint( 240, 43, C_GREEN, "CY=%d", fround(result) );
    }

    return result;
}