<commit_before>/*
 * Send data using byte stuffing protocol
 *
 * Output:
 * Byte in frame has value 0x7E is changed into 2 bytes: 0x7D, 0x5E
 * Byte in frame has value 0x7D is changed into 2 bytes: 0x7D, 0x5D
 * Input:
 * When byte 0x7D is received, discard this byte, and the next byte is XORed with 0x20.
 *
 * The telemetry code will do a best effort to send as much data as possible over the link.
 * There may be more data produced than what can be sent. It is therefore split into a number of queues.
 * 1) priorityQueue    These are messages that should NOT be dropped. For example ping/pong messages.
 * 2) ultrasoundQueue  These are measurements of obstacle distances. These should be mixed fairly with odometerQueue.
 * 3) odometerQueue    These are measurements of the wheel rotations. These should be mixed fairly with ultrasoundQueue.
 *
 * When selecting what to send, first check priority queue. Then consider ultrasoundQueue and odometerQueue in alternating order.
 * If data cannot be sent, then items in ultrasoundQueue and odometerQueue can be replaced with newer measurements.
 *
 *
 */
#include "telemetry.h"
#include <HardwareSerial.h>
#include <QueueList.h>

#define serialPort Serial1

#define START_STOP 0x7e
#define DATA_ESCAPE 0x7d
#define DATA_FRAME 0x10

size_t getPacketLength(packet &p)
{
    switch (p.hdr.cmd)
    {
    case CMD_PING:
    case CMD_PONG:
        return sizeof(pingpong);

    case CMD_US_SET_SEQ:
        return sizeof(sequence);

    case CMD_US_STOP:
    case CMD_US_START:
        return sizeof(header);

    case CMD_US_STATUS:
        return sizeof(distance);

    case CMD_ROT_STATUS:
        return sizeof(rotation);

    case CMD_ROT_RESET:
        return sizeof(header);
    }
    return 0;
}

void setPing(packet &packet, uint32_t val)
{
    packet.pp.hdr.dst = ADDR_RPI;
    packet.pp.hdr.src = ADDR_TEENSY;
    packet.pp.hdr.cmd = CMD_PING;
    packet.pp.hdr.reserved = 0;
    packet.pp.timestamp1 = val;
    packet.pp.timestamp2 = 0;
}

void serialPolling()
{
    if (serialPort.available() > 0)
    {

    }
}

uint16_t rxCrc=0;
uint16_t txCrc=0;

void SerialTransmitByte(uint8_t b)
{
    if ( b == 0x7e ) {
        serialPort.write( 0x7d );
        serialPort.write( 0x5e );
    } else if ( b == 0x7d ) {
        serialPort.write( 0x7d );
        serialPort.write( 0x5d );
    } else {
        serialPort.write( b );
    }
}

void SerialSendByte(uint8_t b)
{
    SerialTransmitByte( b );
    // CRC update
    txCrc += b; //0-1FF
    txCrc += ( txCrc >> 8 ); //0-100
    txCrc &= 0x00ff;
}

inline void crcUpdate(b)
{
    txCrc += b; //0-1FF
    txCrc += ( txCrc >> 8 ); //0-100
    txCrc &= 0x00ff;
}

void SerialSendCrc()
{
    SerialTransmitByte( 0xFF-txCrc );
    txCrc = 0; // CRC reset
}

void SerialSendPacket(uint16_t id, uint32_t value)
{
    SerialSendByte(DATA_FRAME);
    uint8_t *bytes = (uint8_t*)&id;
    SerialSendByte(bytes[0]);
    SerialSendByte(bytes[1]);
    bytes = (uint8_t*)&value;
    SerialSendByte(bytes[0]);
    SerialSendByte(bytes[1]);
    SerialSendByte(bytes[2]);
    SerialSendByte(bytes[3]);
    SerialSendCrc();
    serialPort.flush();
}

enum transmitStates {
    TS_BEGIN,  // Nothing sent yet, deliver 0x7e
    TS_DATA,   // Sending normal data
    TS_ESCAPE, // Escape has been sent, escByte is next
    TS_CHKSUM, // Last data byte sent, checksum is next
    TS_END,    // Checksum sent, frame is next. Can be skipped if there is a next packet in queue
    TS_IDLE,   // No data to transmit.
};

packet *currentPacket;
uint16_t totalSize;
uint16_t currentOffset;
transmitStates txState = TS_BEGIN;
byte escByte;

bool sendPacket(packet &p)
{
    if (currentPacket)
    {
        return false;
    }
    currentPacket = p;
    currentOffset = 0;
    totalSize = getPacketLength(p);
    txCrc = 0;
}

/*
 * Pull bytes one-by-one from buffer with framing, stuffing and checksum calculation
 */
bool getPacketByte(byte &b)
{
    bool ret = true;
    switch (txState)
    {
    case TS_BEGIN:
        b = DATA_FRAME;
        txState = TS_DATA;
        break;
    case TS_DATA:
        b = currentPacket[currentOffset++];
        if ((b == 0x7d)||(b == 0x7e))
        {
            escByte = b ^ 0x20;
            b = 0x7d;
        }
        else
        {
            txState = (currentOffset < totalSize) ? TS_DATA : TS_CHKSUM;
        }
        break;
    case TS_ESCAPE:
        b = escByte;
        txState = (currentOffset < totalSize) ? TS_DATA : TS_CHKSUM;
        break;
    case TS_CHKSUM:
        b = txCrc;
        txState = TS_END;
        break;
    case TS_END:
        // TODO: add call here to check for next buffer. Possibly skip extra framing.
        b = DATA_FRAME;
        txState = TS_IDLE;
        break;
    case TS_IDLE:
        return false;
    }
    crcUpdate(b);
    return ret;
}

void serviceSerial()
{
    int txRoom = Serial1.availableForWrite();
    int rxRoom = Serial1.available();

    while (txRoom > 0)
    {

    }
}

<commit_msg>Fixed framing byte value<commit_after>/*
 * Send data using byte stuffing protocol
 *
 * Output:
 * Byte in frame has value 0x7E is changed into 2 bytes: 0x7D, 0x5E
 * Byte in frame has value 0x7D is changed into 2 bytes: 0x7D, 0x5D
 * Input:
 * When byte 0x7D is received, discard this byte, and the next byte is XORed with 0x20.
 *
 * The telemetry code will do a best effort to send as much data as possible over the link.
 * There may be more data produced than what can be sent. It is therefore split into a number of queues.
 * 1) priorityQueue    These are messages that should NOT be dropped. For example ping/pong messages.
 * 2) ultrasoundQueue  These are measurements of obstacle distances. These should be mixed fairly with odometerQueue.
 * 3) odometerQueue    These are measurements of the wheel rotations. These should be mixed fairly with ultrasoundQueue.
 *
 * When selecting what to send, first check priority queue. Then consider ultrasoundQueue and odometerQueue in alternating order.
 * If data cannot be sent, then items in ultrasoundQueue and odometerQueue can be replaced with newer measurements.
 *
 *
 */
#include "telemetry.h"
#include <HardwareSerial.h>
#include <QueueList.h>

#define serialPort Serial1

#define START_STOP 0x7e
#define DATA_ESCAPE 0x7d
#define DATA_FRAME 0x10

size_t getPacketLength(packet &p)
{
    switch (p.hdr.cmd)
    {
    case CMD_PING:
    case CMD_PONG:
        return sizeof(pingpong);

    case CMD_US_SET_SEQ:
        return sizeof(sequence);

    case CMD_US_STOP:
    case CMD_US_START:
        return sizeof(header);

    case CMD_US_STATUS:
        return sizeof(distance);

    case CMD_ROT_STATUS:
        return sizeof(rotation);

    case CMD_ROT_RESET:
        return sizeof(header);
    }
    return 0;
}

void setPing(packet &packet, uint32_t val)
{
    packet.pp.hdr.dst = ADDR_RPI;
    packet.pp.hdr.src = ADDR_TEENSY;
    packet.pp.hdr.cmd = CMD_PING;
    packet.pp.hdr.reserved = 0;
    packet.pp.timestamp1 = val;
    packet.pp.timestamp2 = 0;
}

void serialPolling()
{
    if (serialPort.available() > 0)
    {

    }
}

uint16_t rxCrc=0;
uint16_t txCrc=0;

void SerialTransmitByte(uint8_t b)
{
    if ( b == 0x7e ) {
        serialPort.write( 0x7d );
        serialPort.write( 0x5e );
    } else if ( b == 0x7d ) {
        serialPort.write( 0x7d );
        serialPort.write( 0x5d );
    } else {
        serialPort.write( b );
    }
}

void SerialSendByte(uint8_t b)
{
    SerialTransmitByte( b );
    // CRC update
    txCrc += b; //0-1FF
    txCrc += ( txCrc >> 8 ); //0-100
    txCrc &= 0x00ff;
}

inline void crcUpdate(b)
{
    txCrc += b; //0-1FF
    txCrc += ( txCrc >> 8 ); //0-100
    txCrc &= 0x00ff;
}

void SerialSendCrc()
{
    SerialTransmitByte( 0xFF-txCrc );
    txCrc = 0; // CRC reset
}

void SerialSendPacket(uint16_t id, uint32_t value)
{
    SerialSendByte(DATA_FRAME);
    uint8_t *bytes = (uint8_t*)&id;
    SerialSendByte(bytes[0]);
    SerialSendByte(bytes[1]);
    bytes = (uint8_t*)&value;
    SerialSendByte(bytes[0]);
    SerialSendByte(bytes[1]);
    SerialSendByte(bytes[2]);
    SerialSendByte(bytes[3]);
    SerialSendCrc();
    serialPort.flush();
}

enum transmitStates {
    TS_BEGIN,  // Nothing sent yet, deliver 0x7e
    TS_DATA,   // Sending normal data
    TS_ESCAPE, // Escape has been sent, escByte is next
    TS_CHKSUM, // Last data byte sent, checksum is next
    TS_END,    // Checksum sent, frame is next. Can be skipped if there is a next packet in queue
    TS_IDLE,   // No data to transmit.
};

packet *currentPacket;
uint16_t totalSize;
uint16_t currentOffset;
transmitStates txState = TS_BEGIN;
byte escByte;

bool sendPacket(packet &p)
{
    if (currentPacket)
    {
        return false;
    }
    currentPacket = p;
    currentOffset = 0;
    totalSize = getPacketLength(p);
    txCrc = 0;
}

/*
 * Pull bytes one-by-one from buffer with framing, stuffing and checksum calculation
 */
bool getPacketByte(byte &b)
{
    bool ret = true;
    switch (txState)
    {
    case TS_BEGIN:
        b = START_STOP;
        txState = TS_DATA;
        break;
    case TS_DATA:
        b = currentPacket[currentOffset++];
        if ((b == 0x7d)||(b == 0x7e))
        {
            escByte = b ^ 0x20;
            b = 0x7d;
        }
        else
        {
            txState = (currentOffset < totalSize) ? TS_DATA : TS_CHKSUM;
        }
        break;
    case TS_ESCAPE:
        b = escByte;
        txState = (currentOffset < totalSize) ? TS_DATA : TS_CHKSUM;
        break;
    case TS_CHKSUM:
        b = txCrc;
        txState = TS_END;
        break;
    case TS_END:
        // TODO: add call here to check for next buffer. Possibly skip extra framing.
        b = START_STOP;
        txState = TS_IDLE;
        break;
    case TS_IDLE:
        return false;
    }
    crcUpdate(b);
    return ret;
}

void serviceSerial()
{
    int txRoom = Serial1.availableForWrite();
    int rxRoom = Serial1.available();

    while (txRoom > 0)
    {

    }
}

<|endoftext|>


<commit_before>#include "Aircraft.hpp"
#include "Math.hpp"
#include <iostream>

Aircraft::Aircraft(const AircraftTemplate &NewTemplate, map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot) : Template(NewTemplate), Land(0), State(FlyingIn), Direction(Aircraft::In)
{
    Setup(Textures, Sounds, Pos, Rot);
    FlySound.play();
}

Aircraft::Aircraft(const AircraftTemplate &NewTemplate, map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot, Runway *NewRunway, OutDirections NewOutDirection) : Template(NewTemplate), Land(NewRunway), State(TakingOff), Direction(Aircraft::Out), OutDirection(NewOutDirection)
{
    Setup(Textures, Sounds, Pos, Rot);
    TakeoffSound.play();
}

Aircraft::Aircraft(const AircraftTemplate &NewTemplate, map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot, Runway *NewRunway, States NewState, OutDirections NewOutDirection) : Template(NewTemplate), Land(NewRunway), State(NewState), Direction(Aircraft::Out), OutDirection(NewOutDirection)
{
    Setup(Textures, Sounds, Pos, Rot);
}

Aircraft::Aircraft(const AircraftTemplate &NewTemplate, map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot, Runway *NewRunway, States NewState, sf::Vector2f NewLandPoint) : Template(NewTemplate), Land(NewRunway), State(NewState), Direction(Aircraft::In), LandPoint(NewLandPoint)
{
    Setup(Textures, Sounds, Pos, Rot);
}

void Aircraft::Setup(map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot)
{
    const sf::Texture &Texture = Textures[Template.Res];
    if (Template.FrameSize.x >= 0 || Template.FrameSize.y >= 0)
    {
        Shape = AnimSprite(Texture, Template.FrameSize, Template.FrameRate);
        Shape.setOrigin(Template.FrameSize.x / 2.f, Template.FrameSize.y / 2.f);
    }
    else
    {
        Shape = AnimSprite(Texture, sf::Vector2i(Texture.getSize()), 0.f);
        Shape.setOrigin(sf::Vector2f(Texture.getSize()) / 2.f);
    }
    Shape.setPosition(Pos);
    Shape.setRotation(Rot);

    TakeoffSound.setBuffer(Sounds[Template.TakeoffRes]);
    TakeoffSound.setAttenuation(0.01f);
    FlySound.setBuffer(Sounds[Template.FlyRes]);
    FlySound.setLoop(true);
    FlySound.setAttenuation(0.01f);
    LandingSound.setBuffer(Sounds[Template.LandingRes]);
    LandingSound.setAttenuation(0.01f);

    Radius = Template.Radius;
    Speed = Template.Speed;
    Turn = Template.Turn;
}

const AircraftTemplate& Aircraft::GetTemplate() const
{
    return Template;
}

sf::Vector2f Aircraft::GetPos() const
{
    return Shape.getPosition();
}

float Aircraft::GetAngle() const
{
    return Shape.getRotation();
}

Runway* const Aircraft::GetLand() const
{
    return Land;
}

float Aircraft::GetRadius() const
{
    return Radius;
}

Path& Aircraft::GetPath()
{
    return P;
}

bool Aircraft::OnMe(sf::Vector2f Pos)
{
    const sf::Vector2f &Me = Shape.getPosition();
    return InRange(Me, Pos, Radius);
}

bool Aircraft::Pathable()
{
    return !OnRunway() && State != FlyingOut;
}

bool Aircraft::OnRunway() const
{
    return State == Landing || State == TakingOff;
}

Aircraft::States Aircraft::GetState()
{
    return State;
}

Aircraft::Directions Aircraft::GetDirection()
{
    return Direction;
}

Aircraft::OutDirections Aircraft::GetOutDirection()
{
    return OutDirection;
}

sf::Vector2f Aircraft::GetLandPoint()
{
    return LandPoint;
}

bool Aircraft::Colliding(const Aircraft &Other) const
{
    const sf::Vector2f &Me = Shape.getPosition();
    const sf::Vector2f &Pos = Other.Shape.getPosition();
    return OnRunway() == Other.OnRunway() &&
           InRange(Me, Pos, (Radius + Other.Radius) / 1.3f);
}

bool Aircraft::Colliding(const Explosion &Exp) const
{
    const sf::Vector2f &Me = Shape.getPosition();
    const sf::Vector2f &Pos = Exp.Shape.getPosition();
    return InRange(Me, Pos, (Radius + Exp.Radius) / 2.5f);
}

void Aircraft::SetRunway(Runway *NewLand)
{
    Land = NewLand;
}

void Aircraft::Pause(bool Status)
{
    if (Status)
    {
        TakeoffSound.pause();
        FlySound.pause();
        LandingSound.pause();
    }
    else
    {
        if (TakeoffSound.getStatus() == sf::Sound::Paused)
        {
            TakeoffSound.play();
        }

        if (FlySound.getStatus() == sf::Sound::Paused)
        {
            FlySound.play();
        }

        if (LandingSound.getStatus() == sf::Sound::Paused)
        {
            LandingSound.play();
        }
    }
}

bool Aircraft::Step(float FT, sf::Vector2f Wind)
{
    bool Die = false;

    const sf::Vector2f &Me = Shape.getPosition();
    TakeoffSound.setPosition(Me.x, Me.y, 0.f);
    FlySound.setPosition(Me.x, Me.y, 0.f);
    LandingSound.setPosition(Me.x, Me.y, 0.f);

    Shape.update(FT);

    float Turning = 0.f;

    switch (State)
    {
        case FlyingIn:
        {
            sf::Vector2f To(400.f, 300.f);
            Shape.setRotation(RadToDeg(atan2(To.y - Me.y, To.x - Me.x)));

            if (P.NumPoints() > 0)
            {
                State = FlyingPath;
            }
            else if (Me.x > 100 && Me.x < 700 && Me.y > 100 && Me.y < 500)
            {
                State = FlyingFree;
                Turning = 0.2f;
            }
            break;
        }
        case FlyingOut:
        {
            sf::Vector2f From(400.f, 300.f);
            Shape.setRotation(RadToDeg(atan2(Me.y - From.y, Me.x - From.x)));

            if (Me.x < -Radius || Me.x > (800 + Radius) || Me.y < -Radius || Me.y > (600 + Radius))
            {
                Die = true;
            }
            break;
        }
        case FlyingFree:
        {
            float Angle = AngleFix(Shape.getRotation()), AddAngle;
            if ((Me.x < 100 && Angle > 180) ||
                (Me.x > 700 && Angle < 180) ||
                (Me.y < 100 && (Angle > 90 && Angle < 270)) ||
                (Me.y > 500 && (Angle > 270 || Angle < 90)))
            {
                AddAngle = Turn;
                Turning = Turn / 10;
            }
            else if ((Me.x < 100 && Angle <= 180) ||
                     (Me.x > 700 && Angle >= 180) ||
                     (Me.y < 100 && (Angle <= 90 || Angle >= 270)) ||
                     (Me.y > 500 && (Angle <= 270 && Angle >= 90)))
            {
                AddAngle = -Turn;
                Turning = -Turn / 10;
            }
            else
            {
                AddAngle = Turning;
            }

            Angle += AddAngle;
            Shape.setRotation(AngleFix(Angle));

            if (P.NumPoints() > 0)
            {
                State = FlyingPath;
            }
            else if (Direction == Out &&
                     ((OutDirection == OutUp && Me.y < 50) ||
                      (OutDirection == OutDown && Me.y > 550) ||
                      (OutDirection == OutLeft && Me.x < 50) ||
                      (OutDirection == OutRight && Me.x > 750)))
            {
                State = FlyingOut;
            }
            break;
        }
        case FlyingPath:
        {
            const sf::Vector2f To = P[0]; // might crash

            if (InRange(Me, To, 5))
                P.RemovePoint(0);

            float Target = RadToDeg(atan2(To.y - Me.y, To.x - Me.x));

            Shape.setRotation(Target);

            if (Land &&
                P.NumPoints() == 0 &&
                Land->OnMe(Me) &&
                P.Highlight
                /*abs(AngleDiff(GetAngle(), Land->GetAngle())) <= Land->GetTemplate().LandAngle*/)
            {
                FlySound.stop();
                LandingSound.play();
                State = Landing;
                LandPoint = Me;
            }
            else if (P.NumPoints() == 0)
            {
                State = FlyingFree;
            }
            break;
        }
        case Landing:
        {
            sf::Vector2f Runway = Land->GetPos() + PolarToRect(sf::Vector2f(Land->GetLength() * 1.5f, Land->GetAngle()));
            float Dist = Distance(Me, LandPoint);

            Speed = Map(Dist, 0.f, Land->GetLength() * 1.1f, Template.Speed, 0.f);

            if (Land->GetTemplate().Directional)
            {
                Shape.rotate(AngleDiff(Shape.getRotation(), Angle(Me - Runway)) * 3 * FT);
            }

            float Scale = Map2(Dist, 0.f, Land->GetLength() * 1.1f, 1.f, 0.65f);
            Shape.setScale(Scale, Scale);
            Radius = Template.Radius * Scale;

            if (Dist > Land->GetLength())
            {
                Die = true;
            }
            break;
        }
        case TakingOff:
        {
            sf::Vector2f Runway = Land->GetPos() + PolarToRect(sf::Vector2f(Land->GetLength() * 1.5f, Land->GetAngle()));
            float Dist = Distance(Me, Land->GetPos());

            Speed = Map(Dist, 0.f, Land->GetLength() * 1.1f, 10.f, Template.Speed);

            if (Land->GetTemplate().Directional)
            {
                Shape.setRotation(Angle(Me - Runway));
            }

            float Scale = Map2(Dist, 0.f, Land->GetLength() * 1.1f, 0.65f, 1.f);
            Shape.setScale(Scale, Scale);
            Radius = Template.Radius * Scale;

            if (Dist > Land->GetLength())
            {
                FlySound.play();
                State = FlyingFree;
                Land = 0;
            }
            break;
        }
    }

    Shape.move(sf::Vector2f(cos(DegToRad(Shape.getRotation())), sin(DegToRad(Shape.getRotation()))) * FT * Speed);
    Shape.move(Wind * FT * Magnitude(Shape.getScale()) / sqrt(2.f));

    return Die;
}

void Aircraft::Draw(sf::RenderWindow& App)
{
    Shape.setColor(sf::Color::White);
    App.draw(Shape);
}

void Aircraft::DrawShadow(sf::RenderWindow &App)
{
    Shape.setColor(sf::Color(0, 0, 0, 127));

    float Scale = Map(Speed / Template.Speed, 0.f, 1.f, 1.f, 0.9f);

    sf::Transform Transform;
    Transform.scale(sf::Vector2f(Scale, Scale), sf::Vector2f(App.getSize().x / 2, App.getSize().y * 0.8f));
    App.draw(Shape, Transform);
}

<commit_msg>Smooth turning on takeoff too<commit_after>#include "Aircraft.hpp"
#include "Math.hpp"
#include <iostream>

Aircraft::Aircraft(const AircraftTemplate &NewTemplate, map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot) : Template(NewTemplate), Land(0), State(FlyingIn), Direction(Aircraft::In)
{
    Setup(Textures, Sounds, Pos, Rot);
    FlySound.play();
}

Aircraft::Aircraft(const AircraftTemplate &NewTemplate, map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot, Runway *NewRunway, OutDirections NewOutDirection) : Template(NewTemplate), Land(NewRunway), State(TakingOff), Direction(Aircraft::Out), OutDirection(NewOutDirection)
{
    Setup(Textures, Sounds, Pos, Rot);
    TakeoffSound.play();
}

Aircraft::Aircraft(const AircraftTemplate &NewTemplate, map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot, Runway *NewRunway, States NewState, OutDirections NewOutDirection) : Template(NewTemplate), Land(NewRunway), State(NewState), Direction(Aircraft::Out), OutDirection(NewOutDirection)
{
    Setup(Textures, Sounds, Pos, Rot);
}

Aircraft::Aircraft(const AircraftTemplate &NewTemplate, map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot, Runway *NewRunway, States NewState, sf::Vector2f NewLandPoint) : Template(NewTemplate), Land(NewRunway), State(NewState), Direction(Aircraft::In), LandPoint(NewLandPoint)
{
    Setup(Textures, Sounds, Pos, Rot);
}

void Aircraft::Setup(map<string, sf::Texture> &Textures, map<string, sf::SoundBuffer> &Sounds, sf::Vector2f Pos, float Rot)
{
    const sf::Texture &Texture = Textures[Template.Res];
    if (Template.FrameSize.x >= 0 || Template.FrameSize.y >= 0)
    {
        Shape = AnimSprite(Texture, Template.FrameSize, Template.FrameRate);
        Shape.setOrigin(Template.FrameSize.x / 2.f, Template.FrameSize.y / 2.f);
    }
    else
    {
        Shape = AnimSprite(Texture, sf::Vector2i(Texture.getSize()), 0.f);
        Shape.setOrigin(sf::Vector2f(Texture.getSize()) / 2.f);
    }
    Shape.setPosition(Pos);
    Shape.setRotation(Rot);

    TakeoffSound.setBuffer(Sounds[Template.TakeoffRes]);
    TakeoffSound.setAttenuation(0.01f);
    FlySound.setBuffer(Sounds[Template.FlyRes]);
    FlySound.setLoop(true);
    FlySound.setAttenuation(0.01f);
    LandingSound.setBuffer(Sounds[Template.LandingRes]);
    LandingSound.setAttenuation(0.01f);

    Radius = Template.Radius;
    Speed = Template.Speed;
    Turn = Template.Turn;
}

const AircraftTemplate& Aircraft::GetTemplate() const
{
    return Template;
}

sf::Vector2f Aircraft::GetPos() const
{
    return Shape.getPosition();
}

float Aircraft::GetAngle() const
{
    return Shape.getRotation();
}

Runway* const Aircraft::GetLand() const
{
    return Land;
}

float Aircraft::GetRadius() const
{
    return Radius;
}

Path& Aircraft::GetPath()
{
    return P;
}

bool Aircraft::OnMe(sf::Vector2f Pos)
{
    const sf::Vector2f &Me = Shape.getPosition();
    return InRange(Me, Pos, Radius);
}

bool Aircraft::Pathable()
{
    return !OnRunway() && State != FlyingOut;
}

bool Aircraft::OnRunway() const
{
    return State == Landing || State == TakingOff;
}

Aircraft::States Aircraft::GetState()
{
    return State;
}

Aircraft::Directions Aircraft::GetDirection()
{
    return Direction;
}

Aircraft::OutDirections Aircraft::GetOutDirection()
{
    return OutDirection;
}

sf::Vector2f Aircraft::GetLandPoint()
{
    return LandPoint;
}

bool Aircraft::Colliding(const Aircraft &Other) const
{
    const sf::Vector2f &Me = Shape.getPosition();
    const sf::Vector2f &Pos = Other.Shape.getPosition();
    return OnRunway() == Other.OnRunway() &&
           InRange(Me, Pos, (Radius + Other.Radius) / 1.3f);
}

bool Aircraft::Colliding(const Explosion &Exp) const
{
    const sf::Vector2f &Me = Shape.getPosition();
    const sf::Vector2f &Pos = Exp.Shape.getPosition();
    return InRange(Me, Pos, (Radius + Exp.Radius) / 2.5f);
}

void Aircraft::SetRunway(Runway *NewLand)
{
    Land = NewLand;
}

void Aircraft::Pause(bool Status)
{
    if (Status)
    {
        TakeoffSound.pause();
        FlySound.pause();
        LandingSound.pause();
    }
    else
    {
        if (TakeoffSound.getStatus() == sf::Sound::Paused)
        {
            TakeoffSound.play();
        }

        if (FlySound.getStatus() == sf::Sound::Paused)
        {
            FlySound.play();
        }

        if (LandingSound.getStatus() == sf::Sound::Paused)
        {
            LandingSound.play();
        }
    }
}

bool Aircraft::Step(float FT, sf::Vector2f Wind)
{
    bool Die = false;

    const sf::Vector2f &Me = Shape.getPosition();
    TakeoffSound.setPosition(Me.x, Me.y, 0.f);
    FlySound.setPosition(Me.x, Me.y, 0.f);
    LandingSound.setPosition(Me.x, Me.y, 0.f);

    Shape.update(FT);

    float Turning = 0.f;

    switch (State)
    {
        case FlyingIn:
        {
            sf::Vector2f To(400.f, 300.f);
            Shape.setRotation(RadToDeg(atan2(To.y - Me.y, To.x - Me.x)));

            if (P.NumPoints() > 0)
            {
                State = FlyingPath;
            }
            else if (Me.x > 100 && Me.x < 700 && Me.y > 100 && Me.y < 500)
            {
                State = FlyingFree;
                Turning = 0.2f;
            }
            break;
        }
        case FlyingOut:
        {
            sf::Vector2f From(400.f, 300.f);
            Shape.setRotation(RadToDeg(atan2(Me.y - From.y, Me.x - From.x)));

            if (Me.x < -Radius || Me.x > (800 + Radius) || Me.y < -Radius || Me.y > (600 + Radius))
            {
                Die = true;
            }
            break;
        }
        case FlyingFree:
        {
            float Angle = AngleFix(Shape.getRotation()), AddAngle;
            if ((Me.x < 100 && Angle > 180) ||
                (Me.x > 700 && Angle < 180) ||
                (Me.y < 100 && (Angle > 90 && Angle < 270)) ||
                (Me.y > 500 && (Angle > 270 || Angle < 90)))
            {
                AddAngle = Turn;
                Turning = Turn / 10;
            }
            else if ((Me.x < 100 && Angle <= 180) ||
                     (Me.x > 700 && Angle >= 180) ||
                     (Me.y < 100 && (Angle <= 90 || Angle >= 270)) ||
                     (Me.y > 500 && (Angle <= 270 && Angle >= 90)))
            {
                AddAngle = -Turn;
                Turning = -Turn / 10;
            }
            else
            {
                AddAngle = Turning;
            }

            Angle += AddAngle;
            Shape.setRotation(AngleFix(Angle));

            if (P.NumPoints() > 0)
            {
                State = FlyingPath;
            }
            else if (Direction == Out &&
                     ((OutDirection == OutUp && Me.y < 50) ||
                      (OutDirection == OutDown && Me.y > 550) ||
                      (OutDirection == OutLeft && Me.x < 50) ||
                      (OutDirection == OutRight && Me.x > 750)))
            {
                State = FlyingOut;
            }
            break;
        }
        case FlyingPath:
        {
            const sf::Vector2f To = P[0]; // might crash

            if (InRange(Me, To, 5))
                P.RemovePoint(0);

            float Target = RadToDeg(atan2(To.y - Me.y, To.x - Me.x));

            Shape.setRotation(Target);

            if (Land &&
                P.NumPoints() == 0 &&
                Land->OnMe(Me) &&
                P.Highlight
                /*abs(AngleDiff(GetAngle(), Land->GetAngle())) <= Land->GetTemplate().LandAngle*/)
            {
                FlySound.stop();
                LandingSound.play();
                State = Landing;
                LandPoint = Me;
            }
            else if (P.NumPoints() == 0)
            {
                State = FlyingFree;
            }
            break;
        }
        case Landing:
        {
            sf::Vector2f Runway = Land->GetPos() + PolarToRect(sf::Vector2f(Land->GetLength() * 1.5f, Land->GetAngle()));
            float Dist = Distance(Me, LandPoint);

            Speed = Map(Dist, 0.f, Land->GetLength() * 1.1f, Template.Speed, 0.f);

            if (Land->GetTemplate().Directional)
            {
                Shape.rotate(AngleDiff(Shape.getRotation(), Angle(Me - Runway)) * 3 * FT);
            }

            float Scale = Map2(Dist, 0.f, Land->GetLength() * 1.1f, 1.f, 0.65f);
            Shape.setScale(Scale, Scale);
            Radius = Template.Radius * Scale;

            if (Dist > Land->GetLength())
            {
                Die = true;
            }
            break;
        }
        case TakingOff:
        {
            sf::Vector2f Runway = Land->GetPos() + PolarToRect(sf::Vector2f(Land->GetLength() * 1.5f, Land->GetAngle()));
            float Dist = Distance(Me, Land->GetPos());

            Speed = Map(Dist, 0.f, Land->GetLength() * 1.1f, 10.f, Template.Speed);

            if (Land->GetTemplate().Directional)
            {
                Shape.rotate(AngleDiff(Shape.getRotation(), Angle(Me - Runway)) * 3 * FT);
            }

            float Scale = Map2(Dist, 0.f, Land->GetLength() * 1.1f, 0.65f, 1.f);
            Shape.setScale(Scale, Scale);
            Radius = Template.Radius * Scale;

            if (Dist > Land->GetLength())
            {
                FlySound.play();
                State = FlyingFree;
                Land = 0;
            }
            break;
        }
    }

    Shape.move(sf::Vector2f(cos(DegToRad(Shape.getRotation())), sin(DegToRad(Shape.getRotation()))) * FT * Speed);
    Shape.move(Wind * FT * Magnitude(Shape.getScale()) / sqrt(2.f));

    return Die;
}

void Aircraft::Draw(sf::RenderWindow& App)
{
    Shape.setColor(sf::Color::White);
    App.draw(Shape);
}

void Aircraft::DrawShadow(sf::RenderWindow &App)
{
    Shape.setColor(sf::Color(0, 0, 0, 127));

    float Scale = Map(Speed / Template.Speed, 0.f, 1.f, 1.f, 0.9f);

    sf::Transform Transform;
    Transform.scale(sf::Vector2f(Scale, Scale), sf::Vector2f(App.getSize().x / 2, App.getSize().y * 0.8f));
    App.draw(Shape, Transform);
}

<|endoftext|>


<commit_before>/*
 *  nextpnr -- Next Generation Place and Route
 *
 *  Copyright (C) 2018  David Shah <david@symbioticeda.com>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "globals.h"
#include <algorithm>
#include <iomanip>
#include <queue>
#include "cells.h"
#include "log.h"
#include "nextpnr.h"

#define fmt_str(x) (static_cast<const std::ostringstream &>(std::ostringstream() << x).str())

NEXTPNR_NAMESPACE_BEGIN

static std::string get_quad_name(GlobalQuadrant quad)
{
    switch (quad) {
    case QUAD_UL:
        return "UL";
    case QUAD_UR:
        return "UR";
    case QUAD_LL:
        return "LL";
    case QUAD_LR:
        return "LR";
    }
    return "";
}

class Ecp5GlobalRouter
{
  public:
    Ecp5GlobalRouter(Context *ctx) : ctx(ctx){};

  private:
    bool is_clock_port(const PortRef &user)
    {
        if (user.cell->type == id_TRELLIS_SLICE && (user.port == id_CLK || user.port == id_WCK))
            return true;
        return false;
    }

    std::vector<NetInfo *> get_clocks()
    {
        std::unordered_map<IdString, int> clockCount;
        for (auto &net : ctx->nets) {
            NetInfo *ni = net.second.get();
            clockCount[ni->name] = 0;
            for (const auto &user : ni->users) {
                if (is_clock_port(user))
                    clockCount[ni->name]++;
            }
            // log_info("clkcount %s: %d\n", ni->name.c_str(ctx),clockCount[ni->name]);
        }
        std::vector<NetInfo *> clocks;
        while (clocks.size() < 16) {
            auto max = std::max_element(clockCount.begin(), clockCount.end(),
                                        [](const decltype(clockCount)::value_type &a,
                                           const decltype(clockCount)::value_type &b) { return a.second < b.second; });
            if (max == clockCount.end() || max->second < 5)
                break;
            clocks.push_back(ctx->nets.at(max->first).get());
            clockCount.erase(max->first);
        }
        return clocks;
    }

    PipId find_tap_pip(WireId tile_glb)
    {
        std::string wireName = ctx->getWireBasename(tile_glb).str(ctx);
        std::string glbName = wireName.substr(2);
        TapDirection td = ctx->globalInfoAtLoc(tile_glb.location).tap_dir;
        WireId tap_wire;
        Location tap_loc;
        tap_loc.x = ctx->globalInfoAtLoc(tile_glb.location).tap_col;
        tap_loc.y = tile_glb.location.y;
        if (td == TAP_DIR_LEFT) {
            tap_wire = ctx->getWireByLocAndBasename(tap_loc, "L_" + glbName);
        } else {
            tap_wire = ctx->getWireByLocAndBasename(tap_loc, "R_" + glbName);
        }
        NPNR_ASSERT(tap_wire != WireId());
        return *(ctx->getPipsUphill(tap_wire).begin());
    }

    PipId find_spine_pip(WireId tap_wire)
    {
        std::string wireName = ctx->getWireBasename(tap_wire).str(ctx);
        Location spine_loc;
        spine_loc.x = ctx->globalInfoAtLoc(tap_wire.location).spine_col;
        spine_loc.y = ctx->globalInfoAtLoc(tap_wire.location).spine_row;
        WireId spine_wire = ctx->getWireByLocAndBasename(spine_loc, wireName);
        return *(ctx->getPipsUphill(spine_wire).begin());
    }

    void route_logic_tile_global(NetInfo *net, int global_index, PortRef user)
    {
        WireId userWire = ctx->getBelPinWire(user.cell->bel, user.port);
        WireId globalWire;
        IdString global_name = ctx->id(fmt_str("G_HPBX" << std::setw(2) << std::setfill('0') << global_index << "00"));
        std::queue<WireId> upstream;
        std::unordered_map<WireId, PipId> backtrace;
        upstream.push(userWire);
        bool already_routed = false;
        WireId next;
        // Search back from the pin until we reach the global network
        while (true) {
            next = upstream.front();
            upstream.pop();

            if (ctx->getBoundWireNet(next) == net) {
                already_routed = true;
                globalWire = next;
                break;
            }

            if (ctx->getWireBasename(next) == global_name) {
                globalWire = next;
                break;
            }
            if (ctx->checkWireAvail(next)) {
                for (auto pip : ctx->getPipsUphill(next)) {
                    WireId src = ctx->getPipSrcWire(pip);
                    backtrace[src] = pip;
                    upstream.push(src);
                }
            }
            if (upstream.size() > 30000) {
                log_error("failed to route HPBX%02d00 to %s.%s\n", global_index,
                          ctx->getBelName(user.cell->bel).c_str(ctx), user.port.c_str(ctx));
            }
        }
        // Set all the pips we found along the way
        WireId cursor = next;
        while (true) {
            auto fnd = backtrace.find(cursor);
            if (fnd == backtrace.end())
                break;
            ctx->bindPip(fnd->second, net, STRENGTH_LOCKED);
            cursor = ctx->getPipDstWire(fnd->second);
        }
        // If the global network inside the tile isn't already set up,
        // we also need to bind the buffers along the way
        if (!already_routed) {
            ctx->bindWire(next, net, STRENGTH_LOCKED);
            PipId tap_pip = find_tap_pip(next);
            NetInfo *tap_net = ctx->getBoundPipNet(tap_pip);
            if (tap_net == nullptr) {
                ctx->bindPip(tap_pip, net, STRENGTH_LOCKED);
                PipId spine_pip = find_spine_pip(ctx->getPipSrcWire(tap_pip));
                NetInfo *spine_net = ctx->getBoundPipNet(spine_pip);
                if (spine_net == nullptr) {
                    ctx->bindPip(spine_pip, net, STRENGTH_LOCKED);
                } else {
                    NPNR_ASSERT(spine_net == net);
                }
            } else {
                NPNR_ASSERT(tap_net == net);
            }
        }
    }

    bool is_global_io(CellInfo *io, std::string &glb_name)
    {
        std::string func_name = ctx->getPioFunctionName(io->bel);
        if (func_name.substr(0, 5) == "PCLKT") {
            func_name.erase(func_name.find('_'), 1);
            glb_name = "G_" + func_name;
            return true;
        }
        return false;
    }

    WireId get_global_wire(GlobalQuadrant quad, int network)
    {
        return ctx->getWireByLocAndBasename(Location(0, 0),
                                            "G_" + get_quad_name(quad) + "PCLK" + std::to_string(network));
    }

    bool simple_router(NetInfo *net, WireId src, WireId dst, bool allow_fail = false)
    {
        std::queue<WireId> visit;
        std::unordered_map<WireId, PipId> backtrace;
        visit.push(src);
        WireId cursor;
        while (true) {

            if (visit.empty() || visit.size() > 50000) {
                if (allow_fail)
                    return false;
                log_error("cannot route global from %s to %s.\n", ctx->getWireName(src).c_str(ctx),
                          ctx->getWireName(dst).c_str(ctx));
            }
            cursor = visit.front();
            visit.pop();
            NetInfo *bound = ctx->getBoundWireNet(cursor);
            if (bound == net) {
            } else if (bound != nullptr) {
                continue;
            }
            if (cursor == dst)
                break;
            for (auto dh : ctx->getPipsDownhill(cursor)) {
                WireId pipDst = ctx->getPipDstWire(dh);
                if (backtrace.count(pipDst))
                    continue;
                backtrace[pipDst] = dh;
                visit.push(pipDst);
            }
        }
        while (true) {
            auto fnd = backtrace.find(cursor);
            if (fnd == backtrace.end())
                break;
            NetInfo *bound = ctx->getBoundWireNet(cursor);
            if (bound != nullptr) {
                NPNR_ASSERT(bound == net);
                break;
            }
            ctx->bindPip(fnd->second, net, STRENGTH_LOCKED);
            cursor = ctx->getPipSrcWire(fnd->second);
        }
        if (ctx->getBoundWireNet(src) == nullptr)
            ctx->bindWire(src, net, STRENGTH_LOCKED);
        return true;
    }

    bool route_onto_global(NetInfo *net, int network)
    {
        WireId glb_src;
        NPNR_ASSERT(net->driver.cell->type == id_DCCA);
        glb_src = ctx->getNetinfoSourceWire(net);
        for (int quad = QUAD_UL; quad < QUAD_LR + 1; quad++) {
            WireId glb_dst = get_global_wire(GlobalQuadrant(quad), network);
            NPNR_ASSERT(glb_dst != WireId());
            bool routed = simple_router(net, glb_src, glb_dst);
            if (!routed)
                return false;
        }
        return true;
    }

    // Attempt to place a DCC
    void place_dcc(CellInfo *dcc)
    {
        for (auto bel : ctx->getBels()) {
            if (ctx->getBelType(bel) == id_DCCA && ctx->checkBelAvail(bel)) {
                if (ctx->isValidBelForCell(dcc, bel)) {
                    ctx->bindBel(bel, dcc, STRENGTH_LOCKED);
                    return;
                }
            }
        }
        NPNR_ASSERT_FALSE("failed to place dcca");
    }

    // Insert a DCC into a net to promote it to a global
    NetInfo *insert_dcc(NetInfo *net)
    {
        auto dcc = create_ecp5_cell(ctx, id_DCCA, "$gbuf$" + net->name.str(ctx));

        std::unique_ptr<NetInfo> glbnet = std::unique_ptr<NetInfo>(new NetInfo);
        glbnet->name = ctx->id("$glbnet$" + net->name.str(ctx));
        glbnet->driver.cell = dcc.get();
        glbnet->driver.port = id_CLKO;
        glbnet->is_global = true;
        dcc->ports[id_CLKO].net = glbnet.get();

        glbnet->users = net->users;
        for (auto user : net->users) {
            user.cell->ports.at(user.port).net = glbnet.get();
        }
        net->users.clear();

        dcc->ports[id_CLKI].net = net;
        PortRef clki_pr;
        clki_pr.port = id_CLKI;
        clki_pr.cell = dcc.get();
        net->users.push_back(clki_pr);

        place_dcc(dcc.get());

        ctx->cells[dcc->name] = std::move(dcc);
        NetInfo *glbptr = glbnet.get();
        ctx->nets[glbnet->name] = std::move(glbnet);
        return glbptr;
    }

    int global_route_priority(const PortRef &load)
    {
        if (load.port == id_WCK || load.port == id_WRE)
            return 90;
        return 99;
    }

    Context *ctx;

  public:
    void promote_and_route_globals()
    {
        log_info("Promoting and routing globals...\n");
        auto clocks = get_clocks();
        std::set<int> all_globals, fab_globals;
        for (int i = 0; i < 16; i++) {
            all_globals.insert(i);
            if (i < 8)
                fab_globals.insert(i);
        }
        for (auto clock : clocks) {
            bool drives_fabric = std::any_of(clock->users.begin(), clock->users.end(),
                                             [this](const PortRef &port) { return !is_clock_port(port); });
            int glbid;
            if (drives_fabric) {
                if (fab_globals.empty())
                    continue;
                glbid = *(fab_globals.begin());
            } else {
                glbid = *(all_globals.begin());
            }
            all_globals.erase(glbid);
            fab_globals.erase(glbid);
            log_info("    promoting clock net %s to global %d\n", clock->name.c_str(ctx), glbid);
            auto old_users = clock->users;
            NetInfo *global = insert_dcc(clock);
            bool routed = route_onto_global(global, glbid);
            NPNR_ASSERT(routed);

            // WCK must have routing priority
            auto sorted_users = global->users;
            std::sort(sorted_users.begin(), sorted_users.end(), [this](const PortRef &a, const PortRef &b) {
                return global_route_priority(a) < global_route_priority(b);
            });
            for (const auto &user : sorted_users) {
                route_logic_tile_global(global, glbid, user);
            }
        }
    }
};

void route_ecp5_globals(Context *ctx) { Ecp5GlobalRouter(ctx).promote_and_route_globals(); }

NEXTPNR_NAMESPACE_END
<commit_msg>ecp5: Optimise DCC placement<commit_after>/*
 *  nextpnr -- Next Generation Place and Route
 *
 *  Copyright (C) 2018  David Shah <david@symbioticeda.com>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "globals.h"
#include <algorithm>
#include <iomanip>
#include <queue>
#include "cells.h"
#include "log.h"
#include "nextpnr.h"
#include "place_common.h"
#define fmt_str(x) (static_cast<const std::ostringstream &>(std::ostringstream() << x).str())

NEXTPNR_NAMESPACE_BEGIN

static std::string get_quad_name(GlobalQuadrant quad)
{
    switch (quad) {
    case QUAD_UL:
        return "UL";
    case QUAD_UR:
        return "UR";
    case QUAD_LL:
        return "LL";
    case QUAD_LR:
        return "LR";
    }
    return "";
}

class Ecp5GlobalRouter
{
  public:
    Ecp5GlobalRouter(Context *ctx) : ctx(ctx){};

  private:
    bool is_clock_port(const PortRef &user)
    {
        if (user.cell->type == id_TRELLIS_SLICE && (user.port == id_CLK || user.port == id_WCK))
            return true;
        return false;
    }

    std::vector<NetInfo *> get_clocks()
    {
        std::unordered_map<IdString, int> clockCount;
        for (auto &net : ctx->nets) {
            NetInfo *ni = net.second.get();
            clockCount[ni->name] = 0;
            for (const auto &user : ni->users) {
                if (is_clock_port(user))
                    clockCount[ni->name]++;
            }
            // log_info("clkcount %s: %d\n", ni->name.c_str(ctx),clockCount[ni->name]);
        }
        std::vector<NetInfo *> clocks;
        while (clocks.size() < 16) {
            auto max = std::max_element(clockCount.begin(), clockCount.end(),
                                        [](const decltype(clockCount)::value_type &a,
                                           const decltype(clockCount)::value_type &b) { return a.second < b.second; });
            if (max == clockCount.end() || max->second < 5)
                break;
            clocks.push_back(ctx->nets.at(max->first).get());
            clockCount.erase(max->first);
        }
        return clocks;
    }

    PipId find_tap_pip(WireId tile_glb)
    {
        std::string wireName = ctx->getWireBasename(tile_glb).str(ctx);
        std::string glbName = wireName.substr(2);
        TapDirection td = ctx->globalInfoAtLoc(tile_glb.location).tap_dir;
        WireId tap_wire;
        Location tap_loc;
        tap_loc.x = ctx->globalInfoAtLoc(tile_glb.location).tap_col;
        tap_loc.y = tile_glb.location.y;
        if (td == TAP_DIR_LEFT) {
            tap_wire = ctx->getWireByLocAndBasename(tap_loc, "L_" + glbName);
        } else {
            tap_wire = ctx->getWireByLocAndBasename(tap_loc, "R_" + glbName);
        }
        NPNR_ASSERT(tap_wire != WireId());
        return *(ctx->getPipsUphill(tap_wire).begin());
    }

    PipId find_spine_pip(WireId tap_wire)
    {
        std::string wireName = ctx->getWireBasename(tap_wire).str(ctx);
        Location spine_loc;
        spine_loc.x = ctx->globalInfoAtLoc(tap_wire.location).spine_col;
        spine_loc.y = ctx->globalInfoAtLoc(tap_wire.location).spine_row;
        WireId spine_wire = ctx->getWireByLocAndBasename(spine_loc, wireName);
        return *(ctx->getPipsUphill(spine_wire).begin());
    }

    void route_logic_tile_global(NetInfo *net, int global_index, PortRef user)
    {
        WireId userWire = ctx->getBelPinWire(user.cell->bel, user.port);
        WireId globalWire;
        IdString global_name = ctx->id(fmt_str("G_HPBX" << std::setw(2) << std::setfill('0') << global_index << "00"));
        std::queue<WireId> upstream;
        std::unordered_map<WireId, PipId> backtrace;
        upstream.push(userWire);
        bool already_routed = false;
        WireId next;
        // Search back from the pin until we reach the global network
        while (true) {
            next = upstream.front();
            upstream.pop();

            if (ctx->getBoundWireNet(next) == net) {
                already_routed = true;
                globalWire = next;
                break;
            }

            if (ctx->getWireBasename(next) == global_name) {
                globalWire = next;
                break;
            }
            if (ctx->checkWireAvail(next)) {
                for (auto pip : ctx->getPipsUphill(next)) {
                    WireId src = ctx->getPipSrcWire(pip);
                    backtrace[src] = pip;
                    upstream.push(src);
                }
            }
            if (upstream.size() > 30000) {
                log_error("failed to route HPBX%02d00 to %s.%s\n", global_index,
                          ctx->getBelName(user.cell->bel).c_str(ctx), user.port.c_str(ctx));
            }
        }
        // Set all the pips we found along the way
        WireId cursor = next;
        while (true) {
            auto fnd = backtrace.find(cursor);
            if (fnd == backtrace.end())
                break;
            ctx->bindPip(fnd->second, net, STRENGTH_LOCKED);
            cursor = ctx->getPipDstWire(fnd->second);
        }
        // If the global network inside the tile isn't already set up,
        // we also need to bind the buffers along the way
        if (!already_routed) {
            ctx->bindWire(next, net, STRENGTH_LOCKED);
            PipId tap_pip = find_tap_pip(next);
            NetInfo *tap_net = ctx->getBoundPipNet(tap_pip);
            if (tap_net == nullptr) {
                ctx->bindPip(tap_pip, net, STRENGTH_LOCKED);
                PipId spine_pip = find_spine_pip(ctx->getPipSrcWire(tap_pip));
                NetInfo *spine_net = ctx->getBoundPipNet(spine_pip);
                if (spine_net == nullptr) {
                    ctx->bindPip(spine_pip, net, STRENGTH_LOCKED);
                } else {
                    NPNR_ASSERT(spine_net == net);
                }
            } else {
                NPNR_ASSERT(tap_net == net);
            }
        }
    }

    bool is_global_io(CellInfo *io, std::string &glb_name)
    {
        std::string func_name = ctx->getPioFunctionName(io->bel);
        if (func_name.substr(0, 5) == "PCLKT") {
            func_name.erase(func_name.find('_'), 1);
            glb_name = "G_" + func_name;
            return true;
        }
        return false;
    }

    WireId get_global_wire(GlobalQuadrant quad, int network)
    {
        return ctx->getWireByLocAndBasename(Location(0, 0),
                                            "G_" + get_quad_name(quad) + "PCLK" + std::to_string(network));
    }

    bool simple_router(NetInfo *net, WireId src, WireId dst, bool allow_fail = false)
    {
        std::queue<WireId> visit;
        std::unordered_map<WireId, PipId> backtrace;
        visit.push(src);
        WireId cursor;
        while (true) {

            if (visit.empty() || visit.size() > 50000) {
                if (allow_fail)
                    return false;
                log_error("cannot route global from %s to %s.\n", ctx->getWireName(src).c_str(ctx),
                          ctx->getWireName(dst).c_str(ctx));
            }
            cursor = visit.front();
            visit.pop();
            NetInfo *bound = ctx->getBoundWireNet(cursor);
            if (bound == net) {
            } else if (bound != nullptr) {
                continue;
            }
            if (cursor == dst)
                break;
            for (auto dh : ctx->getPipsDownhill(cursor)) {
                WireId pipDst = ctx->getPipDstWire(dh);
                if (backtrace.count(pipDst))
                    continue;
                backtrace[pipDst] = dh;
                visit.push(pipDst);
            }
        }
        while (true) {
            auto fnd = backtrace.find(cursor);
            if (fnd == backtrace.end())
                break;
            NetInfo *bound = ctx->getBoundWireNet(cursor);
            if (bound != nullptr) {
                NPNR_ASSERT(bound == net);
                break;
            }
            ctx->bindPip(fnd->second, net, STRENGTH_LOCKED);
            cursor = ctx->getPipSrcWire(fnd->second);
        }
        if (ctx->getBoundWireNet(src) == nullptr)
            ctx->bindWire(src, net, STRENGTH_LOCKED);
        return true;
    }

    bool route_onto_global(NetInfo *net, int network)
    {
        WireId glb_src;
        NPNR_ASSERT(net->driver.cell->type == id_DCCA);
        glb_src = ctx->getNetinfoSourceWire(net);
        for (int quad = QUAD_UL; quad < QUAD_LR + 1; quad++) {
            WireId glb_dst = get_global_wire(GlobalQuadrant(quad), network);
            NPNR_ASSERT(glb_dst != WireId());
            bool routed = simple_router(net, glb_src, glb_dst);
            if (!routed)
                return false;
        }
        return true;
    }

    // Attempt to place a DCC
    void place_dcc(CellInfo *dcc)
    {
        BelId best_bel;
        wirelen_t best_wirelen = 9999999;
        for (auto bel : ctx->getBels()) {
            if (ctx->getBelType(bel) == id_DCCA && ctx->checkBelAvail(bel)) {
                if (ctx->isValidBelForCell(dcc, bel)) {
                    ctx->bindBel(bel, dcc, STRENGTH_LOCKED);
                    float tns;
                    wirelen_t wirelen = get_net_metric(ctx, dcc->ports.at(id_CLKI).net, MetricType::WIRELENGTH, tns);
                    if (wirelen < best_wirelen) {
                        best_bel = bel;
                        best_wirelen = wirelen;
                    }
                    ctx->unbindBel(bel);
                }
            }
        }
        NPNR_ASSERT(best_bel != BelId());
        ctx->bindBel(best_bel, dcc, STRENGTH_LOCKED);
    }

    // Insert a DCC into a net to promote it to a global
    NetInfo *insert_dcc(NetInfo *net)
    {
        auto dcc = create_ecp5_cell(ctx, id_DCCA, "$gbuf$" + net->name.str(ctx));

        std::unique_ptr<NetInfo> glbnet = std::unique_ptr<NetInfo>(new NetInfo);
        glbnet->name = ctx->id("$glbnet$" + net->name.str(ctx));
        glbnet->driver.cell = dcc.get();
        glbnet->driver.port = id_CLKO;
        glbnet->is_global = true;
        dcc->ports[id_CLKO].net = glbnet.get();

        glbnet->users = net->users;
        for (auto user : net->users) {
            user.cell->ports.at(user.port).net = glbnet.get();
        }
        net->users.clear();

        dcc->ports[id_CLKI].net = net;
        PortRef clki_pr;
        clki_pr.port = id_CLKI;
        clki_pr.cell = dcc.get();
        net->users.push_back(clki_pr);

        place_dcc(dcc.get());

        ctx->cells[dcc->name] = std::move(dcc);
        NetInfo *glbptr = glbnet.get();
        ctx->nets[glbnet->name] = std::move(glbnet);
        return glbptr;
    }

    int global_route_priority(const PortRef &load)
    {
        if (load.port == id_WCK || load.port == id_WRE)
            return 90;
        return 99;
    }

    Context *ctx;

  public:
    void promote_and_route_globals()
    {
        log_info("Promoting and routing globals...\n");
        auto clocks = get_clocks();
        std::set<int> all_globals, fab_globals;
        for (int i = 0; i < 16; i++) {
            all_globals.insert(i);
            if (i < 8)
                fab_globals.insert(i);
        }
        for (auto clock : clocks) {
            bool drives_fabric = std::any_of(clock->users.begin(), clock->users.end(),
                                             [this](const PortRef &port) { return !is_clock_port(port); });
            int glbid;
            if (drives_fabric) {
                if (fab_globals.empty())
                    continue;
                glbid = *(fab_globals.begin());
            } else {
                glbid = *(all_globals.begin());
            }
            all_globals.erase(glbid);
            fab_globals.erase(glbid);
            log_info("    promoting clock net %s to global %d\n", clock->name.c_str(ctx), glbid);
            auto old_users = clock->users;
            NetInfo *global = insert_dcc(clock);
            bool routed = route_onto_global(global, glbid);
            NPNR_ASSERT(routed);

            // WCK must have routing priority
            auto sorted_users = global->users;
            std::sort(sorted_users.begin(), sorted_users.end(), [this](const PortRef &a, const PortRef &b) {
                return global_route_priority(a) < global_route_priority(b);
            });
            for (const auto &user : sorted_users) {
                route_logic_tile_global(global, glbid, user);
            }
        }
    }
};

void route_ecp5_globals(Context *ctx) { Ecp5GlobalRouter(ctx).promote_and_route_globals(); }

NEXTPNR_NAMESPACE_END
<|endoftext|>


<commit_before>#ifndef _EIGEN3_HDF5_HPP
#define _EIGEN3_HDF5_HPP

#include <cassert>
#include <complex>
#include <cstddef>
#include <stdexcept>
#include <string>
#include <vector>

//#include <hdf5.h>
#include <H5Cpp.h>
#include <Eigen/Dense>

namespace EigenHDF5
{

template <typename T>
struct DatatypeSpecialization;

// floating-point types

template <>
struct DatatypeSpecialization<float>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_FLOAT;
        }
};

template <>
struct DatatypeSpecialization<double>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_DOUBLE;
        }
};

template <>
struct DatatypeSpecialization<long double>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_LDOUBLE;
        }
};

// integer types

template <>
struct DatatypeSpecialization<short>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_SHORT;
        }
};

template <>
struct DatatypeSpecialization<unsigned short>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_USHORT;
        }
};

template <>
struct DatatypeSpecialization<int>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_INT;
        }
};

template <>
struct DatatypeSpecialization<unsigned int>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_UINT;
        }
};

template <>
struct DatatypeSpecialization<long>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_LONG;
        }
};

template <>
struct DatatypeSpecialization<unsigned long>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_ULONG;
        }
};

template <>
struct DatatypeSpecialization<long long>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_LLONG;
        }
};

template <>
struct DatatypeSpecialization<unsigned long long>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_ULLONG;
        }
};

// complex types
//
// inspired by http://www.mail-archive.com/hdf-forum@hdfgroup.org/msg00759.html

template <typename T>
class ComplexH5Type : public H5::CompType
{
public:
    ComplexH5Type (void)
        : CompType(sizeof(std::complex<T>))
        {
            const H5::DataType * const datatype = DatatypeSpecialization<T>::get();
            assert(datatype->getSize() == sizeof(T));
            // If we call the members "r" and "i", h5py interprets the
            // structure correctly as complex numbers.
            this->insertMember(std::string("r"), 0, *datatype);
            this->insertMember(std::string("i"), sizeof(T), *datatype);
            this->pack();
        }

    static const ComplexH5Type<T> * get_singleton (void)
        {
            // NOTE: constructing this could be a race condition
            static ComplexH5Type<T> singleton;
            return &singleton;
        }
};

template <typename T>
struct DatatypeSpecialization<std::complex<T> >
{
    static inline const H5::DataType * get (void)
        {
            return ComplexH5Type<T>::get_singleton();
        }
};

// string types, to be used mainly for attributes

template <>
struct DatatypeSpecialization<const char *>
{
    static inline const H5::DataType * get (void)
        {
            static const H5::StrType strtype(0, H5T_VARIABLE);
            return &strtype;
        }
};

template <>
struct DatatypeSpecialization<char *>
{
    static inline const H5::DataType * get (void)
        {
            static const H5::StrType strtype(0, H5T_VARIABLE);
            return &strtype;
        }
};

// XXX: for some unknown reason the following two functions segfault if
// H5T_VARIABLE is used.  The passed strings should still be null-terminated,
// so this is a bit worrisome.

template <std::size_t N>
struct DatatypeSpecialization<const char [N]>
{
    static inline const H5::DataType * get (void)
        {
            static const H5::StrType strtype(0, N);
            return &strtype;
        }
};

template <std::size_t N>
struct DatatypeSpecialization<char [N]>
{
    static inline const H5::DataType * get (void)
        {
            static const H5::StrType strtype(0, N);
            return &strtype;
        }
};

namespace internal
{
    template <typename Derived>
    H5::DataSpace create_dataspace (const Eigen::EigenBase<Derived> &mat)
    {
        const std::size_t dimensions_size = 2;
        const hsize_t dimensions[dimensions_size] = {
            static_cast<hsize_t>(mat.rows()),
            static_cast<hsize_t>(mat.cols())
        };
        return H5::DataSpace(dimensions_size, dimensions);
    }

    // I suspect there is a more optimal way to set up the hyperslab that would avoid
    // copying row by row, but I do not know what it is right now.
    template <typename Derived>
    bool write_mat_by_rows(const Eigen::EigenBase<Derived> &mat,
        const H5::DataType * const datatype,
        H5::DataSet *dataset,
        const H5::DataSpace* dataspace)
    {
        Derived::Index mstride = mat.derived().outerStride();

        // slab params for the file data
        hsize_t fstride[2] = { 1, mat.cols() };
        hsize_t count[2] = { 1, 1 };
        hsize_t block[2] = { 1, mat.cols() };

        // slab params for the memory data
        hsize_t mdim[2] = { 1, mat.cols() };
        H5::DataSpace mspace(2, mdim);

        // write each row of mat as a slab
        for (int i = 0; i < mat.rows(); i++)
        {
            hsize_t start[2] = { i, 0 };
            dataspace->selectHyperslab(H5S_SELECT_SET, count, start, fstride, block);
            dataset->write(mat.derived().data() + i*mstride, *datatype, mspace, *dataspace);
        }
        return true;
    }

    template <typename Derived>
    bool write_col_mat(const Eigen::EigenBase<Derived> &mat,
        const H5::DataType * const datatype,
        H5::DataSet *dataset,
        const H5::DataSpace* dspace)
    {
        bool written = false;
        Derived::Index rows = mat.rows();
        Derived::Index cols = mat.cols();

        Derived::Index stride = mat.derived().outerStride();
        if (rows == stride)
        {
            // slab params for the file data
            hsize_t fstride[2] = { 1, 1 };
            hsize_t fcount[2] = { 1, 1 };
            hsize_t fblock[2] = { 1, cols };

            // slab params for the memory data
            hsize_t mdim[2] = { cols, rows };
            H5::DataSpace mspace(2, mdim);
            hsize_t mstride[2] = { rows, 1 };
            hsize_t mcount[2] = { 1, 1 };
            hsize_t mblock[2] = { cols, 1 };

            // write each row of mat as a slab
            for (int i = 0; i < rows; i++)
            {
                hsize_t fstart[2] = { i, 0 };
                hsize_t mstart[2] = { 0, i };
                dspace->selectHyperslab(H5S_SELECT_SET, fcount, fstart, fstride, fblock);
                mspace.selectHyperslab(H5S_SELECT_SET, mcount, mstart, mstride, mblock);
                dataset->write(mat.derived().data(), *datatype, mspace, *dspace);
            }
            written = true;
        }
        return written;
    }
}

template <typename T>
void save_scalar_attribute (const H5::H5Location &h5obj, const std::string &name, const T &value)
{
    const H5::DataType * const datatype = DatatypeSpecialization<T>::get();
    H5::DataSpace dataspace(H5S_SCALAR);
    H5::Attribute att = h5obj.createAttribute(name, *datatype, dataspace);
    att.write(*datatype, &value);
}

template <>
inline void save_scalar_attribute (const H5::H5Location &h5obj, const std::string &name, const std::string &value)
{
    save_scalar_attribute(h5obj, name, value.c_str());
}

// see http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html

template <typename Derived>
void save (H5::CommonFG &h5group, const std::string &name, const Eigen::EigenBase<Derived> &mat, const H5::DSetCreatPropList &plist=H5::DSetCreatPropList::DEFAULT)
{
    typedef typename Derived::Scalar Scalar;
    const H5::DataType * const datatype = DatatypeSpecialization<Scalar>::get();
    const H5::DataSpace dataspace = internal::create_dataspace(mat);
    H5::DataSet dataset = h5group.createDataSet(name, *datatype, dataspace, plist);

    bool written = false;  // flag will be true when the data has been written
    if (mat.derived().Flags & Eigen::RowMajor)
    {
        // the matrix is stored in a row major order; see if it is contiguous
        if (mat.derived().outerStride() == mat.cols())
        {
            // the stride between rows is the number of columns means there is no padding between
            // rows, so the data is contiguous and we can write the data directly from memory
            // without copying.
            dataset.write(mat.derived().data(), *datatype);
            written = true;
        }
        else if (mat.derived().innerStride() == 1)
        {
            // inner stride == 1 is a sanity check. Only matrices made with unconventional maps
            // would have inner stride != 1, and they will be handled below by first copying the
            // input matrix.

            // write the matrix by rows to the dataset using the dataspace made above
            written = internal::write_mat_by_rows(mat, datatype, &dataset, &dataspace);
        }
    }
    else
    {
        written = internal::write_col_mat(mat, datatype, &dataset, &dataspace);
    }

    if (!written)
    {
        // data has not yet been written, so there is nothing else to try but copy the input
        // matrix to a row major matrix and write it.
        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> row_major_mat(mat);
        dataset.write(row_major_mat.data(), *datatype);
    }
}

template <typename Derived>
void save_attribute (const H5::H5Location &h5obj, const std::string &name, const Eigen::EigenBase<Derived> &mat)
{
    typedef typename Derived::Scalar Scalar;
    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> row_major_mat(mat);
    const H5::DataSpace dataspace = internal::create_dataspace(mat);
    const H5::DataType * const datatype = DatatypeSpecialization<Scalar>::get();
    H5::Attribute dataset = h5obj.createAttribute(name, *datatype, dataspace);
    dataset.write(*datatype, row_major_mat.data());
}

namespace internal
{
    // H5::Attribute and H5::DataSet both have similar API's, and although they
    // share a common base class, the relevant methods are not virtual.  Worst
    // of all, they take their arguments in different orders!

    template <typename Scalar>
    inline void read_data (const H5::DataSet &dataset, Scalar *data, const H5::DataType &datatype)
    {
        dataset.read(data, datatype);
    }

    template <typename Scalar>
    inline void read_data (const H5::Attribute &dataset, Scalar *data, const H5::DataType &datatype)
    {
        dataset.read(datatype, data);
    }

    template <typename Derived, typename DataSet>
    void _load (const DataSet &dataset, const Eigen::DenseBase<Derived> &mat)
    {
        typedef typename Derived::Scalar Scalar;
        const H5::DataSpace dataspace = dataset.getSpace();
        const std::size_t ndims = dataspace.getSimpleExtentNdims();
        assert(ndims > 0);
        const std::size_t dimensions_size = 2;
        hsize_t dimensions[dimensions_size];
        dimensions[1] = 1; // in case it's 1D
        if (ndims > dimensions_size) {
            throw std::runtime_error("HDF5 array has too many dimensions.");
        }
        dataspace.getSimpleExtentDims(dimensions);
        const hsize_t rows = dimensions[0], cols = dimensions[1];
        const H5::DataType * const datatype = DatatypeSpecialization<Scalar>::get();
        Eigen::DenseBase<Derived> &mat_ = const_cast<Eigen::DenseBase<Derived> &>(mat);
        bool written = false;
        if (mat.Flags & Eigen::RowMajor || dimensions[0] == 1 || dimensions[1] == 1)
        {
            // mat is already row major; resize it and read into it
            mat_.derived().resize(rows, cols);
            Derived::Index stride = mat_.derived().outerStride();
            if (stride == cols || (stride == rows && cols == 1))
            {
                // mat has natural stride, so read directly into its data block
                internal::read_data(dataset, mat_.derived().data(), *datatype);
                written = true;
            }
        }

        if (!written)
        {
            // input is col major or has unnatural stride; read into a temp and copy it
            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> temp(rows, cols);
            internal::read_data(dataset, temp.data(), *datatype);
            mat_ = temp;
            written = true;
        }
    }
}

template <typename Derived>
void load (const H5::CommonFG &h5group, const std::string &name, const Eigen::DenseBase<Derived> &mat)
{
    const H5::DataSet dataset = h5group.openDataSet(name);
    internal::_load(dataset, mat);
}

template <typename Derived>
void load_attribute (const H5::H5Location &h5obj, const std::string &name, const Eigen::DenseBase<Derived> &mat)
{
    const H5::Attribute dataset = h5obj.openAttribute(name);
    internal::_load(dataset, mat);
}

} // namespace EigenHDF5

#endif
<commit_msg>improve writing by using hyperslabs<commit_after>#ifndef _EIGEN3_HDF5_HPP
#define _EIGEN3_HDF5_HPP

#include <cassert>
#include <complex>
#include <cstddef>
#include <stdexcept>
#include <string>
#include <vector>

//#include <hdf5.h>
#include <H5Cpp.h>
#include <Eigen/Dense>

namespace EigenHDF5
{

template <typename T>
struct DatatypeSpecialization;

// floating-point types

template <>
struct DatatypeSpecialization<float>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_FLOAT;
        }
};

template <>
struct DatatypeSpecialization<double>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_DOUBLE;
        }
};

template <>
struct DatatypeSpecialization<long double>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_LDOUBLE;
        }
};

// integer types

template <>
struct DatatypeSpecialization<short>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_SHORT;
        }
};

template <>
struct DatatypeSpecialization<unsigned short>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_USHORT;
        }
};

template <>
struct DatatypeSpecialization<int>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_INT;
        }
};

template <>
struct DatatypeSpecialization<unsigned int>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_UINT;
        }
};

template <>
struct DatatypeSpecialization<long>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_LONG;
        }
};

template <>
struct DatatypeSpecialization<unsigned long>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_ULONG;
        }
};

template <>
struct DatatypeSpecialization<long long>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_LLONG;
        }
};

template <>
struct DatatypeSpecialization<unsigned long long>
{
    static inline const H5::DataType * get (void)
        {
            return &H5::PredType::NATIVE_ULLONG;
        }
};

// complex types
//
// inspired by http://www.mail-archive.com/hdf-forum@hdfgroup.org/msg00759.html

template <typename T>
class ComplexH5Type : public H5::CompType
{
public:
    ComplexH5Type (void)
        : CompType(sizeof(std::complex<T>))
        {
            const H5::DataType * const datatype = DatatypeSpecialization<T>::get();
            assert(datatype->getSize() == sizeof(T));
            // If we call the members "r" and "i", h5py interprets the
            // structure correctly as complex numbers.
            this->insertMember(std::string("r"), 0, *datatype);
            this->insertMember(std::string("i"), sizeof(T), *datatype);
            this->pack();
        }

    static const ComplexH5Type<T> * get_singleton (void)
        {
            // NOTE: constructing this could be a race condition
            static ComplexH5Type<T> singleton;
            return &singleton;
        }
};

template <typename T>
struct DatatypeSpecialization<std::complex<T> >
{
    static inline const H5::DataType * get (void)
        {
            return ComplexH5Type<T>::get_singleton();
        }
};

// string types, to be used mainly for attributes

template <>
struct DatatypeSpecialization<const char *>
{
    static inline const H5::DataType * get (void)
        {
            static const H5::StrType strtype(0, H5T_VARIABLE);
            return &strtype;
        }
};

template <>
struct DatatypeSpecialization<char *>
{
    static inline const H5::DataType * get (void)
        {
            static const H5::StrType strtype(0, H5T_VARIABLE);
            return &strtype;
        }
};

// XXX: for some unknown reason the following two functions segfault if
// H5T_VARIABLE is used.  The passed strings should still be null-terminated,
// so this is a bit worrisome.

template <std::size_t N>
struct DatatypeSpecialization<const char [N]>
{
    static inline const H5::DataType * get (void)
        {
            static const H5::StrType strtype(0, N);
            return &strtype;
        }
};

template <std::size_t N>
struct DatatypeSpecialization<char [N]>
{
    static inline const H5::DataType * get (void)
        {
            static const H5::StrType strtype(0, N);
            return &strtype;
        }
};

namespace internal
{
    template <typename Derived>
    H5::DataSpace create_dataspace (const Eigen::EigenBase<Derived> &mat)
    {
        const std::size_t dimensions_size = 2;
        const hsize_t dimensions[dimensions_size] = {
            static_cast<hsize_t>(mat.rows()),
            static_cast<hsize_t>(mat.cols())
        };
        return H5::DataSpace(dimensions_size, dimensions);
    }

    template <typename Derived>
    bool write_rowmat(const Eigen::EigenBase<Derived> &mat,
        const H5::DataType * const datatype,
        H5::DataSet *dataset,
        const H5::DataSpace* dspace)
    {
        if (mat.derived().innerStride() != 1)
        {
            // inner stride != 1 is an edge case this function does not (yet) handle. (I think it
            // could by using the inner stride as the first element of mstride below. But I do
            // not have a test case to try it out, so just return false for now.)
            return false;
        }

        Derived::Index rows = mat.rows();
        Derived::Index cols = mat.cols();
        Derived::Index mat_stride = mat.derived().outerStride();

        // slab params for the file data
        hsize_t fstride[2] = { 1, cols };

        // slab params for the memory data
        hsize_t mstride[2] = { 1, mat_stride };

        // slab params for both file and memory data
        hsize_t count[2] = { 1, 1 };
        hsize_t block[2] = { rows, cols };
        hsize_t start[2] = { 0, 0 };

        // memory dataspace
        hsize_t mdim[2] = { rows, mat_stride };
        H5::DataSpace mspace(2, mdim);

        dspace->selectHyperslab(H5S_SELECT_SET, count, start, fstride, block);
        mspace.selectHyperslab(H5S_SELECT_SET, count, start, mstride, block);
        dataset->write(mat.derived().data(), *datatype, mspace, *dspace);

        return true;
    }

    template <typename Derived>
    bool write_colmat(const Eigen::EigenBase<Derived> &mat,
        const H5::DataType * const datatype,
        H5::DataSet *dataset,
        const H5::DataSpace* dspace)
    {
        if (mat.derived().innerStride() != 1)
        {
            // inner stride != 1 is an edge case this function does not (yet) handle. (I think it
            // could by using the inner stride as the first element of mstride below. But I do
            // not have a test case to try it out, so just return false for now.)
            return false;
        }

        bool written = false;
        Derived::Index rows = mat.rows();
        Derived::Index cols = mat.cols();
        Derived::Index stride = mat.derived().outerStride();

        // slab params for the file data
        hsize_t fstride[2] = { 1, cols };
        hsize_t fcount[2] = { 1, 1 };
        hsize_t fblock[2] = { 1, cols };

        // slab params for the memory data
        hsize_t mstride[2] = { stride, 1 };
        hsize_t mcount[2] = { 1, 1 };
        hsize_t mblock[2] = { cols, 1 };

        // memory dataspace
        hsize_t mdim[2] = { cols, stride };
        H5::DataSpace mspace(2, mdim);

        // transpose the column major data in memory to the row major data in the file by
        // writing one row slab at a time.
        for (int i = 0; i < rows; i++)
        {
            hsize_t fstart[2] = { i, 0 };
            hsize_t mstart[2] = { 0, i };
            dspace->selectHyperslab(H5S_SELECT_SET, fcount, fstart, fstride, fblock);
            mspace.selectHyperslab(H5S_SELECT_SET, mcount, mstart, mstride, mblock);
            dataset->write(mat.derived().data(), *datatype, mspace, *dspace);
        }
        return true;
    }
}

template <typename T>
void save_scalar_attribute (const H5::H5Location &h5obj, const std::string &name, const T &value)
{
    const H5::DataType * const datatype = DatatypeSpecialization<T>::get();
    H5::DataSpace dataspace(H5S_SCALAR);
    H5::Attribute att = h5obj.createAttribute(name, *datatype, dataspace);
    att.write(*datatype, &value);
}

template <>
inline void save_scalar_attribute (const H5::H5Location &h5obj, const std::string &name, const std::string &value)
{
    save_scalar_attribute(h5obj, name, value.c_str());
}

// see http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html

template <typename Derived>
void save (H5::CommonFG &h5group, const std::string &name, const Eigen::EigenBase<Derived> &mat, const H5::DSetCreatPropList &plist=H5::DSetCreatPropList::DEFAULT)
{
    typedef typename Derived::Scalar Scalar;
    const H5::DataType * const datatype = DatatypeSpecialization<Scalar>::get();
    const H5::DataSpace dataspace = internal::create_dataspace(mat);
    H5::DataSet dataset = h5group.createDataSet(name, *datatype, dataspace, plist);

    bool written = false;  // flag will be true when the data has been written
    if (mat.derived().Flags & Eigen::RowMajor)
    {
        written = internal::write_rowmat(mat, datatype, &dataset, &dataspace);
    }
    else
    {
        written = internal::write_colmat(mat, datatype, &dataset, &dataspace);
    }

    if (!written)
    {
        // data has not yet been written, so there is nothing else to try but copy the input
        // matrix to a row major matrix and write it.
        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> row_major_mat(mat);
        dataset.write(row_major_mat.data(), *datatype);
    }
}

template <typename Derived>
void save_attribute (const H5::H5Location &h5obj, const std::string &name, const Eigen::EigenBase<Derived> &mat)
{
    typedef typename Derived::Scalar Scalar;
    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> row_major_mat(mat);
    const H5::DataSpace dataspace = internal::create_dataspace(mat);
    const H5::DataType * const datatype = DatatypeSpecialization<Scalar>::get();
    H5::Attribute dataset = h5obj.createAttribute(name, *datatype, dataspace);
    dataset.write(*datatype, row_major_mat.data());
}

namespace internal
{
    // H5::Attribute and H5::DataSet both have similar API's, and although they
    // share a common base class, the relevant methods are not virtual.  Worst
    // of all, they take their arguments in different orders!

    template <typename Scalar>
    inline void read_data (const H5::DataSet &dataset, Scalar *data, const H5::DataType &datatype)
    {
        dataset.read(data, datatype);
    }

    template <typename Scalar>
    inline void read_data (const H5::Attribute &dataset, Scalar *data, const H5::DataType &datatype)
    {
        dataset.read(datatype, data);
    }

    template <typename Derived, typename DataSet>
    void _load (const DataSet &dataset, const Eigen::DenseBase<Derived> &mat)
    {
        typedef typename Derived::Scalar Scalar;
        const H5::DataSpace dataspace = dataset.getSpace();
        const std::size_t ndims = dataspace.getSimpleExtentNdims();
        assert(ndims > 0);
        const std::size_t dimensions_size = 2;
        hsize_t dimensions[dimensions_size];
        dimensions[1] = 1; // in case it's 1D
        if (ndims > dimensions_size) {
            throw std::runtime_error("HDF5 array has too many dimensions.");
        }
        dataspace.getSimpleExtentDims(dimensions);
        const hsize_t rows = dimensions[0], cols = dimensions[1];
        const H5::DataType * const datatype = DatatypeSpecialization<Scalar>::get();
        Eigen::DenseBase<Derived> &mat_ = const_cast<Eigen::DenseBase<Derived> &>(mat);
        bool written = false;
        if (mat.Flags & Eigen::RowMajor || dimensions[0] == 1 || dimensions[1] == 1)
        {
            // mat is already row major; resize it and read into it
            mat_.derived().resize(rows, cols);
            Derived::Index stride = mat_.derived().outerStride();
            if (stride == cols || (stride == rows && cols == 1))
            {
                // mat has natural stride, so read directly into its data block
                internal::read_data(dataset, mat_.derived().data(), *datatype);
                written = true;
            }
        }

        if (!written)
        {
            // input is col major or has unnatural stride; read into a temp and copy it
            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> temp(rows, cols);
            internal::read_data(dataset, temp.data(), *datatype);
            mat_ = temp;
            written = true;
        }
    }
}

template <typename Derived>
void load (const H5::CommonFG &h5group, const std::string &name, const Eigen::DenseBase<Derived> &mat)
{
    const H5::DataSet dataset = h5group.openDataSet(name);
    internal::_load(dataset, mat);
}

template <typename Derived>
void load_attribute (const H5::H5Location &h5obj, const std::string &name, const Eigen::DenseBase<Derived> &mat)
{
    const H5::Attribute dataset = h5obj.openAttribute(name);
    internal::_load(dataset, mat);
}

} // namespace EigenHDF5

#endif
<|endoftext|>