ambrop72/BoundedInt.h

## BoundedInt.h
/*
 * Copyright (c) 2013 Ambroz Bizjak
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMBROLIB_BOUNDED_INT_H
#define AMBROLIB_BOUNDED_INT_H

#include <stdint.h>

#include "PowerOfTwo.h"
#include "MinMax.h"
#include "Modulo.h"
#include "ChooseInt.h"

template <int NumBits, bool Signed = true>
class BoundedInt {
public:
    static const int num_bits = NumBits;

    typedef typename ChooseInt<NumBits, Signed>::Type IntType;

    static IntType minIntValue ()
    {
        return Signed ? -PowerOfTwoMinusOne<IntType, NumBits>::value : 0;
    }

    static IntType maxIntValue ()
    {
        return PowerOfTwoMinusOne<IntType, NumBits>::value;
    }

    static BoundedInt minValue ()
    {
        return BoundedInt::import(minIntValue());
    }

    static BoundedInt maxValue ()
    {
        return BoundedInt::import(maxIntValue());
    }

    static BoundedInt import (IntType the_int)
    {
        BoundedInt res;
        res.m_int = the_int;
        return res;
    }

    IntType value () const
    {
        return m_int;
    }

    template <int NewNumBits, bool NewSigned>
    BoundedInt<NewNumBits, NewSigned> convert () const
    {
        return BoundedInt<NewNumBits, NewSigned>::import(value());
    }

    BoundedInt<NumBits, true> toSigned () const
    {
        return BoundedInt<NumBits, true>::import(value());
    }

    BoundedInt<NumBits, false> toUnsignedUnsafe () const
    {
        return BoundedInt<NumBits, false>::import(value());
    }

    template <int ShiftExp>
    BoundedInt<NumBits - ShiftExp, Signed> shiftRight () const
    {
        return BoundedInt<NumBits - ShiftExp, Signed>::import(value() / PowerOfTwo<IntType, ShiftExp>::value);
    }

    template <int ShiftExp>
    BoundedInt<NumBits + ShiftExp, Signed> shiftLeft () const
    {
        return BoundedInt<NumBits + ShiftExp, Signed>::import(value() * PowerOfTwo<typename BoundedInt<NumBits + ShiftExp, Signed>::IntType, ShiftExp>::value);
    }

    template <int ShiftExp>
    BoundedInt<NumBits - ShiftExp, Signed> shift () const
    {
        return ShiftHelper<ShiftExp, (ShiftExp < 0)>::call(*this);
    }

    BoundedInt<NumBits, false> absVal () const
    {
        return BoundedInt<NumBits, false>::import((value() >= 0) ? value() : -value());
    }

private:
    template <int ShiftExp, bool Left>
    struct ShiftHelper;

    template <int ShiftExp>
    struct ShiftHelper<ShiftExp, false> {
        static BoundedInt<NumBits - ShiftExp, Signed> call (BoundedInt op)
        {
            return op.shiftRight<ShiftExp>();
        }
    };

    template <int ShiftExp>
    struct ShiftHelper<ShiftExp, true> {
        static BoundedInt<NumBits - ShiftExp, Signed> call (BoundedInt op)
        {
            return op.shiftLeft<(-ShiftExp)>();
        }
    };

public:
    IntType m_int;
};

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
bool operator== (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 || Signed2)>::Type TempType;
    return ((TempType)op1.value() == (TempType)op2.value());
}

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
bool operator!= (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 || Signed2)>::Type TempType;
    return ((TempType)op1.value() != (TempType)op2.value());
}

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
bool operator< (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 || Signed2)>::Type TempType;
    return ((TempType)op1.value() < (TempType)op2.value());
}

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
bool operator> (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 || Signed2)>::Type TempType;
    return ((TempType)op1.value() > (TempType)op2.value());
}

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
bool operator<= (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 || Signed2)>::Type TempType;
    return ((TempType)op1.value() <= (TempType)op2.value());
}

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
bool operator>= (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 || Signed2)>::Type TempType;
    return ((TempType)op1.value() >= (TempType)op2.value());
}

template <int NumBits, bool Signed>
BoundedInt<NumBits, true> operator- (BoundedInt<NumBits, Signed> op)
{
    typedef typename ChooseInt<NumBits, true>::Type TempType;
    return BoundedInt<NumBits, true>::import(-(TempType)op.value());
}

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
BoundedInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 || Signed2)> operator+ (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 || Signed2)>::Type TempType;
    return BoundedInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 || Signed2)>::import((TempType)op1.value() + (TempType)op2.value());
}

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
BoundedInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 || Signed2)> operator- (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 || Signed2)>::Type TempType;
    return BoundedInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 || Signed2)>::import((TempType)op1.value() - (TempType)op2.value());
}

template <int RightShift, int NumBits1, bool Signed1, int NumBits2, bool Signed2>
BoundedInt<(NumBits1 + NumBits2 - RightShift), (Signed1 || Signed2)> BoundedMultiply (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<NumBits1, Signed1>::Type Op1Type;
    typedef typename ChooseInt<NumBits2, Signed2>::Type Op2Type;
    typedef typename ChooseInt<(NumBits1 + NumBits2 - RightShift), (Signed1 || Signed2)>::Type ResType;
    typedef typename ChooseInt<(NumBits1 + NumBits2), (Signed1 || Signed2)>::Type TempResType;

    return BoundedInt<(NumBits1 + NumBits2 - RightShift), (Signed1 || Signed2)>::import(
        ((TempResType)op1.value() * (TempResType)op2.value()) / PowerOfTwo<TempResType, RightShift>::value
    );
}

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
BoundedInt<(NumBits1 + NumBits2), (Signed1 || Signed2)> operator* (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    return BoundedMultiply<0>(op1, op2);
}

template <int LeftShift, int ResSatBits, int NumBits1, bool Signed1, int NumBits2, bool Signed2>
BoundedInt<ResSatBits, (Signed1 || Signed2)> BoundedDivide (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    typedef typename ChooseInt<NumBits1, Signed1>::Type Op1Type;
    typedef typename ChooseInt<NumBits2, Signed2>::Type Op2Type;
    typedef typename ChooseInt<ResSatBits, (Signed1 || Signed2)>::Type ResType;

    typedef typename ChooseInt<(NumBits1 + LeftShift), (Signed1 || Signed2)>::Type TempResType;
    typedef typename ChooseInt<NumBits2, (Signed1 || Signed2)>::Type TempType2;

    TempResType res = (((TempResType)op1.value() * PowerOfTwo<TempResType, LeftShift>::value) / (TempType2)op2.value());
    if (ResSatBits < NumBits1 + LeftShift) {
        if (res > PowerOfTwoMinusOne<ResType, ResSatBits>::value) {
            res = PowerOfTwoMinusOne<ResType, ResSatBits>::value;
        } else if (Signed1 || Signed2) {
            if (res < -PowerOfTwoMinusOne<ResType, ResSatBits>::value) {
                res = -PowerOfTwoMinusOne<ResType, ResSatBits>::value;
            }
        }
    }

    return BoundedInt<ResSatBits, (Signed1 || Signed2)>::import(res);
}

template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
BoundedInt<NumBits1, (Signed1 || Signed2)> operator/ (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
{
    return BoundedDivide<0, NumBits1>(op1, op2);
}

template <int NumBits, bool Signed>
BoundedInt<((NumBits + 1) / 2), false> BoundedSquareRoot (BoundedInt<NumBits, Signed> op1)
{
    static const int TempBits = NumBits + Modulo<int, NumBits, 2>::value;
    typedef typename ChooseInt<TempBits, false>::Type TempType;

    TempType op = op1.value();
    TempType res = 0;
    TempType one = PowerOfTwo<TempType, (TempBits - 2)>::value;

    while (one > op) {
        one >>= 2;
    }

    while (one != 0) {
        if (op >= res + one) {
            op = op - (res + one);
            res = res + 2 * one;
        }
        res >>= 1;
        one >>= 2;
    }

    return BoundedInt<((NumBits + 1) / 2), false>::import(res);
}

template <int NumBits>
BoundedInt<NumBits, false> BoundedModuloAdd (BoundedInt<NumBits, false> op1, BoundedInt<NumBits, false> op2)
{
    return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(op1.value() + op2.value()) & BoundedInt<NumBits, false>::maxIntValue());
}

template <int NumBits>
BoundedInt<NumBits, false> BoundedModuloSubtract (BoundedInt<NumBits, false> op1, BoundedInt<NumBits, false> op2)
{
    return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(op1.value() - op2.value()) & BoundedInt<NumBits, false>::maxIntValue());
}

template <int NumBits>
BoundedInt<NumBits, false> BoundedModuloNegative (BoundedInt<NumBits, false> op)
{
    return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(-op.value()) & BoundedInt<NumBits, false>::maxIntValue());
}

template <int NumBits>
BoundedInt<NumBits, false> BoundedModuloInc (BoundedInt<NumBits, false> op)
{
    return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(op.value() + 1) & BoundedInt<NumBits, false>::maxIntValue());
}

template <int NumBits>
BoundedInt<NumBits, false> BoundedModuloDec (BoundedInt<NumBits, false> op)
{
    return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(op.value() - 1) & BoundedInt<NumBits, false>::maxIntValue());
}

#endif

## ChooseInt.h
/*
 * Copyright (c) 2013 Ambroz Bizjak
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMBROLIB_CHOOSE_INT_H
#define AMBROLIB_CHOOSE_INT_H

#include <stdint.h>

#include "If.h"

template <int NumBits, bool Signed>
class ChooseInt {
public:
    typedef
        typename If<(Signed && NumBits < 8), int8_t,
        typename If<(Signed && NumBits < 16), int16_t,
#ifdef AMBROLIB_AVR
        typename If<(Signed && NumBits < 24), __int24,
#endif
        typename If<(Signed && NumBits < 32), int32_t,
        typename If<(Signed && NumBits < 64), int64_t,
        typename If<(!Signed && NumBits <= 8), uint8_t,
#ifdef AMBROLIB_AVR
        typename If<(!Signed && NumBits <= 24), __uint24,
#endif
        typename If<(!Signed && NumBits <= 16), uint16_t,
        typename If<(!Signed && NumBits <= 32), uint32_t,
        typename If<(!Signed && NumBits <= 64), uint64_t,
        void>::Type>::Type>::Type>::Type>::Type>::Type>::Type>::Type
#ifdef AMBROLIB_AVR
        >::Type>::Type
#endif
        Type;
};

#endif

## compile.sh
#!/bin/bash

set -e
set -x

CROSS=${CROSS:-avr-}
CXX=${CXX:-${CROSS}g++}
MCU=${MCU:-atmega1284p}
F_CPU=${F_CPU:-20000000}
BAUD=${BAUD:-115200}
CXXFLAGS="-std=c++98 -mmcu=$MCU -DF_CPU=$F_CPU -DBAUD=$BAUD -DNDEBUG -O4 -g3 -gdwarf-2 \
-ffunction-sections -fdata-sections -Wl,--gc-sections \
-D__STDC_LIMIT_MACROS -D__STDC_FORMAT_MACROS -D__STDC_CONSTANT_MACROS \
-DAMBROLIB_AVR -I. $CXXFLAGS"

${CXX} $CXXFLAGS main.cpp -o main.elf -Wl,-u,vfprintf -lprintf_flt -lm
${CROSS}objcopy -j .text -j .data -O ihex main.elf main.hex

## FixedPoint.h
/*
 * Copyright (c) 2013 Ambroz Bizjak
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMBROLIB_FIXED_POINT_H
#define AMBROLIB_FIXED_POINT_H

#include <math.h>

#include "BoundedInt.h"
#include "MinMax.h"
#include "Modulo.h"

template <int NumBits, bool Signed, int Exp>
class FixedPoint {
public:
    typedef BoundedInt<NumBits, Signed> BoundedIntType;
    typedef typename BoundedIntType::IntType IntType;

    static FixedPoint importBoundedBits (BoundedIntType op)
    {
        FixedPoint res;
        res.m_bits = op;
        return res;
    }

    static FixedPoint importBits (IntType op)
    {
        return importBoundedBits(BoundedIntType::import(op));
    }

    BoundedIntType bitsBoundedValue () const
    {
        return m_bits;
    }

    IntType bitsValue () const
    {
        return m_bits.value();
    }

    template <int ShiftExp>
    FixedPoint<NumBits - ShiftExp, Signed, Exp + ShiftExp> shiftBits () const
    {
        return FixedPoint<NumBits - ShiftExp, Signed, Exp + ShiftExp>::importBoundedBits(bitsBoundedValue().template shift<ShiftExp>());
    }

    template <int ShiftExp>
    FixedPoint<NumBits, Signed, Exp + ShiftExp> shift () const
    {
        return FixedPoint<NumBits, Signed, Exp + ShiftExp>::importBoundedBits(bitsBoundedValue());
    }

public:
    BoundedIntType m_bits;
};

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2, int RightShiftBits>
struct FixedPointMultiply {
    typedef FixedPoint<(NumBits1 + NumBits2 - RightShiftBits), (Signed1 || Signed2), (Exp1 + Exp2 + RightShiftBits)> ResultType;

    static ResultType call (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
    {
        return ResultType::importBoundedBits(BoundedMultiply<RightShiftBits>(op1.bitsBoundedValue(), op2.bitsBoundedValue()));
    }
};

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2>
typename FixedPointMultiply<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2, 0>::ResultType operator* (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
{
    return FixedPointMultiply<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2, 0>::call(op1, op2);
}

template <int ResExp, int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2>
inline typename FixedPointMultiply<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2, (ResExp - (Exp1 + Exp2))>::ResultType FixedResMultiply (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
{
    return FixedPointMultiply<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2, (ResExp - (Exp1 + Exp2))>::call(op1, op2);
}

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2>
struct FixedPointAdd {
    static const int shift_op1 = min<int, 0, Exp2 - Exp1>::value;
    static const int shift_op2 = min<int, 0, Exp1 - Exp2>::value;
    static const int numbits_shift_op1 = NumBits1 - shift_op1;
    static const int numbits_shift_op2 = NumBits2 - shift_op2;
    static const int numbits_result = max<int, numbits_shift_op1, numbits_shift_op2>::value + 1;
    static const int exp_result = Exp1 + shift_op1;

    typedef FixedPoint<numbits_result, (Signed1 || Signed2), exp_result> ResultType;

    static ResultType call (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
    {
        return ResultType::importBoundedBits(op1.bitsBoundedValue().template shift<shift_op1>() + op2.bitsBoundedValue().template shift<shift_op2>());
    }
};

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2>
typename FixedPointAdd<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2>::ResultType operator+ (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
{
    return FixedPointAdd<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2>::call(op1, op2);
}

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2>
typename FixedPointAdd<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2>::ResultType operator- (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
{
    return FixedPointAdd<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2>::call(op1, -op2);
}

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2, int LeftShiftBits, int ResSatBits>
struct FixedPointDivide {
    typedef FixedPoint<ResSatBits, (Signed1 || Signed2), (Exp1 - Exp2 - LeftShiftBits)> ResultType;

    static ResultType call (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
    {
        return ResultType::importBoundedBits(BoundedDivide<LeftShiftBits, ResSatBits>(op1.bitsBoundedValue(), op2.bitsBoundedValue()));
    }
};

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2>
typename FixedPointDivide<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2, NumBits2, NumBits2 + Exp2 - Exp1>::ResultType FixedFracDivide (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
{
    return FixedPointDivide<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2, NumBits2, NumBits2 + Exp2 - Exp1>::call(op1, op2);
}

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2>
struct FixedPointCompare {
    static const int shift_op1 = min<int, 0, Exp2 - Exp1>::value;
    static const int shift_op2 = min<int, 0, Exp1 - Exp2>::value;

    static bool lt (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
    {
        return (op1.bitsBoundedValue().template shift<shift_op1>() < op2.bitsBoundedValue().template shift<shift_op2>());
    }

    static bool le (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
    {
        return (op1.bitsBoundedValue().template shift<shift_op1>() <= op2.bitsBoundedValue().template shift<shift_op2>());
    }
};

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2>
bool operator< (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
{
    return FixedPointCompare<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2>::lt(op1, op2);
}

template <int NumBits1, bool Signed1, int Exp1, int NumBits2, bool Signed2, int Exp2>
bool operator<= (FixedPoint<NumBits1, Signed1, Exp1> op1, FixedPoint<NumBits2, Signed2, Exp2> op2)
{
    return FixedPointCompare<NumBits1, Signed1, Exp1, NumBits2, Signed2, Exp2>::le(op1, op2);
}

template <int NumBits, bool Signed, int Exp>
FixedPoint<((NumBits + Modulo<int, Exp, 2>::value + 1) / 2), false, ((Exp - Modulo<int, Exp, 2>::value) / 2)> FixedSquareRoot (FixedPoint<NumBits, Signed, Exp> op)
{
    return FixedPoint<((NumBits + Modulo<int, Exp, 2>::value + 1) / 2), false, ((Exp - Modulo<int, Exp, 2>::value) / 2)>::importBoundedBits(BoundedSquareRoot(op.bitsBoundedValue().template shiftLeft<Modulo<int, Exp, 2>::value>()));
}

#endif

## If.h
/*
 * Copyright (c) 2013 Ambroz Bizjak
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMBROLIB_IF_H
#define AMBROLIB_IF_H

template <bool Cond, typename T1, typename T2>
struct If;

template <typename T1, typename T2>
struct If<true, T1, T2> {
    typedef T1 Type;
};

template <typename T1, typename T2>
struct If<false, T1, T2> {
    typedef T2 Type;
};

#endif

## main.cpp
#include <avr/io.h>
#include <stdio.h>

#include "FixedPoint.h"

struct AxisController {
    struct Command {
        FixedPoint<19, false, 0> x;
        FixedPoint<19, true, 0> a;
        FixedPoint<28, false, 0> t;
        FixedPoint<19, false, 0> x_pos;
    };

    void init ()
    {
        Command *cmd = &m_entries;
        cmd->x = FixedPoint<19, false, 0>::importBits((uint32_t)1 << 17);
        cmd->a = FixedPoint<19, true, 0>::importBits(0);
        cmd->t =  FixedPoint<28, false, 0>::importBits(0);
        cmd->x_pos = FixedPoint<19, false, 0>::importBits(0);

        send_stepper_command();
    }

    void send_stepper_command ()
    {
        Command *cmd = &m_entries;

        FixedPoint<19, false, 0> new_x = FixedPoint<19, false, 0>::importBits(0);
        FixedPoint<13, true, 0> rel_a = FixedPoint<13, true, 0>::importBits(0);

        if (!(cmd->x <= FixedPoint<19, false, 0>::importBits(0) && cmd->t <= FixedPoint<28, false, 0>::importBits(0))) {
            FixedPoint<28, false, 0> new_t = cmd->t;
            FixedPoint<19, false, 0> remain_x = FixedPoint<19, false, 0>::importBits(cmd->x.bitsValue() - cmd->x_pos.bitsValue());
            if (remain_x.bitsValue() > 0) {
                new_x = FixedPoint<19, false, 0>::importBits(cmd->x_pos.bitsValue());
                FixedPoint<19, false, 0> v0 = FixedPoint<19, false, 0>::importBits(cmd->x.bitsValue());
                FixedPoint<38, false, 0> v02 = v0 * v0;
                FixedPoint<41, true, 0> s = v0 + (cmd->a * new_x).shift<2>();
                FixedPoint<22, false, -1> q = (v0 + FixedSquareRoot(s)).shift<-1>();
                FixedPoint<21, false, -21> t_frac = FixedFracDivide(new_x, q);
                new_t = FixedResMultiply<0>(cmd->t, t_frac);
            }

            FixedPoint<22, false, 0> rel_t = FixedPoint<22, false, 0>::importBits(new_t.bitsValue());
            FixedPoint<28, false, -28> gt_frac = FixedFracDivide(rel_t, cmd->t);
            FixedPoint<19, true, 0> a = FixedResMultiply<0>(cmd->a, (gt_frac * gt_frac).shiftBits<12>());
            rel_a = FixedPoint<13, true, 0>::importBits(a.bitsValue());
        }

        printf("%d\n", (unsigned char)rel_a.bitsValue());
        cmd->x_pos = new_x;
        printf("set\n");
    }

    Command m_entries;
};

static AxisController axis_controller0;

FILE uart_output;

static int uart_putchar (char ch, FILE *stream)
{
    while (!(UCSR0A & (1 << UDRE0)));
    UDR0 = ch;
    return 1;
}

static void setup_uart_stdio ()
{
    uint32_t baud = BAUD;
    uint16_t ubrr = (((2 * (uint32_t)F_CPU) + (16 * baud)) / (2 * 16 * baud)) - 1;
    UBRR0H = (ubrr >> 8);
    UBRR0L = ubrr;
    UCSR0A = 0;
    UCSR0B = (1 << RXCIE0) | (1 << RXEN0) | (1 << TXEN0);
    UCSR0C = (1 << UCSZ01) | (1 << UCSZ00);
    uart_output.put = uart_putchar;
    uart_output.flags = _FDEV_SETUP_WRITE;
    stdout = &uart_output;
    stderr = &uart_output;
}

int main ()
{
    setup_uart_stdio();
    printf("HELLO\n");
    axis_controller0.init();
    printf("we survived\n");
    while (1);
}

## MinMax.h
/*
 * Copyright (c) 2013 Ambroz Bizjak
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMBROLIB_MIN_MAX_H
#define AMBROLIB_MIN_MAX_H

template <typename T, T op1, T op2>
struct min {
    static const T value = (op1 < op2) ? op1 : op2;
};

template <typename T, T op1, T op2>
struct max {
    static const T value = (op1 > op2) ? op1 : op2;
};

#endif

## Modulo.h
/*
 * Copyright (c) 2013 Ambroz Bizjak
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMBROLIB_MODULO_H
#define AMBROLIB_MODULO_H

template <typename T, T x, T m>
struct Modulo {
    static const int value = (x >= 0) ? (x % m) : (((x / m) * m) == x) ? 0 : (m - (((x / m) * m) - x));
};

#endif

## PowerOfTwo.h
/*
 * Copyright (c) 2013 Ambroz Bizjak
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMBROLIB_POWER_OF_TWO_H
#define AMBROLIB_POWER_OF_TWO_H

template <typename T, int E>
struct PowerOfTwo {
    static const T value = 2 * PowerOfTwo<T, E - 1>::value;
};

template <typename T>
struct PowerOfTwo<T, 0> {
    static const T value = 1;
};

template <typename T, int E>
struct PowerOfTwoMinusOne {
    static const T value = 2 * PowerOfTwoMinusOne<T, E - 1>::value + 1;
};

template <typename T>
struct PowerOfTwoMinusOne<T, 0> {
    static const T value = 0;
};

#endif
	/*
	* Copyright (c) 2013 Ambroz Bizjak
	* All rights reserved.
	*
	* Redistribution and use in source and binary forms, with or without
	* modification, are permitted provided that the following conditions are met:
	* 1. Redistributions of source code must retain the above copyright
	* notice, this list of conditions and the following disclaimer.
	* 2. Redistributions in binary form must reproduce the above copyright
	* notice, this list of conditions and the following disclaimer in the
	* documentation and/or other materials provided with the distribution.
	*
	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
	* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
	* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
	* DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
	* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
	* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
	* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
	* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
	* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
	* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
	*/

	#ifndef AMBROLIB_BOUNDED_INT_H
	#define AMBROLIB_BOUNDED_INT_H

	#include <stdint.h>

	#include "PowerOfTwo.h"
	#include "MinMax.h"
	#include "Modulo.h"
	#include "ChooseInt.h"

	template <int NumBits, bool Signed = true>
	class BoundedInt {
	public:
	static const int num_bits = NumBits;

	typedef typename ChooseInt<NumBits, Signed>::Type IntType;

	static IntType minIntValue ()
	{
	return Signed ? -PowerOfTwoMinusOne<IntType, NumBits>::value : 0;
	}

	static IntType maxIntValue ()
	{
	return PowerOfTwoMinusOne<IntType, NumBits>::value;
	}

	static BoundedInt minValue ()
	{
	return BoundedInt::import(minIntValue());
	}

	static BoundedInt maxValue ()
	{
	return BoundedInt::import(maxIntValue());
	}

	static BoundedInt import (IntType the_int)
	{
	BoundedInt res;
	res.m_int = the_int;
	return res;
	}

	IntType value () const
	{
	return m_int;
	}

	template <int NewNumBits, bool NewSigned>
	BoundedInt<NewNumBits, NewSigned> convert () const
	{
	return BoundedInt<NewNumBits, NewSigned>::import(value());
	}

	BoundedInt<NumBits, true> toSigned () const
	{
	return BoundedInt<NumBits, true>::import(value());
	}

	BoundedInt<NumBits, false> toUnsignedUnsafe () const
	{
	return BoundedInt<NumBits, false>::import(value());
	}

	template <int ShiftExp>
	BoundedInt<NumBits - ShiftExp, Signed> shiftRight () const
	{
	return BoundedInt<NumBits - ShiftExp, Signed>::import(value() / PowerOfTwo<IntType, ShiftExp>::value);
	}

	template <int ShiftExp>
	BoundedInt<NumBits + ShiftExp, Signed> shiftLeft () const
	{
	return BoundedInt<NumBits + ShiftExp, Signed>::import(value() * PowerOfTwo<typename BoundedInt<NumBits + ShiftExp, Signed>::IntType, ShiftExp>::value);
	}

	template <int ShiftExp>
	BoundedInt<NumBits - ShiftExp, Signed> shift () const
	{
	return ShiftHelper<ShiftExp, (ShiftExp < 0)>::call(*this);
	}

	BoundedInt<NumBits, false> absVal () const
	{
	return BoundedInt<NumBits, false>::import((value() >= 0) ? value() : -value());
	}

	private:
	template <int ShiftExp, bool Left>
	struct ShiftHelper;

	template <int ShiftExp>
	struct ShiftHelper<ShiftExp, false> {
	static BoundedInt<NumBits - ShiftExp, Signed> call (BoundedInt op)
	{
	return op.shiftRight<ShiftExp>();
	}
	};

	template <int ShiftExp>
	struct ShiftHelper<ShiftExp, true> {
	static BoundedInt<NumBits - ShiftExp, Signed> call (BoundedInt op)
	{
	return op.shiftLeft<(-ShiftExp)>();
	}
	};

	public:
	IntType m_int;
	};

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	bool operator== (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 \|\| Signed2)>::Type TempType;
	return ((TempType)op1.value() == (TempType)op2.value());
	}

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	bool operator!= (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 \|\| Signed2)>::Type TempType;
	return ((TempType)op1.value() != (TempType)op2.value());
	}

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	bool operator< (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 \|\| Signed2)>::Type TempType;
	return ((TempType)op1.value() < (TempType)op2.value());
	}

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	bool operator> (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 \|\| Signed2)>::Type TempType;
	return ((TempType)op1.value() > (TempType)op2.value());
	}

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	bool operator<= (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 \|\| Signed2)>::Type TempType;
	return ((TempType)op1.value() <= (TempType)op2.value());
	}

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	bool operator>= (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<max<int, NumBits1, NumBits2>::value, (Signed1 \|\| Signed2)>::Type TempType;
	return ((TempType)op1.value() >= (TempType)op2.value());
	}

	template <int NumBits, bool Signed>
	BoundedInt<NumBits, true> operator- (BoundedInt<NumBits, Signed> op)
	{
	typedef typename ChooseInt<NumBits, true>::Type TempType;
	return BoundedInt<NumBits, true>::import(-(TempType)op.value());
	}

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	BoundedInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 \|\| Signed2)> operator+ (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 \|\| Signed2)>::Type TempType;
	return BoundedInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 \|\| Signed2)>::import((TempType)op1.value() + (TempType)op2.value());
	}

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	BoundedInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 \|\| Signed2)> operator- (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 \|\| Signed2)>::Type TempType;
	return BoundedInt<(max<int, NumBits1, NumBits2>::value + 1), (Signed1 \|\| Signed2)>::import((TempType)op1.value() - (TempType)op2.value());
	}

	template <int RightShift, int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	BoundedInt<(NumBits1 + NumBits2 - RightShift), (Signed1 \|\| Signed2)> BoundedMultiply (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<NumBits1, Signed1>::Type Op1Type;
	typedef typename ChooseInt<NumBits2, Signed2>::Type Op2Type;
	typedef typename ChooseInt<(NumBits1 + NumBits2 - RightShift), (Signed1 \|\| Signed2)>::Type ResType;
	typedef typename ChooseInt<(NumBits1 + NumBits2), (Signed1 \|\| Signed2)>::Type TempResType;

	return BoundedInt<(NumBits1 + NumBits2 - RightShift), (Signed1 \|\| Signed2)>::import(
	((TempResType)op1.value() * (TempResType)op2.value()) / PowerOfTwo<TempResType, RightShift>::value
	);
	}

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	BoundedInt<(NumBits1 + NumBits2), (Signed1 \|\| Signed2)> operator* (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	return BoundedMultiply<0>(op1, op2);
	}

	template <int LeftShift, int ResSatBits, int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	BoundedInt<ResSatBits, (Signed1 \|\| Signed2)> BoundedDivide (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	typedef typename ChooseInt<NumBits1, Signed1>::Type Op1Type;
	typedef typename ChooseInt<NumBits2, Signed2>::Type Op2Type;
	typedef typename ChooseInt<ResSatBits, (Signed1 \|\| Signed2)>::Type ResType;

	typedef typename ChooseInt<(NumBits1 + LeftShift), (Signed1 \|\| Signed2)>::Type TempResType;
	typedef typename ChooseInt<NumBits2, (Signed1 \|\| Signed2)>::Type TempType2;

	TempResType res = (((TempResType)op1.value() * PowerOfTwo<TempResType, LeftShift>::value) / (TempType2)op2.value());
	if (ResSatBits < NumBits1 + LeftShift) {
	if (res > PowerOfTwoMinusOne<ResType, ResSatBits>::value) {
	res = PowerOfTwoMinusOne<ResType, ResSatBits>::value;
	} else if (Signed1 \|\| Signed2) {
	if (res < -PowerOfTwoMinusOne<ResType, ResSatBits>::value) {
	res = -PowerOfTwoMinusOne<ResType, ResSatBits>::value;
	}
	}
	}

	return BoundedInt<ResSatBits, (Signed1 \|\| Signed2)>::import(res);
	}

	template <int NumBits1, bool Signed1, int NumBits2, bool Signed2>
	BoundedInt<NumBits1, (Signed1 \|\| Signed2)> operator/ (BoundedInt<NumBits1, Signed1> op1, BoundedInt<NumBits2, Signed2> op2)
	{
	return BoundedDivide<0, NumBits1>(op1, op2);
	}

	template <int NumBits, bool Signed>
	BoundedInt<((NumBits + 1) / 2), false> BoundedSquareRoot (BoundedInt<NumBits, Signed> op1)
	{
	static const int TempBits = NumBits + Modulo<int, NumBits, 2>::value;
	typedef typename ChooseInt<TempBits, false>::Type TempType;

	TempType op = op1.value();
	TempType res = 0;
	TempType one = PowerOfTwo<TempType, (TempBits - 2)>::value;

	while (one > op) {
	one >>= 2;
	}

	while (one != 0) {
	if (op >= res + one) {
	op = op - (res + one);
	res = res + 2 * one;
	}
	res >>= 1;
	one >>= 2;
	}

	return BoundedInt<((NumBits + 1) / 2), false>::import(res);
	}

	template <int NumBits>
	BoundedInt<NumBits, false> BoundedModuloAdd (BoundedInt<NumBits, false> op1, BoundedInt<NumBits, false> op2)
	{
	return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(op1.value() + op2.value()) & BoundedInt<NumBits, false>::maxIntValue());
	}

	template <int NumBits>
	BoundedInt<NumBits, false> BoundedModuloSubtract (BoundedInt<NumBits, false> op1, BoundedInt<NumBits, false> op2)
	{
	return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(op1.value() - op2.value()) & BoundedInt<NumBits, false>::maxIntValue());
	}

	template <int NumBits>
	BoundedInt<NumBits, false> BoundedModuloNegative (BoundedInt<NumBits, false> op)
	{
	return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(-op.value()) & BoundedInt<NumBits, false>::maxIntValue());
	}

	template <int NumBits>
	BoundedInt<NumBits, false> BoundedModuloInc (BoundedInt<NumBits, false> op)
	{
	return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(op.value() + 1) & BoundedInt<NumBits, false>::maxIntValue());
	}

	template <int NumBits>
	BoundedInt<NumBits, false> BoundedModuloDec (BoundedInt<NumBits, false> op)
	{
	return BoundedInt<NumBits, false>::import((typename BoundedInt<NumBits, false>::IntType)(op.value() - 1) & BoundedInt<NumBits, false>::maxIntValue());
	}

	#endif
	#!/bin/bash

	set -e
	set -x

	CROSS=${CROSS:-avr-}
	CXX=${CXX:-${CROSS}g++}
	MCU=${MCU:-atmega1284p}
	F_CPU=${F_CPU:-20000000}
	BAUD=${BAUD:-115200}
	CXXFLAGS="-std=c++98 -mmcu=$MCU -DF_CPU=$F_CPU -DBAUD=$BAUD -DNDEBUG -O4 -g3 -gdwarf-2 \
	-ffunction-sections -fdata-sections -Wl,--gc-sections \
	-D__STDC_LIMIT_MACROS -D__STDC_FORMAT_MACROS -D__STDC_CONSTANT_MACROS \
	-DAMBROLIB_AVR -I. $CXXFLAGS"

	${CXX} $CXXFLAGS main.cpp -o main.elf -Wl,-u,vfprintf -lprintf_flt -lm
	${CROSS}objcopy -j .text -j .data -O ihex main.elf main.hex
	#include <avr/io.h>
	#include <stdio.h>

	#include "FixedPoint.h"

	struct AxisController {
	struct Command {
	FixedPoint<19, false, 0> x;
	FixedPoint<19, true, 0> a;
	FixedPoint<28, false, 0> t;
	FixedPoint<19, false, 0> x_pos;
	};

	void init ()
	{
	Command *cmd = &m_entries;
	cmd->x = FixedPoint<19, false, 0>::importBits((uint32_t)1 << 17);
	cmd->a = FixedPoint<19, true, 0>::importBits(0);
	cmd->t = FixedPoint<28, false, 0>::importBits(0);
	cmd->x_pos = FixedPoint<19, false, 0>::importBits(0);

	send_stepper_command();
	}

	void send_stepper_command ()
	{
	Command *cmd = &m_entries;

	FixedPoint<19, false, 0> new_x = FixedPoint<19, false, 0>::importBits(0);
	FixedPoint<13, true, 0> rel_a = FixedPoint<13, true, 0>::importBits(0);

	if (!(cmd->x <= FixedPoint<19, false, 0>::importBits(0) && cmd->t <= FixedPoint<28, false, 0>::importBits(0))) {
	FixedPoint<28, false, 0> new_t = cmd->t;
	FixedPoint<19, false, 0> remain_x = FixedPoint<19, false, 0>::importBits(cmd->x.bitsValue() - cmd->x_pos.bitsValue());
	if (remain_x.bitsValue() > 0) {
	new_x = FixedPoint<19, false, 0>::importBits(cmd->x_pos.bitsValue());
	FixedPoint<19, false, 0> v0 = FixedPoint<19, false, 0>::importBits(cmd->x.bitsValue());
	FixedPoint<38, false, 0> v02 = v0 * v0;
	FixedPoint<41, true, 0> s = v0 + (cmd->a * new_x).shift<2>();
	FixedPoint<22, false, -1> q = (v0 + FixedSquareRoot(s)).shift<-1>();
	FixedPoint<21, false, -21> t_frac = FixedFracDivide(new_x, q);
	new_t = FixedResMultiply<0>(cmd->t, t_frac);
	}

	FixedPoint<22, false, 0> rel_t = FixedPoint<22, false, 0>::importBits(new_t.bitsValue());
	FixedPoint<28, false, -28> gt_frac = FixedFracDivide(rel_t, cmd->t);
	FixedPoint<19, true, 0> a = FixedResMultiply<0>(cmd->a, (gt_frac * gt_frac).shiftBits<12>());
	rel_a = FixedPoint<13, true, 0>::importBits(a.bitsValue());
	}

	printf("%d\n", (unsigned char)rel_a.bitsValue());
	cmd->x_pos = new_x;
	printf("set\n");
	}

	Command m_entries;
	};

	static AxisController axis_controller0;

	FILE uart_output;

	static int uart_putchar (char ch, FILE *stream)
	{
	while (!(UCSR0A & (1 << UDRE0)));
	UDR0 = ch;
	return 1;
	}

	static void setup_uart_stdio ()
	{
	uint32_t baud = BAUD;
	uint16_t ubrr = (((2 * (uint32_t)F_CPU) + (16 * baud)) / (2 * 16 * baud)) - 1;
	UBRR0H = (ubrr >> 8);
	UBRR0L = ubrr;
	UCSR0A = 0;
	UCSR0B = (1 << RXCIE0) \| (1 << RXEN0) \| (1 << TXEN0);
	UCSR0C = (1 << UCSZ01) \| (1 << UCSZ00);
	uart_output.put = uart_putchar;
	uart_output.flags = _FDEV_SETUP_WRITE;
	stdout = &uart_output;
	stderr = &uart_output;
	}

	int main ()
	{
	setup_uart_stdio();
	printf("HELLO\n");
	axis_controller0.init();
	printf("we survived\n");
	while (1);
	}